spatial-math 0.4.0-beta.1

// Copyright (C) 2020-2025 spatial-math authors. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

//! Spatial transformations using Plücker coordinates.
//!
//! This module implements spatial coordinate transformations following Featherstone's
//! spatial algebra formulation. Plücker coordinates provide an elegant way to represent
//! both rotation and translation in a unified 6×6 transformation matrix.
//!
//! # Plücker Transform Theory
//!
//! A spatial transform X combines rotation E and translation r into a single 6×6 matrix:
//! ```text
//!     | E   0 |
//! X = |       |
//!     | -E×r E |
//! ```
//!
//! where:
//! - **E** is the 3×3 rotation matrix
//! - **r** is the translation vector (parent frame origin to child frame origin)
//! - **E×r** is the cross-product matrix of the translated vector
//!
//! # Coordinate Frame Conventions
//!
//! - Transforms convert from **parent/reference frame** to **child/local frame**
//! - Translation r is expressed in parent frame coordinates
//! - Rotation E transforms vectors from parent to child frame
//! - These conventions follow Featherstone's spatial algebra
//!
//! # Key Operations
//!
//! - **Motion vector transformation**: `v_child = X · v_parent`
//! - **Force vector transformation**: `f_child = X · f_parent`
//! - **Inertia transformation**: `I_child = X · I_parent · Xᵀ`
//! - **Transform composition**: `X_total = X_child · X_parent`

use std::ops::Mul;

use nalgebra::Rotation3;

use super::exts::Vec3Ext;
use super::{
    ArticulatedBodyInertia, Mat3, SpatialForceVector, SpatialMotionVector, UnitQuat, UnitVec3, Vec3,
};
use crate::exts::MatrixExt;
use crate::{Real, SMatrix, VEC3_ZERO, unit_quat};

/// A Plücker rotation representing a 3D rotation from reference to local frame.
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PlukerRotation(Rotation3<Real>);

impl PlukerRotation {
    /// Create a Plücker rotation from an axis and angle that rotates from reference to local frame.
    ///
    /// # Coordinate Frame Convention
    ///
    /// This creates a rotation that transforms vectors from the parent/reference frame
    /// to the child/local frame, following Featherstone's spatial algebra conventions.
    #[inline]
    pub fn from_axis_angle(axis: UnitVec3, angle: Real) -> Self {
        // Note: we should do inverse here because the nalgebra rotation is defined to convert a
        // vector from local to reference frame.
        Self(Rotation3::from_axis_angle(&axis, angle).inverse())
    }

    /// Create a Plücker rotation from a unit quaternion.
    ///
    /// The quaternion should represent the rotation from reference to local frame
    /// to be consistent with spatial algebra conventions.
    #[inline]
    pub fn from_quat(quat: UnitQuat) -> Self {
        Self(Rotation3::from(quat))
    }

    /// Convert the Plücker rotation to a unit quaternion.
    #[inline]
    pub fn into_quat(self) -> UnitQuat {
        self.0.into()
    }

    /// Convert the Plücker rotation to a 3x3 matrix.
    #[inline]
    pub fn into_matrix(self) -> Mat3 {
        self.0.into_inner()
    }

    /// Create a identity [`PlukerRotation`].
    #[inline]
    pub fn identity() -> Self {
        Self(Rotation3::identity())
    }

    /// Transpose of the rotation is identical to the inverse.
    ///
    /// Returns the transpose of the rotation matrix, which is equivalent to the inverse
    /// for orthogonal rotation matrices.
    #[inline]
    #[must_use]
    pub fn transpose(&self) -> Self {
        Self(self.0.transpose())
    }

    /// Convert the rotation to a 3x3 matrix.
    ///
    /// Returns the underlying rotation matrix representation.
    #[inline]
    pub fn matrix(&self) -> Mat3 {
        self.0.into_inner()
    }

