spatial_motion/
transform.rs

1use super::Movement;
2
3use scalars::{Exp, Inv, One, Zero};
4use vector_space::{AffineSpace, VectorSpace, interpolate};
5
6use core::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
7
8#[derive(Copy, Clone, Debug, PartialEq, Eq)]
9pub struct Transform<P, R> {
10    pub position: P,
11    pub orientation: R,
12}
13
14impl<P, R> Transform<P, R>
15where
16    P: Clone + AffineSpace + Add<P::Diff, Output = P>,
17    P::Diff: VectorSpace,
18    R: Clone + AffineSpace + Add<R::Diff, Output = R>,
19    R::Diff: VectorSpace<Scalar = <P::Diff as VectorSpace>::Scalar>,
20{
21    pub fn lerp(&self, other: &Self, ratio: <P::Diff as VectorSpace>::Scalar) -> Self {
22        Self {
23            position: interpolate(self.position.clone(), other.position.clone(), ratio),
24            orientation: interpolate(self.orientation.clone(), other.orientation.clone(), ratio),
25        }
26    }
27}
28
29impl<P: Copy + Add<Output = P>, R: vector_space::Transform<P>> Transform<P, R> {
30    pub fn apply(&self, point: P) -> P {
31        self.position + self.orientation.apply_point(point)
32    }
33}
34
35impl<P: Zero, R: One> One for Transform<P, R> {
36    fn one() -> Self {
37        Self {
38            position: P::zero(),
39            orientation: R::one(),
40        }
41    }
42
43    fn is_one(&self) -> bool {
44        self.position.is_zero() && self.orientation.is_one()
45    }
46}
47
48impl<P, R> Mul for Transform<P, R>
49where
50    P: Add<Output = P>,
51    R: Copy + Mul<Output = R> + vector_space::Transform<P>,
52{
53    type Output = Self;
54
55    fn mul(self, other: Self) -> Self {
56        Self {
57            position: self.position + self.orientation.apply_point(other.position),
58            orientation: self.orientation * other.orientation,
59        }
60    }
61}
62
63impl<P, R> Div for Transform<P, R>
64where
65    P: Add<Output = P> + Neg<Output = P>,
66    R: Copy + Mul<Output = R> + Inv<Output = R> + vector_space::Transform<P>,
67{
68    type Output = Self;
69
70    fn div(self, other: Self) -> Self {
71        self * other.inv()
72    }
73}
74
75impl<P, R> Inv for Transform<P, R>
76where
77    P: Neg<Output = P>,
78    R: Copy + Inv<Output = R> + vector_space::Transform<P>,
79{
80    type Output = Self;
81
82    fn inv(self) -> Self {
83        let orientation = self.orientation.inv();
84        Self {
85            position: -orientation.apply_point(self.position),
86            orientation,
87        }
88    }
89}
90
91impl<P, R> MulAssign for Transform<P, R>
92where
93    P: AddAssign,
94    R: Copy + MulAssign + vector_space::Transform<P>,
95{
96    fn mul_assign(&mut self, other: Self) {
97        self.position += self.orientation.apply_point(other.position);
98        self.orientation *= other.orientation;
99    }
100}
101
102impl<P, R> DivAssign for Transform<P, R>
103where
104    P: SubAssign,
105    R: Copy + DivAssign + vector_space::Transform<P>,
106{
107    fn div_assign(&mut self, other: Self) {
108        self.orientation /= other.orientation;
109        self.position -= self.orientation.apply_point(other.position);
110    }
111}
112
113impl<P, R, V, B> Add<Movement<V, B>> for Transform<P, R>
114where
115    P: Add<V, Output = P>,
116    B: Exp<Output = R>,
117    R: Mul<Output = R>,
118{
119    type Output = Self;
120
121    fn add(self, movement: Movement<V, B>) -> Self {
122        Self {
123            position: self.position + movement.velocity,
124            orientation: self.orientation * movement.spin.exp(),
125        }
126    }
127}
128
129impl<P, R, V, B> Sub<Movement<V, B>> for Transform<P, R>
130where
131    P: Sub<V, Output = P>,
132    B: Exp<Output = R>,
133    R: Div<Output = R>,
134{
135    type Output = Self;
136
137    fn sub(self, movement: Movement<V, B>) -> Self {
138        Self {
139            position: self.position - movement.velocity,
140            orientation: self.orientation / movement.spin.exp(),
141        }
142    }
143}
144
145impl<P, R, V, B> AddAssign<Movement<V, B>> for Transform<P, R>
146where
147    P: AddAssign<V>,
148    B: Exp<Output = R>,
149    R: MulAssign,
150{
151    fn add_assign(&mut self, movement: Movement<V, B>) {
152        self.position += movement.velocity;
153        self.orientation *= movement.spin.exp();
154    }
155}
156
157impl<P, R, V, B> SubAssign<Movement<V, B>> for Transform<P, R>
158where
159    P: SubAssign<V>,
160    B: Exp<Output = R>,
161    R: DivAssign,
162{
163    fn sub_assign(&mut self, movement: Movement<V, B>) {
164        self.position -= movement.velocity;
165        self.orientation /= movement.spin.exp();
166    }
167}
168
169impl<P, R> vector_space::Transform<P> for Transform<P, R>
170where
171    P: Copy + Add<Output = P>,
172    R: vector_space::Transform<P>,
173{
174    fn apply_point(&self, point: P) -> P {
175        self.apply(point)
176    }
177}
spatial_motion/transform.rs

spatial_motion/
transform.rs
