ear_algae/
rotor.rs

1use crate::{
2    Mat, Nrml, Vect,
3    op_wrapper::{Sc, scs},
4    ops::{Apl, BefAft},
5    traits::Field,
6};
7use std::ops::Mul;
8
9use culit::culit;
10use maybe_trait::Maybe;
11
12#[cfg(feature = "serde")]
13use serde::{Deserialize, Deserializer, Serialize, Serializer};
14
15mod cross_product;
16mod rot2;
17mod rot3;
18
19pub trait RotInner<const N: usize, S: Field>: Sized + Copy {
20    type Bivector;
21    type Axis: Mul<S, Output = Self::Bivector>;
22
23    const IDENT: Self;
24
25    fn angle_axis(angle: S, axis: Self::Axis) -> Self;
26    fn from_to(from: Nrml<N, S>, to: Nrml<N, S>) -> Self;
27
28    /// This is like angle_axis, except that the angle is the magnitude of the input.
29    fn from_torq(axis: Self::Bivector) -> Self;
30
31    /// .
32    ///
33    /// # Safety
34    /// Safe when `w*w` + `bi.dot(bi)` = 1.
35    /// That is, the created rotor is properly unitized.
36    ///
37    /// .
38    unsafe fn from_w_bi_unchecked(w: S, bi: Self::Bivector) -> Self;
39
40    fn angle(self) -> S;
41    fn axis(self) -> Option<Self::Axis>;
42
43    fn axis_or_zero(self) -> Self::Bivector;
44
45    fn w(self) -> S;
46    fn bi(self) -> Self::Bivector;
47
48    fn to_torq(self) -> Self::Bivector;
49
50    fn part(self, t: S) -> Self;
51    fn inv(self) -> Self;
52
53    fn aft(self, other: Self) -> Self;
54
55    fn apl(self, vect: Vect<N, S>) -> Vect<N, S>;
56
57    fn normalize_bivector(vector: Self::Bivector) -> Option<Self::Axis>;
58
59    fn mat(self) -> Mat<N, N, S>;
60}
61
62pub struct RotBivectorRepr<R>(R);
63
64pub trait RotDim<const N: usize> {
65    type Inner<S: Field>: RotInner<N, S>;
66}
67
68#[derive(Copy, Clone)]
69pub struct Rot<const N: usize, S: Field>(<() as RotDim<N>>::Inner<S>)
70where
71    (): RotDim<N>;
72pub(crate) type Axis<const N: usize, S> = <<() as RotDim<N>>::Inner<S> as RotInner<N, S>>::Axis;
73pub(crate) type Bivector<const N: usize, S> =
74    <<() as RotDim<N>>::Inner<S> as RotInner<N, S>>::Bivector;
75
76impl<const N: usize, S: Field> Default for Rot<N, S>
77where
78    (): RotDim<N>,
79{
80    fn default() -> Self {
81        Self(RotInner::IDENT)
82    }
83}
84
85impl<const N: usize, S: Field> Rot<N, S>
86where
87    (): RotDim<N>,
88{
89    pub const IDENT: Self = Self(RotInner::IDENT);
90
91    pub fn angle_axis(angle: S, axis: impl Maybe<Axis<N, S>>) -> Self {
92        if let Some(axis) = axis.maybe() {
93            Self(RotInner::angle_axis(angle, axis))
94        } else {
95            Self::IDENT
96        }
97    }
98
99    pub fn from_to(from: impl Maybe<Nrml<N, S>>, to: impl Maybe<Nrml<N, S>>) -> Self {
100        if let Some(from) = from.maybe()
101            && let Some(to) = to.maybe()
102        {
103            Self(RotInner::from_to(from, to))
104        } else {
105            Self::IDENT
106        }
107    }
108
109    /// # Safety
110    /// Safe when `w*w + bi.dot(bi) =~ 1.`
111    /// That is, the created rotor is properly unitized.
112    pub unsafe fn from_w_bi_unchecked(w: S, bi: Bivector<N, S>) -> Self {
113        Self(unsafe { RotInner::from_w_bi_unchecked(w, bi) })
114    }
115
116    pub fn from_torq(torq: Bivector<N, S>) -> Self {
117        Self(RotInner::from_torq(torq))
118    }
119
120    pub fn angle(self) -> S {
121        self.0.angle()
122    }
123    pub fn axis(self) -> Option<Axis<N, S>> {
124        self.0.axis()
125    }
126
127    pub fn axis_or_zero(self) -> Bivector<N, S> {
128        self.0.axis_or_zero()
129    }
130
131    pub fn w(self) -> S {
132        self.0.w()
133    }
134
135    pub fn bi(self) -> Bivector<N, S> {
136        self.0.bi()
137    }
138
139    pub fn to_torq(self) -> Bivector<N, S> {
140        self.0.to_torq()
141    }
142
143    pub fn part(self, t: S) -> Self {
144        Self(self.0.part(t))
145    }
146    pub fn inv(self) -> Self {
147        Self(self.0.inv())
148    }
149
150    pub fn mat(self) -> Mat<N, N, S> {
151        self.0.mat()
152    }
153}
154
155impl<const N: usize, S: Field> BefAft for Rot<N, S>
156where
157    (): RotDim<N>,
158{
159    fn aft(self, other: Self) -> Self {
160        Self(self.0.aft(other.0))
161    }
162}
163
164impl<const N: usize, S: Field> Apl<Vect<N, S>> for Rot<N, S>
165where
166    (): RotDim<N>,
167{
168    type Output = Vect<N, S>;
169
170    fn apl(self, other: Vect<N, S>) -> Self::Output {
171        RotInner::apl(self.0, other)
172    }
173}
174
175impl<const N: usize, S: Field> Apl<Nrml<N, S>> for Rot<N, S>
176where
177    (): RotDim<N>,
178{
179    type Output = Nrml<N, S>;
180
181    fn apl(self, other: Nrml<N, S>) -> Self::Output {
182        RotInner::apl(self.0, other.into()).normal().unwrap()
183    }
184}
185
186impl<S: Field> Rot<2, S> {
187    pub fn angle2(angle: S) -> Self {
188        Self::angle_axis(angle, Nrml::axis(0))
189    }
190
191    pub fn signed_angle(self) -> S {
192        self.angle()
193            .mul(self.bi().normal_or_zero()[0])
194            .add(S::PI)
195            .rem_euclid(S::PI.add(S::PI))
196            .sub(S::PI)
197    }
198
199    pub fn lift<const N: usize, R: RotInner<N, S>>(self, axis: R::Axis) -> R {
200        let w = self.w();
201        let bi = axis * self.bi()[0];
202        unsafe { R::from_w_bi_unchecked(w, bi) }
203    }
204}
205
206impl<S: Field> Rot<3, S> {
207    #[culit]
208    pub fn pitch(angle: S) -> Self {
209        let (sin, cos) = (Sc(angle) / 2Sc).sin_cos();
210        unsafe { Self::from_w_bi_unchecked(cos.0, Vect::axis(0, sin.0)) }
211    }
212
213    #[culit]
214    pub fn yaw(angle: S) -> Self {
215        let (sin, cos) = (Sc(angle) / 2Sc).sin_cos();
216        unsafe { Self::from_w_bi_unchecked(cos.0, Vect::axis(1, sin.0)) }
217    }
218
219    #[culit]
220    pub fn roll(angle: S) -> Self {
221        let (sin, cos) = (Sc(angle) / 2Sc).sin_cos();
222        unsafe { Self::from_w_bi_unchecked(cos.0, Vect::axis(2, sin.0)) }
223    }
224
225    #[culit]
226    pub fn euler_angles(self) -> (S, S, S) {
227        let w = self.w();
228        let Vect([x, y, z]) = self.bi();
229        scs!(w, x, y, z);
230
231        (
232            Sc::atan2(2Sc * (w * y + z * x), 1Sc - 2Sc * (y.pow(2) + x.pow(2))).0,
233            (2Sc * (w * x - (y - z))).clamp(-1Sc, 1Sc).asin().0,
234            Sc::atan2(2Sc * (w * z + x * y), 1Sc - 2Sc * (z.pow(2) + x.pow(2))).0,
235        )
236    }
237}
238
239#[cfg(feature = "serde")]
240impl<const N: usize, S: Field> Serialize for Rot<N, S>
241where
242    (): RotDim<N>,
243    Bivector<N, S>: Serialize,
244{
245    fn serialize<Ser: Serializer>(&self, serializer: Ser) -> Result<Ser::Ok, Ser::Error> {
246        self.to_torq().serialize(serializer)
247    }
248}
249
250#[cfg(feature = "serde")]
251impl<'de, const N: usize, S: Field> Deserialize<'de> for Rot<N, S>
252where
253    (): RotDim<N>,
254    Bivector<N, S>: Deserialize<'de>,
255{
256    fn deserialize<D: Deserializer<'de>>(deserializer: D) -> Result<Self, D::Error> {
257        Ok(Self::from_torq(Bivector::<N, S>::deserialize(
258            deserializer,
259        )?))
260    }
261}
ear_algae/rotor.rs

ear_algae/
rotor.rs