    /// Transform a vector from local frame to reference frame.
    ///
    /// This is the inverse operation of multiplying a vector by the rotation.
    #[inline]
    pub fn inverse_transform_vector(&self, v: Vec3) -> Vec3 {
        self.0.inverse_transform_vector(&v)
    }

    /// Transform a unit vector from local frame to reference frame.
    ///
    /// This is the inverse operation of multiplying a unit vector by the rotation.
    #[inline]
    pub fn inverse_transform_unitvec(&self, v: UnitVec3) -> UnitVec3 {
        self.0.inverse_transform_unit_vector(&v)
    }

    /// Get the axis and angle of the rotation.
    #[inline]
    pub fn axis_angle(&self) -> Option<(UnitVec3, Real)> {
        self.0.axis_angle().map(|(axis, angle)| (axis, -angle))
    }

    /// Create a Plücker rotation from a 3x3 matrix without validation.
    ///
    /// # Safety
    ///
    /// The input matrix must be a valid rotation matrix (orthogonal with determinant 1).
    /// This function does not perform validation, so invalid input may cause undefined behavior.
    #[inline]
    pub fn from_matrix_unchecked(matrix: Mat3) -> Self {
        Self(Rotation3::from_matrix_unchecked(matrix))
    }

    /// Get the rotation matrix as a slice.
    ///
    /// Returns the 3x3 rotation matrix as a contiguous slice of 9 elements in column-major order.
    #[inline]
    pub fn as_slice(&self) -> &[Real] {
        self.0.matrix().as_slice()
    }
}

impl Mul<Vec3> for PlukerRotation {
    type Output = Vec3;

    #[inline]
    fn mul(self, rhs: Vec3) -> Vec3 {
        self.0 * rhs
    }
}

impl Mul<Self> for PlukerRotation {
    type Output = Self;

    #[inline]
    fn mul(self, rhs: Self) -> Self {
        Self(self.0 * rhs.0)
    }
}

impl Mul<Mat3> for PlukerRotation {
    type Output = Mat3;

    #[inline]
    fn mul(self, rhs: Mat3) -> Mat3 {
        self.0 * rhs
    }
}

/// A spatial transform using Plücker coordinates.
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PluckerTransform {
    /// The rotation component of the spatial transform (E).
    ///
    /// This is a 3×3 rotation matrix that transforms angular components
    /// from the parent/reference frame to the child/local frame.
    pub rotation: PlukerRotation,

    /// The translation component of the spatial transform (r).
    ///
    /// This is a 3D vector representing the position of the child frame
    /// origin relative to the parent frame origin, expressed in parent frame coordinates.
    pub translation: Vec3,
}

impl Default for PluckerTransform {
    #[inline]
    fn default() -> Self {
        Self {
            rotation: PlukerRotation::identity(),
            translation: Vec3::zeros(),
        }
    }
}

impl PluckerTransform {
    /// Create an identity `PluckerTransform`.
    ///
    /// Returns a transform with no rotation or translation.
    #[inline]
    pub fn identity() -> Self {
        Self {
            rotation: PlukerRotation::identity(),
            translation: VEC3_ZERO,
        }
    }

    /// Create a plucker transform from a pose. The returned transform converts from parent to
    /// local.
    #[inline]
    pub fn from_pose(pose: Pose) -> Self {
        Self {
            // Note: we should do inverse here because the pose rotation is defined to convert a
            // vector from local to reference frame, but the plucker rotation is defined to convert
            // a vector from reference to local frame.
            rotation: PlukerRotation::from_quat(pose.rotation.inverse()),
            translation: pose.translation,
        }
    }

    /// Check if the transform contains any NaN values.
    #[inline]
    pub fn any_nan(&self) -> bool {
        self.rotation.matrix().any_nan() || self.translation.any_nan()
    }

