16 旋转操作模块（rotation.rs）

一、rotation.rs源码

use crate::approxeq::ApproxEq;
use crate::trig::Trig;
use crate::{point2, point3, vec3, Angle, Point2D, Point3D, Vector2D, Vector3D};
use crate::{Transform2D, Transform3D, UnknownUnit};

use core::cmp::{Eq, PartialEq};
use core::fmt;
use core::hash::Hash;
use core::marker::PhantomData;
use core::ops::{Add, Mul, Neg, Sub};

#[cfg(feature = "bytemuck")]
use bytemuck::{Pod, Zeroable};
use num_traits::real::Real;
use num_traits::{NumCast, One, Zero};
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};

/// A transform that can represent rotations in 2d, represented as an angle in radians.
#[repr(C)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[cfg_attr(
    feature = "serde",
    serde(bound(
        serialize = "T: serde::Serialize",
        deserialize = "T: serde::Deserialize<'de>"
    ))
)]
pub struct Rotation2D<T, Src, Dst> {
    /// Angle in radians
    pub angle: T,
    #[doc(hidden)]
    pub _unit: PhantomData<(Src, Dst)>,
}

impl<T: Copy, Src, Dst> Copy for Rotation2D<T, Src, Dst> {}

impl<T: Clone, Src, Dst> Clone for Rotation2D<T, Src, Dst> {
    fn clone(&self) -> Self {
        Rotation2D {
            angle: self.angle.clone(),
            _unit: PhantomData,
        }
    }
}

impl<T, Src, Dst> Eq for Rotation2D<T, Src, Dst> where T: Eq {}

impl<T, Src, Dst> PartialEq for Rotation2D<T, Src, Dst>
where
    T: PartialEq,
{
    fn eq(&self, other: &Self) -> bool {
        self.angle == other.angle
    }
}

impl<T, Src, Dst> Hash for Rotation2D<T, Src, Dst>
where
    T: Hash,
{
    fn hash<H: core::hash::Hasher>(&self, h: &mut H) {
        self.angle.hash(h);
    }
}

#[cfg(feature = "arbitrary")]
impl<'a, T, Src, Dst> arbitrary::Arbitrary<'a> for Rotation2D<T, Src, Dst>
where
    T: arbitrary::Arbitrary<'a>,
{
    fn arbitrary(u: &mut arbitrary::Unstructured<'a>) -> arbitrary::Result<Self> {
        Ok(Rotation2D::new(arbitrary::Arbitrary::arbitrary(u)?))
    }
}

#[cfg(feature = "bytemuck")]
unsafe impl<T: Zeroable, Src, Dst> Zeroable for Rotation2D<T, Src, Dst> {}

#[cfg(feature = "bytemuck")]
unsafe impl<T: Pod, Src: 'static, Dst: 'static> Pod for Rotation2D<T, Src, Dst> {}

impl<T, Src, Dst> Rotation2D<T, Src, Dst> {
    /// Creates a rotation from an angle in radians.
    #[inline]
    pub fn new(angle: Angle<T>) -> Self {
        Rotation2D {
            angle: angle.radians,
            _unit: PhantomData,
        }
    }

    /// Creates a rotation from an angle in radians.
    pub fn radians(angle: T) -> Self {
        Self::new(Angle::radians(angle))
    }

    /// Creates the identity rotation.
    #[inline]
    pub fn identity() -> Self
    where
        T: Zero,
    {
        Self::radians(T::zero())
    }
}

impl<T: Copy, Src, Dst> Rotation2D<T, Src, Dst> {
    /// Cast the unit, preserving the numeric value.
    ///
    /// # Example
    ///
    /// ```rust
    /// # use euclid::Rotation2D;
    /// enum Local {}
    /// enum World {}
    ///
    /// enum Local2 {}
    /// enum World2 {}
    ///
    /// let to_world: Rotation2D<_, Local, World> = Rotation2D::radians(42);
    ///
    /// assert_eq!(to_world.angle, to_world.cast_unit::().angle);
    /// ```
    #[inline]
    pub fn cast_unit<Src2, Dst2>(&self) -> Rotation2D<T, Src2, Dst2> {
        Rotation2D {
            angle: self.angle,
            _unit: PhantomData,
        }
    }

    /// Drop the units, preserving only the numeric value.
    ///
    /// # Example
    ///
    /// ```rust
    /// # use euclid::Rotation2D;
    /// enum Local {}
    /// enum World {}
    ///
    /// let to_world: Rotation2D<_, Local, World> = Rotation2D::radians(42);
    ///
    /// assert_eq!(to_world.angle, to_world.to_untyped().angle);
    /// ```
    #[inline]
    pub fn to_untyped(&self) -> Rotation2D<T, UnknownUnit, UnknownUnit> {
        self.cast_unit()
    }

    /// Tag a unitless value with units.
    ///
    /// # Example
    ///
    /// ```rust
    /// # use euclid::Rotation2D;
    /// use euclid::UnknownUnit;
    /// enum Local {}
    /// enum World {}
    ///
    /// let rot: Rotation2D<_, UnknownUnit, UnknownUnit> = Rotation2D::radians(42);
    ///
    /// assert_eq!(rot.angle, Rotation2D::<_, Local, World>::from_untyped(&rot).angle);
    /// ```
    #[inline]
    pub fn from_untyped(r: &Rotation2D<T, UnknownUnit, UnknownUnit>) -> Self {
        r.cast_unit()
    }
}

