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 std::fmt::Display;
use std::ops::{Add, AddAssign, Mul, Neg, Sub, SubAssign};

use super::traits::SpatialVec;
use super::{SVector, VEC3_ZERO, Vec3};
use crate::exts::MatrixExt;
use crate::{Real, Vec6};

/// Type marker for spatial motion vectors.
///
/// This zero-sized type is used at the type level to distinguish
/// motion vectors from force vectors, enabling compile-time type safety.
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct Motion;

/// Type marker for spatial force vectors.
///
/// This zero-sized type is used at the type level to distinguish
/// force vectors from motion vectors, enabling compile-time type safety.
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct Force;

/// A 6D spatial vector representing either motion or force.
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct SpatialVector<T = ()> {
    /// The top (angular) component of the spatial vector.
    ///
    /// - For motion vectors: angular velocity ω
    /// - For force vectors: moment/torque n
    pub top: Vec3,

    /// The bottom (linear) component of the spatial vector.
    ///
    /// - For motion vectors: linear velocity v
    /// - For force vectors: force f
    pub bottom: Vec3,

    /// Phantom data for type-level distinction between motion and force vectors.
    /// This ensures that `SpatialMotionVector` and `SpatialForceVector` are
    /// different types at compile time, preventing misuse.
    _phantom: std::marker::PhantomData<T>,
}

impl<T> Display for SpatialVector<T> {
    #[inline]
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "[{},{},{}, {},{},{}]",
            self.top[0], self.top[1], self.top[2], self.bottom[0], self.bottom[1], self.bottom[2]
        )
    }
}

impl<T> From<SpatialVector<T>> for Vec6 {
    #[inline]
    fn from(v: SpatialVector<T>) -> Self {
        v.into_vec6()
    }
}

impl<T> SpatialVector<T> {
    /// Zero spatial vector (both angular and linear components are zero).
    ///
    /// This is useful for initialization and as the identity element
    /// for vector addition operations.
    pub const ZERO: Self = Self {
        top: VEC3_ZERO,
        bottom: VEC3_ZERO,
        _phantom: std::marker::PhantomData,
    };

    /// Create a spatial vector from a 6-element array.
    ///
    /// # Array Format
    ///
    /// - **Motion vectors**: [ωx, ωy, ωz, vx, vy, vz] where ω is angular velocity and v is linear
    ///   velocity
    /// - **Force vectors**: [nx, ny, nz, fx, fy, fz] where n is moment/torque and f is force
    ///
    /// # Example
    ///
    /// ```rust
    /// use spatial_math::SpatialMotionVector;
    ///
    /// let velocity = SpatialMotionVector::from_array([1.0, 0.0, 0.0, 0.0, 1.0, 0.0]);
    /// // ω = [1, 0, 0], v = [0, 1, 0]
    /// ```
    #[inline]
    pub const fn from_array(array: [Real; 6]) -> Self {
        Self {
            top: Vec3::new(array[0], array[1], array[2]),
            bottom: Vec3::new(array[3], array[4], array[5]),
            _phantom: std::marker::PhantomData,
        }
    }

    /// Create a spatial vector from a 6D static vector.
    ///
    /// This is useful when converting from other linear algebra
    /// representations that use `SVector<6>`.
    #[inline]
    pub fn from_vec6(vec: SVector<6>) -> Self {
        Self {
            top: Vec3::new(vec[0], vec[1], vec[2]),
            bottom: Vec3::new(vec[3], vec[4], vec[5]),
            _phantom: std::marker::PhantomData,
        }
    }

    /// Create a spatial vector from separate 3D top and bottom components.
    ///
    /// This is often used when you have separate angular and linear quantities
    /// that need to be combined into a spatial vector.
    ///
    /// # Arguments
    ///
    /// * `top` - The angular component (ω for motion, n for force)
    /// * `bottom` - The linear component (v for motion, f for force)
    #[inline]
    pub const fn from_pair(top: Vec3, bottom: Vec3) -> Self {
        Self {
            top,
            bottom,
            _phantom: std::marker::PhantomData,
        }
    }

