spatial-motion 0.2.0

use super::Movement;

use scalars::{Exp, Inv, One, Zero};
use vector_space::{AffineSpace, VectorSpace, interpolate};

use core::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct Transform<P, R> {
    pub position: P,
    pub orientation: R,
}

impl<P, R> Transform<P, R>
where
    P: Clone + AffineSpace + Add<P::Diff, Output = P>,
    P::Diff: VectorSpace,
    R: Clone + AffineSpace + Add<R::Diff, Output = R>,
    R::Diff: VectorSpace<Scalar = <P::Diff as VectorSpace>::Scalar>,
{
    pub fn lerp(&self, other: &Self, ratio: <P::Diff as VectorSpace>::Scalar) -> Self {
        Self {
            position: interpolate(self.position.clone(), other.position.clone(), ratio),
            orientation: interpolate(self.orientation.clone(), other.orientation.clone(), ratio),
        }
    }
}

impl<P: Copy + Add<Output = P>, R: vector_space::Transform<P>> Transform<P, R> {
    pub fn apply(&self, point: P) -> P {
        self.position + self.orientation.apply_point(point)
    }
}

impl<P: Zero, R: One> One for Transform<P, R> {
    fn one() -> Self {
        Self {
            position: P::zero(),
            orientation: R::one(),
        }
    }

    fn is_one(&self) -> bool {
        self.position.is_zero() && self.orientation.is_one()
    }
}

impl<P, R> Mul for Transform<P, R>
where
    P: Add<Output = P>,
    R: Copy + Mul<Output = R> + vector_space::Transform<P>,
{
    type Output = Self;

    fn mul(self, other: Self) -> Self {
        Self {
            position: self.position + self.orientation.apply_point(other.position),
            orientation: self.orientation * other.orientation,
        }
    }
}

impl<P, R> Div for Transform<P, R>
where
    P: Add<Output = P> + Neg<Output = P>,
    R: Copy + Mul<Output = R> + Inv<Output = R> + vector_space::Transform<P>,
{
    type Output = Self;

    fn div(self, other: Self) -> Self {
        self * other.inv()
    }
}

impl<P, R> Inv for Transform<P, R>
where
    P: Neg<Output = P>,
    R: Copy + Inv<Output = R> + vector_space::Transform<P>,
{
    type Output = Self;

    fn inv(self) -> Self {
        let orientation = self.orientation.inv();
        Self {
            position: -orientation.apply_point(self.position),
            orientation,
        }
    }
}

impl<P, R> MulAssign for Transform<P, R>
where
    P: AddAssign,
    R: Copy + MulAssign + vector_space::Transform<P>,
{
    fn mul_assign(&mut self, other: Self) {
        self.position += self.orientation.apply_point(other.position);
        self.orientation *= other.orientation;
    }
}

impl<P, R> DivAssign for Transform<P, R>
where
    P: SubAssign,
    R: Copy + DivAssign + vector_space::Transform<P>,
{
    fn div_assign(&mut self, other: Self) {
        self.orientation /= other.orientation;
        self.position -= self.orientation.apply_point(other.position);
    }
}

impl<P, R, V, B> Add<Movement<V, B>> for Transform<P, R>
where
    P: Add<V, Output = P>,
    B: Exp<Output = R>,
    R: Mul<Output = R>,
{
    type Output = Self;

    fn add(self, movement: Movement<V, B>) -> Self {
        Self {
            position: self.position + movement.velocity,
            orientation: self.orientation * movement.spin.exp(),
        }
    }
}

impl<P, R, V, B> Sub<Movement<V, B>> for Transform<P, R>
where
    P: Sub<V, Output = P>,
    B: Exp<Output = R>,
    R: Div<Output = R>,
{
    type Output = Self;

    fn sub(self, movement: Movement<V, B>) -> Self {
        Self {
            position: self.position - movement.velocity,
            orientation: self.orientation / movement.spin.exp(),
        }
    }
}

impl<P, R, V, B> AddAssign<Movement<V, B>> for Transform<P, R>
where
    P: AddAssign<V>,
    B: Exp<Output = R>,
    R: MulAssign,
{
    fn add_assign(&mut self, movement: Movement<V, B>) {
        self.position += movement.velocity;
        self.orientation *= movement.spin.exp();
    }
}

impl<P, R, V, B> SubAssign<Movement<V, B>> for Transform<P, R>
where
    P: SubAssign<V>,
    B: Exp<Output = R>,
    R: DivAssign,
{
    fn sub_assign(&mut self, movement: Movement<V, B>) {
        self.position -= movement.velocity;
        self.orientation /= movement.spin.exp();
    }
}

impl<P, R> vector_space::Transform<P> for Transform<P, R>
where
    P: Copy + Add<Output = P>,
    R: vector_space::Transform<P>,
{
    fn apply_point(&self, point: P) -> P {
        self.apply(point)
    }
}