impl<T, Src, Dst> Rotation2D<T, Src, Dst>
where
    T: Copy,
{
    /// Returns self.angle as a strongly typed `Angle`.
    pub fn get_angle(&self) -> Angle<T> {
        Angle::radians(self.angle)
    }
}

impl<T: Real, Src, Dst> Rotation2D<T, Src, Dst> {
    /// Creates a 3d rotation (around the z axis) from this 2d rotation.
    #[inline]
    pub fn to_3d(&self) -> Rotation3D<T, Src, Dst> {
        Rotation3D::around_z(self.get_angle())
    }

    /// Returns the inverse of this rotation.
    #[inline]
    pub fn inverse(&self) -> Rotation2D<T, Dst, Src> {
        Rotation2D::radians(-self.angle)
    }

    /// Returns a rotation representing the other rotation followed by this rotation.
    #[inline]
    pub fn then<NewSrc>(&self, other: &Rotation2D<T, NewSrc, Src>) -> Rotation2D<T, NewSrc, Dst> {
        Rotation2D::radians(self.angle + other.angle)
    }

    /// Returns the given 2d point transformed by this rotation.
    ///
    /// The input point must be use the unit Src, and the returned point has the unit Dst.
    #[inline]
    pub fn transform_point(&self, point: Point2D<T, Src>) -> Point2D<T, Dst> {
        let (sin, cos) = Real::sin_cos(self.angle);
        point2(point.x * cos - point.y * sin, point.y * cos + point.x * sin)
    }

    /// Returns the given 2d vector transformed by this rotation.
    ///
    /// The input point must be use the unit Src, and the returned point has the unit Dst.
    #[inline]
    pub fn transform_vector(&self, vector: Vector2D<T, Src>) -> Vector2D<T, Dst> {
        self.transform_point(vector.to_point()).to_vector()
    }
}

impl<T, Src, Dst> Rotation2D<T, Src, Dst>
where
    T: Copy + Add<Output = T> + Sub<Output = T> + Mul<Output = T> + Zero + Trig,
{
    /// Returns the matrix representation of this rotation.
    #[inline]
    pub fn to_transform(&self) -> Transform2D<T, Src, Dst> {
        Transform2D::rotation(self.get_angle())
    }
}

impl<T: fmt::Debug, Src, Dst> fmt::Debug for Rotation2D<T, Src, Dst> {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "Rotation({:?} rad)", self.angle)
    }
}

impl<T, Src, Dst> ApproxEq<T> for Rotation2D<T, Src, Dst>
where
    T: Copy + Neg<Output = T> + ApproxEq<T>,
{
    fn approx_epsilon() -> T {
        T::approx_epsilon()
    }

    fn approx_eq_eps(&self, other: &Self, eps: &T) -> bool {
        self.angle.approx_eq_eps(&other.angle, eps)
    }
}

/// A transform that can represent rotations in 3d, represented as a quaternion.
///
/// Most methods expect the quaternion to be normalized.
/// When in doubt, use [`unit_quaternion`] instead of [`quaternion`] to create
/// a rotation as the former will ensure that its result is normalized.
///
/// Some people use the `x, y, z, w` (or `w, x, y, z`) notations. The equivalence is
/// as follows: `x -> i`, `y -> j`, `z -> k`, `w -> r`.
/// The memory layout of this type corresponds to the `x, y, z, w` notation
///
/// [`quaternion`]: Self::quaternion
/// [`unit_quaternion`]: Self::unit_quaternion
#[repr(C)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[cfg_attr(
    feature = "serde",
    serde(bound(
        serialize = "T: serde::Serialize",
        deserialize = "T: serde::Deserialize<'de>"
    ))
)]
pub struct Rotation3D<T, Src, Dst> {
    /// Component multiplied by the imaginary number `i`.
    pub i: T,
    /// Component multiplied by the imaginary number `j`.
    pub j: T,
    /// Component multiplied by the imaginary number `k`.
    pub k: T,
    /// The real part.
    pub r: T,
    #[doc(hidden)]
    pub _unit: PhantomData<(Src, Dst)>,
}

impl<T: Copy, Src, Dst> Copy for Rotation3D<T, Src, Dst> {}

impl<T: Clone, Src, Dst> Clone for Rotation3D<T, Src, Dst> {
    fn clone(&self) -> Self {
        Rotation3D {
            i: self.i.clone(),
            j: self.j.clone(),
            k: self.k.clone(),
            r: self.r.clone(),
            _unit: PhantomData,
        }
    }
}

impl<T, Src, Dst> Eq for Rotation3D<T, Src, Dst> where T: Eq {}

impl<T, Src, Dst> PartialEq for Rotation3D<T, Src, Dst>
where
    T: PartialEq,
{
    fn eq(&self, other: &Self) -> bool {
        self.i == other.i && self.j == other.j && self.k == other.k && self.r == other.r
    }
}

impl<T, Src, Dst> Hash for Rotation3D<T, Src, Dst>
where
    T: Hash,
{
    fn hash<H: core::hash::Hasher>(&self, h: &mut H) {
        self.i.hash(h);
        self.j.hash(h);
        self.k.hash(h);
        self.r.hash(h);
    }
}

/// Note: the quaternions produced by this implementation are not normalized
/// (nor even necessarily finite). That is, this is not appropriate to use to
/// choose an actual “arbitrary rotation”, at least not without postprocessing.
#[cfg(feature = "arbitrary")]
impl<'a, T, Src, Dst> arbitrary::Arbitrary<'a> for Rotation3D<T, Src, Dst>
where
    T: arbitrary::Arbitrary<'a>,
{
    fn arbitrary(u: &mut arbitrary::Unstructured<'a>) -> arbitrary::Result<Self> {
        let (i, j, k, r) = arbitrary::Arbitrary::arbitrary(u)?;
        Ok(Rotation3D::quaternion(i, j, k, r))
    }
}

#[cfg(feature = "bytemuck")]
unsafe impl<T: Zeroable, Src, Dst> Zeroable for Rotation3D<T, Src, Dst> {}

#[cfg(feature = "bytemuck")]
unsafe impl<T: Pod, Src: 'static, Dst: 'static> Pod for Rotation3D<T, Src, Dst> {}

impl<T, Src, Dst> Rotation3D<T, Src, Dst> {
    /// Creates a rotation around from a quaternion representation.
    ///
    /// The parameters are a, b, c and r compose the quaternion `a*i + b*j + c*k + r`
    /// where `a`, `b` and `c` describe the vector part and the last parameter `r` is
    /// the real part.
    ///
    /// The resulting quaternion is not necessarily normalized. See [`unit_quaternion`].
    ///
    /// [`unit_quaternion`]: Self::unit_quaternion
    #[inline]
    pub fn quaternion(a: T, b: T, c: T, r: T) -> Self {
        Rotation3D {
            i: a,
            j: b,
            k: c,
            r,
            _unit: PhantomData,
        }
    }

    /// Creates the identity rotation.
    #[inline]
    pub fn identity() -> Self
    where
        T: Zero + One,
    {
        Self::quaternion(T::zero(), T::zero(), T::zero(), T::one())
    }
}

impl<T, Src, Dst> Rotation3D<T, Src, Dst>
where
    T: Copy,
{
    /// Returns the vector part (i, j, k) of this quaternion.
    #[inline]
    pub fn vector_part(&self) -> Vector3D<T, UnknownUnit> {
        vec3(self.i, self.j, self.k)
    }

    /// Cast the unit, preserving the numeric value.
    ///
    /// # Example
    ///
    /// ```rust
    /// # use euclid::Rotation3D;
    /// enum Local {}
    /// enum World {}
    ///
    /// enum Local2 {}
    /// enum World2 {}
    ///
    /// let to_world: Rotation3D<_, Local, World> = Rotation3D::quaternion(1, 2, 3, 4);
    ///
    /// assert_eq!(to_world.i, to_world.cast_unit::().i);
    /// assert_eq!(to_world.j, to_world.cast_unit::().j);
    /// assert_eq!(to_world.k, to_world.cast_unit::().k);
    /// assert_eq!(to_world.r, to_world.cast_unit::().r);
    /// ```
    #[inline]
    pub fn cast_unit<Src2, Dst2>(&self) -> Rotation3D<T, Src2, Dst2> {
        Rotation3D {
            i: self.i,
            j: self.j,
            k: self.k,
            r: self.r,
            _unit: PhantomData,
        }
    }

    /// Drop the units, preserving only the numeric value.
    ///
    /// # Example
    ///
    /// ```rust
    /// # use euclid::Rotation3D;
    /// enum Local {}
    /// enum World {}
    ///
    /// let to_world: Rotation3D<_, Local, World> = Rotation3D::quaternion(1, 2, 3, 4);
    ///
    /// assert_eq!(to_world.i, to_world.to_untyped().i);
    /// assert_eq!(to_world.j, to_world.to_untyped().j);
    /// assert_eq!(to_world.k, to_world.to_untyped().k);
    /// assert_eq!(to_world.r, to_world.to_untyped().r);
    /// ```
    #[inline]
    pub fn to_untyped(&self) -> Rotation3D<T, UnknownUnit, UnknownUnit> {
        self.cast_unit()
    }

    /// Tag a unitless value with units.
    ///
    /// # Example
    ///
    /// ```rust
    /// # use euclid::Rotation3D;
    /// use euclid::UnknownUnit;
    /// enum Local {}
    /// enum World {}
    ///
    /// let rot: Rotation3D<_, UnknownUnit, UnknownUnit> = Rotation3D::quaternion(1, 2, 3, 4);
    ///
    /// assert_eq!(rot.i, Rotation3D::<_, Local, World>::from_untyped(&rot).i);
    /// assert_eq!(rot.j, Rotation3D::<_, Local, World>::from_untyped(&rot).j);
    /// assert_eq!(rot.k, Rotation3D::<_, Local, World>::from_untyped(&rot).k);
    /// assert_eq!(rot.r, Rotation3D::<_, Local, World>::from_untyped(&rot).r);
    /// ```
    #[inline]
    pub fn from_untyped(r: &Rotation3D<T, UnknownUnit, UnknownUnit>) -> Self {
        r.cast_unit()
    }
}

