linkage_blaze/
math.rs

1use core::{
2    f32::consts::PI,
3    ops::{Add, AddAssign, Index, IndexMut, Mul, Sub},
4};
5
6/// 3D position or vector `[x, y, z]`.
7///
8/// `Vec3` is used for linkage positions and directions. Its arithmetic,
9/// indexing, array conversion, and approximate comparison are demonstrated in
10/// the [pose and coordinate example](crate::Pose#pose-and-coordinate-values).
11#[derive(Clone, Copy, Debug, PartialEq)]
12pub struct Vec3(pub [f32; 3]);
13
14trait F32Ext {
15    fn close_to(self, other: Self, tolerance: Self) -> bool;
16}
17
18impl F32Ext for f32 {
19    fn close_to(self, other: Self, tolerance: Self) -> bool {
20        (self - other).abs() <= tolerance
21    }
22}
23
24impl Vec3 {
25    /// The zero position or direction.
26    pub const ZERO: Self = Self([0.0, 0.0, 0.0]);
27
28    /// Borrow the underlying array.
29    #[must_use]
30    pub const fn as_array(&self) -> &[f32; 3] {
31        &self.0
32    }
33
34    /// Return the underlying array.
35    #[must_use]
36    pub const fn into_array(self) -> [f32; 3] {
37        self.0
38    }
39
40    /// Return true when all components are within `tolerance`.
41    #[must_use]
42    pub fn is_close_to(&self, other: &Self, tolerance: f32) -> bool {
43        self.0
44            .iter()
45            .zip(other.0.iter())
46            .all(|(left, right)| left.close_to(*right, tolerance))
47    }
48
49    /// Return the dot product with `rhs`.
50    #[must_use]
51    pub fn dot(self, rhs: Self) -> f32 {
52        self[0] * rhs[0] + self[1] * rhs[1] + self[2] * rhs[2]
53    }
54
55    /// Return the Euclidean length.
56    #[must_use]
57    pub fn length(self) -> f32 {
58        libm::sqrtf(self.dot(self))
59    }
60
61    /// Return the Euclidean distance to `other`.
62    #[must_use]
63    pub fn distance_to(self, other: Self) -> f32 {
64        (self - other).length()
65    }
66}
67
68impl From<[f32; 3]> for Vec3 {
69    fn from(value: [f32; 3]) -> Self {
70        Self(value)
71    }
72}
73
74impl From<Vec3> for [f32; 3] {
75    fn from(value: Vec3) -> Self {
76        value.into_array()
77    }
78}
79
80impl Index<usize> for Vec3 {
81    type Output = f32;
82
83    fn index(&self, index: usize) -> &Self::Output {
84        &self.0[index]
85    }
86}
87
88impl IndexMut<usize> for Vec3 {
89    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
90        &mut self.0[index]
91    }
92}
93
94impl Add for Vec3 {
95    type Output = Self;
96
97    fn add(self, rhs: Self) -> Self::Output {
98        Self([self[0] + rhs[0], self[1] + rhs[1], self[2] + rhs[2]])
99    }
100}
101
102impl AddAssign for Vec3 {
103    fn add_assign(&mut self, rhs: Self) {
104        *self = *self + rhs;
105    }
106}
107
108impl Sub for Vec3 {
109    type Output = Self;
110
111    fn sub(self, rhs: Self) -> Self::Output {
112        Self([self[0] - rhs[0], self[1] - rhs[1], self[2] - rhs[2]])
113    }
114}
115
116impl Mul<f32> for Vec3 {
117    type Output = Self;
118
119    fn mul(self, rhs: f32) -> Self::Output {
120        Self([self[0] * rhs, self[1] * rhs, self[2] * rhs])
121    }
122}
123
124/// Local-frame orientation matrix stored row-major: `mat[row][col]`.
125///
126/// Columns are local-frame axes: column 0 = +X (forward), column 1 = +Y
127/// (left), and column 2 = +Z (up).
128///
129/// Rotation constructors, axis accessors, matrix multiplication, array access,
130/// and approximate comparison are demonstrated in the
131/// [pose and coordinate example](crate::Pose#pose-and-coordinate-values).
132#[derive(Clone, Copy, Debug, PartialEq)]
133pub struct Mat3(pub [[f32; 3]; 3]);
134
135impl Mat3 {
136    /// Identity orientation with model and local axes aligned.
137    pub const IDENTITY: Self = Self([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]);
138
139    /// Borrow the underlying array.
140    #[must_use]
141    pub const fn as_array(&self) -> &[[f32; 3]; 3] {
142        &self.0
143    }
144
145    /// Return the underlying array.
146    #[must_use]
147    pub const fn into_array(self) -> [[f32; 3]; 3] {
148        self.0
149    }
150
151    /// Rotation around z. Yaw = Rz: \[\[c,-s,0\],\[s,c,0\],\[0,0,1\]\].
152    #[must_use]
153    pub fn yaw(radians: f32) -> Self {
154        let cos = libm::cosf(radians);
155        let sin = libm::sinf(radians);
156        Self([[cos, -sin, 0.0], [sin, cos, 0.0], [0.0, 0.0, 1.0]])
157    }
158
159    /// Rotation around y. Pitch = Ry: \[\[c,0,s\],\[0,1,0\],\[-s,0,c\]\].
160    #[must_use]
161    pub fn pitch(radians: f32) -> Self {
162        let cos = libm::cosf(radians);
163        let sin = libm::sinf(radians);
164        Self([[cos, 0.0, sin], [0.0, 1.0, 0.0], [-sin, 0.0, cos]])
165    }
166
167    /// Rotation around x. Roll = Rx: \[\[1,0,0\],\[0,c,-s\],\[0,s,c\]\].
168    #[must_use]
169    pub fn roll(radians: f32) -> Self {
170        let cos = libm::cosf(radians);
171        let sin = libm::sinf(radians);
172        Self([[1.0, 0.0, 0.0], [0.0, cos, -sin], [0.0, sin, cos]])
173    }
174
175    /// Return local +X, the forward axis stored in column 0.
176    #[must_use]
177    pub fn forward(&self) -> Vec3 {
178        Vec3([self[0][0], self[1][0], self[2][0]])
179    }
180
181    /// Return local +Y, the left axis stored in column 1.
182    #[must_use]
183    pub fn left(&self) -> Vec3 {
184        Vec3([self[0][1], self[1][1], self[2][1]])
185    }
186
187    /// Return local +Z, the up axis stored in column 2.
188    #[must_use]
189    pub fn up(&self) -> Vec3 {
190        Vec3([self[0][2], self[1][2], self[2][2]])
191    }
192
193    /// Return true when all components are within `tolerance`.
194    #[must_use]
195    pub fn is_close_to(&self, other: &Self, tolerance: f32) -> bool {
196        self.0
197            .iter()
198            .zip(other.0.iter())
199            .all(|(left, right)| Vec3::from(*left).is_close_to(&Vec3::from(*right), tolerance))
200    }
201}
202
203impl From<[[f32; 3]; 3]> for Mat3 {
204    fn from(value: [[f32; 3]; 3]) -> Self {
205        Self(value)
206    }
207}
208
209impl From<Mat3> for [[f32; 3]; 3] {
210    fn from(value: Mat3) -> Self {
211        value.into_array()
212    }
213}
214
215impl Index<usize> for Mat3 {
216    type Output = [f32; 3];
217
218    fn index(&self, index: usize) -> &Self::Output {
219        &self.0[index]
220    }
221}
222
223impl IndexMut<usize> for Mat3 {
224    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
225        &mut self.0[index]
226    }
227}
228
229impl Mul for Mat3 {
230    type Output = Self;
231
232    fn mul(self, rhs: Self) -> Self::Output {
233        let mut out = [[0.0f32; 3]; 3];
234        for row in 0..3 {
235            for col in 0..3 {
236                for component in 0..3 {
237                    out[row][col] += self[row][component] * rhs[component][col];
238                }
239            }
240        }
241        Self(out)
242    }
243}
244
245#[must_use]
246pub const fn degrees_to_radians(degrees: f32) -> f32 {
247    degrees * (PI / 180.0)
248}
249
250#[cfg(test)]
251mod tests {
252    use super::{F32Ext, Mat3, Vec3, degrees_to_radians};
253    use core::f32::consts::PI;
254
255    #[test]
256    fn test_degrees_to_radians() {
257        assert!(degrees_to_radians(180.0).close_to(PI, 1e-6));
258        assert!(degrees_to_radians(90.0).close_to(PI / 2.0, 1e-6));
259    }
260
261    #[test]
262    fn test_vec3_add_and_scale() {
263        let actual = Vec3::from([1.0, 2.0, 3.0]) + Vec3::from([4.0, -1.0, 0.5]) * 2.0;
264        let expected = Vec3::from([9.0, 0.0, 4.0]);
265
266        assert!(actual.is_close_to(&expected, 1e-6));
267    }
268
269    #[test]
270    fn test_vec3_sub() {
271        let actual = Vec3::from([5.0, 7.0, 9.0]) - Vec3::from([1.0, 2.0, 3.0]);
272        let expected = Vec3::from([4.0, 5.0, 6.0]);
273
274        assert!(actual.is_close_to(&expected, 1e-6));
275    }
276
277    #[test]
278    fn test_vec3_dot() {
279        let left = Vec3::from([1.0, 2.0, 3.0]);
280        let right = Vec3::from([4.0, -5.0, 6.0]);
281
282        assert!(left.dot(right).close_to(12.0, 1e-6));
283    }
284
285    #[test]
286    fn test_vec3_length() {
287        let vec3 = Vec3::from([3.0, 4.0, 0.0]);
288
289        assert!(vec3.length().close_to(5.0, 1e-6));
290    }
291
292    #[test]
293    fn test_vec3_distance_to_is_symmetric() {
294        let first = Vec3::from([1.0, 2.0, 3.0]);
295        let second = Vec3::from([4.0, 6.0, 3.0]);
296
297        let first_to_second = first.distance_to(second);
298        let second_to_first = second.distance_to(first);
299
300        assert!(first_to_second.close_to(5.0, 1e-6));
301        assert!(first_to_second.close_to(second_to_first, 1e-6));
302    }
303
304    #[test]
305    fn test_vec3_array_conversions() {
306        let vec = Vec3::from([1.0, 2.0, 3.0]);
307
308        assert_eq!(vec.as_array(), &[1.0, 2.0, 3.0]);
309        assert_eq!(vec.into_array(), [1.0, 2.0, 3.0]);
310        assert_eq!(<[f32; 3]>::from(vec), [1.0, 2.0, 3.0]);
311    }
312
313    #[test]
314    fn test_mat3_mul() {
315        let left = Mat3::from([[1.0, 2.0, 3.0], [0.0, 1.0, 4.0], [5.0, 6.0, 0.0]]);
316        let right = Mat3::from([[-2.0, 1.0, 0.0], [3.0, 0.0, 0.0], [4.0, 5.0, 1.0]]);
317        let expected = Mat3::from([[16.0, 16.0, 3.0], [19.0, 20.0, 4.0], [8.0, 5.0, 0.0]]);
318
319        assert!((left * right).is_close_to(&expected, 1e-6));
320    }
321
322    #[test]
323    fn test_mat3_array_conversions() {
324        let mat = Mat3::from([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]]);
325        let expected = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]];
326
327        assert_eq!(mat.as_array(), &expected);
328        assert_eq!(mat.into_array(), expected);
329        assert_eq!(<[[f32; 3]; 3]>::from(mat), expected);
330    }
331
332    #[test]
333    fn test_rotation_forward_axes() {
334        let yaw_forward = Mat3::yaw(degrees_to_radians(90.0)).forward();
335        let pitch_forward = Mat3::pitch(degrees_to_radians(90.0)).forward();
336        let roll_forward = Mat3::roll(degrees_to_radians(90.0)).forward();
337
338        assert!(yaw_forward.is_close_to(&Vec3::from([0.0, 1.0, 0.0]), 1e-6));
339        assert!(pitch_forward.is_close_to(&Vec3::from([0.0, 0.0, -1.0]), 1e-6));
340        assert!(roll_forward.is_close_to(&Vec3::from([1.0, 0.0, 0.0]), 1e-6));
341    }
342
343    #[test]
344    fn test_rotation_local_axes() {
345        let yaw = Mat3::yaw(degrees_to_radians(90.0));
346        let pitch = Mat3::pitch(degrees_to_radians(90.0));
347        let roll = Mat3::roll(degrees_to_radians(90.0));
348
349        assert!(column(yaw, 2).is_close_to(&Vec3::from([0.0, 0.0, 1.0]), 1e-6));
350        assert!(column(pitch, 1).is_close_to(&Vec3::from([0.0, 1.0, 0.0]), 1e-6));
351        assert!(column(roll, 0).is_close_to(&Vec3::from([1.0, 0.0, 0.0]), 1e-6));
352    }
353
354    #[test]
355    fn test_mat3_is_close_to() {
356        let actual = Mat3::from([[1.0001, 0.0, 0.0], [0.0, 0.9999, 0.0], [0.0, 0.0, 1.0001]]);
357        let expected = Mat3::IDENTITY;
358
359        assert!(actual.is_close_to(&expected, 0.001));
360        assert!(!actual.is_close_to(&expected, 0.00001));
361    }
362
363    fn column(mat: Mat3, column_index: usize) -> Vec3 {
364        Vec3::from([
365            mat[0][column_index],
366            mat[1][column_index],
367            mat[2][column_index],
368        ])
369    }
370}
linkage_blaze/math.rs

linkage_blaze/
math.rs
