Added determinants and inversion.
This commit is contained in:
Luqman Aden
parent
34add64ffd
commit
f7fb7f7100
2 changed files with 190 additions and 18 deletions
161
src/matrix.rs
161
src/matrix.rs
|
|
@ -49,6 +49,8 @@ pub trait Matrix<T, ColVec, RowVec> {
|
|||
|
||||
pure fn col(i: uint) -> ColVec;
|
||||
pure fn row(i: uint) -> RowVec;
|
||||
|
||||
pure fn det() -> T;
|
||||
}
|
||||
|
||||
pub trait NumericMatrix<T, ColVec> {
|
||||
|
|
@ -61,13 +63,14 @@ pub trait SquareMatrix<T> {
|
|||
pure fn sub_m(other: &self) -> self;
|
||||
pure fn mul_m(other: &self) -> self;
|
||||
|
||||
// pure fn invert(other: &self) -> self;
|
||||
pure fn invert() -> Option<self>;
|
||||
pure fn transpose() -> self;
|
||||
|
||||
pure fn is_identity() -> bool;
|
||||
pure fn is_symmetric() -> bool;
|
||||
pure fn is_diagonal() -> bool;
|
||||
pure fn is_rotated() -> bool;
|
||||
pure fn is_invertible() -> bool;
|
||||
}
|
||||
|
||||
pub trait Matrix2<T> {
|
||||
|
|
@ -131,7 +134,7 @@ pub mod Mat2 {
|
|||
}
|
||||
}
|
||||
|
||||
pub impl<T:Copy> Mat2<T>: Matrix<T, Vec2<T>, Vec2<T>> {
|
||||
pub impl<T:Copy Num NumCast> Mat2<T>: Matrix<T, Vec2<T>, Vec2<T>> {
|
||||
#[inline(always)]
|
||||
pure fn rows() -> uint { 2 }
|
||||
|
||||
|
|
@ -152,9 +155,13 @@ pub impl<T:Copy> Mat2<T>: Matrix<T, Vec2<T>, Vec2<T>> {
|
|||
Vec2::new(self[0][i],
|
||||
self[1][i])
|
||||
}
|
||||
|
||||
pure fn det() -> T {
|
||||
self[0][0] * self[1][1]
|
||||
}
|
||||
}
|
||||
|
||||
pub impl<T:Copy Num> Mat2<T>: NumericMatrix<T, Vec2<T>> {
|
||||
pub impl<T:Copy Num NumCast> Mat2<T>: NumericMatrix<T, Vec2<T>> {
|
||||
#[inline(always)]
|
||||
pure fn mul_t(value: T) -> Mat2<T> {
|
||||
Mat2::from_cols(self[0].mul_t(value),
|
||||
|
|
@ -187,9 +194,17 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat2<T>: SquareMatrix<T> {
|
|||
self.row(0).dot(&other.col(1)), self.row(1).dot(&other.col(1)))
|
||||
}
|
||||
|
||||
// TODO - inversion is harrrd D:
|
||||
// #[inline(always)]
|
||||
// pure fn invert(other: &Mat2<T>) -> Mat2<T> {}
|
||||
#[inline(always)]
|
||||
pure fn invert() -> Option<Mat2<T>> {
|
||||
let _0 = cast(0);
|
||||
let d = self.det();
|
||||
if d.fuzzy_eq(&_0) {
|
||||
None
|
||||
} else {
|
||||
Some(Mat2::new(self[1][1]/d, -self[1][0]/d,
|
||||
-self[0][1]/d, self[0][0]/d))
|
||||
}
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
pure fn transpose() -> Mat2<T> {
|
||||
|
|
@ -219,6 +234,12 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat2<T>: SquareMatrix<T> {
|
|||
pure fn is_rotated() -> bool {
|
||||
!self.fuzzy_eq(&Mat2::identity())
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
pure fn is_invertible() -> bool {
|
||||
let _0 = cast(0);
|
||||
!self.det().fuzzy_eq(&_0)
|
||||
}
|
||||
}
|
||||
|
||||
pub impl<T:Copy Num NumCast FuzzyEq> Mat2<T>: Matrix2<T> {
|
||||
|
|
@ -342,7 +363,7 @@ pub mod Mat3 {
|
|||
}
|
||||
}
|
||||
|
||||
pub impl<T:Copy> Mat3<T>: Matrix<T, Vec3<T>, Vec3<T>> {
|
||||
pub impl<T:Copy Num NumCast> Mat3<T>: Matrix<T, Vec3<T>, Vec3<T>> {
|
||||
#[inline(always)]
|
||||
pure fn rows() -> uint { 3 }
|
||||
|
||||
|
|
@ -364,9 +385,13 @@ pub impl<T:Copy> Mat3<T>: Matrix<T, Vec3<T>, Vec3<T>> {
|
|||
self[1][i],
|
||||
self[2][i])
|
||||
}
|
||||
|
||||
pure fn det() -> T {
|
||||
self.col(0).dot(&self.col(1).cross(&self.col(2)))
|
||||
}
|
||||
}
|
||||
|
||||
pub impl<T:Copy Num> Mat3<T>: NumericMatrix<T, Vec3<T>> {
|
||||
pub impl<T:Copy Num NumCast> Mat3<T>: NumericMatrix<T, Vec3<T>> {
|
||||
#[inline(always)]
|
||||
pure fn mul_t(value: T) -> Mat3<T> {
|
||||
Mat3::from_cols(self[0].mul_t(value),
|
||||
|
|
@ -404,9 +429,19 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat3<T>: SquareMatrix<T> {
|
|||
self.row(0).dot(&other.col(2)), self.row(1).dot(&other.col(2)), self.row(2).dot(&other.col(2)))
|
||||
}
|
||||
|
||||
// TODO - inversion is harrrd D:
|
||||
// #[inline(always)]
|
||||
// pure fn invert(other: &Mat3) -> Mat3 {}
|
||||
pure fn invert() -> Option<Mat3<T>> {
|
||||
let d = self.det();
|
||||
let _0 = cast(0);
|
||||
if d.fuzzy_eq(&_0) {
|
||||
None
|
||||
} else {
|
||||
Some(Mat3::from_cols(self[1].cross(&self[2]).div_t(d),
|
||||
self[2].cross(&self[0]).div_t(d),
|
||||
self[0].cross(&self[1]).div_t(d))
|
||||
.transpose())
|
||||
}
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
pure fn transpose() -> Mat3<T> {
|
||||
|
|
@ -449,6 +484,12 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat3<T>: SquareMatrix<T> {
|
|||
pure fn is_rotated() -> bool {
|
||||
!self.fuzzy_eq(&Mat3::identity())
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
pure fn is_invertible() -> bool {
|
||||
let _0 = cast(0);
|
||||
!self.det().fuzzy_eq(&_0)
|
||||
}
|
||||
}
|
||||
|
||||
pub impl<T:Copy Num NumCast FuzzyEq> Mat3<T>: Matrix3<T> {
|
||||
|
|
@ -636,7 +677,7 @@ pub mod Mat4 {
|
|||
}
|
||||
}
|
||||
|
||||
pub impl<T:Copy> Mat4<T>: Matrix<T, Vec4<T>, Vec4<T>> {
|
||||
pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: Matrix<T, Vec4<T>, Vec4<T>> {
|
||||
#[inline(always)]
|
||||
pure fn rows() -> uint { 4 }
|
||||
|
||||
|
|
@ -659,9 +700,24 @@ pub impl<T:Copy> Mat4<T>: Matrix<T, Vec4<T>, Vec4<T>> {
|
|||
self[2][i],
|
||||
self[3][i])
|
||||
}
|
||||
|
||||
pure fn det() -> T {
|
||||
self[0][0]*Mat3::new(self[1][1], self[2][1], self[3][1],
|
||||
self[1][2], self[2][2], self[3][2],
|
||||
self[1][3], self[2][3], self[3][3]).det() -
|
||||
self[1][0]*Mat3::new(self[0][1], self[2][1], self[3][1],
|
||||
self[0][2], self[2][2], self[3][2],
|
||||
self[0][3], self[2][3], self[3][3]).det() +
|
||||
self[2][0]*Mat3::new(self[0][1], self[1][1], self[3][1],
|
||||
self[0][2], self[1][2], self[3][2],
|
||||
self[0][3], self[1][3], self[3][3]).det() -
|
||||
self[3][0]*Mat3::new(self[0][1], self[1][1], self[2][1],
|
||||
self[0][2], self[1][2], self[2][2],
|
||||
self[0][3], self[1][3], self[2][3]).det()
|
||||
}
|
||||
}
|
||||
|
||||
pub impl<T:Copy Num> Mat4<T>: NumericMatrix<T, Vec4<T>> {
|
||||
pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: NumericMatrix<T, Vec4<T>> {
|
||||
#[inline(always)]
|
||||
pure fn mul_t(value: T) -> Mat4<T> {
|
||||
Mat4::from_cols(self[0].mul_t(value),
|
||||
|
|
@ -679,7 +735,7 @@ pub impl<T:Copy Num> Mat4<T>: NumericMatrix<T, Vec4<T>> {
|
|||
}
|
||||
}
|
||||
|
||||
pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: SquareMatrix<T> {
|
||||
pub impl<T:Copy Num NumCast FuzzyEq Ord> Mat4<T>: SquareMatrix<T> {
|
||||
#[inline(always)]
|
||||
pure fn add_m(other: &Mat4<T>) -> Mat4<T> {
|
||||
Mat4::from_cols(self[0].add_v(&other[0]),
|
||||
|
|
@ -707,9 +763,74 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: SquareMatrix<T> {
|
|||
self.row(0).dot(&other.col(3)), self.row(1).dot(&other.col(3)), self.row(2).dot(&other.col(3)), self.row(3).dot(&other.col(3)))
|
||||
}
|
||||
|
||||
// TODO - inversion is harrrd D:
|
||||
// #[inline(always)]
|
||||
// pure fn invert(other: &Mat4<T>) -> Mat4<T> {}
|
||||
pure fn invert() -> Option<Mat4<T>> {
|
||||
let d = self.det();
|
||||
let _0 = cast(0);
|
||||
if d.fuzzy_eq(&_0) {
|
||||
None
|
||||
} else {
|
||||
|
||||
// Gauss Jordan Elimination with partial pivoting
|
||||
|
||||
let mut a = self.transpose();
|
||||
let mut inv = Mat4::identity::<T>();
|
||||
|
||||
// Find largest pivot column j among rows j..3
|
||||
uint::range(0, 4, |j| {
|
||||
let mut i1 = j;
|
||||
uint::range(j + 1, 4, |i| {
|
||||
// There should really be a generic abs function
|
||||
let one = a[i][j];
|
||||
let two = a[i1][j];
|
||||
if one < _0 && two < _0 && -one > -two {
|
||||
i1 = i;
|
||||
} else if one > _0 && two > _0 && one > two {
|
||||
i1 = i;
|
||||
} else if one < _0 && two > _0 && -one > two {
|
||||
i1 = i;
|
||||
} else if one > _0 && two < _0 && one > -two {
|
||||
i1 = i;
|
||||
}
|
||||
true
|
||||
});
|
||||
|
||||
// Swap rows i1 and j in a and inv to
|
||||
// put pivot on diagonal
|
||||
let c = [mut a.x, a.y, a.z, a.w];
|
||||
c[i1] <-> c[j];
|
||||
a = Mat4::from_cols(c[0], c[1], c[2], c[3]);
|
||||
let c = [mut inv.x, inv.y, inv.z, inv.w];
|
||||
c[i1] <-> c[j];
|
||||
inv = Mat4::from_cols(c[0], c[1], c[2], c[3]);
|
||||
|
||||
// Scale row j to have a unit diagonal
|
||||
let c = [mut inv.x, inv.y, inv.z, inv.w];
|
||||
c[j] = c[j].div_t(a[j][j]);
|
||||
inv = Mat4::from_cols(c[0], c[1], c[2], c[3]);
|
||||
let c = [mut a.x, a.y, a.z, a.w];
|
||||
c[j] = c[j].div_t(a[j][j]);
|
||||
a = Mat4::from_cols(c[0], c[1], c[2], c[3]);
|
||||
|
||||
// Eliminate off-diagonal elems in col j of a,
|
||||
// doing identical ops to inv
|
||||
uint::range(0, 4, |i| {
|
||||
if i != j {
|
||||
let c = [mut inv.x, inv.y, inv.z, inv.w];
|
||||
c[i] = c[i].sub_v(&c[j].mul_t(a[i][j]));
|
||||
inv = Mat4::from_cols(c[0], c[1], c[2], c[3]);
|
||||
|
||||
let c = [mut a.x, a.y, a.z, a.w];
|
||||
c[i] = c[i].sub_v(&c[j].mul_t(a[i][j]));
|
||||
a = Mat4::from_cols(c[0], c[1], c[2], c[3]);
|
||||
}
|
||||
true
|
||||
});
|
||||
|
||||
true
|
||||
});
|
||||
Some(inv.transpose())
|
||||
}
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
pure fn transpose() -> Mat4<T> {
|
||||
|
|
@ -767,6 +888,12 @@ pub impl<T:Copy Num NumCast FuzzyEq> Mat4<T>: SquareMatrix<T> {
|
|||
pure fn is_rotated() -> bool {
|
||||
!self.fuzzy_eq(&Mat4::identity())
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
pure fn is_invertible() -> bool {
|
||||
let _0 = cast(0);
|
||||
!self.det().fuzzy_eq(&_0)
|
||||
}
|
||||
}
|
||||
|
||||
pub impl<T> Mat4<T>: Matrix4<T> {
|
||||
|
|
@ -828,4 +955,4 @@ pub impl<T:Copy> Mat4<T>: ToPtr<T> {
|
|||
pure fn to_ptr() -> *T {
|
||||
self[0].to_ptr()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,6 +29,8 @@ fn test_Mat2() {
|
|||
|
||||
assert a.col(0) == Vec2::new(1f, 3f);
|
||||
assert a.col(1) == Vec2::new(2f, 4f);
|
||||
|
||||
assert a.det() == 4f;
|
||||
|
||||
assert a.neg() == Mat2::new(-1f, -3f,
|
||||
-2f, -4f);
|
||||
|
|
@ -47,6 +49,12 @@ fn test_Mat2() {
|
|||
|
||||
assert a.transpose() == Mat2::new(1f, 2f,
|
||||
3f, 4f);
|
||||
|
||||
assert option::unwrap(a.invert()) == Mat2::new(1f, -0.5f,
|
||||
-0.75f, 0.25f);
|
||||
|
||||
assert Mat2::new(0f, 2f,
|
||||
3f, 5f).invert().is_none();
|
||||
|
||||
// exact_eq
|
||||
// fuzzy_eq
|
||||
|
|
@ -56,11 +64,13 @@ fn test_Mat2() {
|
|||
assert Mat2::identity::<float>().is_symmetric();
|
||||
assert Mat2::identity::<float>().is_diagonal();
|
||||
assert !Mat2::identity::<float>().is_rotated();
|
||||
assert Mat2::identity::<float>().is_invertible();
|
||||
|
||||
assert !a.is_identity();
|
||||
assert !a.is_symmetric();
|
||||
assert !a.is_diagonal();
|
||||
assert a.is_rotated();
|
||||
assert a.is_invertible();
|
||||
|
||||
let c = Mat2::new(2f, 1f,
|
||||
1f, 2f);
|
||||
|
|
@ -68,6 +78,7 @@ fn test_Mat2() {
|
|||
assert c.is_symmetric();
|
||||
assert !c.is_diagonal();
|
||||
assert c.is_rotated();
|
||||
assert c.is_invertible();
|
||||
|
||||
assert Mat2::from_value(6f).is_diagonal();
|
||||
|
||||
|
|
@ -120,6 +131,8 @@ fn test_Mat3() {
|
|||
assert a.col(0) == Vec3::new(1f, 4f, 7f);
|
||||
assert a.col(1) == Vec3::new(2f, 5f, 8f);
|
||||
assert a.col(2) == Vec3::new(3f, 6f, 9f);
|
||||
|
||||
assert a.det() == 0f;
|
||||
|
||||
assert a.neg() == Mat3::new(-1f, -4f, -7f,
|
||||
-2f, -5f, -8f,
|
||||
|
|
@ -144,7 +157,18 @@ fn test_Mat3() {
|
|||
assert a.transpose() == Mat3::new(1f, 2f, 3f,
|
||||
4f, 5f, 6f,
|
||||
7f, 8f, 9f);
|
||||
|
||||
assert a.invert().is_none();
|
||||
|
||||
assert option::unwrap(Mat3::identity::<float>().invert())
|
||||
== Mat3::identity::<float>();
|
||||
|
||||
assert option::unwrap(Mat3::new(2f, 4f, 6f,
|
||||
0f, 2f, 4f,
|
||||
0f, 0f, 1f).invert())
|
||||
== Mat3::new(0.5f, -1f, 1f,
|
||||
0f, 0.5f, -2f,
|
||||
0f, 0f, 1f);
|
||||
// exact_eq
|
||||
// fuzzy_eq
|
||||
// eq
|
||||
|
|
@ -153,11 +177,13 @@ fn test_Mat3() {
|
|||
assert Mat3::identity::<float>().is_symmetric();
|
||||
assert Mat3::identity::<float>().is_diagonal();
|
||||
assert !Mat3::identity::<float>().is_rotated();
|
||||
assert Mat3::identity::<float>().is_invertible();
|
||||
|
||||
assert !a.is_identity();
|
||||
assert !a.is_symmetric();
|
||||
assert !a.is_diagonal();
|
||||
assert a.is_rotated();
|
||||
assert !a.is_invertible();
|
||||
|
||||
let c = Mat3::new(3f, 2f, 1f,
|
||||
2f, 3f, 2f,
|
||||
|
|
@ -166,6 +192,7 @@ fn test_Mat3() {
|
|||
assert c.is_symmetric();
|
||||
assert !c.is_diagonal();
|
||||
assert c.is_rotated();
|
||||
assert c.is_invertible();
|
||||
|
||||
assert Mat3::from_value(6f).is_diagonal();
|
||||
|
||||
|
|
@ -187,6 +214,10 @@ fn test_Mat4() {
|
|||
y: Vec4 { x: 3f, y: 7f, z: 11f, w: 15f },
|
||||
z: Vec4 { x: 4f, y: 8f, z: 12f, w: 16f },
|
||||
w: Vec4 { x: 5f, y: 9f, z: 13f, w: 17f } };
|
||||
let c = Mat4 { x: Vec4 { x: 3f, y: 2f, z: 1f, w: 1f },
|
||||
y: Vec4 { x: 2f, y: 3f, z: 2f, w: 2f },
|
||||
z: Vec4 { x: 1f, y: 2f, z: 3f, w: 3f },
|
||||
w: Vec4 { x: 0f, y: 1f, z: 1f, w: 0f } };
|
||||
let v1 = Vec4::new(1f, 2f, 3f, 4f);
|
||||
let f1 = 0.5f;
|
||||
|
||||
|
|
@ -232,6 +263,8 @@ fn test_Mat4() {
|
|||
assert a.col(1) == Vec4::new(2f, 6f, 10f, 14f);
|
||||
assert a.col(2) == Vec4::new(3f, 7f, 11f, 15f);
|
||||
assert a.col(3) == Vec4::new(4f, 8f, 12f, 16f);
|
||||
|
||||
assert a.det() == 0f;
|
||||
|
||||
assert a.neg() == Mat4::new(-1f, -5f, -9f, -13f,
|
||||
-2f, -6f, -10f, -14f,
|
||||
|
|
@ -262,6 +295,15 @@ fn test_Mat4() {
|
|||
5f, 6f, 7f, 8f,
|
||||
9f, 10f, 11f, 12f,
|
||||
13f, 14f, 15f, 16f);
|
||||
|
||||
assert option::unwrap(c.invert())
|
||||
== Mat4::new( 5f, -4f, 1f, 0f,
|
||||
-4f, 8f, -4f, 0f,
|
||||
4f, -8f, 4f, 8f,
|
||||
-3f, 4f, 1f, -8f).mul_t(0.125f);
|
||||
|
||||
assert option::unwrap(Mat4::identity::<float>().invert())
|
||||
== Mat4::identity::<float>();
|
||||
|
||||
// exact_eq
|
||||
// fuzzy_eq
|
||||
|
|
@ -271,11 +313,13 @@ fn test_Mat4() {
|
|||
assert Mat4::identity::<float>().is_symmetric();
|
||||
assert Mat4::identity::<float>().is_diagonal();
|
||||
assert !Mat4::identity::<float>().is_rotated();
|
||||
assert Mat4::identity::<float>().is_invertible();
|
||||
|
||||
assert !a.is_identity();
|
||||
assert !a.is_symmetric();
|
||||
assert !a.is_diagonal();
|
||||
assert a.is_rotated();
|
||||
assert !a.is_invertible();
|
||||
|
||||
let c = Mat4::new(4f, 3f, 2f, 1f,
|
||||
3f, 4f, 3f, 2f,
|
||||
|
|
@ -285,6 +329,7 @@ fn test_Mat4() {
|
|||
assert c.is_symmetric();
|
||||
assert !c.is_diagonal();
|
||||
assert c.is_rotated();
|
||||
assert c.is_invertible();
|
||||
|
||||
assert Mat4::from_value(6f).is_diagonal();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue