ear_algae/rotor/
rot3.rs

1use culit::culit;
2
3use super::*;
4
5use crate::{
6    op_wrapper::{Sc, scs},
7    ops::{Cross as _, Dot as _},
8};
9
10impl RotDim<3> for () {
11    type Inner<S: Field> = RotInner3<S>;
12}
13
14#[derive(Copy, Clone)]
15pub struct RotInner3<S: Field>(S, Vect<3, S>);
16
17impl<S: Field> RotInner3<S> {
18    fn renormalize(self) -> Self {
19        if let Some(normal) = Vect([self.0, self.1[0], self.1[1], self.1[2]]).normal() {
20            Self(normal[0], Vect([normal[1], normal[2], normal[3]]))
21        } else {
22            Self::IDENT
23        }
24    }
25}
26
27impl<S: Field> RotInner<3, S> for RotInner3<S> {
28    type Bivector = Vect<3, S>;
29    type Axis = Nrml<3, S>;
30
31    const IDENT: Self = Self(S::ONE, Vect::ZERO);
32
33    #[culit]
34    fn angle_axis(angle: S, axis: Nrml<3, S>) -> Self {
35        let (sin, cos) = (Sc(angle) / 2Sc).sin_cos();
36        Self(cos.0, axis * sin.0)
37    }
38
39    fn from_to(from: Nrml<3, S>, to: Nrml<3, S>) -> Self {
40        let dot = to.dot(from);
41        let cross = from.cross(to);
42        if cross == Vect::ZERO {
43            //This all handles the edge case where from == -to
44            if dot > S::ZERO {
45                return Self::IDENT;
46            }
47
48            if let Some(axis) = from.cross(Nrml::axis(0)).normal() {
49                return Self(S::ZERO, axis.into());
50            } else {
51                return Self(S::ZERO, Vect::axis(1, S::ONE));
52            }
53        }
54
55        let sqrt = dot.add(S::ONE).max(S::ZERO).sqrt();
56        Self(sqrt.div(S::SQRT_2), cross / sqrt.mul(S::SQRT_2))
57    }
58
59    fn from_torq(ang: Self::Bivector) -> Self {
60        if let Some((angle, axis)) = ang.magn_normal() {
61            Self::angle_axis(angle, axis)
62        } else {
63            Self::IDENT
64        }
65    }
66
67    unsafe fn from_w_bi_unchecked(w: S, bi: Self::Bivector) -> Self {
68        Self(w, bi)
69    }
70
71    #[culit]
72    fn angle(self) -> S {
73        let w = Sc(self.0).clamp(-1Sc, 1Sc);
74        (2Sc * (w.acos())).0
75    }
76
77    fn axis(self) -> Option<Self::Axis> {
78        self.1.normal()
79    }
80
81    fn axis_or_zero(self) -> Self::Bivector {
82        self.1.normal_or_zero()
83    }
84
85    fn w(self) -> S {
86        self.0
87    }
88
89    fn bi(self) -> Self::Bivector {
90        self.1
91    }
92
93    fn to_torq(self) -> Self::Bivector {
94        self.1.normal_or_zero() * self.angle()
95    }
96
97    fn part(self, t: S) -> Self {
98        if let Some(normal) = self.1.normal() {
99            Self::angle_axis(self.angle().mul(t), normal)
100        } else {
101            Self::IDENT
102        }
103    }
104
105    fn inv(self) -> Self {
106        Self(self.0, -self.1)
107    }
108
109    fn aft(self, other: Self) -> Self {
110        Self(
111            self.0.mul(other.0).sub(self.1.dot(other.1)),
112            self.1.cross(other.1) + other.1 * self.0 + self.1 * other.0,
113        )
114        .renormalize()
115    }
116
117    fn apl(self, vect: Vect<3, S>) -> Vect<3, S> {
118        (vect * self.0.pow(2).sub(self.1.dot(self.1)))
119            + (self.1 * self.1.dot(vect) + self.1.cross(vect) * self.0) * S::TWO
120    }
121
122    fn normalize_bivector(vector: Self::Bivector) -> Option<Self::Axis> {
123        vector.normal()
124    }
125
126    #[culit]
127    fn mat(self) -> Mat<3, 3, S> {
128        let Self(w, Vect([x, y, z])) = self;
129        scs!(w, x, y, z);
130
131        Mat::IDENT
132            + Mat::from_scs([
133                [-(y.pow(2) + z.pow(2)), x * y - z * w, z * x + y * w],
134                [x * y + z * w, -(z.pow(2) + x.pow(2)), y * z - x * w],
135                [z * x - y * w, y * z + x * w, -(x.pow(2) + y.pow(2))],
136            ]) * 2S
137    }
138}
139
140#[cfg(test)]
141mod tests {
142    use super::*;
143
144    #[test]
145    fn test_from_to() {
146        let v1: Nrml<3, f32> = Vect([-2.34, 5.8, -0.8]).normal().unwrap();
147        let v2 = Vect([-8.2, 1.1, 4.]).normal().unwrap();
148
149        let q = Rot::from_to(v1, v2);
150        if (Vect::from(v2) - q.apl(Vect::from(v1))).magn() > 0.0001 {
151            panic!("{v2} != {}", q.apl(Vect::from(v1)));
152        }
153    }
154}
ear_algae/rotor/rot3.rs

ear_algae/rotor/
rot3.rs
