use std::fmt;
use std::ops::{Mul, MulAssign, Neg};
use std::str::FromStr;
use crate::utilities::math_utilities::MathUtilities;
use crate::utilities::span_helpers::InvariantF64;
use crate::utilities::{FormatError, SpanStringTokenizer};
use crate::{Point, Vector};
#[derive(Clone, Copy, Debug, PartialEq, Default)]
pub struct Matrix {
pub m11: f64,
pub m12: f64,
pub m13: f64,
pub m21: f64,
pub m22: f64,
pub m23: f64,
pub m31: f64,
pub m32: f64,
pub m33: f64,
}
#[derive(Clone, Copy, Debug, PartialEq, Default)]
pub struct MatrixDecomposed {
pub translate: Vector,
pub scale: Vector,
pub skew: Vector,
pub angle: f64,
}
impl Matrix {
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);
#[inline]
pub const fn new(
scale_x: f64,
skew_y: f64,
skew_x: f64,
scale_y: f64,
offset_x: f64,
offset_y: f64,
) -> Self {
Self::new_3x3(
scale_x, skew_y, 0.0, skew_x, scale_y, 0.0, offset_x, offset_y, 1.0,
)
}
#[inline]
#[allow(clippy::too_many_arguments)]
pub const fn new_3x3(
scale_x: f64,
skew_y: f64,
persp_x: f64,
skew_x: f64,
scale_y: f64,
persp_y: f64,
offset_x: f64,
offset_y: f64,
persp_z: f64,
) -> Self {
Self {
m11: scale_x,
m12: skew_y,
m13: persp_x,
m21: skew_x,
m22: scale_y,
m23: persp_y,
m31: offset_x,
m32: offset_y,
m33: persp_z,
}
}
#[inline]
pub fn is_identity(&self) -> bool {
*self == Matrix::IDENTITY
}
#[inline]
pub fn has_inverse(&self) -> bool {
!MathUtilities::is_zero(self.get_determinant())
}
#[inline]
pub fn create_rotation(radians: f64) -> Matrix {
let cos = radians.cos();
let sin = radians.sin();
Matrix::new(cos, sin, -sin, cos, 0.0, 0.0)
}
#[inline]
pub fn create_rotation_at(radians: f64, center: Point) -> Matrix {
let cos = radians.cos();
let sin = radians.sin();
let x = center.x;
let y = center.y;
Matrix::new(
cos,
sin,
-sin,
cos,
x * (1.0 - cos) + y * sin,
y * (1.0 - cos) - x * sin,
)
}
#[inline]
pub fn create_skew(x_angle: f64, y_angle: f64) -> Matrix {
let tan_x = x_angle.tan();
let tan_y = y_angle.tan();
Matrix::new(1.0, tan_y, tan_x, 1.0, 0.0, 0.0)
}
#[inline]
pub const fn create_scale(x_scale: f64, y_scale: f64) -> Matrix {
Matrix::new(x_scale, 0.0, 0.0, y_scale, 0.0, 0.0)
}
#[inline]
pub const fn create_scale_vector(scales: Vector) -> Matrix {
Matrix::create_scale(scales.x, scales.y)
}
#[inline]
pub const fn create_translation_vector(position: Vector) -> Matrix {
Matrix::create_translation(position.x, position.y)
}
#[inline]
pub const fn create_translation(x_position: f64, y_position: f64) -> Matrix {
Matrix::new(1.0, 0.0, 0.0, 1.0, x_position, y_position)
}
#[inline]
pub fn to_radians(angle: f64) -> f64 {
angle * 0.0174532925
}
#[inline]
pub fn append(&self, value: Matrix) -> Matrix {
*self * value
}
#[inline]
pub fn prepend(&self, value: Matrix) -> Matrix {
value * *self
}
#[inline]
pub fn get_determinant(&self) -> f64 {
self.m11 * (self.m22 * self.m33 - self.m23 * self.m32)
- self.m12 * (self.m21 * self.m33 - self.m23 * self.m31)
+ self.m13 * (self.m21 * self.m32 - self.m22 * self.m31)
}
#[inline]
pub fn transform(&self, p: Point) -> Point {
if self.contains_perspective() {
let (x, y) = (p.x as f32, p.y as f32);
let tx = x * self.m11 as f32 + y * self.m21 as f32 + self.m31 as f32;
let ty = x * self.m12 as f32 + y * self.m22 as f32 + self.m32 as f32;
let tz = x * self.m13 as f32 + y * self.m23 as f32 + self.m33 as f32;
let z = 1.0 / tz;
Point::new((tx * z) as f64, (ty * z) as f64)
} else {
Point::new(
(p.x * self.m11) + (p.y * self.m21) + self.m31,
(p.x * self.m12) + (p.y * self.m22) + self.m32,
)
}
}
#[inline]
pub fn equals(&self, other: Matrix) -> bool {
*self == other
}
#[inline]
pub fn contains_perspective(&self) -> bool {
self.m13 != 0.0 || self.m23 != 0.0 || self.m33 != 1.0
}
pub fn try_invert(&self) -> Option<Matrix> {
let d = self.get_determinant();
if MathUtilities::is_zero(d) {
return None;
}
let invdet = 1.0 / d;
Some(Matrix::new_3x3(
(self.m22 * self.m33 - self.m32 * self.m23) * invdet,
(self.m13 * self.m32 - self.m12 * self.m33) * invdet,
(self.m12 * self.m23 - self.m13 * self.m22) * invdet,
(self.m23 * self.m31 - self.m21 * self.m33) * invdet,
(self.m11 * self.m33 - self.m13 * self.m31) * invdet,
(self.m21 * self.m13 - self.m11 * self.m23) * invdet,
(self.m21 * self.m32 - self.m31 * self.m22) * invdet,
(self.m31 * self.m12 - self.m11 * self.m32) * invdet,
(self.m11 * self.m22 - self.m21 * self.m12) * invdet,
))
}
#[inline]
pub fn invert(&self) -> Matrix {
match self.try_invert() {
Some(inverted) => inverted,
None => panic!("Transform is not invertible."),
}
}
pub fn parse(s: &str) -> Result<Self, FormatError> {
SpanStringTokenizer::with_message(s, "Invalid Matrix.").scope(|t| {
let v1 = t.read_double()?;
let v2 = t.read_double()?;
let v3 = t.read_double()?;
let v4 = t.read_double()?;
let v5 = t.read_double()?;
let v6 = t.read_double()?;
let mut persp = None;
if let Some(v7) = t.try_read_double()? {
if let Some(v8) = t.try_read_double()? {
if let Some(v9) = t.try_read_double()? {
persp = Some((v7, v8, v9));
}
}
}
Ok(match persp {
Some((v7, v8, v9)) => Matrix::new_3x3(v1, v2, v7, v3, v4, v8, v5, v6, v9),
None => Matrix::new(v1, v2, v3, v4, v5, v6),
})
})
}
pub fn try_decompose_transform(matrix: Matrix) -> Option<MatrixDecomposed> {
let determinant = matrix.get_determinant();
if MathUtilities::is_zero(determinant) || matrix.contains_perspective() {
return None;
}
let mut m11 = matrix.m11;
let mut m21 = matrix.m21;
let mut m12 = matrix.m12;
let mut m22 = matrix.m22;
let translate = Vector::new(matrix.m31, matrix.m32);
let mut scale_x = 1.0;
let mut scale_y = 1.0;
if determinant < 0.0 {
if m11 < m22 {
scale_x *= -1.0;
} else {
scale_y *= -1.0;
}
}
scale_x *= (m11 * m11 + m12 * m12).sqrt();
m11 /= scale_x;
m12 /= scale_x;
let scaled_shear = m11 * m21 + m12 * m22;
m21 -= m11 * scaled_shear;
m22 -= m12 * scaled_shear;
scale_y *= (m21 * m21 + m22 * m22).sqrt();
Some(MatrixDecomposed {
translate,
scale: Vector::new(scale_x, scale_y),
skew: Vector::new(scaled_shear / scale_y, 0.0),
angle: m12.atan2(m11),
})
}
}
impl Mul for Matrix {
type Output = Matrix;
#[inline]
fn mul(self, value2: Matrix) -> Matrix {
let value1 = self;
Matrix::new_3x3(
(value1.m11 * value2.m11) + (value1.m12 * value2.m21) + (value1.m13 * value2.m31),
(value1.m11 * value2.m12) + (value1.m12 * value2.m22) + (value1.m13 * value2.m32),
(value1.m11 * value2.m13) + (value1.m12 * value2.m23) + (value1.m13 * value2.m33),
(value1.m21 * value2.m11) + (value1.m22 * value2.m21) + (value1.m23 * value2.m31),
(value1.m21 * value2.m12) + (value1.m22 * value2.m22) + (value1.m23 * value2.m32),
(value1.m21 * value2.m13) + (value1.m22 * value2.m23) + (value1.m23 * value2.m33),
(value1.m31 * value2.m11) + (value1.m32 * value2.m21) + (value1.m33 * value2.m31),
(value1.m31 * value2.m12) + (value1.m32 * value2.m22) + (value1.m33 * value2.m32),
(value1.m31 * value2.m13) + (value1.m32 * value2.m23) + (value1.m33 * value2.m33),
)
}
}
impl MulAssign for Matrix {
#[inline]
fn mul_assign(&mut self, value: Matrix) {
*self = *self * value;
}
}
impl Neg for Matrix {
type Output = Matrix;
#[inline]
fn neg(self) -> Matrix {
self.invert()
}
}
impl FromStr for Matrix {
type Err = FormatError;
#[inline]
fn from_str(s: &str) -> Result<Self, FormatError> {
Matrix::parse(s)
}
}
impl fmt::Display for Matrix {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
if self.contains_perspective() {
write!(
f,
"{{ {{M11:{} M12:{} M13:{}}} {{M21:{} M22:{} M23:{}}} {{M31:{} M32:{} M33:{}}} }}",
InvariantF64(self.m11),
InvariantF64(self.m12),
InvariantF64(self.m13),
InvariantF64(self.m21),
InvariantF64(self.m22),
InvariantF64(self.m23),
InvariantF64(self.m31),
InvariantF64(self.m32),
InvariantF64(self.m33)
)
} else {
write!(
f,
"{{ {{M11:{} M12:{}}} {{M21:{} M22:{}}} {{M31:{} M32:{}}} }}",
InvariantF64(self.m11),
InvariantF64(self.m12),
InvariantF64(self.m21),
InvariantF64(self.m22),
InvariantF64(self.m31),
InvariantF64(self.m32)
)
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[derive(Clone, Copy)]
struct Matrix3x2 {
m11: f32,
m12: f32,
m21: f32,
m22: f32,
m31: f32,
m32: f32,
}
impl Matrix3x2 {
fn create_translation(x: f32, y: f32) -> Self {
Self {
m11: 1.0,
m12: 0.0,
m21: 0.0,
m22: 1.0,
m31: x,
m32: y,
}
}
fn create_scale(x: f32, y: f32) -> Self {
Self {
m11: x,
m12: 0.0,
m21: 0.0,
m22: y,
m31: 0.0,
m32: 0.0,
}
}
fn create_rotation(radians: f32) -> Self {
let (s, c) = radians.sin_cos();
Self {
m11: c,
m12: s,
m21: -s,
m22: c,
m31: 0.0,
m32: 0.0,
}
}
fn create_rotation_at(radians: f32, center: (f32, f32)) -> Self {
let (s, c) = radians.sin_cos();
let x = center.0 * (1.0 - c) + center.1 * s;
let y = center.1 * (1.0 - c) - center.0 * s;
Self {
m11: c,
m12: s,
m21: -s,
m22: c,
m31: x,
m32: y,
}
}
fn create_skew(radians_x: f32, radians_y: f32) -> Self {
Self {
m11: 1.0,
m12: radians_y.tan(),
m21: radians_x.tan(),
m22: 1.0,
m31: 0.0,
m32: 0.0,
}
}
fn transform(&self, v: (f32, f32)) -> (f32, f32) {
(
v.0 * self.m11 + v.1 * self.m21 + self.m31,
v.0 * self.m12 + v.1 * self.m22 + self.m32,
)
}
}
fn assert_coordinates_equal_with_reduced_precision(expected: (f32, f32), actual: Point) {
fn reduce_precision(input: f64) -> f64 {
(input * 10000.0).trunc()
}
let expected_x = reduce_precision(expected.0 as f64);
let expected_y = reduce_precision(expected.1 as f64);
let actual_x = reduce_precision(actual.x);
let actual_y = reduce_precision(actual.y);
assert_eq!(expected_x, actual_x);
assert_eq!(expected_y, actual_y);
}
#[test]
fn transform_point_should_return_correct_value_for_translated_matrix() {
let vector2 = Matrix3x2::create_translation(2.0, 2.0).transform((1.0, 1.0));
let expected = Point::new(vector2.0 as f64, vector2.1 as f64);
let matrix = Matrix::create_translation(2.0, 2.0);
let point = Point::new(1.0, 1.0);
let transformed_point = matrix.transform(point);
assert_eq!(expected, transformed_point);
}
#[test]
fn transform_point_should_return_correct_value_for_rotated_matrix() {
let expected =
Matrix3x2::create_rotation(Matrix::to_radians(45.0) as f32).transform((0.0, 10.0));
let matrix = Matrix::create_rotation(Matrix::to_radians(45.0));
let point = Point::new(0.0, 10.0);
let actual = matrix.transform(point);
assert_coordinates_equal_with_reduced_precision(expected, actual);
}
#[test]
fn transform_point_should_return_correct_value_for_rotate_matrix_with_center_point() {
let expected = Matrix3x2::create_rotation_at(Matrix::to_radians(30.0) as f32, (3.0, 5.0))
.transform((0.0, 10.0));
let matrix = Matrix::create_rotation_at(Matrix::to_radians(30.0), Point::new(3.0, 5.0));
let point = Point::new(0.0, 10.0);
let actual = matrix.transform(point);
assert_coordinates_equal_with_reduced_precision(expected, actual);
}
#[test]
fn transform_point_should_return_correct_value_for_scaled_matrix() {
let vector2 = Matrix3x2::create_scale(2.0, 2.0).transform((1.0, 1.0));
let expected = Point::new(vector2.0 as f64, vector2.1 as f64);
let matrix = Matrix::create_scale(2.0, 2.0);
let point = Point::new(1.0, 1.0);
let actual = matrix.transform(point);
assert_eq!(expected, actual);
}
#[test]
fn transform_point_should_return_correct_value_for_skewed_matrix() {
let expected = Matrix3x2::create_skew(30.0, 20.0).transform((1.0, 1.0));
let matrix = Matrix::create_skew(30.0, 20.0);
let point = Point::new(1.0, 1.0);
let actual = matrix.transform(point);
assert_coordinates_equal_with_reduced_precision(expected, actual);
}
#[test]
fn can_parse() {
let matrix = Matrix::parse("1,2,3,-4,5 6").unwrap();
let expected = Matrix::new(1.0, 2.0, 3.0, -4.0, 5.0, 6.0);
assert_eq!(expected, matrix);
}
#[test]
fn singular_has_no_inverse() {
let matrix = Matrix::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
assert!(!matrix.has_inverse());
}
#[test]
fn identity_has_inverse() {
let matrix = Matrix::IDENTITY;
assert!(matrix.has_inverse());
}
#[test]
fn invert_should_work() {
let matrix = Matrix::new_3x3(1.0, 2.0, 3.0, 0.0, 1.0, 4.0, 5.0, 6.0, 0.0);
let inverted = matrix.invert();
assert_eq!(matrix * inverted, Matrix::IDENTITY);
assert_eq!(inverted * matrix, Matrix::IDENTITY);
}
#[test]
fn can_decompose_translation() {
let matrix = Matrix::create_translation(5.0, 10.0);
let decomposed = Matrix::try_decompose_transform(matrix).unwrap();
assert_eq!(5.0, decomposed.translate.x);
assert_eq!(10.0, decomposed.translate.y);
}
fn normalize_angle(mut rad: f64) -> f64 {
let two_pi = 2.0 * std::f64::consts::PI;
while rad < 0.0 {
rad += two_pi;
}
while rad > two_pi {
rad -= two_pi;
}
rad
}
fn round(value: f64, digits: i32) -> f64 {
let power = 10f64.powi(digits);
(value * power).round_ties_even() / power
}
#[test]
fn can_decompose_angle() {
for angle_deg in [30.0, 0.0, 90.0, 270.0] {
let angle_rad = MathUtilities::deg2rad(angle_deg);
let matrix = Matrix::create_rotation(angle_rad);
let decomposed = Matrix::try_decompose_transform(matrix).unwrap();
let expected = normalize_angle(angle_rad);
let actual = normalize_angle(decomposed.angle);
assert_eq!(round(expected, 4), round(actual, 4), "angle {angle_deg}");
}
}
#[test]
fn can_decompose_scale() {
for (x, y) in [(1.0, 1.0), (-1.0, 1.0), (1.0, -1.0), (5.0, 10.0)] {
let matrix = Matrix::create_scale(x, y);
let decomposed = Matrix::try_decompose_transform(matrix).unwrap();
assert_eq!(x, decomposed.scale.x);
assert_eq!(y, decomposed.scale.y);
}
}
#[test]
fn parse_nine_values_and_errors() {
let m = Matrix::parse("1,2,3,4,5,6,7,8,9").unwrap();
assert_eq!(
m,
Matrix::new_3x3(1.0, 2.0, 7.0, 3.0, 4.0, 8.0, 5.0, 6.0, 9.0)
);
assert_eq!(
Matrix::parse("1,2,3").unwrap_err().message(),
"Invalid Matrix."
);
assert!(Matrix::parse("1,2,3,4,5,6,7,8,9,10").is_err());
}
#[test]
fn display() {
assert_eq!(
Matrix::IDENTITY.to_string(),
"{ {M11:1 M12:0} {M21:0 M22:1} {M31:0 M32:0} }"
);
assert_eq!(
Matrix::new_3x3(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.5).to_string(),
"{ {M11:1 M12:2 M13:3} {M21:4 M22:5 M23:6} {M31:7 M32:8 M33:9.5} }"
);
}
#[test]
fn identity_default_and_perspective() {
assert!(Matrix::IDENTITY.is_identity());
assert!(!Matrix::default().is_identity());
assert!(!Matrix::IDENTITY.contains_perspective());
assert!(Matrix::default().contains_perspective());
assert_eq!(Matrix::default().try_invert(), None);
assert_eq!(Matrix::try_decompose_transform(Matrix::default()), None);
}
#[test]
#[should_panic(expected = "Transform is not invertible.")]
fn invert_of_singular_matrix_panics() {
let _ = Matrix::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).invert();
}
#[test]
fn perspective_transform_divides_by_w() {
let m = Matrix::new_3x3(1.0, 0.0, 0.5, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
assert_eq!(m.transform(Point::new(2.0, 4.0)), Point::new(1.0, 2.0));
}
#[test]
fn append_prepend_and_negation() {
let t = Matrix::create_translation(10.0, 0.0);
let s = Matrix::create_scale(2.0, 2.0);
assert_eq!(t.append(s), t * s);
assert_eq!(t.prepend(s), s * t);
assert_eq!(
(t * s).transform(Point::new(1.0, 1.0)),
Point::new(22.0, 2.0)
);
assert_eq!(
(s * t).transform(Point::new(1.0, 1.0)),
Point::new(12.0, 2.0)
);
assert_eq!(-t, Matrix::create_translation(-10.0, 0.0));
}
}