multicalc 0.10.0

//! Typed spatial force.

use core::ops::{Add, Neg, Sub};

use crate::linear_algebra::{Vector, Vector3D, Vector6D};
use crate::scalar::Numeric;

/// A spatial force (wrench), stored force-first in the `[force; torque]` ordering — reciprocal to a
/// [`Twist`](crate::spatial::Twist), so the two line up component-for-component.
///
/// Like `Twist`, the type owns its layout: the only value constructor takes the force and torque
/// parts by name, and the flat converters emit `[f; τ]`. `Add`/`Sub`/`Neg`/[`scale`](Wrench::scale)
/// act component-wise; the spatial *algebra* (coordinate transforms, the `twist · wrench` power
/// product) is not defined here.
///
/// ```
/// use multicalc::spatial::Wrench;
/// let a = Wrench::from_array([1.0_f64, 2.0, 3.0, 4.0, 5.0, 6.0]);
/// let b = Wrench::from_array([1.0_f64; 6]);
/// assert_eq!((a + b).as_array(), [2.0, 3.0, 4.0, 5.0, 6.0, 7.0]);
/// assert_eq!((a - b).as_array(), [0.0, 1.0, 2.0, 3.0, 4.0, 5.0]);
/// assert_eq!((-a).as_array(), [-1.0, -2.0, -3.0, -4.0, -5.0, -6.0]);
/// assert_eq!(a.scale(2.0).as_array(), [2.0, 4.0, 6.0, 8.0, 10.0, 12.0]);
/// ```
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Wrench<T: Numeric = f64> {
    force: Vector3D<T>,
    torque: Vector3D<T>,
}

impl<T: Numeric> Wrench<T> {
    /// A wrench from its force and torque parts.
    ///
    /// ```
    /// use multicalc::linear_algebra::Vector;
    /// use multicalc::spatial::Wrench;
    /// let force = Vector::new([1.0_f64, 2.0, 3.0]);
    /// let torque = Vector::new([4.0, 5.0, 6.0]);
    /// let w = Wrench::new(force, torque);
    /// assert_eq!(w.force(), Vector::new([1.0, 2.0, 3.0]));
    /// assert_eq!(w.torque(), Vector::new([4.0, 5.0, 6.0]));
    /// ```
    #[inline]
    #[must_use]
    pub fn new(force: Vector3D<T>, torque: Vector3D<T>) -> Self {
        Wrench { force, torque }
    }

    /// The zero wrench.
    ///
    /// ```
    /// use multicalc::spatial::Wrench;
    /// assert_eq!(Wrench::<f64>::zeros().as_array(), [0.0; 6]);
    /// ```
    #[inline]
    #[must_use]
    pub fn zeros() -> Self {
        Wrench {
            force: Vector::zeros(),
            torque: Vector::zeros(),
        }
    }

    /// A wrench from a `[fx, fy, fz, τx, τy, τz]` array.
    ///
    /// ```
    /// use multicalc::linear_algebra::Vector;
    /// use multicalc::spatial::Wrench;
    /// let w = Wrench::from_array([1.0_f64, 2.0, 3.0, 4.0, 5.0, 6.0]);
    /// assert_eq!(w.torque(), Vector::new([4.0, 5.0, 6.0]));
    /// ```
    #[inline]
    #[must_use]
    pub fn from_array(a: [T; 6]) -> Self {
        let [vx, vy, vz, wx, wy, wz] = a;
        Wrench {
            force: Vector::new([vx, vy, vz]),
            torque: Vector::new([wx, wy, wz]),
        }
    }

    /// The wrench as a `[fx, fy, fz, τx, τy, τz]` array.
    ///
    /// ```
    /// use multicalc::spatial::Wrench;
    /// let w = Wrench::from_array([1.0_f64, 2.0, 3.0, 4.0, 5.0, 6.0]);
    /// assert_eq!(w.as_array(), [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
    /// ```
    #[inline]
    #[must_use]
    pub fn as_array(self) -> [T; 6] {
        let [fx, fy, fz] = *self.force.as_array();
        let [tx, ty, tz] = *self.torque.as_array();
        [fx, fy, fz, tx, ty, tz]
    }