    /// Multiply this transform by another transform.
    ///
    /// Computes the composition of two transforms: result = self * rhs
    /// where rhs is applied first, then self.
    ///
    /// The formula is: X1 * X2 = plx(E1E2, r2 + E2^T r1)
    #[inline]
    #[must_use]
    pub fn mul_transform(&self, rhs: &PluckerTransform) -> PluckerTransform {
        // X1 * X2 = plx(E1E2, r2 + E2^T r1)
        PluckerTransform {
            rotation: self.rotation * rhs.rotation,
            translation: rhs.translation + rhs.rotation.transpose() * self.translation,
        }
    }

    /// Compute the inverse of the transform.
    ///
    /// Returns a transform that undoes this transform when applied.
    ///
    /// The formula is: X^-1 = plx(E^T, -Er)
    #[inline]
    #[must_use]
    pub fn inverse(&self) -> PluckerTransform {
        // X^-1 = plx(E^T, -Er)
        PluckerTransform {
            rotation: self.rotation.transpose(),
            translation: -(self.rotation * self.translation),
        }
    }

    /// Transform a spatial motion vector from parent frame to child frame.
    ///
    /// Transforms a 6D motion vector containing angular and linear velocity components.
    /// The formula is: X * v^ = mv(Ew, E(v - r x w))
    ///
    /// # Arguments
    /// * `v` - Spatial motion vector in parent frame coordinates
    ///
    /// # Returns
    /// Spatial motion vector in child frame coordinates
    #[inline]
    pub fn transform_motion_vec(&self, v: SpatialMotionVector) -> SpatialMotionVector {
        // X * v^ = mv(Ew, E(v - r x w))
        let w = v.top;
        let v = v.bottom;

        let w1 = self.rotation * w;
        let v1 = self.rotation * (v - self.translation.cross(&w));

        SpatialMotionVector::from_pair(w1, v1)
    }

    /// Transform a spatial motion vector from child frame to parent frame.
    ///
    /// This is the inverse operation of `transform_motion_vec`.
    /// The formula is: X^-1 * v^ = mv(E^T w, E^T v + r x E^T w)
    ///
    /// # Arguments
    /// * `v` - Spatial motion vector in child frame coordinates
    ///
    /// # Returns
    /// Spatial motion vector in parent frame coordinates
    #[inline]
    pub fn inverse_transform_motion_vec(&self, v: SpatialMotionVector) -> SpatialMotionVector {
        // X^-1 * v^ = mv(E^T w, E^T v + r x E^T w)
        let w = v.top;
        let v = v.bottom;

        let e_transpose = self.rotation.transpose();

        let w = e_transpose * w;
        let v = e_transpose * v + self.translation.cross(&w);

        SpatialMotionVector::from_pair(w, v)
    }

    /// Transform a spatial force vector from parent frame to child frame.
    ///
    /// Transforms a 6D force vector containing moment (torque) and force components.
    /// The formula is: X * f = fv(E(n - r x f), Ef)
    ///
    /// # Arguments
    /// * `f` - Spatial force vector in parent frame coordinates
    ///
    /// # Returns
    /// Spatial force vector in child frame coordinates
    #[inline]
    pub fn transform_force_vec(&self, f: SpatialForceVector) -> SpatialForceVector {
        // X * f = fv(E(n - r x f), Ef)

        let n = f.top;
        let f = f.bottom;

        let e = self.rotation;
        let r = self.translation;

        let n = e * (n - r.cross(&f));
        let f = e * f;

        SpatialForceVector::from_pair(n, f)
    }

    /// Transform a spatial force vector from child frame to parent frame.
    ///
    /// This is the inverse operation of `transform_force_vec`.
    /// The formula is: X^-1 * f = fv(E^T n + r x E^T f, E^T f)
    ///
    /// # Arguments
    /// * `f` - Spatial force vector in child frame coordinates
    ///
    /// # Returns
    /// Spatial force vector in parent frame coordinates
    #[inline]
    pub fn inverse_transform_force_vec(&self, f: SpatialForceVector) -> SpatialForceVector {
        // X^-1 * f = fv(E^T n + r x E^T f, E^T f)

        let n = f.top;
        let f = f.bottom;

        let e_transpose = self.rotation.transpose();

        let f = e_transpose * f;
        let n = e_transpose * n + self.translation.cross(&f);

        SpatialForceVector::from_pair(n, f)
    }