impl<T, Src, Dst> Rotation3D<T, Src, Dst>
where
    T: Real,
{
    /// Creates a rotation around from a quaternion representation and normalizes it.
    ///
    /// The parameters are a, b, c and r compose the quaternion `a*i + b*j + c*k + r`
    /// before normalization, where `a`, `b` and `c` describe the vector part and the
    /// last parameter `r` is the real part.
    #[inline]
    pub fn unit_quaternion(i: T, j: T, k: T, r: T) -> Self {
        Self::quaternion(i, j, k, r).normalize()
    }

    /// Creates a rotation around a given axis.
    pub fn around_axis(axis: Vector3D<T, Src>, angle: Angle<T>) -> Self {
        let axis = axis.normalize();
        let two = T::one() + T::one();
        let (sin, cos) = Angle::sin_cos(angle / two);
        Self::quaternion(axis.x * sin, axis.y * sin, axis.z * sin, cos)
    }

    /// Creates a rotation around the x axis.
    pub fn around_x(angle: Angle<T>) -> Self {
        let zero = Zero::zero();
        let two = T::one() + T::one();
        let (sin, cos) = Angle::sin_cos(angle / two);
        Self::quaternion(sin, zero, zero, cos)
    }

    /// Creates a rotation around the y axis.
    pub fn around_y(angle: Angle<T>) -> Self {
        let zero = Zero::zero();
        let two = T::one() + T::one();
        let (sin, cos) = Angle::sin_cos(angle / two);
        Self::quaternion(zero, sin, zero, cos)
    }

    /// Creates a rotation around the z axis.
    pub fn around_z(angle: Angle<T>) -> Self {
        let zero = Zero::zero();
        let two = T::one() + T::one();
        let (sin, cos) = Angle::sin_cos(angle / two);
        Self::quaternion(zero, zero, sin, cos)
    }

    /// Creates a rotation from Euler angles.
    ///
    /// The rotations are applied in roll then pitch then yaw order.
    ///
    ///  - Roll (also called bank) is a rotation around the x axis.
    ///  - Pitch (also called bearing) is a rotation around the y axis.
    ///  - Yaw (also called heading) is a rotation around the z axis.
    pub fn euler(roll: Angle<T>, pitch: Angle<T>, yaw: Angle<T>) -> Self {
        let half = T::one() / (T::one() + T::one());

        let (sy, cy) = Real::sin_cos(half * yaw.get());
        let (sp, cp) = Real::sin_cos(half * pitch.get());
        let (sr, cr) = Real::sin_cos(half * roll.get());

        Self::quaternion(
            cy * sr * cp - sy * cr * sp,
            cy * cr * sp + sy * sr * cp,
            sy * cr * cp - cy * sr * sp,
            cy * cr * cp + sy * sr * sp,
        )
    }

    /// Returns the inverse of this rotation.
    #[inline]
    pub fn inverse(&self) -> Rotation3D<T, Dst, Src> {
        Rotation3D::quaternion(-self.i, -self.j, -self.k, self.r)
    }

    /// Computes the norm of this quaternion.
    #[inline]
    pub fn norm(&self) -> T {
        self.square_norm().sqrt()
    }

    /// Computes the squared norm of this quaternion.
    #[inline]
    pub fn square_norm(&self) -> T {
        self.i * self.i + self.j * self.j + self.k * self.k + self.r * self.r
    }

    /// Returns a [unit quaternion] from this one.
    ///
    /// [unit quaternion]: https://en.wikipedia.org/wiki/Quaternion#Unit_quaternion
    #[inline]
    pub fn normalize(&self) -> Self {
        self.mul(T::one() / self.norm())
    }

    /// Returns `true` if [norm] of this quaternion is (approximately) one.
    ///
    /// [norm]: Self::norm
    #[inline]
    pub fn is_normalized(&self) -> bool
    where
        T: ApproxEq<T>,
    {
        let eps = NumCast::from(1.0e-5).unwrap();
        self.square_norm().approx_eq_eps(&T::one(), &eps)
    }

    /// Spherical linear interpolation between this rotation and another rotation.
    ///
    /// `t` is expected to be between zero and one.
    pub fn slerp(&self, other: &Self, t: T) -> Self
    where
        T: ApproxEq<T>,
    {
        debug_assert!(self.is_normalized());
        debug_assert!(other.is_normalized());

        let r1 = *self;
        let mut r2 = *other;

        let mut dot = r1.i * r2.i + r1.j * r2.j + r1.k * r2.k + r1.r * r2.r;

        let one = T::one();

        if dot.approx_eq(&T::one()) {
            // If the inputs are too close, linearly interpolate to avoid precision issues.
            return r1.lerp(&r2, t);
        }

        // If the dot product is negative, the quaternions
        // have opposite handed-ness and slerp won't take
        // the shorter path. Fix by reversing one quaternion.
        if dot < T::zero() {
            r2 = r2.mul(-T::one());
            dot = -dot;
        }

        // For robustness, stay within the domain of acos.
        dot = Real::min(dot, one);

        // Angle between r1 and the result.
        let theta = Real::acos(dot) * t;

        // r1 and r3 form an orthonormal basis.
        let r3 = r2.sub(r1.mul(dot)).normalize();
        let (sin, cos) = Real::sin_cos(theta);
        r1.mul(cos).add(r3.mul(sin))
    }

    /// Basic Linear interpolation between this rotation and another rotation.
    #[inline]
    pub fn lerp(&self, other: &Self, t: T) -> Self {
        let one_t = T::one() - t;
        self.mul(one_t).add(other.mul(t)).normalize()
    }

    /// Returns the given 3d point transformed by this rotation.
    ///
    /// The input point must be use the unit Src, and the returned point has the unit Dst.
    pub fn transform_point3d(&self, point: Point3D<T, Src>) -> Point3D<T, Dst>
    where
        T: ApproxEq<T>,
    {
        debug_assert!(self.is_normalized());

        let two = T::one() + T::one();
        let cross = self.vector_part().cross(point.to_vector().to_untyped()) * two;

        point3(
            point.x + self.r * cross.x + self.j * cross.z - self.k * cross.y,
            point.y + self.r * cross.y + self.k * cross.x - self.i * cross.z,
            point.z + self.r * cross.z + self.i * cross.y - self.j * cross.x,
        )
    }

    /// Returns the given 2d point transformed by this rotation then projected on the xy plane.
    ///
    /// The input point must be use the unit Src, and the returned point has the unit Dst.
    #[inline]
    pub fn transform_point2d(&self, point: Point2D<T, Src>) -> Point2D<T, Dst>
    where
        T: ApproxEq<T>,
    {
        self.transform_point3d(point.to_3d()).xy()
    }

    /// Returns the given 3d vector transformed by this rotation.
    ///
    /// The input vector must be use the unit Src, and the returned point has the unit Dst.
    #[inline]
    pub fn transform_vector3d(&self, vector: Vector3D<T, Src>) -> Vector3D<T, Dst>
    where
        T: ApproxEq<T>,
    {
        self.transform_point3d(vector.to_point()).to_vector()
    }

    /// Returns the given 2d vector transformed by this rotation then projected on the xy plane.
    ///
    /// The input vector must be use the unit Src, and the returned point has the unit Dst.
    #[inline]
    pub fn transform_vector2d(&self, vector: Vector2D<T, Src>) -> Vector2D<T, Dst>
    where
        T: ApproxEq<T>,
    {
        self.transform_vector3d(vector.to_3d()).xy()
    }

    /// Returns the matrix representation of this rotation.
    #[inline]
    #[rustfmt::skip]
    pub fn to_transform(&self) -> Transform3D<T, Src, Dst>
    where
        T: ApproxEq<T>,
    {
        debug_assert!(self.is_normalized());

        let i2 = self.i + self.i;
        let j2 = self.j + self.j;
        let k2 = self.k + self.k;
        let ii = self.i * i2;
        let ij = self.i * j2;
        let ik = self.i * k2;
        let jj = self.j * j2;
        let jk = self.j * k2;
        let kk = self.k * k2;
        let ri = self.r * i2;
        let rj = self.r * j2;
        let rk = self.r * k2;

        let one = T::one();
        let zero = T::zero();

        let m11 = one - (jj + kk);
        let m12 = ij + rk;
        let m13 = ik - rj;

        let m21 = ij - rk;
        let m22 = one - (ii + kk);
        let m23 = jk + ri;

        let m31 = ik + rj;
        let m32 = jk - ri;
        let m33 = one - (ii + jj);

        Transform3D::new(
            m11, m12, m13, zero,
            m21, m22, m23, zero,
            m31, m32, m33, zero,
            zero, zero, zero, one,
        )
    }

    /// Returns a rotation representing this rotation followed by the other rotation.
    #[inline]
    pub fn then<NewDst>(&self, other: &Rotation3D<T, Dst, NewDst>) -> Rotation3D<T, Src, NewDst>
    where
        T: ApproxEq<T>,
    {
        debug_assert!(self.is_normalized());
        Rotation3D::quaternion(
            other.i * self.r + other.r * self.i + other.j * self.k - other.k * self.j,
            other.j * self.r + other.r * self.j + other.k * self.i - other.i * self.k,
            other.k * self.r + other.r * self.k + other.i * self.j - other.j * self.i,
            other.r * self.r - other.i * self.i - other.j * self.j - other.k * self.k,
        )
    }

    // add, sub and mul are used internally for intermediate computation but aren't public
    // because they don't carry real semantic meanings (I think?).

    #[inline]
    fn add(&self, other: Self) -> Self {
        Self::quaternion(
            self.i + other.i,
            self.j + other.j,
            self.k + other.k,
            self.r + other.r,
        )
    }

    #[inline]
    fn sub(&self, other: Self) -> Self {
        Self::quaternion(
            self.i - other.i,
            self.j - other.j,
            self.k - other.k,
            self.r - other.r,
        )
    }

    #[inline]
    fn mul(&self, factor: T) -> Self {
        Self::quaternion(
            self.i * factor,
            self.j * factor,
            self.k * factor,
            self.r * factor,
        )
    }
}

