lox_frames/transformations/
rotations.rs

1use glam::{DMat3, DVec3};
2
3pub fn rotation_matrix_derivative(m: DMat3, v: DVec3) -> DMat3 {
4    let sx = DVec3::new(0.0, v.z, v.y);
5    let sy = DVec3::new(-v.z, 0.0, v.x);
6    let sz = DVec3::new(v.y, -v.x, 0.0);
7    let s = DMat3::from_cols(sx, sy, sz);
8    -s * m
9}
10
11pub struct Rotation {
12    m: DMat3,
13    dm: DMat3,
14}
15
16impl Rotation {
17    pub(crate) const IDENTITY: Self = Self {
18        m: DMat3::IDENTITY,
19        dm: DMat3::ZERO,
20    };
21
22    pub fn new(m: DMat3) -> Self {
23        Self { m, dm: DMat3::ZERO }
24    }
25
26    pub fn with_derivative(mut self, dm: DMat3) -> Self {
27        self.dm = dm;
28        self
29    }
30
31    pub fn with_angular_velocity(mut self, v: DVec3) -> Self {
32        self.dm = rotation_matrix_derivative(self.m, v);
33        self
34    }
35
36    pub fn position_matrix(&self) -> DMat3 {
37        self.m
38    }
39
40    pub fn velocity_matrix(&self) -> DMat3 {
41        self.dm
42    }
43
44    pub fn compose(&self, other: &Self) -> Self {
45        Self {
46            m: other.m * self.m,
47            dm: other.dm * self.m + other.m * self.dm,
48        }
49    }
50
51    pub fn transpose(&self) -> Self {
52        let m = self.m.transpose();
53        let dm = self.dm.transpose();
54        Self { m, dm }
55    }
56
57    pub fn rotate_position(&self, pos: DVec3) -> DVec3 {
58        self.m * pos
59    }
60
61    pub fn rotate_velocity(&self, pos: DVec3, vel: DVec3) -> DVec3 {
62        self.dm * pos + self.m * vel
63    }
64
65    pub fn rotate_state(&self, pos: DVec3, vel: DVec3) -> (DVec3, DVec3) {
66        (self.rotate_position(pos), self.rotate_velocity(pos, vel))
67    }
68}
lox_frames/transformations/rotations.rs

lox_frames/transformations/
rotations.rs