    /// Transform a point from reference frame to local
    #[inline]
    pub fn transform_point(&self, point: Vec3) -> Vec3 {
        let v = point - self.translation;
        self.rotation * v
    }

    /// Transform a 3D vector from reference frame to local frame.
    ///
    /// Applies only the rotation component of the transform, ignoring translation.
    /// This is appropriate for transforming directions and vectors that represent
    // quantities like velocity or force rather than positions.
    #[inline]
    pub fn transform_vec3(&self, vec3: Vec3) -> Vec3 {
        self.rotation * vec3
    }

    /// Transform a point from local frame to reference frame
    #[inline]
    pub fn inverse_transform_point(&self, point: Vec3) -> Vec3 {
        let v = self.rotation.inverse_transform_vector(point);
        v + self.translation
    }

    /// Transform a 3D vector from local frame to reference frame.
    ///
    /// Applies only the inverse rotation component of the transform, ignoring translation.
    /// This is the inverse operation of `transform_vec3`.
    #[inline]
    pub fn inverse_transform_vec3(&self, vec3: Vec3) -> Vec3 {
        self.rotation.inverse_transform_vector(vec3)
    }

    /// Transform a unit vector from local frame to reference frame.
    ///
    /// Applies only the inverse rotation component of the transform, ignoring translation.
    /// The input vector is assumed to be normalized and will remain normalized.
    #[inline]
    pub fn inverse_transform_unitvec3(&self, vec3: UnitVec3) -> UnitVec3 {
        self.rotation.inverse_transform_unitvec(vec3)
    }

    /// Transform an articulated body inertia from parent frame to child frame.
    ///
    /// Transforms a 6×6 articulated body inertia matrix using the similarity transform:
    /// `I_child = X * I_parent * Xᵀ`
    ///
    /// The formula involves three components:
    /// - Mass matrix: M = E * M * Eᵀ
    /// - Coriolis matrix: H = E * (H - r * M) * Eᵀ
    /// - Rotational inertia: I = E * (I - r×Hᵀ + (H - r×M)r×) * Eᵀ
    ///
    /// # Arguments
    /// * `inertia` - Articulated body inertia in parent frame coordinates
    ///
    /// # Returns
    /// Articulated body inertia in child frame coordinates
    #[inline]
    pub fn transform_articulated_inertia(
        &self,
        inertia: &ArticulatedBodyInertia,
    ) -> ArticulatedBodyInertia {
        let rotation = self.rotation.matrix();
        let rotation_t = rotation.transpose();
        let m = inertia.mass.mat3();
        let r = self.translation.into_cross_matrix();
        let i = inertia.top_left.mat3();
        let h = inertia.h;

        // M = E * M * E^T
        let new_m = rotation * m * rotation_t;
        // H = E * (H - r * M) * E^T
        let new_h = rotation * (h - r * m) * rotation_t;
        // E(I −r×HT +(H −r×M)r×)ET
        let new_inertia = rotation * (i - r * h.transpose() + (h - r * m) * r) * rotation_t;
        ArticulatedBodyInertia {
            top_left: new_inertia.into(),
            h: new_h,
            mass: new_m.into(),
        }
    }