impl<T: fmt::Debug, Src, Dst> fmt::Debug for Rotation3D<T, Src, Dst> {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(
            f,
            "Quat({:?}*i + {:?}*j + {:?}*k + {:?})",
            self.i, self.j, self.k, self.r
        )
    }
}

impl<T, Src, Dst> ApproxEq<T> for Rotation3D<T, Src, Dst>
where
    T: Copy + Neg<Output = T> + ApproxEq<T>,
{
    fn approx_epsilon() -> T {
        T::approx_epsilon()
    }

    fn approx_eq_eps(&self, other: &Self, eps: &T) -> bool {
        (self.i.approx_eq_eps(&other.i, eps)
            && self.j.approx_eq_eps(&other.j, eps)
            && self.k.approx_eq_eps(&other.k, eps)
            && self.r.approx_eq_eps(&other.r, eps))
            || (self.i.approx_eq_eps(&-other.i, eps)
                && self.j.approx_eq_eps(&-other.j, eps)
                && self.k.approx_eq_eps(&-other.k, eps)
                && self.r.approx_eq_eps(&-other.r, eps))
    }
}

#[test]
fn simple_rotation_2d() {
    use crate::default::Rotation2D;
    use core::f32::consts::{FRAC_PI_2, PI};

    let ri = Rotation2D::identity();
    let r90 = Rotation2D::radians(FRAC_PI_2);
    let rm90 = Rotation2D::radians(-FRAC_PI_2);
    let r180 = Rotation2D::radians(PI);

    assert!(ri
        .transform_point(point2(1.0, 2.0))
        .approx_eq(&point2(1.0, 2.0)));
    assert!(r90
        .transform_point(point2(1.0, 2.0))
        .approx_eq(&point2(-2.0, 1.0)));
    assert!(rm90
        .transform_point(point2(1.0, 2.0))
        .approx_eq(&point2(2.0, -1.0)));
    assert!(r180
        .transform_point(point2(1.0, 2.0))
        .approx_eq(&point2(-1.0, -2.0)));

    assert!(r90
        .inverse()
        .inverse()
        .transform_point(point2(1.0, 2.0))
        .approx_eq(&r90.transform_point(point2(1.0, 2.0))));
}

