ga3/
rotor.rs

1use crate::*;
2
3use std::{
4    fmt::{self, Display, Formatter},
5    ops::{Div, DivAssign, Mul, MulAssign},
6    str::FromStr,
7};
8
9use derive_more::{Add, AddAssign, Neg, Sub, SubAssign};
10use inner_space::{DotProduct, InnerSpace, VectorSpace};
11use scalars::{Inv, One, Real, Zero};
12use vector_space::Transform;
13
14/// The 3D rotor type.
15#[derive(Copy, Clone, Debug, PartialEq, Eq, Add, AddAssign, Sub, SubAssign, Neg)]
16pub struct Rotor<T> {
17    /// The scalar component.
18    pub scalar: T,
19    /// The component representing the xy plane.
20    pub xy: T,
21    /// The component representing the xz plane.
22    pub xz: T,
23    /// The component representing the yz plane.
24    pub yz: T,
25}
26
27impl<T> Rotor<T> {
28    /// Creates a new rotor from its components.
29    pub fn new(scalar: T, xy: T, xz: T, yz: T) -> Self {
30        Self { scalar, xy, xz, yz }
31    }
32}
33
34impl<T: Real> Rotor<T> {
35    /// Rotates a vector by the rotor.
36    pub fn rotate(self, vector: Vector<T>) -> Vector<T> {
37        self.normalize().rotate_normalized(vector)
38    }
39
40    /// Rotates a vector by the rotor using the sandwich product R v R†.
41    /// The rotor has to be normalized already.
42    pub fn rotate_normalized(self, v: Vector<T>) -> Vector<T> {
43        let s = self.scalar;
44        let a = self.xy;
45        let b = self.xz;
46        let c = self.yz;
47
48        let two = T::one() + T::one();
49
50        let s2 = s * s;
51        let a2 = a * a;
52        let b2 = b * b;
53        let c2 = c * c;
54
55        Vector {
56            x: (s2 + c2 - a2 - b2) * v.x
57                + two * (s * a - b * c) * v.y
58                + two * (s * b + a * c) * v.z,
59            y: -two * (s * a + b * c) * v.x
60                + (s2 + b2 - a2 - c2) * v.y
61                + two * (s * c - a * b) * v.z,
62            z: two * (a * c - s * b) * v.x - two * (a * b + s * c) * v.y
63                + (s2 + a2 - b2 - c2) * v.z,
64        }
65    }
66
67    /// Rotates a bivector by the rotor.
68    pub fn rotate_bivector(self, bivector: Bivector<T>) -> Bivector<T> {
69        self.normalize().rotate_bivector_normalized(bivector)
70    }
71
72    /// Rotates a bivector by the rotor using the sandwich product R B R†.
73    /// The rotor has to be normalized already.
74    pub fn rotate_bivector_normalized(self, b: Bivector<T>) -> Bivector<T> {
75        let s = self.scalar;
76        let a = self.xy;
77        let br = self.xz;
78        let c = self.yz;
79
80        let two = T::one() + T::one();
81
82        let s2 = s * s;
83        let a2 = a * a;
84        let b2 = br * br;
85        let c2 = c * c;
86
87        Bivector {
88            xy: two * (a * c - s * br) * b.yz
89                + two * (a * br + s * c) * b.xz
90                + (s2 + a2 - b2 - c2) * b.xy,
91            xz: two * (s * a + br * c) * b.yz + (s2 + b2 - a2 - c2) * b.xz
92                - two * (s * c - a * br) * b.xy,
93            yz: (s2 + c2 - a2 - b2) * b.yz - two * (s * a - br * c) * b.xz
94                + two * (s * br + a * c) * b.xy,
95        }
96    }
97
98    /// The reverse of the rotor.
99    pub fn reverse(self) -> Self {
100        Self {
101            scalar: self.scalar,
102            xy: -self.xy,
103            xz: -self.xz,
104            yz: -self.yz,
105        }
106    }
107}
108
109impl<T: Zero> Rotor<T> {
110    /// Creates a new scalar rotor.
111    pub fn scalar(scalar: T) -> Self {
112        Self {
113            scalar,
114            xy: T::zero(),
115            xz: T::zero(),
116            yz: T::zero(),
117        }
118    }
119
120    /// Creates a new rotor along the xy plane.
121    pub fn xy(xy: T) -> Self {
122        Self {
123            scalar: T::zero(),
124            xy,
125            xz: T::zero(),
126            yz: T::zero(),
127        }
128    }
129
130    /// Creates a new rotor along the xz plane.
131    pub fn xz(xz: T) -> Self {
132        Self {
133            scalar: T::zero(),
134            xy: T::zero(),
135            xz,
136            yz: T::zero(),
137        }
138    }
139
140    /// Creates a new rotor along the yz plane.
141    pub fn yz(yz: T) -> Self {
142        Self {
143            scalar: T::zero(),
144            xy: T::zero(),
145            xz: T::zero(),
146            yz,
147        }
148    }
149}
150
151impl<T: Zero> Zero for Rotor<T> {
152    fn zero() -> Self {
153        Self {
154            scalar: T::zero(),
155            xy: T::zero(),
156            xz: T::zero(),
157            yz: T::zero(),
158        }
159    }
160
161    fn is_zero(&self) -> bool {
162        self.scalar.is_zero() && self.xy.is_zero() && self.xz.is_zero() && self.yz.is_zero()
163    }
164}
165
166impl<T: Real> One for Rotor<T> {
167    fn one() -> Self {
168        Self {
169            scalar: T::one(),
170            xy: T::zero(),
171            xz: T::zero(),
172            yz: T::zero(),
173        }
174    }
175
176    fn is_one(&self) -> bool {
177        self.scalar.is_one() && self.xy.is_zero() && self.xz.is_zero() && self.yz.is_zero()
178    }
179}
180
181impl<T: Real> Inv for Rotor<T> {
182    type Output = Self;
183
184    fn inv(self) -> Self {
185        self.reverse() / self.magnitude2()
186    }
187}
188
189impl<T: Real> From<T> for Rotor<T> {
190    fn from(scalar: T) -> Self {
191        Self::scalar(scalar)
192    }
193}
194
195impl<T> Mul<T> for Rotor<T>
196where
197    T: Mul<Output = T> + Copy,
198{
199    type Output = Self;
200    fn mul(self, other: T) -> Self {
201        Self {
202            scalar: self.scalar * other,
203            xy: self.xy * other,
204            xz: self.xz * other,
205            yz: self.yz * other,
206        }
207    }
208}
209
210impl<T> MulAssign<T> for Rotor<T>
211where
212    T: MulAssign + Copy,
213{
214    fn mul_assign(&mut self, other: T) {
215        self.scalar *= other;
216        self.xy *= other;
217        self.xz *= other;
218        self.yz *= other;
219    }
220}
221
222impl<T> Div<T> for Rotor<T>
223where
224    T: Div<Output = T> + Copy,
225{
226    type Output = Self;
227    fn div(self, other: T) -> Self {
228        Self {
229            scalar: self.scalar / other,
230            xy: self.xy / other,
231            xz: self.xz / other,
232            yz: self.yz / other,
233        }
234    }
235}
236
237impl<T> DivAssign<T> for Rotor<T>
238where
239    T: DivAssign + Copy,
240{
241    fn div_assign(&mut self, other: T) {
242        self.scalar /= other;
243        self.xy /= other;
244        self.xz /= other;
245        self.yz /= other;
246    }
247}
248
249impl<T: Real> VectorSpace for Rotor<T> {
250    type Scalar = T;
251}
252
253impl<T: Real> DotProduct for Rotor<T> {
254    type Output = Self::Scalar;
255
256    fn dot(&self, other: &Self) -> T {
257        self.scalar * other.scalar - self.xy * other.xy - self.xz * other.xz - self.yz * other.yz
258    }
259
260    fn scalar(&self, other: &Self) -> T {
261        self.scalar * other.scalar + self.xy * other.xy + self.xz * other.xz + self.yz * other.yz
262    }
263}
264
265impl<T: Real> DotProduct<Vector<T>> for Rotor<T> {
266    type Output = Vector<T>;
267    #[inline]
268    fn dot(&self, other: &Vector<T>) -> Vector<T> {
269        other.dot(self)
270    }
271}
272
273impl<T: Real> DotProduct<Bivector<T>> for Rotor<T> {
274    type Output = Self;
275    #[inline]
276    fn dot(&self, other: &Bivector<T>) -> Self {
277        other.dot(self)
278    }
279}
280
281impl<T: Real> Mul for Rotor<T> {
282    type Output = Self;
283    fn mul(self, other: Self) -> Self {
284        Self {
285            scalar: self.scalar * other.scalar
286                - self.xy * other.xy
287                - self.xz * other.xz
288                - self.yz * other.yz,
289            xy: self.scalar * other.xy + other.scalar * self.xy + self.xz * other.yz
290                - self.yz * other.xz,
291            xz: self.scalar * other.xz + other.scalar * self.xz - self.xy * other.yz
292                + self.yz * other.xy,
293            yz: self.scalar * other.yz + other.scalar * self.yz + self.xy * other.xz
294                - self.xz * other.xy,
295        }
296    }
297}
298
299impl<T: Real> MulAssign for Rotor<T> {
300    fn mul_assign(&mut self, other: Self) {
301        *self = *self * other;
302    }
303}
304
305impl<T: Real> Div for Rotor<T> {
306    type Output = Self;
307    fn div(self, other: Self) -> Self {
308        self * other.inv()
309    }
310}
311
312impl<T: Real> DivAssign for Rotor<T> {
313    fn div_assign(&mut self, other: Self) {
314        *self = *self / other;
315    }
316}
317
318impl<T: Real> Transform<Vector<T>> for Rotor<T> {
319    fn apply_point(&self, point: Vector<T>) -> Vector<T> {
320        self.rotate(point)
321    }
322}
323
324impl<T: Real> Transform<Bivector<T>> for Rotor<T> {
325    fn apply_point(&self, bivector: Bivector<T>) -> Bivector<T> {
326        self.rotate_bivector(bivector)
327    }
328}
329
330impl<T: Display> Display for Rotor<T> {
331    fn fmt(&self, formatter: &mut Formatter<'_>) -> fmt::Result {
332        write!(
333            formatter,
334            "({}, {}, {}, {})",
335            self.scalar, self.xy, self.xz, self.yz
336        )
337    }
338}
339
340impl<T: FromStr> FromStr for Rotor<T> {
341    type Err = ParseRotorError;
342
343    fn from_str(source: &str) -> Result<Self, Self::Err> {
344        let trimmed = source.trim();
345        let inner = trimmed
346            .strip_prefix('(')
347            .and_then(|s| s.strip_suffix(')'))
348            .ok_or(ParseRotorError)?;
349        let parts: Vec<&str> = inner.split(',').collect();
350        if parts.len() != 4 {
351            return Err(ParseRotorError);
352        }
353        let scalar = parts[0].trim().parse().map_err(|_| ParseRotorError)?;
354        let xy = parts[1].trim().parse().map_err(|_| ParseRotorError)?;
355        let xz = parts[2].trim().parse().map_err(|_| ParseRotorError)?;
356        let yz = parts[3].trim().parse().map_err(|_| ParseRotorError)?;
357        Ok(Self { scalar, xy, xz, yz })
358    }
359}
360
361/// Error returned when parsing a rotor from a string fails.
362#[derive(Debug, Clone, PartialEq, Eq)]
363pub struct ParseRotorError;
364
365impl Display for ParseRotorError {
366    fn fmt(&self, formatter: &mut Formatter<'_>) -> fmt::Result {
367        write!(formatter, "invalid rotor format")
368    }
369}
370
371impl std::error::Error for ParseRotorError {}
ga3/rotor.rs

ga3/
rotor.rs
