diff --git a/src/mat.rs b/src/mat.rs index ae7339f..166a022 100644 --- a/src/mat.rs +++ b/src/mat.rs @@ -1702,17 +1702,97 @@ impl MatTranspose> for Mat43 where T: Number { } } +/// trait for the minor of a matrix +/// the minor is the determinant of the matrix without the given row and column +pub trait MatMinor { + fn minor(&self, row: u32, column: u32) -> T; +} + +impl MatMinor for Mat2 where T: Number { + fn minor(&self, row: u32, column: u32) -> T { + self.at(1 - row, 1 - column) + } +} + +impl MatMinor> for Mat3 where T: Number { + fn minor(&self, row: u32, column: u32) -> T { + let mut m = Mat2::zero(); + let mut pos = 0; + + for l in 0..3 { + for k in 0..3 { + if l != row && k != column { + m.set(pos / 2, pos % 2, self.at(l, k)); + pos += 1; + } + } + } + + m.determinant() + } +} + +impl MatMinor> for Mat4 where T: Number { + fn minor(&self, row: u32, column: u32) -> T { + let mut m = Mat3::zero(); + let mut pos = 0; + + for l in 0..4 { + for k in 0..4 { + if l != row && k != column { + m.set(pos / 3, pos % 3, self.at(l, k)); + pos += 1; + } + } + } + + m.determinant() + } +} + +/// trait for the cofactor of a matrix +/// the cofactor is the minor with an alternating sign, multiplied by (-1)^(row+column) +pub trait MatCofactor { + fn cofactor(&self, row: u32, column: u32) -> T; +} + +impl MatCofactor for Mat2 where T: SignedNumber { + fn cofactor(&self, row: u32, column: u32) -> T { + let sign = if (row + column & 1) == 1 { T::minus_one() } else { T::one() }; + sign * self.minor(row, column) + } +} + +impl MatCofactor for Mat3 where T: SignedNumber { + fn cofactor(&self, row: u32, column: u32) -> T { + let sign = if (row + column & 1) == 1 { T::minus_one() } else { T::one() }; + sign * self.minor(row, column) + } +} + +impl MatCofactor for Mat4 where T: SignedNumber { + fn cofactor(&self, row: u32, column: u32) -> T { + let sign = if (row + column & 1) == 1 { T::minus_one() } else { T::one() }; + sign * self.minor(row, column) + } +} + +/// trait for the adjugate of a matrix +/// the adjugate is the transpose of the cofactor matrix +/// the cofactor matrix is the matrix where each element is replaced by its cofactor pub trait MatAdjugate { fn adjugate(&self) -> Self; } impl MatAdjugate for Mat2 where T: SignedNumber { fn adjugate(&self) -> Self { - // 2x2 is a simple hardcoded case - Mat2::from(( - self.m[3], -self.m[1], - -self.m[2], self.m[0] - )) + let mut output = Mat2::zero(); + output.set(0, 0, self.cofactor(0, 0)); + output.set(0, 1, self.cofactor(1, 0)); + output.set(1, 0, self.cofactor(0, 1)); + output.set(1, 1, self.cofactor(1, 1)); + + output } } @@ -1721,54 +1801,23 @@ impl MatAdjugate for Mat3 where T: SignedNumber { let mut output = Mat3::zero(); for j in 0..3 { for i in 0..3 { - // gather minor matrix - let mut mm = Mat2::zero(); - let mut pos = 0; - for l in 0..3 { - for k in 0..3 { - if l != j && k != i { - let sign = if (k+l & 1) == 1 { - T::minus_one() - } else { - T::one() - }; - mm.m[pos] = self.at(k, l) * sign; - pos = pos + 1; - } - } - } - output.set(i, j, mm.determinant()); + output.set(i, j, self.cofactor(i, j)); } } + output.transpose() } } - impl MatAdjugate for Mat4 where T: SignedNumber { fn adjugate(&self) -> Self { let mut output = Mat4::zero(); for j in 0..4 { for i in 0..4 { - // gather minor matrix - let mut mm = Mat3::zero(); - let mut pos = 0; - for l in 0..4 { - for k in 0..4 { - if l != j && k != i { - let sign = if (k+l & 1) == 1 { - T::minus_one() - } else { - T::one() - }; - mm.m[pos] = self.at(k, l) * sign; - pos = pos + 1; - } - } - } - output.set(i, j, mm.determinant()); + output.set(i, j, self.cofactor(i, j)); } } + output.transpose() } } diff --git a/src/quat.rs b/src/quat.rs index 29c5558..8313b77 100644 --- a/src/quat.rs +++ b/src/quat.rs @@ -78,14 +78,14 @@ impl Quat where T: Float + FloatOps + SignedNumberOps { pub fn from_matrix(m: Mat3) -> Self { // https://math.stackexchange.com/questions/893984/conversion-of-rotation-matrix-to-quaternion let m00 = m.m[0]; - let m01 = m.m[4]; - let m02 = m.m[8]; + let m01 = m.m[3]; + let m02 = m.m[6]; let m10 = m.m[1]; - let m11 = m.m[5]; - let m12 = m.m[9]; + let m11 = m.m[4]; + let m12 = m.m[7]; let m20 = m.m[2]; - let m21 = m.m[6]; - let m22 = m.m[10]; + let m21 = m.m[5]; + let m22 = m.m[8]; let t = T::zero(); let t0 = T::zero(); diff --git a/tests/tests.rs b/tests/tests.rs index 70541b2..9b013d7 100644 --- a/tests/tests.rs +++ b/tests/tests.rs @@ -6172,4 +6172,84 @@ fn test_adjugate() { -399, -288, 69, 177, 123, 75, -228, -27 ))); +} + +#[test] +fn test_minor() { + let m = Mat2::from(( + 2, 3, + 1, 4 + )); + + assert_eq!(m.minor(0, 0), 4); + assert_eq!(m.minor(0, 1), 1); + assert_eq!(m.minor(1, 0), 3); + assert_eq!(m.minor(1, 1), 2); + + let m = Mat3::from(( + 1, -1, 2, + 2, 3, 5, + 1, 0, 3 + )); + + assert_eq!(m.minor(0, 0), Mat2::new(3, 5,0, 3).determinant()); + assert_eq!(m.minor(0, 1), Mat2::new(2, 5,1, 3).determinant()); + assert_eq!(m.minor(0, 2), Mat2::new(2, 3,1, 0).determinant()); + assert_eq!(m.minor(1, 0), Mat2::new(-1, 2,0, 3).determinant()); + assert_eq!(m.minor(1, 1), Mat2::new(1, 2,1, 3).determinant()); + assert_eq!(m.minor(1, 2), Mat2::new(1, -1,1, 0).determinant()); + assert_eq!(m.minor(2, 0), Mat2::new(-1, 2,3, 5).determinant()); + assert_eq!(m.minor(2, 1), Mat2::new(1, 2,2, 5).determinant()); + assert_eq!(m.minor(2, 2), Mat2::new(1, -1,2, 3).determinant()); + + let m = Mat4::from(( + 1, 3, -1, 2, + 5, -6, 7, 0, + 1, 0, 2, 9, + 10, -3, -2, 1 + )); + + assert_eq!(m.minor(0, 0), Mat3::new(-6, 7, 0, 0, 2, 9, -3, -2, 1).determinant()); + assert_eq!(m.minor(0, 1), Mat3::new(5, 7, 0, 1, 2, 9, 10, -2, 1).determinant()); + assert_eq!(m.minor(0, 2), Mat3::new(5, -6, 0, 1, 0, 9, 10, -3, 1).determinant()); + assert_eq!(m.minor(0, 3), Mat3::new(5, -6, 7, 1, 0, 2, 10, -3, -2).determinant()); + assert_eq!(m.minor(1, 0), Mat3::new(3, -1, 2, 0, 2, 9, -3, -2, 1).determinant()); + assert_eq!(m.minor(1, 1), Mat3::new(1, -1, 2, 1, 2, 9, 10, -2, 1).determinant()); + assert_eq!(m.minor(1, 2), Mat3::new(1, 3, 2, 1, 0, 9, 10, -3, 1).determinant()); + assert_eq!(m.minor(1, 3), Mat3::new(1, 3, -1, 1, 0, 2, 10, -3, -2).determinant()); + assert_eq!(m.minor(2, 0), Mat3::new(3, -1, 2, -6, 7, 0, -3, -2, 1).determinant()); + assert_eq!(m.minor(2, 1), Mat3::new(1, -1, 2, 5, 7, 0, 10, -2, 1).determinant()); + assert_eq!(m.minor(2, 2), Mat3::new(1, 3, 2, 5, -6, 0, 10, -3, 1).determinant()); + assert_eq!(m.minor(2, 3), Mat3::new(1, 3, -1, 5, -6, 7, 10, -3, -2).determinant()); + assert_eq!(m.minor(3, 0), Mat3::new(3, -1, 2, -6, 7, 0, 0, 2, 9).determinant()); + assert_eq!(m.minor(3, 1), Mat3::new(1, -1, 2, 5, 7, 0, 1, 2, 9).determinant()); + assert_eq!(m.minor(3, 2), Mat3::new(1, 3, 2, 5, -6, 0, 1, 0, 9).determinant()); + assert_eq!(m.minor(3, 3), Mat3::new(1, 3, -1, 5, -6, 7, 1, 0, 2).determinant()); +} + +#[test] +fn test_cofactor() { + let m = Mat4::from(( + 1, 3, -1, 2, + 5, -6, 7, 0, + 1, 0, 2, 9, + 10, -3, -2, 1 + )); + + assert_eq!(m.cofactor(0, 0), Mat3::new(-6, 7, 0, 0, 2, 9, -3, -2, 1).determinant()); + assert_eq!(m.cofactor(0, 1), -Mat3::new(5, 7, 0, 1, 2, 9, 10, -2, 1).determinant()); + assert_eq!(m.cofactor(0, 2), Mat3::new(5, -6, 0, 1, 0, 9, 10, -3, 1).determinant()); + assert_eq!(m.cofactor(0, 3), -Mat3::new(5, -6, 7, 1, 0, 2, 10, -3, -2).determinant()); + assert_eq!(m.cofactor(1, 0), -Mat3::new(3, -1, 2, 0, 2, 9, -3, -2, 1).determinant()); + assert_eq!(m.cofactor(1, 1), Mat3::new(1, -1, 2, 1, 2, 9, 10, -2, 1).determinant()); + assert_eq!(m.cofactor(1, 2), -Mat3::new(1, 3, 2, 1, 0, 9, 10, -3, 1).determinant()); + assert_eq!(m.cofactor(1, 3), Mat3::new(1, 3, -1, 1, 0, 2, 10, -3, -2).determinant()); + assert_eq!(m.cofactor(2, 0), Mat3::new(3, -1, 2, -6, 7, 0, -3, -2, 1).determinant()); + assert_eq!(m.cofactor(2, 1), -Mat3::new(1, -1, 2, 5, 7, 0, 10, -2, 1).determinant()); + assert_eq!(m.cofactor(2, 2), Mat3::new(1, 3, 2, 5, -6, 0, 10, -3, 1).determinant()); + assert_eq!(m.cofactor(2, 3), -Mat3::new(1, 3, -1, 5, -6, 7, 10, -3, -2).determinant()); + assert_eq!(m.cofactor(3, 0), -Mat3::new(3, -1, 2, -6, 7, 0, 0, 2, 9).determinant()); + assert_eq!(m.cofactor(3, 1), Mat3::new(1, -1, 2, 5, 7, 0, 1, 2, 9).determinant()); + assert_eq!(m.cofactor(3, 2), -Mat3::new(1, 3, 2, 5, -6, 0, 1, 0, 9).determinant()); + assert_eq!(m.cofactor(3, 3), Mat3::new(1, 3, -1, 5, -6, 7, 1, 0, 2).determinant()); } \ No newline at end of file