#[test]
fn simple_rotation_3d_in_2d() {
    use crate::default::Rotation3D;
    use core::f32::consts::{FRAC_PI_2, PI};

    let ri = Rotation3D::identity();
    let r90 = Rotation3D::around_z(Angle::radians(FRAC_PI_2));
    let rm90 = Rotation3D::around_z(Angle::radians(-FRAC_PI_2));
    let r180 = Rotation3D::around_z(Angle::radians(PI));

    assert!(ri
        .transform_point2d(point2(1.0, 2.0))
        .approx_eq(&point2(1.0, 2.0)));
    assert!(r90
        .transform_point2d(point2(1.0, 2.0))
        .approx_eq(&point2(-2.0, 1.0)));
    assert!(rm90
        .transform_point2d(point2(1.0, 2.0))
        .approx_eq(&point2(2.0, -1.0)));
    assert!(r180
        .transform_point2d(point2(1.0, 2.0))
        .approx_eq(&point2(-1.0, -2.0)));

    assert!(r90
        .inverse()
        .inverse()
        .transform_point2d(point2(1.0, 2.0))
        .approx_eq(&r90.transform_point2d(point2(1.0, 2.0))));
}

#[test]
fn pre_post() {
    use crate::default::Rotation3D;
    use core::f32::consts::FRAC_PI_2;

    let r1 = Rotation3D::around_x(Angle::radians(FRAC_PI_2));
    let r2 = Rotation3D::around_y(Angle::radians(FRAC_PI_2));
    let r3 = Rotation3D::around_z(Angle::radians(FRAC_PI_2));

    let t1 = r1.to_transform();
    let t2 = r2.to_transform();
    let t3 = r3.to_transform();

    let p = point3(1.0, 2.0, 3.0);

    // Check that the order of transformations is correct (corresponds to what
    // we do in Transform3D).
    let p1 = r1.then(&r2).then(&r3).transform_point3d(p);
    let p2 = t1.then(&t2).then(&t3).transform_point3d(p);

    assert!(p1.approx_eq(&p2.unwrap()));

    // Check that changing the order indeed matters.
    let p3 = t3.then(&t1).then(&t2).transform_point3d(p);
    assert!(!p1.approx_eq(&p3.unwrap()));
}

#[test]
fn to_transform3d() {
    use crate::default::Rotation3D;

    use core::f32::consts::{FRAC_PI_2, PI};
    let rotations = [
        Rotation3D::identity(),
        Rotation3D::around_x(Angle::radians(FRAC_PI_2)),
        Rotation3D::around_x(Angle::radians(-FRAC_PI_2)),
        Rotation3D::around_x(Angle::radians(PI)),
        Rotation3D::around_y(Angle::radians(FRAC_PI_2)),
        Rotation3D::around_y(Angle::radians(-FRAC_PI_2)),
        Rotation3D::around_y(Angle::radians(PI)),
        Rotation3D::around_z(Angle::radians(FRAC_PI_2)),
        Rotation3D::around_z(Angle::radians(-FRAC_PI_2)),
        Rotation3D::around_z(Angle::radians(PI)),
    ];

    let points = [
        point3(0.0, 0.0, 0.0),
        point3(1.0, 2.0, 3.0),
        point3(-5.0, 3.0, -1.0),
        point3(-0.5, -1.0, 1.5),
    ];

    for rotation in &rotations {
        for &point in &points {
            let p1 = rotation.transform_point3d(point);
            let p2 = rotation.to_transform().transform_point3d(point);
            assert!(p1.approx_eq(&p2.unwrap()));
        }
    }
}