    /// Convert the Plücker transform to a 6×6 matrix representation.
    ///
    /// Returns the full 6×6 spatial transform matrix:
    /// ```text
    ///     | E   0 |
    /// X = |       |
    ///     | -E×r E |
    /// ```
    ///
    /// where E is the 3×3 rotation matrix and r is the translation vector.
    /// This matrix can be used for direct multiplication with 6D vectors.
    #[inline]
    pub fn matrix(&self) -> SMatrix<6, 6> {
        let mut result = SMatrix::<6, 6>::zeros();

        let rotation = self.rotation.matrix();

        let bottom_left = -(rotation * self.translation.into_cross_matrix());

        // top left
        result.fixed_view_mut::<3, 3>(0, 0).copy_from(&rotation);
        // right bottom
        result.fixed_view_mut::<3, 3>(3, 3).copy_from(&rotation);

        // body left
        result.fixed_view_mut::<3, 3>(3, 0).copy_from(&bottom_left);

        result
    }

    /// Convert the Plücker transform to its dual matrix representation.
    ///
    /// Returns the dual transform matrix where the translation component appears
    /// in the top-right block instead of the bottom-left block:
    /// ```text
    ///     | E  -E×r |
    /// X* = |        |
    ///     | 0    E  |
    /// ```
    ///
    /// The dual matrix is the transpose inverse of the regular matrix.
    /// This representation is useful for transforming certain spatial quantities.
    #[inline]
    pub fn dual_matrix(&self) -> SMatrix<6, 6> {
        let mut result = SMatrix::<6, 6>::zeros();

        let rotation = self.rotation.matrix();
        // top left
        result.fixed_view_mut::<3, 3>(0, 0).copy_from(&rotation);
        // right bottom
        result.fixed_view_mut::<3, 3>(3, 3).copy_from(&rotation);

        let top_right = -rotation * self.translation.into_cross_matrix();

        result.fixed_view_mut::<3, 3>(0, 3).copy_from(&top_right);

        result
    }

    /// Check whether the transform is close to identity.
    /// Check whether the transform is close to identity.
    ///
    /// Returns true if both the rotation angle and translation magnitude
    /// are below the given epsilon threshold.
    ///
    /// # Arguments
    /// * `epsilon` - Maximum allowed deviation from identity
    ///
    /// # Returns
    /// True if the transform is approximately identity
    #[inline]
    pub fn is_near_identity(&self, epsilon: Real) -> bool {
        self.rotation.0.angle() < epsilon && self.translation.iter().all(|x| x.abs() < epsilon)
    }
}

impl Mul<Self> for PluckerTransform {
    type Output = Self;

    #[inline]
    fn mul(self, rhs: Self) -> Self {
        self.mul_transform(&rhs)
    }
}

impl Mul<SpatialMotionVector> for PluckerTransform {
    type Output = SpatialMotionVector;

    #[inline]
    fn mul(self, rhs: SpatialMotionVector) -> SpatialMotionVector {
        self.transform_motion_vec(rhs)
    }
}

impl Mul<SpatialForceVector> for PluckerTransform {
    type Output = SpatialForceVector;

    #[inline]
    fn mul(self, rhs: SpatialForceVector) -> SpatialForceVector {
        self.transform_force_vec(rhs)
    }
}

impl Mul<&ArticulatedBodyInertia> for PluckerTransform {
    type Output = ArticulatedBodyInertia;

    #[inline]
    fn mul(self, rhs: &ArticulatedBodyInertia) -> ArticulatedBodyInertia {
        self.transform_articulated_inertia(rhs)
    }
}

impl PluckerTransform {
    /// Convert the Plücker transform to a pose.
    ///
    /// Returns a pose that represents the same transformation but with
    /// the rotation component inverted to match pose conventions.
    /// This is because poses typically represent the transformation
    /// from local to world space, while Plücker transforms represent
    /// the transformation from parent to child frame.
    #[inline]
    pub fn pose(&self) -> Pose {
        let quat: UnitQuat = self.rotation.0.into();
        Pose {
            rotation: quat.inverse(),
            translation: self.translation,
        }
    }
}