    /// A wrench from a flat `[f; τ]` `Vector6D`.
    ///
    /// ```
    /// use multicalc::linear_algebra::Vector;
    /// use multicalc::spatial::Wrench;
    /// let w = Wrench::from_vector(Vector::new([1.0_f64, 2.0, 3.0, 4.0, 5.0, 6.0]));
    /// assert_eq!(w.force(), Vector::new([1.0, 2.0, 3.0]));
    /// ```
    #[inline]
    #[must_use]
    pub fn from_vector(v: Vector6D<T>) -> Self {
        Self::from_array(v.into_array())
    }

    /// The wrench as a flat `[f; τ]` `Vector6D`.
    ///
    /// ```
    /// use multicalc::linear_algebra::Vector;
    /// use multicalc::spatial::Wrench;
    /// let w = Wrench::from_array([1.0_f64, 2.0, 3.0, 4.0, 5.0, 6.0]);
    /// assert_eq!(w.to_vector(), Vector::new([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]));
    /// ```
    #[inline]
    pub fn to_vector(self) -> Vector6D<T> {
        Vector::new(self.as_array())
    }

    /// The force part `f`.
    #[inline]
    pub fn force(self) -> Vector3D<T> {
        self.force
    }

    /// The torque (moment) part `τ`.
    #[inline]
    pub fn torque(self) -> Vector3D<T> {
        self.torque
    }

    /// Multiplies both parts by `scalar`.
    ///
    /// ```
    /// use multicalc::spatial::Wrench;
    /// let w = Wrench::from_array([1.0_f64, 2.0, 3.0, 4.0, 5.0, 6.0]);
    /// assert_eq!(w.scale(2.0).as_array(), [2.0, 4.0, 6.0, 8.0, 10.0, 12.0]);
    /// ```
    #[inline]
    #[must_use]
    pub fn scale(self, scalar: T) -> Self {
        Wrench {
            force: self.force.scale(scalar),
            torque: self.torque.scale(scalar),
        }
    }
}

impl<T: Numeric> Add for Wrench<T> {
    type Output = Self;

    #[inline]
    fn add(self, rhs: Self) -> Self {
        Wrench {
            force: self.force + rhs.force,
            torque: self.torque + rhs.torque,
        }
    }
}

impl<T: Numeric> Sub for Wrench<T> {
    type Output = Self;

    #[inline]
    fn sub(self, rhs: Self) -> Self {
        Wrench {
            force: self.force - rhs.force,
            torque: self.torque - rhs.torque,
        }
    }
}

impl<T: Numeric> Neg for Wrench<T> {
    type Output = Self;

    #[inline]
    fn neg(self) -> Self {
        Wrench {
            force: -self.force,
            torque: -self.torque,
        }
    }
}

impl<T: Numeric> From<Vector6D<T>> for Wrench<T> {
    /// Reinterprets a flat `[f; τ]` `Vector6D` as a wrench.
    #[inline]
    fn from(v: Vector6D<T>) -> Self {
        Self::from_vector(v)
    }
}

impl<T: Numeric> From<Wrench<T>> for Vector6D<T> {
    /// Flattens a wrench into `[f; τ]`.
    #[inline]
    fn from(w: Wrench<T>) -> Self {
        w.to_vector()
    }
}

impl<T: Numeric> Default for Wrench<T> {
    /// Returns the zero wrench as default.
    ///
    /// ```
    /// use multicalc::Wrench;
    ///
    /// let default_wrench = Wrench::default();
    /// let wrench = Wrench::<f64>::from_array([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
    ///
    /// assert_eq!(wrench + default_wrench, wrench);
    /// ```
    fn default() -> Self {
        Self::zeros()
    }
}