#[test]
fn slerp() {
    use crate::default::Rotation3D;

    let q1 = Rotation3D::quaternion(1.0, 0.0, 0.0, 0.0);
    let q2 = Rotation3D::quaternion(0.0, 1.0, 0.0, 0.0);
    let q3 = Rotation3D::quaternion(0.0, 0.0, -1.0, 0.0);

    // The values below can be obtained with a python program:
    // import numpy
    // import quaternion
    // q1 = numpy.quaternion(1, 0, 0, 0)
    // q2 = numpy.quaternion(0, 1, 0, 0)
    // quaternion.slerp_evaluate(q1, q2, 0.2)

    assert!(q1.slerp(&q2, 0.0).approx_eq(&q1));
    assert!(q1.slerp(&q2, 0.2).approx_eq(&Rotation3D::quaternion(
        0.951056516295154,
        0.309016994374947,
        0.0,
        0.0
    )));
    assert!(q1.slerp(&q2, 0.4).approx_eq(&Rotation3D::quaternion(
        0.809016994374947,
        0.587785252292473,
        0.0,
        0.0
    )));
    assert!(q1.slerp(&q2, 0.6).approx_eq(&Rotation3D::quaternion(
        0.587785252292473,
        0.809016994374947,
        0.0,
        0.0
    )));
    assert!(q1.slerp(&q2, 0.8).approx_eq(&Rotation3D::quaternion(
        0.309016994374947,
        0.951056516295154,
        0.0,
        0.0
    )));
    assert!(q1.slerp(&q2, 1.0).approx_eq(&q2));

    assert!(q1.slerp(&q3, 0.0).approx_eq(&q1));
    assert!(q1.slerp(&q3, 0.2).approx_eq(&Rotation3D::quaternion(
        0.951056516295154,
        0.0,
        -0.309016994374947,
        0.0
    )));
    assert!(q1.slerp(&q3, 0.4).approx_eq(&Rotation3D::quaternion(
        0.809016994374947,
        0.0,
        -0.587785252292473,
        0.0
    )));
    assert!(q1.slerp(&q3, 0.6).approx_eq(&Rotation3D::quaternion(
        0.587785252292473,
        0.0,
        -0.809016994374947,
        0.0
    )));
    assert!(q1.slerp(&q3, 0.8).approx_eq(&Rotation3D::quaternion(
        0.309016994374947,
        0.0,
        -0.951056516295154,
        0.0
    )));
    assert!(q1.slerp(&q3, 1.0).approx_eq(&q3));
}

#[test]
fn around_axis() {
    use crate::default::Rotation3D;
    use core::f32::consts::{FRAC_PI_2, PI};

    // Two sort of trivial cases:
    let r1 = Rotation3D::around_axis(vec3(1.0, 1.0, 0.0), Angle::radians(PI));
    let r2 = Rotation3D::around_axis(vec3(1.0, 1.0, 0.0), Angle::radians(FRAC_PI_2));
    assert!(r1
        .transform_point3d(point3(1.0, 2.0, 0.0))
        .approx_eq(&point3(2.0, 1.0, 0.0)));
    assert!(r2
        .transform_point3d(point3(1.0, 0.0, 0.0))
        .approx_eq(&point3(0.5, 0.5, -0.5.sqrt())));

    // A more arbitrary test (made up with numpy):
    let r3 = Rotation3D::around_axis(vec3(0.5, 1.0, 2.0), Angle::radians(2.291288));
    assert!(r3
        .transform_point3d(point3(1.0, 0.0, 0.0))
        .approx_eq(&point3(-0.58071821, 0.81401868, -0.01182979)));
}

#[test]
fn from_euler() {
    use crate::default::Rotation3D;
    use core::f32::consts::FRAC_PI_2;

    // First test simple separate yaw pitch and roll rotations, because it is easy to come
    // up with the corresponding quaternion.
    // Since several quaternions can represent the same transformation we compare the result
    // of transforming a point rather than the values of each quaternions.
    let p = point3(1.0, 2.0, 3.0);

    let angle = Angle::radians(FRAC_PI_2);
    let zero = Angle::radians(0.0);

    // roll
    let roll_re = Rotation3D::euler(angle, zero, zero);
    let roll_rq = Rotation3D::around_x(angle);
    let roll_pe = roll_re.transform_point3d(p);
    let roll_pq = roll_rq.transform_point3d(p);

    // pitch
    let pitch_re = Rotation3D::euler(zero, angle, zero);
    let pitch_rq = Rotation3D::around_y(angle);
    let pitch_pe = pitch_re.transform_point3d(p);
    let pitch_pq = pitch_rq.transform_point3d(p);

    // yaw
    let yaw_re = Rotation3D::euler(zero, zero, angle);
    let yaw_rq = Rotation3D::around_z(angle);
    let yaw_pe = yaw_re.transform_point3d(p);
    let yaw_pq = yaw_rq.transform_point3d(p);

    assert!(roll_pe.approx_eq(&roll_pq));
    assert!(pitch_pe.approx_eq(&pitch_pq));
    assert!(yaw_pe.approx_eq(&yaw_pq));

    // Now check that the yaw pitch and roll transformations when combined are applied in
    // the proper order: roll -> pitch -> yaw.
    let ypr_e = Rotation3D::euler(angle, angle, angle);
    let ypr_q = roll_rq.then(&pitch_rq).then(&yaw_rq);
    let ypr_pe = ypr_e.transform_point3d(p);
    let ypr_pq = ypr_q.transform_point3d(p);

    assert!(ypr_pe.approx_eq(&ypr_pq));
}

二、Rotation2D 结构体

Rotation2D 结构体是一个灵活且功能强大的表示二维旋转的方式，它通过泛型支持多种数值类型，同时考虑了与C语言的兼容性、序列化和反序列化的支持，以及避免未使用类型参数警告的设计。

1. 泛型结构体：Rotation2D 是一个泛型结构体，有三个类型参数：T、Src 和 Dst。T 用于表示角度的类型，Src 和 Dst 通常是坐标转换的源和目标类型的标记，但在结构体中并没有直接使用它们，而是通过 PhantomData 来持有，这是为了避免编译器因未使用类型参数而发出的警告，同时也保留了类型信息，这可能用于泛型编程中的类型检查和约束。
2. repr© 表示法：#[repr©] 属性指示编译器为这个结构体使用C语言的布局。这对于与C语言代码交互或者在需要精确控制内存布局的场景下非常有用。
3. 条件编译指令：#[cfg_attr(feature = “serde”, derive(Serialize, Deserialize))] 是一个条件编译指令，它检查是否定义了名为 serde 的特性（feature）。如果定义了，就为 Rotation2D 结构体派生 Serialize 和 Deserialize 这两个trait。这对于序列化和反序列化结构体非常有用，尤其是在需要JSON等格式的数据交换时。
4. Serde的边界条件：紧接着的条件编译指令还使用了serde宏来指定序列化和反序列化的边界条件。这里指定了当进行序列化时，T 类型必须实现 serde::Serialize trait；当进行反序列化时，T 类型必须实现 serde::Deserialize<'de> trait。这确保了只有满足这些条件的类型参数才能被用于 Rotation2D 结构体的序列化和反序列化。
5. 字段：