    /// Convert the spatial vector to a 6-element array.
    ///
    /// Returns the components in the same format as expected by `from_array()`:
    /// [top.x, top.y, top.z, bottom.x, bottom.y, bottom.z]
    #[inline]
    pub fn into_array(self) -> [Real; 6] {
        [
            self.top.x,
            self.top.y,
            self.top.z,
            self.bottom.x,
            self.bottom.y,
            self.bottom.z,
        ]
    }

    #[inline]
    pub fn any_nan(&self) -> bool {
        self.top.any_nan() || self.bottom.any_nan()
    }

    #[inline]
    #[must_use]
    pub fn add(&self, rhs: &Self) -> Self {
        Self::from_pair(self.top + rhs.top, self.bottom + rhs.bottom)
    }

    #[inline]
    #[must_use]
    pub fn sub(&self, rhs: &Self) -> Self {
        Self::from_pair(self.top - rhs.top, self.bottom - rhs.bottom)
    }

    #[inline]
    #[must_use]
    pub fn neg(&self) -> Self {
        Self::from_pair(-self.top, -self.bottom)
    }

    #[inline]
    #[must_use]
    pub fn scale(&self, scalar: Real) -> Self {
        Self::from_pair(self.top * scalar, self.bottom * scalar)
    }

    #[inline]
    #[must_use]
    pub fn cross(&self, rhs: &Self) -> Self {
        Self::from_pair(
            self.top.cross(&rhs.top),
            self.top.cross(&rhs.bottom) + self.bottom.cross(&rhs.top),
        )
    }

    #[inline]
    pub fn into_vec6(self) -> SVector<6> {
        SVector::from_iterator([
            self.top.x,
            self.top.y,
            self.top.z,
            self.bottom.x,
            self.bottom.y,
            self.bottom.z,
        ])
    }
}

impl<T> Add<Self> for SpatialVector<T> {
    type Output = Self;

    #[inline]
    fn add(self, rhs: Self) -> Self::Output {
        SpatialVector::add(&self, &rhs)
    }
}

impl<T> AddAssign<Self> for SpatialVector<T> {
    #[inline]
    fn add_assign(&mut self, rhs: Self) {
        *self = SpatialVector::add(&*self, &rhs);
    }
}

impl<T> Sub<Self> for SpatialVector<T> {
    type Output = Self;

    #[inline]
    fn sub(self, rhs: Self) -> Self::Output {
        SpatialVector::sub(&self, &rhs)
    }
}

impl<T> SubAssign<Self> for SpatialVector<T> {
    #[inline]
    fn sub_assign(&mut self, rhs: Self) {
        *self = SpatialVector::sub(&*self, &rhs);
    }
}

impl<T> Mul<Real> for SpatialVector<T> {
    type Output = Self;

    #[inline]
    fn mul(self, rhs: Real) -> Self::Output {
        self.scale(rhs)
    }
}

impl<T> Neg for SpatialVector<T> {
    type Output = Self;

    #[inline]
    fn neg(self) -> Self::Output {
        Self::neg(&self)
    }
}

pub type SpatialMotionVector = SpatialVector<Motion>;
pub type SpatialForceVector = SpatialVector<Force>;

impl SpatialVec for SpatialMotionVector {
    type DualType = SpatialForceVector;

    #[inline]
    fn from_pair(top: Vec3, bottom: Vec3) -> Self {
        Self::from_pair(top, bottom)
    }

    #[inline]
    fn top(&self) -> Vec3 {
        self.top
    }

    #[inline]
    fn bottom(&self) -> Vec3 {
        self.bottom
    }
}

impl SpatialVec for SpatialForceVector {
    type DualType = SpatialMotionVector;

    #[inline]
    fn from_pair(top: Vec3, bottom: Vec3) -> Self {
        Self::from_pair(top, bottom)
    }

    #[inline]
    fn top(&self) -> Vec3 {
        self.top
    }

    #[inline]
    fn bottom(&self) -> Vec3 {
        self.bottom
    }
}

#[cfg(feature = "approx")]
mod approx_eq {
    use crate::Real;

