mini_math/
matrix.rs

1use crate::{NearlyEqual, Point, Vector3, Vector4};
2
3/// A 4x4 matrix, suitable for 3D transformations.
4#[derive(Copy, Clone, Debug, PartialEq)]
5#[repr(C)]
6pub struct Matrix4(pub [Vector4; 4]);
7
8impl Matrix4 {
9    /// A new matrix from a 1D array.
10    pub const fn from_1d_array(a: [f32; 16]) -> Self {
11        Self([
12            Vector4::new(a[0], a[1], a[2], a[3]),
13            Vector4::new(a[4], a[5], a[6], a[4]),
14            Vector4::new(a[8], a[9], a[10], a[11]),
15            Vector4::new(a[12], a[13], a[14], a[15]),
16        ])
17    }
18
19    /// A new matrix from a 2D array.
20    pub const fn from_2d_array(a: [[f32; 4]; 4]) -> Self {
21        Self([
22            Vector4::new(a[0][0], a[0][1], a[0][2], a[0][3]),
23            Vector4::new(a[1][0], a[1][1], a[1][2], a[1][3]),
24            Vector4::new(a[2][0], a[2][1], a[2][2], a[2][3]),
25            Vector4::new(a[3][0], a[3][1], a[3][2], a[3][3]),
26        ])
27    }
28
29    /// The identity matrix.
30    pub const fn identity() -> Self {
31        Self([
32            Vector4::new(1.0, 0.0, 0.0, 0.0),
33            Vector4::new(0.0, 1.0, 0.0, 0.0),
34            Vector4::new(0.0, 0.0, 1.0, 0.0),
35            Vector4::new(0.0, 0.0, 0.0, 1.0),
36        ])
37    }
38
39    /// A matrix composed entirely of zeroes.
40    pub const fn zero() -> Self {
41        Self([
42            Vector4::new(0.0, 0.0, 0.0, 0.0),
43            Vector4::new(0.0, 0.0, 0.0, 0.0),
44            Vector4::new(0.0, 0.0, 0.0, 0.0),
45            Vector4::new(0.0, 0.0, 0.0, 0.0),
46        ])
47    }
48
49    /// A look-at matrix suitable for positioning a camera.
50    pub fn look_at(eye: Point, target: Point, up: Vector3) -> Self {
51        let z_axis = (target - eye).normalized();
52        let x_axis = z_axis.cross(up).normalized();
53        let y_axis = x_axis.cross(z_axis);
54
55        let eye_vec = eye.into();
56
57        Self([
58            Vector4::new(x_axis.x, y_axis.x, -z_axis.x, 0.0),
59            Vector4::new(x_axis.y, y_axis.y, -z_axis.y, 0.0),
60            Vector4::new(x_axis.z, y_axis.z, -z_axis.z, 0.0),
61            Vector4::new(
62                -x_axis.dot(eye_vec),
63                -y_axis.dot(eye_vec),
64                z_axis.dot(eye_vec),
65                1.0,
66            ),
67        ])
68    }
69
70    /// A perspective matrix suitable for use as a camera projection.
71    pub fn perspective(aspect_ratio: f32, fov_radians: f32, znear: f32, zfar: f32) -> Self {
72        let f = 1.0 / (fov_radians / 2.0).tan();
73
74        Self([
75            Vector4::new(f / aspect_ratio, 0.0, 0.0, 0.0),
76            Vector4::new(0.0, f, 0.0, 0.0),
77            Vector4::new(0.0, 0.0, (zfar + znear) / (znear - zfar), -1.0),
78            Vector4::new(0.0, 0.0, (2.0 * zfar * znear) / (znear - zfar), 0.0),
79        ])
80    }
81
82    /// An orthographic matrix suitable for rendering user interfaces.
83    pub fn orthographic(left: f32, right: f32, bottom: f32, top: f32, near: f32, far: f32) -> Self {
84        Self([
85            Vector4::new(2.0 / (right - left), 0.0, 0.0, 0.0),
86            Vector4::new(0.0, 2.0 / (top - bottom), 0.0, 0.0),
87            Vector4::new(0.0, 0.0, -2.0 / (far - near), -1.0),
88            Vector4::new(
89                -(right + left) / (right - left),
90                -(top + bottom) / (top - bottom),
91                -(far + near) / (far - near),
92                1.0,
93            ),
94        ])
95    }
96
97    /// A matrix that translates by the given vector.
98    pub fn translation(v: Vector3) -> Self {
99        Self([
100            Vector4::new(1.0, 0.0, 0.0, 0.0),
101            Vector4::new(0.0, 1.0, 0.0, 0.0),
102            Vector4::new(0.0, 0.0, 1.0, 0.0),
103            Vector4::new(v.x, v.y, v.z, 1.0),
104        ])
105    }
106
107    /// A matrix that rotates around the x-axis.
108    pub fn rotation_x(angle_radians: f32) -> Self {
109        Self([
110            Vector4::new(1.0, 0.0, 0.0, 0.0),
111            Vector4::new(0.0, angle_radians.cos(), -angle_radians.sin(), 0.0),
112            Vector4::new(0.0, angle_radians.sin(), angle_radians.cos(), 0.0),
113            Vector4::new(0.0, 0.0, 0.0, 1.0),
114        ])
115    }
116
117    /// A matrix that rotates around the y-axis.
118    pub fn rotation_y(angle_radians: f32) -> Self {
119        Self([
120            Vector4::new(angle_radians.cos(), 0.0, angle_radians.sin(), 0.0),
121            Vector4::new(0.0, 1.0, 0.0, 0.0),
122            Vector4::new(-angle_radians.sin(), 0.0, angle_radians.cos(), 0.0),
123            Vector4::new(0.0, 0.0, 0.0, 1.0),
124        ])
125    }
126
127    /// A matrix that rotates around the z-axis.
128    pub fn rotation_z(angle_radians: f32) -> Self {
129        Self([
130            Vector4::new(angle_radians.cos(), -angle_radians.sin(), 0.0, 0.0),
131            Vector4::new(angle_radians.sin(), angle_radians.cos(), 0.0, 0.0),
132            Vector4::new(0.0, 0.0, 1.0, 0.0),
133            Vector4::new(0.0, 0.0, 0.0, 1.0),
134        ])
135    }
136
137    /// A matrix that rotates around an arbitrary axis.
138    pub fn rotation_axis_angle(axis: Vector3, angle_radians: f32) -> Self {
139        let sin = angle_radians.sin();
140        let cos = angle_radians.cos();
141        let k = 1.0 - cos;
142
143        Self([
144            Vector4::new(
145                axis.x * axis.x * k + cos,
146                axis.y * axis.x * k - sin * axis.z,
147                axis.z * axis.x * k + sin * axis.y,
148                0.0,
149            ),
150            Vector4::new(
151                axis.x * axis.y * k + sin * axis.z,
152                axis.y * axis.y * k + cos,
153                axis.z * axis.y * k - sin * axis.x,
154                0.0,
155            ),
156            Vector4::new(
157                axis.x * axis.z * k - sin * axis.y,
158                axis.y * axis.z * k + sin * axis.x,
159                axis.z * axis.z * k + cos,
160                0.0,
161            ),
162            Vector4::new(0.0, 0.0, 0.0, 1.0),
163        ])
164    }
165
166    /// A matrix that rotates from a given vector to another.
167    pub fn rotation_from_vector_to_vector(start: Vector3, end: Vector3) -> Self {
168        let axis = -start.cross(end);
169
170        let cos = start.dot(end);
171        let k = 1.0 / (1.0 + cos);
172
173        Self([
174            Vector4::new(
175                axis.x * axis.x * k + cos,
176                axis.y * axis.x * k - axis.z,
177                axis.z * axis.x * k + axis.y,
178                0.0,
179            ),
180            Vector4::new(
181                axis.x * axis.y * k + axis.z,
182                axis.y * axis.y * k + cos,
183                axis.z * axis.y * k - axis.x,
184                0.0,
185            ),
186            Vector4::new(
187                axis.x * axis.z * k - axis.y,
188                axis.y * axis.z * k + axis.x,
189                axis.z * axis.z * k + cos,
190                0.0,
191            ),
192            Vector4::new(0.0, 0.0, 0.0, 1.0),
193        ])
194    }
195
196    /// A matrix that scales uniformly in all dimensions.
197    pub fn uniform_scale(scale: f32) -> Self {
198        Self([
199            Vector4::new(scale, 0.0, 0.0, 0.0),
200            Vector4::new(0.0, scale, 0.0, 0.0),
201            Vector4::new(0.0, 0.0, scale, 0.0),
202            Vector4::new(0.0, 0.0, 0.0, 1.0),
203        ])
204    }
205
206    /// Obtain the specified row vector of this matrix.
207    pub fn row(&self, i: usize) -> Vector4 {
208        Vector4::new(self.0[0][i], self.0[1][i], self.0[2][i], self.0[3][i])
209    }
210    /// Obtain the specified column vector of this matrix.
211    pub fn column(&self, i: usize) -> Vector4 {
212        self.0[i]
213    }
214
215    /// The transpose of this matrix (i.e. this matrix flipped along the diagonal)
216    pub fn transpose(&self) -> Self {
217        let mut r = Self::zero();
218
219        for i in 0..4 {
220            for j in 0..4 {
221                r.0[i][j] = self.0[j][i];
222            }
223        }
224
225        r
226    }
227
228    /// The inverse of this matrix.
229    pub fn invert(&self) -> Self {
230        let mut inv = Matrix4::zero();
231
232        inv.0[0][0] = self.0[1][1] * self.0[2][2] * self.0[3][3]
233            - self.0[1][1] * self.0[2][3] * self.0[3][2]
234            - self.0[2][1] * self.0[1][2] * self.0[3][3]
235            + self.0[2][1] * self.0[1][3] * self.0[3][2]
236            + self.0[3][1] * self.0[1][2] * self.0[2][3]
237            - self.0[3][1] * self.0[1][3] * self.0[2][2];
238
239        inv.0[1][0] = -self.0[1][0] * self.0[2][2] * self.0[3][3]
240            + self.0[1][0] * self.0[2][3] * self.0[3][2]
241            + self.0[2][0] * self.0[1][2] * self.0[3][3]
242            - self.0[2][0] * self.0[1][3] * self.0[3][2]
243            - self.0[3][0] * self.0[1][2] * self.0[2][3]
244            + self.0[3][0] * self.0[1][3] * self.0[2][2];
245
246        inv.0[2][0] = self.0[1][0] * self.0[2][1] * self.0[3][3]
247            - self.0[1][0] * self.0[2][3] * self.0[3][1]
248            - self.0[2][0] * self.0[1][1] * self.0[3][3]
249            + self.0[2][0] * self.0[1][3] * self.0[3][1]
250            + self.0[3][0] * self.0[1][1] * self.0[2][3]
251            - self.0[3][0] * self.0[1][3] * self.0[2][1];
252
253        inv.0[3][0] = -self.0[1][0] * self.0[2][1] * self.0[3][2]
254            + self.0[1][0] * self.0[2][2] * self.0[3][1]
255            + self.0[2][0] * self.0[1][1] * self.0[3][2]
256            - self.0[2][0] * self.0[1][2] * self.0[3][1]
257            - self.0[3][0] * self.0[1][1] * self.0[2][2]
258            + self.0[3][0] * self.0[1][2] * self.0[2][1];
259
260        inv.0[0][1] = -self.0[0][1] * self.0[2][2] * self.0[3][3]
261            + self.0[0][1] * self.0[2][3] * self.0[3][2]
262            + self.0[2][1] * self.0[0][2] * self.0[3][3]
263            - self.0[2][1] * self.0[0][3] * self.0[3][2]
264            - self.0[3][1] * self.0[0][2] * self.0[2][3]
265            + self.0[3][1] * self.0[0][3] * self.0[2][2];
266
267        inv.0[1][1] = self.0[0][0] * self.0[2][2] * self.0[3][3]
268            - self.0[0][0] * self.0[2][3] * self.0[3][2]
269            - self.0[2][0] * self.0[0][2] * self.0[3][3]
270            + self.0[2][0] * self.0[0][3] * self.0[3][2]
271            + self.0[3][0] * self.0[0][2] * self.0[2][3]
272            - self.0[3][0] * self.0[0][3] * self.0[2][2];
273
274        inv.0[2][1] = -self.0[0][0] * self.0[2][1] * self.0[3][3]
275            + self.0[0][0] * self.0[2][3] * self.0[3][1]
276            + self.0[2][0] * self.0[0][1] * self.0[3][3]
277            - self.0[2][0] * self.0[0][3] * self.0[3][1]
278            - self.0[3][0] * self.0[0][1] * self.0[2][3]
279            + self.0[3][0] * self.0[0][3] * self.0[2][1];
280
281        inv.0[3][1] = self.0[0][0] * self.0[2][1] * self.0[3][2]
282            - self.0[0][0] * self.0[2][2] * self.0[3][1]
283            - self.0[2][0] * self.0[0][1] * self.0[3][2]
284            + self.0[2][0] * self.0[0][2] * self.0[3][1]
285            + self.0[3][0] * self.0[0][1] * self.0[2][2]
286            - self.0[3][0] * self.0[0][2] * self.0[2][1];
287
288        inv.0[0][2] = self.0[0][1] * self.0[1][2] * self.0[3][3]
289            - self.0[0][1] * self.0[1][3] * self.0[3][2]
290            - self.0[1][1] * self.0[0][2] * self.0[3][3]
291            + self.0[1][1] * self.0[0][3] * self.0[3][2]
292            + self.0[3][1] * self.0[0][2] * self.0[1][3]
293            - self.0[3][1] * self.0[0][3] * self.0[1][2];
294
295        inv.0[1][2] = -self.0[0][0] * self.0[1][2] * self.0[3][3]
296            + self.0[0][0] * self.0[1][3] * self.0[3][2]
297            + self.0[1][0] * self.0[0][2] * self.0[3][3]
298            - self.0[1][0] * self.0[0][3] * self.0[3][2]
299            - self.0[3][0] * self.0[0][2] * self.0[1][3]
300            + self.0[3][0] * self.0[0][3] * self.0[1][2];
301
302        inv.0[2][2] = self.0[0][0] * self.0[1][1] * self.0[3][3]
303            - self.0[0][0] * self.0[1][3] * self.0[3][1]
304            - self.0[1][0] * self.0[0][1] * self.0[3][3]
305            + self.0[1][0] * self.0[0][3] * self.0[3][1]
306            + self.0[3][0] * self.0[0][1] * self.0[1][3]
307            - self.0[3][0] * self.0[0][3] * self.0[1][1];
308
309        inv.0[3][2] = -self.0[0][0] * self.0[1][1] * self.0[3][2]
310            + self.0[0][0] * self.0[1][2] * self.0[3][1]
311            + self.0[1][0] * self.0[0][1] * self.0[3][2]
312            - self.0[1][0] * self.0[0][2] * self.0[3][1]
313            - self.0[3][0] * self.0[0][1] * self.0[1][2]
314            + self.0[3][0] * self.0[0][2] * self.0[1][1];
315
316        inv.0[0][3] = -self.0[0][1] * self.0[1][2] * self.0[2][3]
317            + self.0[0][1] * self.0[1][3] * self.0[2][2]
318            + self.0[1][1] * self.0[0][2] * self.0[2][3]
319            - self.0[1][1] * self.0[0][3] * self.0[2][2]
320            - self.0[2][1] * self.0[0][2] * self.0[1][3]
321            + self.0[2][1] * self.0[0][3] * self.0[1][2];
322
323        inv.0[1][3] = self.0[0][0] * self.0[1][2] * self.0[2][3]
324            - self.0[0][0] * self.0[1][3] * self.0[2][2]
325            - self.0[1][0] * self.0[0][2] * self.0[2][3]
326            + self.0[1][0] * self.0[0][3] * self.0[2][2]
327            + self.0[2][0] * self.0[0][2] * self.0[1][3]
328            - self.0[2][0] * self.0[0][3] * self.0[1][2];
329
330        inv.0[2][3] = -self.0[0][0] * self.0[1][1] * self.0[2][3]
331            + self.0[0][0] * self.0[1][3] * self.0[2][1]
332            + self.0[1][0] * self.0[0][1] * self.0[2][3]
333            - self.0[1][0] * self.0[0][3] * self.0[2][1]
334            - self.0[2][0] * self.0[0][1] * self.0[1][3]
335            + self.0[2][0] * self.0[0][3] * self.0[1][1];
336
337        inv.0[3][3] = self.0[0][0] * self.0[1][1] * self.0[2][2]
338            - self.0[0][0] * self.0[1][2] * self.0[2][1]
339            - self.0[1][0] * self.0[0][1] * self.0[2][2]
340            + self.0[1][0] * self.0[0][2] * self.0[2][1]
341            + self.0[2][0] * self.0[0][1] * self.0[1][2]
342            - self.0[2][0] * self.0[0][2] * self.0[1][1];
343
344        let mut det = self.0[0][0] * inv.0[0][0]
345            + self.0[0][1] * inv.0[1][0]
346            + self.0[0][2] * inv.0[2][0]
347            + self.0[0][3] * inv.0[3][0];
348        det = 1.0 / det;
349
350        for i in 0..4 {
351            for j in 0..4 {
352                inv.0[i][j] *= det;
353            }
354        }
355
356        inv
357    }
358
359    pub fn as_slice(&self) -> &[f32] {
360        unsafe {
361            std::slice::from_raw_parts(
362                &self.0[0][0],
363                std::mem::size_of::<Self>() / std::mem::size_of::<f32>(),
364            )
365        }
366    }
367}
368
369impl NearlyEqual for &Matrix4 {
370    fn nearly_equals(self, rhs: Self) -> bool {
371        for i in 0..4 {
372            if !self.0[i].nearly_equals(&rhs.0[i]) {
373                return false;
374            }
375        }
376
377        true
378    }
379}
380
381#[cfg(test)]
382mod tests {
383    use crate::*;
384
385    #[test]
386    fn identity() {
387        let m = Matrix4::identity();
388        let p = Point::new(1.0, 2.0, 3.0);
389
390        assert_eq!(p, m * p);
391        assert_eq!(m, m.transpose());
392        assert_eq!(m, m.invert());
393    }
394
395    #[test]
396    fn invert() {
397        let m = Matrix4::from_2d_array([
398            [3.0, 2.0, 1.0, 1.0],
399            [2.0, 3.0, 2.0, 2.0],
400            [1.0, 2.0, 3.0, 3.0],
401            [0.0, 1.0, 1.0, 0.0],
402        ]);
403
404        assert_eq!(m.invert() * m, Matrix4::identity());
405
406        let n = Matrix4([
407            Vector4 {
408                x: 0.9742785,
409                y: 0.0,
410                z: 0.0,
411                w: 0.0,
412            },
413            Vector4 {
414                x: 0.0,
415                y: 1.7320507,
416                z: 0.0,
417                w: 0.0,
418            },
419            Vector4 {
420                x: 0.0,
421                y: 0.0,
422                z: -1.0002,
423                w: -1.0,
424            },
425            Vector4 {
426                x: 0.0,
427                y: 0.0,
428                z: -2.0002,
429                w: 0.0,
430            },
431        ]);
432        let inverse = Matrix4([
433            Vector4 {
434                x: 1.0264006,
435                y: -0.0,
436                z: -0.0,
437                w: -0.0,
438            },
439            Vector4 {
440                x: -0.0,
441                y: 0.5773504,
442                z: -0.0,
443                w: -0.0,
444            },
445            Vector4 {
446                x: -0.0,
447                y: -0.0,
448                z: -0.0,
449                w: -0.49995005,
450            },
451            Vector4 {
452                x: -0.0,
453                y: -0.0,
454                z: -1.0000001,
455                w: 0.50005007,
456            },
457        ]);
458        assert_eq!(n.invert(), inverse);
459    }
460
461    #[test]
462    fn translate() {
463        let m = Matrix4::translation(Vector3::new(10.0, 1.0, 0.0));
464        assert_eq!(m * Point::zero(), Point::new(10.0, 1.0, 0.0));
465
466        let n = Matrix4::translation(Vector3::new(-2.0, -5.0, 0.0));
467        assert_eq!(n * Point::zero(), Point::new(-2.0, -5.0, 0.0));
468
469        let t = m * n;
470        assert_eq!(t * Point::zero(), Point::new(8.0, -4.0, 0.0));
471    }
472
473    #[test]
474    fn rotate_axis_angle() {
475        let m =
476            Matrix4::rotation_axis_angle(Vector3::new(0.0, 1.0, 0.0), std::f32::consts::FRAC_PI_2);
477        assert_eq!(m * Point::zero(), Point::zero());
478        assert_nearly_eq!(m * Point::new(1.0, 0.0, 0.0), &Point::new(0.0, 0.0, 1.0));
479
480        let m =
481            Matrix4::rotation_axis_angle(Vector3::new(1.0, 0.0, 0.0), std::f32::consts::FRAC_PI_2);
482        assert_nearly_eq!(m * Point::new(0.0, 0.0, 1.0), &Point::new(0.0, 1.0, 0.0));
483
484        let m =
485            Matrix4::rotation_axis_angle(Vector3::new(0.0, 0.0, 1.0), std::f32::consts::FRAC_PI_2);
486        assert_nearly_eq!(m * Point::new(1.0, 0.0, 0.0), &Point::new(0.0, -1.0, 0.0));
487    }
488
489    #[test]
490    fn rotation_from_vector_to_vector() {
491        let m = Matrix4::rotation_from_vector_to_vector(
492            Vector3::new(1.0, 0.0, 0.0),
493            Vector3::new(0.0, 0.0, 1.0),
494        );
495        assert_eq!(m * Point::zero(), Point::zero());
496        assert_nearly_eq!(m * Point::new(1.0, 0.0, 0.0), &Point::new(0.0, 0.0, 1.0));
497        assert_nearly_eq!(m * Point::new(0.0, 0.0, 1.0), &Point::new(-1.0, 0.0, 0.0));
498    }
499
500    #[test]
501    fn slice() {
502        let a = [
503            3.0, 2.0, 1.0, 1.0, 2.0, 3.0, 2.0, 2.0, 1.0, 2.0, 3.0, 3.0, 0.0, 1.0, 1.0, 0.0,
504        ];
505        let m = Matrix4::from_1d_array(a);
506
507        assert_eq!(m.as_slice(), &a);
508    }
509}
mini_math/matrix.rs

mini_math/
matrix.rs