• pub angle: T：这是一个公开的字段，表示旋转的角度，其类型由泛型参数 T 指定。
• #[doc(hidden)] pub _unit: PhantomData<(Src, Dst)>：这是一个隐藏的字段，使用了 PhantomData 来携带 Src 和 Dst 类型的信息，但不占用额外的运行时内存。#[doc(hidden)] 属性确保这个字段在生成的文档中不可见。

四、Rotation2D 方法

1. Copy和Clone的实现：

• Copy的实现语法上没问题，但前提是Rotation2D的所有字段也都需要实现Copy。
• Clone的实现中，PhantomData字段没有初始化。PhantomData是一个零大小的标记类型，用于在类型系统中表达所有权、借用或生命周期等关系，因此不需要实际的初始化值，但需要在结构体定义中指定其类型。例如，如果_unit是用来表示Src到Dst的转换，则应在Rotation2D结构体定义中包含PhantomData Dst>或类似的类型（具体类型取决于您的设计）。

2. Eq和PartialEq的实现：
   这些实现看起来是合理的，只要T（即角度类型）实现了Eq或PartialEq。
3. Hash的实现：
   实现看起来合理，只要T实现了Hash。
4. arbitrary::Arbitrary的实现：
   这个实现假设了Rotation2D有一个名为new的构造函数，该函数接受一个T类型的值作为角度并返回一个新的Rotation2D实例。这个假设需要在Rotation2D的定义中得到验证。
5. then方法和transform_point/transform_vector方法：

• then方法应该返回一个新的Rotation2D实例，其角度是两个旋转角度的和。这里假设了Real::sin_cos函数能够计算给定角度的正弦和余弦值，以及point2函数能够创建一个新的二维点。这些函数或宏需要在其他地方定义。
• transform_point和transform_vector方法分别用于旋转点和向量。这里同样假设了Real类型提供了sin_cos方法，以及Point2D和Vector2D类型及其相关方法的存在。

6. Rotation2D的实现，针对泛型类型T、Src和Dst：

• 这个实现块要求T类型实现了一些trait，包括Copy（可以被复制）、Add（加法操作，结果类型为T）、Sub（减法操作，结果类型为T）、Mul（乘法操作，结果类型为T）、Zero（零值，通常是一个常量方法返回类型T的零值）、Trig（这个trait没有在标准库中定义，可能是一个自定义trait，用于三角函数计算）。
• to_transform方法：这个方法返回当前旋转的矩阵表示（Transform2D）。它通过调用Transform2D::rotation方法，并传入当前旋转的角度（self.get_angle()）来实现。

7. Debug实现：

• 这个实现块允许Rotation2D类型使用{:?}格式化符进行调试打印。它要求T实现了fmt::Debug trait。
• fmt方法：这个方法使用write!宏将旋转的角度（以弧度为单位）格式化输出到提供的格式化器f中。

8. ApproxEq实现：

• 这个实现块允许Rotation2D类型使用近似相等比较。它要求T类型实现了Copy、Neg（取负操作，结果类型为T）和ApproxEq trait。
• approx_epsilon方法：返回用于近似相等比较时使用的容差（epsilon）值。这个值是通过调用T类型的approx_epsilon方法获得的。
• approx_eq_eps方法：使用给定的容差（eps）来比较两个Rotation2D实例是否近似相等。它通过比较两个实例的角度值是否近似相等来实现。

五、Rotation3D结构体

用于在三维空间中表示旋转，这种旋转通过四元数（quaternion）来体现。四元数是一种在三维空间中表示旋转的非常有效的方式，它由一个实部（real part）和三个虚部（imaginary parts）组成。

1. 在这个Rotation3D结构体中，有以下成员：

• i：表示四元数的第一个虚部，对应于虚数单位i的系数。
• j：表示四元数的第二个虚部，对应于虚数单位j的系数。
• k：表示四元数的第三个虚部，对应于虚数单位k的系数。
• r：表示四元数的实部。
• 此外，还有一个隐藏的字段_unit，它是PhantomData<(Src, Dst)>类型的，用于在类型系统中表达一些额外的信息，而不占用实际的内存空间。这里的Src和Dst可能是用于标记旋转的源坐标系和目标坐标系的类型参数，但在这段代码中并没有详细说明它们的具体用途。

2. 结构体上还有一些属性（attributes）：

• #[repr©]：这个属性指定了结构体的内存布局应当与C语言中的布局兼容，这对于与C语言代码进行互操作很重要。
• #[cfg_attr(feature = “serde”, derive(Serialize, Deserialize))]和相关的serde属性：这些属性条件性地为结构体实现了Serialize和Deserialize特性，如果启用了serde特性的话。这使得结构体可以被序列化（转换为JSON等格式）和反序列化（从JSON等格式转换回来），前提是其泛型类型T也实现了相应的serde特性。