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> RotInner2<S> {
28    pub fn angle(angle: S) -> Self {
29        Self::angle_axis(angle, Nrml::axis(0))
30    }
31
32    pub fn signed_angle(self) -> S {
33        self.angle()
34            .mul(self.1.normal_or_zero()[0])
35            .add(S::PI)
36            .rem_euclid(S::PI.add(S::PI))
37            .sub(S::PI)
38    }
39
40    pub fn lift<const N: usize, R: RotInner<N, S>>(self, axis: R::Axis) -> R {
41        let w = self.0;
42        let bi = axis * self.1[0];
43        unsafe { R::from_w_bi_unchecked(w, bi) }
44    }
45}
46
47impl<S: Field> RotInner<2, S> for RotInner2<S> {
48    type Bivector = Vect<1, S>;
49    type Axis = Nrml<1, S>;
50
51    const IDENT: Self = Self(S::ONE, Vect::ZERO);
52
53    #[culit]
54    fn angle_axis(angle: S, axis: Self::Axis) -> Self {
55        let (sin, cos) = (Sc(angle) / 2Sc).sin_cos();
56        Self(cos.0, axis * sin.0)
57    }
58
59    #[culit]
60    fn from_to(from: Nrml<2, S>, to: Nrml<2, S>) -> Self {
61        let dot = to.dot(from);
62        let cross = from.cross(to);
63        if cross == Vect::ZERO {
64            if dot > 0S {
65                return Self::IDENT;
66            } else {
67                return Self(0S, Vect::axis(0, 1S));
68            }
69        }
70
71        let sqrt = (Sc(dot) + 1Sc).max(0Sc).sqrt();
72        Self((sqrt / Sc::SQRT_2).0, cross / (sqrt * Sc::SQRT_2).0)
73    }
74
75    fn from_torq(ang: Self::Bivector) -> Self {
76        if let Some((angle, axis)) = ang.magn_normal() {
77            Self::angle_axis(angle, axis)
78        } else {
79            Self::IDENT
80        }
81    }
82
83    unsafe fn from_w_bi_unchecked(w: S, bi: Self::Bivector) -> Self {
84        Self(w, bi)
85    }
86
87    #[culit]
88    fn angle(self) -> S {
89        let w = Sc(self.0).clamp(-1Sc, 1Sc);
90        (2Sc * w.acos()).0
91    }
92
93    fn axis(self) -> Option<Self::Axis> {
94        self.1.normal()
95    }
96
97    fn axis_or_zero(self) -> Self::Bivector {
98        self.1.normal_or_zero()
99    }
100
101    fn w(self) -> S {
102        self.0
103    }
104
105    fn bi(self) -> Self::Bivector {
106        self.1
107    }
108
109    fn to_torq(self) -> Self::Bivector {
110        self.1.normal_or_zero() * self.angle()
111    }
112
113    fn part(self, t: S) -> Self {
114        if let Some(normal) = self.1.normal() {
115            Self::angle_axis(self.angle().mul(t), normal)
116        } else {
117            Self::IDENT
118        }
119    }
120
121    fn inv(self) -> Self {
122        Self(self.0, -self.1)
123    }
124
125    fn aft(self, other: Self) -> Self {
126        Self(
127            self.0.mul(other.0).sub(self.1.dot(other.1)),
128            other.1 * self.0 + self.1 * other.0,
129        )
130        .reunitize()
131    }
132
133    #[culit]
134    fn apl(self, vect: Vect<2, S>) -> Vect<2, S> {
135        (vect * self.0.pow(2).sub(self.1.dot(self.1)))
136            + (Vect([vect[1].neg(), vect[0]]) * self.1[0] * self.0) * 2S
137    }
138
139    fn normalize_bivector(vector: Self::Bivector) -> Option<Self::Axis> {
140        vector.normal()
141    }
142
143    fn mat(self) -> Mat<2, 2, S> {
144        let Self(cos, Vect([sin])) = self;
145        scs!(cos, sin);
146        Mat::from_scs([[cos, -sin], [sin, cos]])
147    }
148}
ear_algae/rotor/rot2.rs

ear_algae/rotor/
rot2.rs
