1use std::fmt;
4use std::ops::{Mul, MulAssign, Neg};
5use std::str::FromStr;
6
7use crate::utilities::math_utilities::MathUtilities;
8use crate::utilities::span_helpers::InvariantF64;
9use crate::utilities::{FormatError, SpanStringTokenizer};
10use crate::{Point, Vector};
11
12#[derive(Clone, Copy, Debug, PartialEq, Default)]
23pub struct Matrix {
24 pub m11: f64,
26 pub m12: f64,
28 pub m13: f64,
30 pub m21: f64,
32 pub m22: f64,
34 pub m23: f64,
36 pub m31: f64,
38 pub m32: f64,
40 pub m33: f64,
42}
43
44#[derive(Clone, Copy, Debug, PartialEq, Default)]
46pub struct MatrixDecomposed {
47 pub translate: Vector,
48 pub scale: Vector,
49 pub skew: Vector,
50 pub angle: f64,
51}
52
53impl Matrix {
54 pub const IDENTITY: Matrix = Matrix::new_3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
56
57 #[inline]
59 pub const fn new(
60 scale_x: f64,
61 skew_y: f64,
62 skew_x: f64,
63 scale_y: f64,
64 offset_x: f64,
65 offset_y: f64,
66 ) -> Self {
67 Self::new_3x3(
68 scale_x, skew_y, 0.0, skew_x, scale_y, 0.0, offset_x, offset_y, 1.0,
69 )
70 }
71
72 #[inline]
74 #[allow(clippy::too_many_arguments)]
75 pub const fn new_3x3(
76 scale_x: f64,
77 skew_y: f64,
78 persp_x: f64,
79 skew_x: f64,
80 scale_y: f64,
81 persp_y: f64,
82 offset_x: f64,
83 offset_y: f64,
84 persp_z: f64,
85 ) -> Self {
86 Self {
87 m11: scale_x,
88 m12: skew_y,
89 m13: persp_x,
90 m21: skew_x,
91 m22: scale_y,
92 m23: persp_y,
93 m31: offset_x,
94 m32: offset_y,
95 m33: persp_z,
96 }
97 }
98
99 #[inline]
101 pub fn is_identity(&self) -> bool {
102 *self == Matrix::IDENTITY
103 }
104
105 #[inline]
107 pub fn has_inverse(&self) -> bool {
108 !MathUtilities::is_zero(self.get_determinant())
109 }
110
111 #[inline]
113 pub fn create_rotation(radians: f64) -> Matrix {
114 let cos = radians.cos();
115 let sin = radians.sin();
116 Matrix::new(cos, sin, -sin, cos, 0.0, 0.0)
117 }
118
119 #[inline]
121 pub fn create_rotation_at(radians: f64, center: Point) -> Matrix {
122 let cos = radians.cos();
123 let sin = radians.sin();
124 let x = center.x;
125 let y = center.y;
126 Matrix::new(
127 cos,
128 sin,
129 -sin,
130 cos,
131 x * (1.0 - cos) + y * sin,
132 y * (1.0 - cos) - x * sin,
133 )
134 }
135
136 #[inline]
138 pub fn create_skew(x_angle: f64, y_angle: f64) -> Matrix {
139 let tan_x = x_angle.tan();
140 let tan_y = y_angle.tan();
141 Matrix::new(1.0, tan_y, tan_x, 1.0, 0.0, 0.0)
142 }
143
144 #[inline]
146 pub const fn create_scale(x_scale: f64, y_scale: f64) -> Matrix {
147 Matrix::new(x_scale, 0.0, 0.0, y_scale, 0.0, 0.0)
148 }
149
150 #[inline]
152 pub const fn create_scale_vector(scales: Vector) -> Matrix {
153 Matrix::create_scale(scales.x, scales.y)
154 }
155
156 #[inline]
158 pub const fn create_translation_vector(position: Vector) -> Matrix {
159 Matrix::create_translation(position.x, position.y)
160 }
161
162 #[inline]
164 pub const fn create_translation(x_position: f64, y_position: f64) -> Matrix {
165 Matrix::new(1.0, 0.0, 0.0, 1.0, x_position, y_position)
166 }
167
168 #[inline]
170 pub fn to_radians(angle: f64) -> f64 {
171 angle * 0.0174532925
172 }
173
174 #[inline]
177 pub fn append(&self, value: Matrix) -> Matrix {
178 *self * value
179 }
180
181 #[inline]
184 pub fn prepend(&self, value: Matrix) -> Matrix {
185 value * *self
186 }
187
188 #[inline]
190 pub fn get_determinant(&self) -> f64 {
191 self.m11 * (self.m22 * self.m33 - self.m23 * self.m32)
193 - self.m12 * (self.m21 * self.m33 - self.m23 * self.m31)
194 + self.m13 * (self.m21 * self.m32 - self.m22 * self.m31)
195 }
196
197 #[inline]
199 pub fn transform(&self, p: Point) -> Point {
200 if self.contains_perspective() {
206 let (x, y) = (p.x as f32, p.y as f32);
207
208 let tx = x * self.m11 as f32 + y * self.m21 as f32 + self.m31 as f32;
209 let ty = x * self.m12 as f32 + y * self.m22 as f32 + self.m32 as f32;
210 let tz = x * self.m13 as f32 + y * self.m23 as f32 + self.m33 as f32;
211
212 let z = 1.0 / tz;
213
214 Point::new((tx * z) as f64, (ty * z) as f64)
215 } else {
216 Point::new(
217 (p.x * self.m11) + (p.y * self.m21) + self.m31,
218 (p.x * self.m12) + (p.y * self.m22) + self.m32,
219 )
220 }
221 }
222
223 #[inline]
226 pub fn equals(&self, other: Matrix) -> bool {
227 *self == other
228 }
229
230 #[inline]
232 pub fn contains_perspective(&self) -> bool {
233 self.m13 != 0.0 || self.m23 != 0.0 || self.m33 != 1.0
234 }
235
236 pub fn try_invert(&self) -> Option<Matrix> {
238 let d = self.get_determinant();
239
240 if MathUtilities::is_zero(d) {
241 return None;
242 }
243
244 let invdet = 1.0 / d;
245
246 Some(Matrix::new_3x3(
247 (self.m22 * self.m33 - self.m32 * self.m23) * invdet,
248 (self.m13 * self.m32 - self.m12 * self.m33) * invdet,
249 (self.m12 * self.m23 - self.m13 * self.m22) * invdet,
250 (self.m23 * self.m31 - self.m21 * self.m33) * invdet,
251 (self.m11 * self.m33 - self.m13 * self.m31) * invdet,
252 (self.m21 * self.m13 - self.m11 * self.m23) * invdet,
253 (self.m21 * self.m32 - self.m31 * self.m22) * invdet,
254 (self.m31 * self.m12 - self.m11 * self.m32) * invdet,
255 (self.m11 * self.m22 - self.m21 * self.m12) * invdet,
256 ))
257 }
258
259 #[inline]
265 pub fn invert(&self) -> Matrix {
266 match self.try_invert() {
267 Some(inverted) => inverted,
268 None => panic!("Transform is not invertible."),
269 }
270 }
271
272 pub fn parse(s: &str) -> Result<Self, FormatError> {
276 SpanStringTokenizer::with_message(s, "Invalid Matrix.").scope(|t| {
277 let v1 = t.read_double()?;
278 let v2 = t.read_double()?;
279 let v3 = t.read_double()?;
280 let v4 = t.read_double()?;
281 let v5 = t.read_double()?;
282 let v6 = t.read_double()?;
283
284 let mut persp = None;
286 if let Some(v7) = t.try_read_double()? {
287 if let Some(v8) = t.try_read_double()? {
288 if let Some(v9) = t.try_read_double()? {
289 persp = Some((v7, v8, v9));
290 }
291 }
292 }
293
294 Ok(match persp {
295 Some((v7, v8, v9)) => Matrix::new_3x3(v1, v2, v7, v3, v4, v8, v5, v6, v9),
296 None => Matrix::new(v1, v2, v3, v4, v5, v6),
297 })
298 })
299 }
300
301 pub fn try_decompose_transform(matrix: Matrix) -> Option<MatrixDecomposed> {
304 let determinant = matrix.get_determinant();
305
306 if MathUtilities::is_zero(determinant) || matrix.contains_perspective() {
307 return None;
308 }
309
310 let mut m11 = matrix.m11;
311 let mut m21 = matrix.m21;
312 let mut m12 = matrix.m12;
313 let mut m22 = matrix.m22;
314
315 let translate = Vector::new(matrix.m31, matrix.m32);
317
318 let mut scale_x = 1.0;
320 let mut scale_y = 1.0;
321
322 if determinant < 0.0 {
323 if m11 < m22 {
324 scale_x *= -1.0;
325 } else {
326 scale_y *= -1.0;
327 }
328 }
329
330 scale_x *= (m11 * m11 + m12 * m12).sqrt();
332
333 m11 /= scale_x;
334 m12 /= scale_x;
335
336 let scaled_shear = m11 * m21 + m12 * m22;
338
339 m21 -= m11 * scaled_shear;
340 m22 -= m12 * scaled_shear;
341
342 scale_y *= (m21 * m21 + m22 * m22).sqrt();
344
345 Some(MatrixDecomposed {
346 translate,
347 scale: Vector::new(scale_x, scale_y),
348 skew: Vector::new(scaled_shear / scale_y, 0.0),
349 angle: m12.atan2(m11),
350 })
351 }
352}
353
354impl Mul for Matrix {
356 type Output = Matrix;
357 #[inline]
358 fn mul(self, value2: Matrix) -> Matrix {
359 let value1 = self;
360 Matrix::new_3x3(
361 (value1.m11 * value2.m11) + (value1.m12 * value2.m21) + (value1.m13 * value2.m31),
362 (value1.m11 * value2.m12) + (value1.m12 * value2.m22) + (value1.m13 * value2.m32),
363 (value1.m11 * value2.m13) + (value1.m12 * value2.m23) + (value1.m13 * value2.m33),
364 (value1.m21 * value2.m11) + (value1.m22 * value2.m21) + (value1.m23 * value2.m31),
365 (value1.m21 * value2.m12) + (value1.m22 * value2.m22) + (value1.m23 * value2.m32),
366 (value1.m21 * value2.m13) + (value1.m22 * value2.m23) + (value1.m23 * value2.m33),
367 (value1.m31 * value2.m11) + (value1.m32 * value2.m21) + (value1.m33 * value2.m31),
368 (value1.m31 * value2.m12) + (value1.m32 * value2.m22) + (value1.m33 * value2.m32),
369 (value1.m31 * value2.m13) + (value1.m32 * value2.m23) + (value1.m33 * value2.m33),
370 )
371 }
372}
373
374impl MulAssign for Matrix {
375 #[inline]
376 fn mul_assign(&mut self, value: Matrix) {
377 *self = *self * value;
378 }
379}
380
381impl Neg for Matrix {
386 type Output = Matrix;
387 #[inline]
388 fn neg(self) -> Matrix {
389 self.invert()
390 }
391}
392
393impl FromStr for Matrix {
394 type Err = FormatError;
395 #[inline]
396 fn from_str(s: &str) -> Result<Self, FormatError> {
397 Matrix::parse(s)
398 }
399}
400
401impl fmt::Display for Matrix {
402 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
403 if self.contains_perspective() {
404 write!(
405 f,
406 "{{ {{M11:{} M12:{} M13:{}}} {{M21:{} M22:{} M23:{}}} {{M31:{} M32:{} M33:{}}} }}",
407 InvariantF64(self.m11),
408 InvariantF64(self.m12),
409 InvariantF64(self.m13),
410 InvariantF64(self.m21),
411 InvariantF64(self.m22),
412 InvariantF64(self.m23),
413 InvariantF64(self.m31),
414 InvariantF64(self.m32),
415 InvariantF64(self.m33)
416 )
417 } else {
418 write!(
419 f,
420 "{{ {{M11:{} M12:{}}} {{M21:{} M22:{}}} {{M31:{} M32:{}}} }}",
421 InvariantF64(self.m11),
422 InvariantF64(self.m12),
423 InvariantF64(self.m21),
424 InvariantF64(self.m22),
425 InvariantF64(self.m31),
426 InvariantF64(self.m32)
427 )
428 }
429 }
430}
431
432#[cfg(test)]
433mod tests {
434 use super::*;
435
436 #[derive(Clone, Copy)]
439 struct Matrix3x2 {
440 m11: f32,
441 m12: f32,
442 m21: f32,
443 m22: f32,
444 m31: f32,
445 m32: f32,
446 }
447
448 impl Matrix3x2 {
449 fn create_translation(x: f32, y: f32) -> Self {
450 Self {
451 m11: 1.0,
452 m12: 0.0,
453 m21: 0.0,
454 m22: 1.0,
455 m31: x,
456 m32: y,
457 }
458 }
459
460 fn create_scale(x: f32, y: f32) -> Self {
461 Self {
462 m11: x,
463 m12: 0.0,
464 m21: 0.0,
465 m22: y,
466 m31: 0.0,
467 m32: 0.0,
468 }
469 }
470
471 fn create_rotation(radians: f32) -> Self {
472 let (s, c) = radians.sin_cos();
473 Self {
474 m11: c,
475 m12: s,
476 m21: -s,
477 m22: c,
478 m31: 0.0,
479 m32: 0.0,
480 }
481 }
482
483 fn create_rotation_at(radians: f32, center: (f32, f32)) -> Self {
484 let (s, c) = radians.sin_cos();
485 let x = center.0 * (1.0 - c) + center.1 * s;
486 let y = center.1 * (1.0 - c) - center.0 * s;
487 Self {
488 m11: c,
489 m12: s,
490 m21: -s,
491 m22: c,
492 m31: x,
493 m32: y,
494 }
495 }
496
497 fn create_skew(radians_x: f32, radians_y: f32) -> Self {
498 Self {
499 m11: 1.0,
500 m12: radians_y.tan(),
501 m21: radians_x.tan(),
502 m22: 1.0,
503 m31: 0.0,
504 m32: 0.0,
505 }
506 }
507
508 fn transform(&self, v: (f32, f32)) -> (f32, f32) {
509 (
510 v.0 * self.m11 + v.1 * self.m21 + self.m31,
511 v.0 * self.m12 + v.1 * self.m22 + self.m32,
512 )
513 }
514 }
515
516 fn assert_coordinates_equal_with_reduced_precision(expected: (f32, f32), actual: Point) {
520 fn reduce_precision(input: f64) -> f64 {
521 (input * 10000.0).trunc()
522 }
523
524 let expected_x = reduce_precision(expected.0 as f64);
525 let expected_y = reduce_precision(expected.1 as f64);
526
527 let actual_x = reduce_precision(actual.x);
528 let actual_y = reduce_precision(actual.y);
529
530 assert_eq!(expected_x, actual_x);
531 assert_eq!(expected_y, actual_y);
532 }
533
534 #[test]
535 fn transform_point_should_return_correct_value_for_translated_matrix() {
536 let vector2 = Matrix3x2::create_translation(2.0, 2.0).transform((1.0, 1.0));
537 let expected = Point::new(vector2.0 as f64, vector2.1 as f64);
538
539 let matrix = Matrix::create_translation(2.0, 2.0);
540 let point = Point::new(1.0, 1.0);
541 let transformed_point = matrix.transform(point);
542
543 assert_eq!(expected, transformed_point);
544 }
545
546 #[test]
547 fn transform_point_should_return_correct_value_for_rotated_matrix() {
548 let expected =
549 Matrix3x2::create_rotation(Matrix::to_radians(45.0) as f32).transform((0.0, 10.0));
550
551 let matrix = Matrix::create_rotation(Matrix::to_radians(45.0));
552 let point = Point::new(0.0, 10.0);
553 let actual = matrix.transform(point);
554
555 assert_coordinates_equal_with_reduced_precision(expected, actual);
556 }
557
558 #[test]
559 fn transform_point_should_return_correct_value_for_rotate_matrix_with_center_point() {
560 let expected = Matrix3x2::create_rotation_at(Matrix::to_radians(30.0) as f32, (3.0, 5.0))
561 .transform((0.0, 10.0));
562
563 let matrix = Matrix::create_rotation_at(Matrix::to_radians(30.0), Point::new(3.0, 5.0));
564 let point = Point::new(0.0, 10.0);
565 let actual = matrix.transform(point);
566
567 assert_coordinates_equal_with_reduced_precision(expected, actual);
568 }
569
570 #[test]
571 fn transform_point_should_return_correct_value_for_scaled_matrix() {
572 let vector2 = Matrix3x2::create_scale(2.0, 2.0).transform((1.0, 1.0));
573 let expected = Point::new(vector2.0 as f64, vector2.1 as f64);
574 let matrix = Matrix::create_scale(2.0, 2.0);
575 let point = Point::new(1.0, 1.0);
576 let actual = matrix.transform(point);
577
578 assert_eq!(expected, actual);
579 }
580
581 #[test]
582 fn transform_point_should_return_correct_value_for_skewed_matrix() {
583 let expected = Matrix3x2::create_skew(30.0, 20.0).transform((1.0, 1.0));
584
585 let matrix = Matrix::create_skew(30.0, 20.0);
586 let point = Point::new(1.0, 1.0);
587 let actual = matrix.transform(point);
588
589 assert_coordinates_equal_with_reduced_precision(expected, actual);
590 }
591
592 #[test]
593 fn can_parse() {
594 let matrix = Matrix::parse("1,2,3,-4,5 6").unwrap();
595 let expected = Matrix::new(1.0, 2.0, 3.0, -4.0, 5.0, 6.0);
596 assert_eq!(expected, matrix);
597 }
598
599 #[test]
600 fn singular_has_no_inverse() {
601 let matrix = Matrix::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
602 assert!(!matrix.has_inverse());
603 }
604
605 #[test]
606 fn identity_has_inverse() {
607 let matrix = Matrix::IDENTITY;
608 assert!(matrix.has_inverse());
609 }
610
611 #[test]
612 fn invert_should_work() {
613 let matrix = Matrix::new_3x3(1.0, 2.0, 3.0, 0.0, 1.0, 4.0, 5.0, 6.0, 0.0);
614 let inverted = matrix.invert();
615
616 assert_eq!(matrix * inverted, Matrix::IDENTITY);
617 assert_eq!(inverted * matrix, Matrix::IDENTITY);
618 }
619
620 #[test]
621 fn can_decompose_translation() {
622 let matrix = Matrix::create_translation(5.0, 10.0);
623
624 let decomposed = Matrix::try_decompose_transform(matrix).unwrap();
625
626 assert_eq!(5.0, decomposed.translate.x);
627 assert_eq!(10.0, decomposed.translate.y);
628 }
629
630 fn normalize_angle(mut rad: f64) -> f64 {
631 let two_pi = 2.0 * std::f64::consts::PI;
632
633 while rad < 0.0 {
634 rad += two_pi;
635 }
636
637 while rad > two_pi {
638 rad -= two_pi;
639 }
640
641 rad
642 }
643
644 fn round(value: f64, digits: i32) -> f64 {
646 let power = 10f64.powi(digits);
647 (value * power).round_ties_even() / power
648 }
649
650 #[test]
651 fn can_decompose_angle() {
652 for angle_deg in [30.0, 0.0, 90.0, 270.0] {
653 let angle_rad = MathUtilities::deg2rad(angle_deg);
654
655 let matrix = Matrix::create_rotation(angle_rad);
656
657 let decomposed = Matrix::try_decompose_transform(matrix).unwrap();
658
659 let expected = normalize_angle(angle_rad);
660 let actual = normalize_angle(decomposed.angle);
661
662 assert_eq!(round(expected, 4), round(actual, 4), "angle {angle_deg}");
663 }
664 }
665
666 #[test]
667 fn can_decompose_scale() {
668 for (x, y) in [(1.0, 1.0), (-1.0, 1.0), (1.0, -1.0), (5.0, 10.0)] {
669 let matrix = Matrix::create_scale(x, y);
670
671 let decomposed = Matrix::try_decompose_transform(matrix).unwrap();
672
673 assert_eq!(x, decomposed.scale.x);
674 assert_eq!(y, decomposed.scale.y);
675 }
676 }
677
678 #[test]
681 fn parse_nine_values_and_errors() {
682 let m = Matrix::parse("1,2,3,4,5,6,7,8,9").unwrap();
683 assert_eq!(
684 m,
685 Matrix::new_3x3(1.0, 2.0, 7.0, 3.0, 4.0, 8.0, 5.0, 6.0, 9.0)
686 );
687 assert_eq!(
688 Matrix::parse("1,2,3").unwrap_err().message(),
689 "Invalid Matrix."
690 );
691 assert!(Matrix::parse("1,2,3,4,5,6,7,8,9,10").is_err());
692 }
693
694 #[test]
695 fn display() {
696 assert_eq!(
697 Matrix::IDENTITY.to_string(),
698 "{ {M11:1 M12:0} {M21:0 M22:1} {M31:0 M32:0} }"
699 );
700 assert_eq!(
701 Matrix::new_3x3(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.5).to_string(),
702 "{ {M11:1 M12:2 M13:3} {M21:4 M22:5 M23:6} {M31:7 M32:8 M33:9.5} }"
703 );
704 }
705
706 #[test]
707 fn identity_default_and_perspective() {
708 assert!(Matrix::IDENTITY.is_identity());
709 assert!(!Matrix::default().is_identity());
710 assert!(!Matrix::IDENTITY.contains_perspective());
711 assert!(Matrix::default().contains_perspective());
712 assert_eq!(Matrix::default().try_invert(), None);
713 assert_eq!(Matrix::try_decompose_transform(Matrix::default()), None);
714 }
715
716 #[test]
717 #[should_panic(expected = "Transform is not invertible.")]
718 fn invert_of_singular_matrix_panics() {
719 let _ = Matrix::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).invert();
720 }
721
722 #[test]
723 fn perspective_transform_divides_by_w() {
724 let m = Matrix::new_3x3(1.0, 0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
726 assert_eq!(m.transform(Point::new(2.0, 4.0)), Point::new(1.0, 2.0));
727 }
728
729 #[test]
730 fn append_prepend_and_negation() {
731 let t = Matrix::create_translation(10.0, 0.0);
732 let s = Matrix::create_scale(2.0, 2.0);
733 assert_eq!(t.append(s), t * s);
734 assert_eq!(t.prepend(s), s * t);
735 assert_eq!(
736 (t * s).transform(Point::new(1.0, 1.0)),
737 Point::new(22.0, 2.0)
738 );
739 assert_eq!(
740 (s * t).transform(Point::new(1.0, 1.0)),
741 Point::new(12.0, 2.0)
742 );
743 assert_eq!(-t, Matrix::create_translation(-10.0, 0.0));
744 }
745}