ear_algae/rotor/
rot2.rs

1use culit::culit;
2
3use super::*;
4
5use crate::{
6    op_wrapper::{Sc, scs},
7    ops::{Cross, Dot},
8};
9
10impl RotDim<2> for () {
11    type Inner<S: Field> = RotInner2<S>;
12}
13
14#[derive(Copy, Clone, Debug)]
15pub struct RotInner2<S: Field>(S, Vect<1, S>);
16
17impl<S: Field> RotInner2<S> {
18    fn reunitize(self) -> Self {
19        if let Some(normal) = Vect([self.0, self.1[0]]).normal() {
20            Self(normal[0], Vect([normal[1]]))
21        } else {
22            Self::IDENT
23        }
24    }
25}
26
27impl<S: Field> RotInner<2, S> for RotInner2<S> {
28    type Bivector = Vect<1, S>;
29    type Axis = Nrml<1, S>;
30
31    const IDENT: Self = Self(S::ONE, Vect::ZERO);
32
33    #[culit]
34    fn angle_axis(angle: S, axis: Self::Axis) -> Self {
35        let (sin, cos) = (Sc(angle) / 2Sc).sin_cos();
36        Self(cos.0, axis * sin.0)
37    }
38
39    #[culit]
40    fn from_to(from: Nrml<2, S>, to: Nrml<2, S>) -> Self {
41        let dot = to.dot(from);
42        let cross = from.cross(to);
43        if cross == Vect::ZERO {
44            if dot > 0S {
45                return Self::IDENT;
46            } else {
47                return Self(0S, Vect::axis(0, 1S));
48            }
49        }
50
51        let sqrt = (Sc(dot) + 1Sc).max(0Sc).sqrt();
52        Self((sqrt / Sc::SQRT_2).0, cross / (sqrt * Sc::SQRT_2).0)
53    }
54
55    fn from_torq(ang: Self::Bivector) -> Self {
56        if let Some((angle, axis)) = ang.magn_normal() {
57            Self::angle_axis(angle, axis)
58        } else {
59            Self::IDENT
60        }
61    }
62
63    unsafe fn from_w_bi_unchecked(w: S, bi: Self::Bivector) -> Self {
64        Self(w, bi)
65    }
66
67    #[culit]
68    fn angle(self) -> S {
69        let w = Sc(self.0).clamp(-1Sc, 1Sc);
70        (2Sc * w.acos()).0
71    }
72
73    fn axis(self) -> Option<Self::Axis> {
74        self.1.normal()
75    }
76
77    fn axis_or_zero(self) -> Self::Bivector {
78        self.1.normal_or_zero()
79    }
80
81    fn w(self) -> S {
82        self.0
83    }
84
85    fn bi(self) -> Self::Bivector {
86        self.1
87    }
88
89    fn to_torq(self) -> Self::Bivector {
90        self.1.normal_or_zero() * self.angle()
91    }
92
93    fn part(self, t: S) -> Self {
94        if let Some(normal) = self.1.normal() {
95            Self::angle_axis(self.angle().mul(t), normal)
96        } else {
97            Self::IDENT
98        }
99    }
100
101    fn inv(self) -> Self {
102        Self(self.0, -self.1)
103    }
104
105    fn aft(self, other: Self) -> Self {
106        Self(
107            self.0.mul(other.0).sub(self.1.dot(other.1)),
108            other.1 * self.0 + self.1 * other.0,
109        )
110        .reunitize()
111    }
112
113    #[culit]
114    fn apl(self, vect: Vect<2, S>) -> Vect<2, S> {
115        (vect * self.0.pow(2).sub(self.1.dot(self.1)))
116            + (Vect([vect[1].neg(), vect[0]]) * self.1[0] * self.0) * 2S
117    }
118
119    fn normalize_bivector(vector: Self::Bivector) -> Option<Self::Axis> {
120        vector.normal()
121    }
122
123    fn mat(self) -> Mat<2, 2, S> {
124        let Self(cos, Vect([sin])) = self;
125        scs!(cos, sin);
126        Mat::from_scs([[cos, -sin], [sin, cos]])
127    }
128}
ear_algae/rotor/rot2.rs

ear_algae/rotor/
rot2.rs