/// A 6DOF pose represented by rotation and translation.
///
/// This represents a rigid body transformation in 3D space with a quaternion
/// for rotation and a 3D vector for translation. The pose transforms points
/// from local/object coordinates to world/reference coordinates.
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[derive(Debug, Default, Clone, Copy)]
pub struct Pose {
    /// Rotation component as a unit quaternion
    pub rotation: UnitQuat,
    /// Translation component as a 3D vector
    pub translation: Vec3,
}

impl Pose {
    /// Transform a point from local/object coordinates to world/reference coordinates.
    ///
    /// Applies both rotation and translation to transform a point position.
    /// The formula is: `world_point = rotation * local_point + translation`
    ///
    /// # Arguments
    /// * `local` - Point in local coordinates
    ///
    /// # Returns
    /// Point in world/reference coordinates
    #[inline]
    pub fn transform_point(&self, local: Vec3) -> Vec3 {
        self.rotation * local + self.translation
    }

    /// Transform a vector from local/object coordinates to world/reference coordinates.
    ///
    /// Applies only the rotation component, ignoring translation.
    /// This is appropriate for transforming directions, velocities, forces,
    /// or any vector quantity that represents a direction rather than a position.
    ///
    /// # Arguments
    /// * `local` - Vector in local coordinates
    ///
    /// # Returns
    /// Vector in world/reference coordinates
    #[inline]
    pub fn transform_vec(&self, local: Vec3) -> Vec3 {
        self.rotation * local
    }

    /// Compute the inverse of the pose.
    ///
    /// Returns a pose that transforms from world/reference coordinates back to
    /// local/object coordinates. The formula is: X⁻¹ = [R⁻¹, -R⁻¹·t]
    ///
    /// # Returns
    /// Inverse pose that undoes this transformation
    #[inline]
    #[must_use]
    pub fn inverse(&self) -> Pose {
        let r = self.rotation.inverse();
        let t = -self.translation;
        Pose {
            rotation: r,
            translation: r * t,
        }
    }

    /// Check whether the pose is close to identity.
    ///
    /// Returns true if both the rotation angle and translation magnitude
    /// are below the given epsilon threshold.
    ///
    /// # Arguments
    /// * `epsilon` - Maximum allowed deviation from identity
    ///
    /// # Returns
    /// True if the pose is approximately identity
    #[inline]
    pub fn is_identity(&self, epsilon: Real) -> bool {
        self.rotation.angle() < epsilon && self.translation.iter().all(|x| x.abs() < epsilon)
    }

    /// Convert the pose to a `[Real; 7]` array with the format `[x, y, z, i, j, k, w]`.
    #[inline]
    pub fn into_array(self) -> [Real; 7] {
        let p = self.translation;
        let r = self.rotation;
        [p.x, p.y, p.z, r.i, r.j, r.k, r.w]
    }

    /// Create a pose from a `[Real; 7]` array with the format `[x, y, z, i, j, k, w]`.
    #[inline]
    pub fn from_array(arr: [Real; 7]) -> Self {
        Pose {
            translation: Vec3::new(arr[0], arr[1], arr[2]),
            rotation: unit_quat(arr[3], arr[4], arr[5], arr[6]),
        }
    }

    /// Create a pose from a slice with the format `[x, y, z, i, j, k, w]`.
    ///
    /// # Panics
    ///
    /// Panics if the slice length is not 7.
    #[inline]
    pub fn from_slice(arr: &[Real]) -> Self {
        assert_eq!(arr.len(), 7);
        Pose {
            translation: Vec3::new(arr[0], arr[1], arr[2]),
            rotation: unit_quat(arr[3], arr[4], arr[5], arr[6]),
        }
    }
}

impl From<Pose> for PluckerTransform {
    #[inline]
    fn from(pose: Pose) -> Self {
        PluckerTransform::from_pose(pose)
    }
}

impl Mul<Pose> for Pose {
    type Output = Pose;

