lox_frames/transformations/
rotations.rs

1// SPDX-FileCopyrightText: 2024 Helge Eichhorn <git@helgeeichhorn.de>
2//
3// SPDX-License-Identifier: MPL-2.0
4
5use std::ops::Mul;
6
7use glam::{DMat3, DVec3};
8use lox_core::coords::Cartesian;
9
10pub fn rotation_matrix_derivative(m: DMat3, v: DVec3) -> DMat3 {
11    let sx = DVec3::new(0.0, v.z, v.y);
12    let sy = DVec3::new(-v.z, 0.0, v.x);
13    let sz = DVec3::new(v.y, -v.x, 0.0);
14    let s = DMat3::from_cols(sx, sy, sz);
15    -s * m
16}
17
18pub struct Rotation {
19    /// Rotation matrix
20    m: DMat3,
21    /// Time derivative of the rotation matrix
22    dm: DMat3,
23}
24
25impl Rotation {
26    pub const IDENTITY: Self = Self {
27        m: DMat3::IDENTITY,
28        dm: DMat3::ZERO,
29    };
30
31    pub fn new(m: DMat3) -> Self {
32        Self { m, dm: DMat3::ZERO }
33    }
34
35    pub fn with_derivative(mut self, dm: DMat3) -> Self {
36        self.dm = dm;
37        self
38    }
39
40    pub fn with_angular_velocity(mut self, v: DVec3) -> Self {
41        self.dm = rotation_matrix_derivative(self.m, v);
42        self
43    }
44
45    pub fn position_matrix(&self) -> DMat3 {
46        self.m
47    }
48
49    pub fn velocity_matrix(&self) -> DMat3 {
50        self.dm
51    }
52
53    pub fn compose(self, other: Self) -> Self {
54        Self {
55            m: other.m * self.m,
56            dm: other.dm * self.m + other.m * self.dm,
57        }
58    }
59
60    pub fn transpose(&self) -> Self {
61        let m = self.m.transpose();
62        let dm = self.dm.transpose();
63        Self { m, dm }
64    }
65
66    pub fn rotate_position(&self, pos: DVec3) -> DVec3 {
67        self.m * pos
68    }
69
70    pub fn rotate_velocity(&self, pos: DVec3, vel: DVec3) -> DVec3 {
71        self.dm * pos + self.m * vel
72    }
73
74    pub fn rotate_state(&self, pos: DVec3, vel: DVec3) -> (DVec3, DVec3) {
75        (self.rotate_position(pos), self.rotate_velocity(pos, vel))
76    }
77}
78
79impl Mul<DVec3> for Rotation {
80    type Output = DVec3;
81
82    fn mul(self, rhs: DVec3) -> Self::Output {
83        self.m * rhs
84    }
85}
86
87impl Mul<Cartesian> for Rotation {
88    type Output = Cartesian;
89
90    fn mul(self, rhs: Cartesian) -> Self::Output {
91        let pos = self.m * rhs.position();
92        let vel = self.dm * rhs.position() + self.m * rhs.velocity();
93        Cartesian::from_vecs(pos, vel)
94    }
95}
lox_frames/transformations/rotations.rs

lox_frames/transformations/
rotations.rs
