spatialrust_math/
mat.rs

1use crate::{Scalar, Vec3};
2
3/// 3x3 matrix stored in row-major order.
4#[derive(Clone, Copy, Debug, PartialEq)]
5#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
6pub struct Mat3<T: Scalar> {
7    /// Row-major matrix elements.
8    pub m: [[T; 3]; 3],
9}
10
11/// 4x4 matrix stored in row-major order.
12#[derive(Clone, Copy, Debug, PartialEq)]
13#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
14pub struct Mat4<T: Scalar> {
15    /// Row-major matrix elements.
16    pub m: [[T; 4]; 4],
17}
18
19impl<T: Scalar> Mat3<T> {
20    /// Creates a matrix from row vectors.
21    #[must_use]
22    pub const fn from_rows(row0: [T; 3], row1: [T; 3], row2: [T; 3]) -> Self {
23        Self { m: [row0, row1, row2] }
24    }
25}
26
27impl Mat3<f32> {
28    /// Identity matrix for `f32`.
29    #[must_use]
30    pub fn identity() -> Self {
31        Self::from_rows([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0])
32    }
33
34    /// Transposed matrix.
35    #[must_use]
36    pub fn transpose(self) -> Self {
37        Self::from_rows(
38            [self.m[0][0], self.m[1][0], self.m[2][0]],
39            [self.m[0][1], self.m[1][1], self.m[2][1]],
40            [self.m[0][2], self.m[1][2], self.m[2][2]],
41        )
42    }
43
44    /// Matrix-vector multiplication.
45    #[must_use]
46    pub fn mul_vec3(self, v: Vec3<f32>) -> Vec3<f32> {
47        Vec3::new(
48            self.m[0][0] * v.x + self.m[0][1] * v.y + self.m[0][2] * v.z,
49            self.m[1][0] * v.x + self.m[1][1] * v.y + self.m[1][2] * v.z,
50            self.m[2][0] * v.x + self.m[2][1] * v.y + self.m[2][2] * v.z,
51        )
52    }
53
54    /// Matrix multiplication.
55    #[must_use]
56    pub fn mul_mat3(self, other: Self) -> Self {
57        Self::from_rows(
58            [
59                self.m[0][0] * other.m[0][0]
60                    + self.m[0][1] * other.m[1][0]
61                    + self.m[0][2] * other.m[2][0],
62                self.m[0][0] * other.m[0][1]
63                    + self.m[0][1] * other.m[1][1]
64                    + self.m[0][2] * other.m[2][1],
65                self.m[0][0] * other.m[0][2]
66                    + self.m[0][1] * other.m[1][2]
67                    + self.m[0][2] * other.m[2][2],
68            ],
69            [
70                self.m[1][0] * other.m[0][0]
71                    + self.m[1][1] * other.m[1][0]
72                    + self.m[1][2] * other.m[2][0],
73                self.m[1][0] * other.m[0][1]
74                    + self.m[1][1] * other.m[1][1]
75                    + self.m[1][2] * other.m[2][1],
76                self.m[1][0] * other.m[0][2]
77                    + self.m[1][1] * other.m[1][2]
78                    + self.m[1][2] * other.m[2][2],
79            ],
80            [
81                self.m[2][0] * other.m[0][0]
82                    + self.m[2][1] * other.m[1][0]
83                    + self.m[2][2] * other.m[2][0],
84                self.m[2][0] * other.m[0][1]
85                    + self.m[2][1] * other.m[1][1]
86                    + self.m[2][2] * other.m[2][1],
87                self.m[2][0] * other.m[0][2]
88                    + self.m[2][1] * other.m[1][2]
89                    + self.m[2][2] * other.m[2][2],
90            ],
91        )
92    }
93}
94
95impl Mat3<f64> {
96    /// Identity matrix for `f64`.
97    #[must_use]
98    pub fn identity() -> Self {
99        Self::from_rows([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0])
100    }
101
102    /// Transposed matrix.
103    #[must_use]
104    pub fn transpose(self) -> Self {
105        Self::from_rows(
106            [self.m[0][0], self.m[1][0], self.m[2][0]],
107            [self.m[0][1], self.m[1][1], self.m[2][1]],
108            [self.m[0][2], self.m[1][2], self.m[2][2]],
109        )
110    }
111
112    /// Matrix-vector multiplication.
113    #[must_use]
114    pub fn mul_vec3(self, v: Vec3<f64>) -> Vec3<f64> {
115        Vec3::new(
116            self.m[0][0] * v.x + self.m[0][1] * v.y + self.m[0][2] * v.z,
117            self.m[1][0] * v.x + self.m[1][1] * v.y + self.m[1][2] * v.z,
118            self.m[2][0] * v.x + self.m[2][1] * v.y + self.m[2][2] * v.z,
119        )
120    }
121
122    /// Matrix multiplication.
123    #[must_use]
124    pub fn mul_mat3(self, other: Self) -> Self {
125        Self::from_rows(
126            [
127                self.m[0][0] * other.m[0][0]
128                    + self.m[0][1] * other.m[1][0]
129                    + self.m[0][2] * other.m[2][0],
130                self.m[0][0] * other.m[0][1]
131                    + self.m[0][1] * other.m[1][1]
132                    + self.m[0][2] * other.m[2][1],
133                self.m[0][0] * other.m[0][2]
134                    + self.m[0][1] * other.m[1][2]
135                    + self.m[0][2] * other.m[2][2],
136            ],
137            [
138                self.m[1][0] * other.m[0][0]
139                    + self.m[1][1] * other.m[1][0]
140                    + self.m[1][2] * other.m[2][0],
141                self.m[1][0] * other.m[0][1]
142                    + self.m[1][1] * other.m[1][1]
143                    + self.m[1][2] * other.m[2][1],
144                self.m[1][0] * other.m[0][2]
145                    + self.m[1][1] * other.m[1][2]
146                    + self.m[1][2] * other.m[2][2],
147            ],
148            [
149                self.m[2][0] * other.m[0][0]
150                    + self.m[2][1] * other.m[1][0]
151                    + self.m[2][2] * other.m[2][0],
152                self.m[2][0] * other.m[0][1]
153                    + self.m[2][1] * other.m[1][1]
154                    + self.m[2][2] * other.m[2][1],
155                self.m[2][0] * other.m[0][2]
156                    + self.m[2][1] * other.m[1][2]
157                    + self.m[2][2] * other.m[2][2],
158            ],
159        )
160    }
161}
162
163impl<T: Scalar> Mat4<T> {
164    /// Creates a matrix from row vectors.
165    #[must_use]
166    pub const fn from_rows(row0: [T; 4], row1: [T; 4], row2: [T; 4], row3: [T; 4]) -> Self {
167        Self { m: [row0, row1, row2, row3] }
168    }
169}
170
171impl Mat4<f32> {
172    /// Identity matrix for `f32`.
173    #[must_use]
174    pub fn identity() -> Self {
175        Self::from_rows(
176            [1.0, 0.0, 0.0, 0.0],
177            [0.0, 1.0, 0.0, 0.0],
178            [0.0, 0.0, 1.0, 0.0],
179            [0.0, 0.0, 0.0, 1.0],
180        )
181    }
182
183    /// Homogeneous point transform.
184    #[must_use]
185    pub fn transform_point(self, point: Vec3<f32>) -> Vec3<f32> {
186        let x =
187            self.m[0][0] * point.x + self.m[0][1] * point.y + self.m[0][2] * point.z + self.m[0][3];
188        let y =
189            self.m[1][0] * point.x + self.m[1][1] * point.y + self.m[1][2] * point.z + self.m[1][3];
190        let z =
191            self.m[2][0] * point.x + self.m[2][1] * point.y + self.m[2][2] * point.z + self.m[2][3];
192        let w =
193            self.m[3][0] * point.x + self.m[3][1] * point.y + self.m[3][2] * point.z + self.m[3][3];
194        if w == 0.0 {
195            return Vec3::new(x, y, z);
196        }
197        Vec3::new(x / w, y / w, z / w)
198    }
199
200    /// Homogeneous vector transform (ignores translation).
201    #[must_use]
202    pub fn transform_vector(self, vector: Vec3<f32>) -> Vec3<f32> {
203        Vec3::new(
204            self.m[0][0] * vector.x + self.m[0][1] * vector.y + self.m[0][2] * vector.z,
205            self.m[1][0] * vector.x + self.m[1][1] * vector.y + self.m[1][2] * vector.z,
206            self.m[2][0] * vector.x + self.m[2][1] * vector.y + self.m[2][2] * vector.z,
207        )
208    }
209
210    /// Builds a rigid transform matrix from rotation and translation.
211    #[must_use]
212    pub fn from_rotation_translation(rotation: Mat3<f32>, translation: Vec3<f32>) -> Self {
213        Self::from_rows(
214            [rotation.m[0][0], rotation.m[0][1], rotation.m[0][2], translation.x],
215            [rotation.m[1][0], rotation.m[1][1], rotation.m[1][2], translation.y],
216            [rotation.m[2][0], rotation.m[2][1], rotation.m[2][2], translation.z],
217            [0.0, 0.0, 0.0, 1.0],
218        )
219    }
220}
221
222impl Mat4<f64> {
223    /// Identity matrix for `f64`.
224    #[must_use]
225    pub fn identity() -> Self {
226        Self::from_rows(
227            [1.0, 0.0, 0.0, 0.0],
228            [0.0, 1.0, 0.0, 0.0],
229            [0.0, 0.0, 1.0, 0.0],
230            [0.0, 0.0, 0.0, 1.0],
231        )
232    }
233
234    /// Homogeneous point transform.
235    #[must_use]
236    pub fn transform_point(self, point: Vec3<f64>) -> Vec3<f64> {
237        let x =
238            self.m[0][0] * point.x + self.m[0][1] * point.y + self.m[0][2] * point.z + self.m[0][3];
239        let y =
240            self.m[1][0] * point.x + self.m[1][1] * point.y + self.m[1][2] * point.z + self.m[1][3];
241        let z =
242            self.m[2][0] * point.x + self.m[2][1] * point.y + self.m[2][2] * point.z + self.m[2][3];
243        let w =
244            self.m[3][0] * point.x + self.m[3][1] * point.y + self.m[3][2] * point.z + self.m[3][3];
245        if w == 0.0 {
246            return Vec3::new(x, y, z);
247        }
248        Vec3::new(x / w, y / w, z / w)
249    }
250
251    /// Builds a rigid transform matrix from rotation and translation.
252    #[must_use]
253    pub fn from_rotation_translation(rotation: Mat3<f64>, translation: Vec3<f64>) -> Self {
254        Self::from_rows(
255            [rotation.m[0][0], rotation.m[0][1], rotation.m[0][2], translation.x],
256            [rotation.m[1][0], rotation.m[1][1], rotation.m[1][2], translation.y],
257            [rotation.m[2][0], rotation.m[2][1], rotation.m[2][2], translation.z],
258            [0.0, 0.0, 0.0, 1.0],
259        )
260    }
261}
262
263#[cfg(test)]
264mod tests {
265    use super::{Mat3, Mat4, Vec3};
266
267    #[test]
268    fn mat3_mul_vec3() {
269        let rot_y: Mat3<f32> = Mat3::from_rows([0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]);
270        let v = Vec3::new(1.0_f32, 0.0, 0.0);
271        let out = rot_y.mul_vec3(v);
272        assert!((out.x - 0.0).abs() < 1e-6);
273        assert!((out.z - (-1.0)).abs() < 1e-6);
274    }
275
276    #[test]
277    fn mat4_transform_point() {
278        let transform = Mat4::<f32>::from_rotation_translation(
279            Mat3::<f32>::identity(),
280            Vec3::new(1.0, 2.0, 3.0),
281        );
282        let p = Vec3::new(0.0_f32, 0.0, 0.0);
283        let out = transform.transform_point(p);
284        assert!((out.x - 1.0).abs() < 1e-6);
285        assert!((out.y - 2.0).abs() < 1e-6);
286        assert!((out.z - 3.0).abs() < 1e-6);
287    }
288
289    #[test]
290    fn mat4_f64_roundtrip() {
291        let m = Mat4::<f64>::identity();
292        let p = Vec3::new(1.0_f64, 2.0, 3.0);
293        let out = m.transform_point(p);
294        assert!((out.x - 1.0).abs() < 1e-12);
295    }
296}
spatialrust_math/mat.rs

spatialrust_math/
mat.rs