    impl<T> approx::AbsDiffEq for super::SpatialVector<T>
    where
        T: PartialEq,
    {
        type Epsilon = Real;

        fn default_epsilon() -> Self::Epsilon {
            Real::EPSILON
        }

        fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
            self.top.abs_diff_eq(&other.top, epsilon)
                && self.bottom.abs_diff_eq(&other.bottom, epsilon)
        }
    }

    impl<T> approx::RelativeEq for super::SpatialVector<T>
    where
        T: PartialEq,
    {
        fn default_max_relative() -> Self::Epsilon {
            Real::EPSILON
        }

        fn relative_eq(
            &self,
            other: &Self,
            epsilon: Self::Epsilon,
            max_relative: Self::Epsilon,
        ) -> bool {
            self.top.relative_eq(&other.top, epsilon, max_relative)
                && self
                    .bottom
                    .relative_eq(&other.bottom, epsilon, max_relative)
        }
    }
}

#[cfg(test)]
mod tests {

    use approx::assert_relative_eq;

    use super::*;
    use crate::vec3;

    #[test]
    fn test_construct() {
        let v = SpatialVector::<()>::from_array([1., 2., 3., 4., 5., 6.]);
        assert_eq!(v.top, vec3(1., 2., 3.));
        assert_eq!(v.bottom, vec3(4., 5., 6.));

        let v = SpatialVector::<()>::from_vec6(SVector::from_iterator([1., 2., 3., 4., 5., 6.]));
        assert_eq!(v.top, vec3(1., 2., 3.));
        assert_eq!(v.bottom, vec3(4., 5., 6.));
    }

    #[test]
    fn test_add() {
        let v1 = SpatialVector::<()>::from_array([1., 2., 3., 4., 5., 6.]);
        let v2 = SpatialVector::<()>::from_array([7., 8., 9., 10., 11., 12.]);

        let result = v1 + v2;

        assert_eq!(result.top, vec3(8., 10., 12.));
        assert_eq!(result.bottom, vec3(14., 16., 18.));
    }

    #[test]
    fn test_add_assign() {
        let mut v1 = SpatialVector::<()>::from_array([1., 2., 3., 4., 5., 6.]);
        let v2 = SpatialVector::<()>::from_array([7., 8., 9., 10., 11., 12.]);

        v1 += v2;

        assert_eq!(v1.top, vec3(8., 10., 12.));
        assert_eq!(v1.bottom, vec3(14., 16., 18.));
    }

    #[test]
    fn test_sub() {
        let v1 = SpatialVector::<()>::from_array([1., 2., 3., 4., 5., 6.]);
        let v2 = SpatialVector::<()>::from_array([7., 8., 9., 10., 11., 12.]);

        let result = v1 - v2;

        assert_eq!(result.top, vec3(-6., -6., -6.));
        assert_eq!(result.bottom, vec3(-6., -6., -6.));
    }

    #[test]
    fn test_sub_assign() {
        let mut v1 = SpatialVector::<()>::from_array([1., 2., 3., 4., 5., 6.]);
        let v2 = SpatialVector::<()>::from_array([7., 8., 9., 10., 11., 12.]);

        v1 -= v2;

        assert_eq!(v1.top, vec3(-6., -6., -6.));
        assert_eq!(v1.bottom, vec3(-6., -6., -6.));
    }

    #[test]
    fn test_mul() {
        let v = SpatialVector::<()>::from_array([1., 2., 3., 4., 5., 6.]);
        let scalar = 2.0;

        let result = v * scalar;

        assert_eq!(result.top, vec3(2., 4., 6.));
        assert_eq!(result.bottom, vec3(8., 10., 12.));
    }

    #[test]
    fn test_neg() {
        let v = SpatialVector::<()>::from_array([1., 2., 3., 4., 5., 6.]);

        let result = v.neg();

        assert_eq!(result.top, vec3(-1., -2., -3.));
        assert_eq!(result.bottom, vec3(-4., -5., -6.));
    }

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

        let result = spatial_v1.cross(&spatial_v2);

        let w1 = spatial_v1.top;
        let v1 = spatial_v1.bottom;
        let w2 = spatial_v2.top;
        let v2 = spatial_v2.bottom;

        assert_relative_eq!(result.top, w1.cross(&w2));
        assert_relative_eq!(result.bottom, w1.cross(&v2) + v1.cross(&w2));
    }
}