    #[inline]
    fn mul(self, rhs: Pose) -> Pose {
        let rotation = self.rotation * rhs.rotation;
        let translation = self.rotation * rhs.translation + self.translation;
        Pose {
            rotation,
            translation,
        }
    }
}

#[cfg(feature = "approx")]
mod approx_eq {

    use crate::Real;

    impl approx::AbsDiffEq for super::PlukerRotation {
        type Epsilon = Real;

        #[inline]
        fn default_epsilon() -> Self::Epsilon {
            Real::EPSILON
        }

        #[inline]
        fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
            self.0.abs_diff_eq(&other.0, epsilon)
        }
    }

    impl approx::RelativeEq for super::PlukerRotation {
        #[inline]
        fn default_max_relative() -> Self::Epsilon {
            Real::default_max_relative()
        }

        #[inline]
        fn relative_eq(
            &self,
            other: &Self,
            epsilon: Self::Epsilon,
            max_relative: Self::Epsilon,
        ) -> bool {
            self.0.relative_eq(&other.0, epsilon, max_relative)
        }
    }

    impl approx::AbsDiffEq for super::PluckerTransform {
        type Epsilon = Real;

        #[inline]
        fn default_epsilon() -> Self::Epsilon {
            Real::EPSILON
        }

        #[inline]
        fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
            self.rotation.abs_diff_eq(&other.rotation, epsilon)
                & self.translation.abs_diff_eq(&other.translation, epsilon)
        }
    }

    impl approx::RelativeEq for super::PluckerTransform {
        #[inline]
        fn default_max_relative() -> Self::Epsilon {
            Real::default_max_relative()
        }

        #[inline]
        fn relative_eq(
            &self,
            other: &Self,
            epsilon: Self::Epsilon,
            max_relative: Self::Epsilon,
        ) -> bool {
            self.rotation
                .relative_eq(&other.rotation, epsilon, max_relative)
                & self
                    .translation
                    .relative_eq(&other.translation, epsilon, max_relative)
        }
    }
}

#[cfg(test)]
mod tests {
    use approx::assert_relative_eq;

    use super::PluckerTransform;
    use crate::exts::Vec3Ext;
    use crate::transform::PlukerRotation;
    use crate::{
        ArticulatedBodyInertia, Mat3, Real, RigidBodyInertia, SpatialMotionVector, SymmetricMat3,
        UnitQuat, UnitVec3, Vec3, unit_vec3, vec3,
    };

