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.

use super::Vec3;
use crate::Real;

/// Trait defining the core interface for spatial vectors in Featherstone's spatial algebra.
///
/// # Spatial Vector Duality
///
/// In Featherstone's formulation, spatial vectors come in dual pairs:
/// - **Motion vectors** (velocity, acceleration): [ω, v] where ω is angular velocity and v is
///   linear velocity
/// - **Force vectors** (force, torque): [n, f] where n is moment/torque and f is force
///
/// This duality is fundamental to spatial algebra:
/// - Motion and force vectors are duals of each other
/// - The dot product between a motion vector and force vector gives power
/// - Cross products have specific meanings for each combination
///
/// # Mathematical Conventions
///
/// ```text
/// Motion Vector:  v = [ω]  (angular velocity)
///                      [v]  (linear velocity)
///
/// Force Vector:   f = [n]  (moment/torque)
///                      [f]  (force)
///
/// Power:          P = v ⋅ f = ω·n + v·f
/// ```
pub trait SpatialVec: Sized {
    /// The dual type of this spatial vector.
    ///
    /// For `SpatialMotionVector`, this is `SpatialForceVector`.
    /// For `SpatialForceVector`, this is `SpatialMotionVector`.
    /// This enforces type safety at compile time to prevent mixing
    /// incompatible vector types in operations.
    type DualType: SpatialVec;

    /// Create a spatial vector from its top and bottom 3D components.
    ///
    /// # Arguments
    ///
    /// * `top` - The angular component (ω for motion, n for force)
    /// * `bottom` - The linear component (v for motion, f for force)
    fn from_pair(top: Vec3, bottom: Vec3) -> Self;

    /// Get the top (angular) 3D component of the spatial vector.
    ///
    /// - For motion vectors: returns angular velocity ω
    /// - For force vectors: returns moment/torque n
    fn top(&self) -> Vec3;

    /// Get the bottom (linear) 3D component of the spatial vector.
    ///
    /// - For motion vectors: returns linear velocity v
    /// - For force vectors: returns force f
    fn bottom(&self) -> Vec3;

    /// Calculate the dot product with the dual vector type.
    ///
    /// This computes the scalar product between a motion vector and force vector,
    /// which represents power in physics: P = ω·n + v·f
    ///
    /// # Mathematical Meaning
    ///
    /// For motion vector v = [ω, v] and force vector f = [n, f]:
    /// ```text
    /// v ⋅ f = ω·n + v·f
    /// ```
    /// where:
    /// - ω·n is the power from rotational motion
    /// - v·f is the power from linear motion
    #[inline]
    fn dot(&self, rhs: &Self::DualType) -> Real {
        self.top().dot(&rhs.top()) + self.bottom().dot(&rhs.bottom())
    }

    /// Calculate the cross product with the dual vector type.
    ///
    /// This operation represents the spatial cross product between motion and force
    /// vectors, following Featherstone's spatial algebra conventions.
    ///
    /// # Mathematical Formulation
    ///
    /// For motion vector m = [ω, v] and force vector f = [n, f]:
    /// ```text
    /// m ×* f = [ω×n + v×f]
    ///         [ω×f      ]
    /// ```
    /// where ×* denotes the spatial cross product.
    ///
    /// This operation is used in dynamics calculations, particularly for
    /// transforming forces between rotating reference frames.
    #[inline]
    fn cross_dual(&self, rhs: &Self::DualType) -> Self::DualType {
        Self::DualType::from_pair(
            self.top().cross(&rhs.top()) + self.bottom().cross(&rhs.bottom()),
            self.top().cross(&rhs.bottom()),
        )
    }

    /// Compute the transpose (dual) of the spatial vector.
    ///
    /// In spatial algebra, the transpose operation converts between motion
    /// and force vectors, effectively changing the vector's interpretation
    /// while preserving its components.
    ///
    /// # Mathematical Meaning
    ///
    /// This is equivalent to the adjoint operation in spatial algebra,
    /// which is used for coordinate transformations and energy calculations.
    #[inline]
    fn transpose(&self) -> Self::DualType {
        Self::DualType::from_pair(self.top(), self.bottom())
    }
}

#[cfg(test)]
mod tests {

    use approx::assert_relative_eq;

    use super::*;
    use crate::{SpatialForceVector, SpatialMotionVector};

    #[test]
    fn test_cross_dual() {
        let spatial_v = SpatialMotionVector::from_array([1., 2., 3., 4., 5., 6.]);
        let spatial_f = SpatialForceVector::from_array([7., 8., 9., 10., 11., 12.]);

        let result = spatial_v.cross_dual(&spatial_f);

        let w = spatial_v.top;
        let v = spatial_v.bottom;
        let n = spatial_f.top;
        let f = spatial_f.bottom;

        assert_relative_eq!(result.top, w.cross(&n) + v.cross(&f));
        assert_relative_eq!(result.bottom, w.cross(&f));
    }

    #[test]
    fn test_dot() {
        let spatial_v = SpatialMotionVector::from_array([1., 2., 3., 4., 5., 6.]);
        let spatial_f = SpatialForceVector::from_array([7., 8., 9., 10., 11., 12.]);

        let result = spatial_v.dot(&spatial_f);

        let w = spatial_v.top;
        let v = spatial_v.bottom;
        let n = spatial_f.top;
        let f = spatial_f.bottom;

        assert_relative_eq!(result, w.dot(&n) + v.dot(&f));
    }
}