    #[test]
    fn test_transform_motion_vec() {
        let transform = PluckerTransform {
            rotation: PlukerRotation::from_axis_angle(Vec3::z_axis(), Real::to_radians(90.0)),
            translation: vec3(1., 2., 3.),
        };

        let v0 = SpatialMotionVector::from_array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0]);
        let v1 = transform * v0;

        assert_relative_eq!(v1.top, transform.rotation * v0.top);
        assert_relative_eq!(
            v1.bottom,
            transform.rotation * (v0.bottom + v0.top.cross(&transform.translation))
        );
    }

    #[test]
    fn test_transform_mul() {
        let a_to_b = PluckerTransform {
            rotation: PlukerRotation::from_axis_angle(Vec3::z_axis(), Real::to_radians(90.0)),
            translation: Vec3::zeros(),
        };

        let b_to_c = PluckerTransform {
            rotation: PlukerRotation::identity(),
            translation: vec3(1.0, 0.0, 0.0),
        };

        let a_to_c = b_to_c * a_to_b;

        assert_relative_eq!(a_to_c.translation, vec3(0.0, 1.0, 0.0));
    }

    #[test]
    fn test_transform_inverse() {
        let transform = PluckerTransform {
            rotation: PlukerRotation::from_axis_angle(Vec3::z_axis(), Real::to_radians(90.0)),
            translation: vec3(1.0, 0.0, 0.0),
        };

        let transform_inv = transform.inverse();

        let result = transform_inv * transform;

        assert_relative_eq!(result.rotation.matrix(), Mat3::identity());
        assert_relative_eq!(result.translation, Vec3::zeros());
    }

    #[test]
    fn test_transform_to_matrix() {
        let transform = PluckerTransform {
            rotation: PlukerRotation::from_axis_angle(
                UnitVec3::new_normalize(vec3(1.0, 2.0, 3.0)),
                Real::to_radians(45.0),
            ),
            translation: vec3(1.0, 2.0, 3.0),
        };

        let matrix = transform.matrix();

        let e = transform.rotation.matrix();
        let rx = transform.translation.into_cross_matrix();

        {
            let top_left = matrix.fixed_view::<3, 3>(0, 0);
            let bottom_right = matrix.fixed_view::<3, 3>(3, 3);
            assert_relative_eq!(bottom_right, top_left);

            let top_left: Vec<_> = top_left.iter().collect();
            let e_data = e.iter().collect::<Vec<_>>();
            assert_eq!(top_left, e_data);
        }

        {
            let top_right = matrix.fixed_view::<3, 3>(0, 3);
            top_right.iter().for_each(|&x| assert_relative_eq!(x, 0.0));
        }

        {
            let bottom_left = matrix.fixed_view::<3, 3>(3, 0);

            let expected = -e * rx;

            bottom_left.iter().zip(expected.iter()).for_each(|(a, b)| {
                assert_relative_eq!(a, b);
            });
        }

        let motion_vec = SpatialMotionVector::from_pair(vec3(1.0, 2.0, 3.0), vec3(4.0, 5.0, 6.0));

        assert_relative_eq!(
            (transform * motion_vec).into_vec6(),
            matrix * motion_vec.into_vec6(),
            epsilon = 1e-6
        );
    }

    #[test]
    fn test_transform_to_dual_matrix() {
        let transform = PluckerTransform {
            rotation: PlukerRotation::from_axis_angle(
                UnitVec3::new_normalize(vec3(1.0, 2.0, 3.0)),
                Real::to_radians(90.0),
            ),
            translation: vec3(1.0, 2.0, 3.0),
        };

        let matrix = transform.matrix();
        let dual_matrix = transform.dual_matrix();

        assert_relative_eq!(
            matrix.transpose().try_inverse().unwrap(),
            dual_matrix,
            epsilon = 1e-6
        );
    }

    #[test]
    fn test_transform_articulated_inertia() {
        let transform = PluckerTransform {
            rotation: PlukerRotation::from_axis_angle(Vec3::z_axis(), Real::to_radians(90.0)),
            translation: vec3(1.0, 0.0, 0.0),
        };

        let inertia: ArticulatedBodyInertia =
            RigidBodyInertia::new(1.0, vec3(1.0, 2.0, 3.0), SymmetricMat3::ONE).into();

        let new_inertia = transform.transform_articulated_inertia(&inertia);

        let dual_x = transform.dual_matrix();
        let inverse_x = transform.matrix().try_inverse().unwrap();

        let expected_inertia = dual_x * inertia.matrix() * inverse_x;

        assert_relative_eq!(new_inertia.matrix(), expected_inertia, epsilon = 1e-6);

        assert_relative_eq!(
            new_inertia.mass.mat3(),
            transform.rotation.matrix()
                * inertia.mass.mat3()
                * transform.rotation.matrix().transpose(),
            epsilon = 1e-6
        );
    }

    #[test]
    fn test_pose_inverse() {
        let pose = super::Pose {
            rotation: UnitQuat::from_axis_angle(&unit_vec3(1., 2., 3.), Real::to_radians(36.0)),
            translation: vec3(1.0, 2.0, 3.0),
        };

        let inv = pose.inverse();

        let result = inv * pose;

        assert_relative_eq!(result.rotation, UnitQuat::identity());
        assert_relative_eq!(result.translation, Vec3::zeros());
    }
}