From 2b8e36d4a3b76f7a5a7436614345bced000f37f4 Mon Sep 17 00:00:00 2001
From: Brendan Zabarauskas <bjzaba@yahoo.com.au>
Date: Tue, 3 Sep 2013 23:36:03 +1000
Subject: [PATCH] Impl ApproxEq for vectors, matrices and points

---
 src/cgmath/array.rs  | 41 +++++++++----------
 src/cgmath/matrix.rs | 96 +++++++++++++++++++++++++++++++++++++++++---
 src/cgmath/point.rs  |  3 ++
 src/cgmath/vector.rs |  5 +++
 4 files changed, 118 insertions(+), 27 deletions(-)

diff --git a/src/cgmath/array.rs b/src/cgmath/array.rs
index af73176..04ff408 100644
--- a/src/cgmath/array.rs
+++ b/src/cgmath/array.rs
@@ -103,27 +103,26 @@ macro_rules! array(
     )
 )
 
-macro_rules! array_op(
-    (impl<$S:ident> ($Op:ident, $op:ident) for ($Self:ty, $Other:ty) -> $Result:ty) => (
-        impl<$S: Field> $Op<$Other, $Result> for $Self {
-            #[inline(always)]
-            fn $op(&self, other: &$Other) -> $Result {
-                self.bimap(other, |a, b| a.$op(b))
+macro_rules! approx_eq(
+    (impl<$S:ident> $Self:ty) => (
+        impl<$S: Clone + ApproxEq<$S>> ApproxEq<$S> for $Self {
+            #[inline]
+            fn approx_epsilon() -> $S {
+                // TODO: fix this after static methods are fixed in rustc
+                fail!(~"Doesn't work!");
+            }
+
+            #[inline]
+            fn approx_eq(&self, other: &$Self) -> bool {
+                self.iter().zip(other.iter())
+                           .all(|(a, b)| a.approx_eq(b))
+            }
+
+            #[inline]
+            fn approx_eq_eps(&self, other: &$Self, approx_epsilon: &$S) -> bool {
+                self.iter().zip(other.iter())
+                           .all(|(a, b)| a.approx_eq_eps(b, approx_epsilon))
             }
         }
-    );
-    (impl<$S:ident> ($Op:ident, $op:ident) for $Self:ty -> $Result:ty) => (
-        impl<$S: Field> $Op<$Result> for $Self {
-            #[inline(always)]
-            fn $op(&self) -> $Result {
-                self.map(|a| a.$op())
-            }
-        }
-    );
-    (impl<$S:ident> -$Self:ty -> $Result:ty) => (array_op!(impl<$S> (Neg, neg) for $Self -> $Result));
-    (impl<$S:ident> $Self:ty + $Other:ty -> $Result:ty) => (array_op!(impl<$S> (Add, add) for ($Self, $Other) -> $Result));
-    (impl<$S:ident> $Self:ty - $Other:ty -> $Result:ty) => (array_op!(impl<$S> (Sub, sub) for ($Self, $Other) -> $Result));
-    (impl<$S:ident> $Self:ty * $Other:ty -> $Result:ty) => (array_op!(impl<$S> (Mul, mul) for ($Self, $Other) -> $Result));
-    (impl<$S:ident> $Self:ty / $Other:ty -> $Result:ty) => (array_op!(impl<$S> (Div, div) for ($Self, $Other) -> $Result));
-    (impl<$S:ident> $Self:ty % $Other:ty -> $Result:ty) => (array_op!(impl<$S> (Rem, rem) for ($Self, $Other) -> $Result));
+    )
 )
diff --git a/src/cgmath/matrix.rs b/src/cgmath/matrix.rs
index 2e70715..74622de 100644
--- a/src/cgmath/matrix.rs
+++ b/src/cgmath/matrix.rs
@@ -34,6 +34,10 @@ pub struct Mat3<S> { x: Vec3<S>, y: Vec3<S>, z: Vec3<S> }
 #[deriving(Clone, Eq, Zero)]
 pub struct Mat4<S> { x: Vec4<S>, y: Vec4<S>, z: Vec4<S>, w: Vec4<S> }
 
+approx_eq!(impl<S> Mat2<S>)
+approx_eq!(impl<S> Mat3<S>)
+approx_eq!(impl<S> Mat4<S>)
+
 // Conversion traits
 pub trait ToMat2<S: Clone + Num> { fn to_mat2(&self) -> Mat2<S>; }
 pub trait ToMat3<S: Clone + Num> { fn to_mat3(&self) -> Mat3<S>; }
@@ -175,6 +179,7 @@ pub trait Matrix
 :   Array<V, Slice>
 +   Neg<Self>
 +   Zero + One
++   ApproxEq<S>
 {
     #[inline]
     fn c<'a>(&'a self, c: uint) -> &'a V { self.i(c) }
@@ -237,13 +242,21 @@ pub trait Matrix
 
     #[inline]
     fn is_invertible(&self) -> bool {
-        !self.determinant().approx_eq(&zero::<S>())
+        !self.determinant().approx_eq(&zero())
     }
 
-    // pub fn is_identity(&self) -> bool;
-    // pub fn is_diagonal(&self) -> bool;
-    // pub fn is_rotated(&self) -> bool;
-    // pub fn is_symmetric(&self) -> bool;
+    #[inline]
+    fn is_identity(&self) -> bool {
+        self.approx_eq(&one())
+    }
+
+    #[inline]
+    fn is_rotated(&self) -> bool {
+        !self.approx_eq(&one())
+    }
+
+    fn is_diagonal(&self) -> bool;
+    fn is_symmetric(&self) -> bool;
 }
 
 impl<S: Clone + Float> Neg<Mat2<S>> for Mat2<S> { #[inline] fn neg(&self) -> Mat2<S> { self.map(|c| c.neg()) } }
@@ -280,6 +293,7 @@ for Mat2<S>
         *self.cr(0, 0) * *self.cr(1, 1) - *self.cr(1, 0) * *self.cr(0, 1)
     }
 
+    #[inline]
     fn invert(&self) -> Option<Mat2<S>> {
         let det = self.determinant();
         if det.approx_eq(&zero()) {
@@ -289,6 +303,19 @@ for Mat2<S>
                            -self.cr(1, 0) / det,  self.cr(0, 0) / det))
         }
     }
+
+    #[inline]
+    fn is_diagonal(&self) -> bool {
+        self.cr(0, 1).approx_eq(&zero()) &&
+        self.cr(1, 0).approx_eq(&zero())
+    }
+
+
+    #[inline]
+    fn is_symmetric(&self) -> bool {
+        self.cr(0, 1).approx_eq(self.cr(1, 0)) &&
+        self.cr(1, 0).approx_eq(self.cr(0, 1))
+    }
 }
 
 impl<S: Clone + Float>
@@ -336,6 +363,28 @@ for Mat3<S>
                                  self.c(0).cross(self.c(1)).div_s(det.clone())).transpose())
         }
     }
+
+    fn is_diagonal(&self) -> bool {
+        self.cr(0, 1).approx_eq(&zero()) &&
+        self.cr(0, 2).approx_eq(&zero()) &&
+
+        self.cr(1, 0).approx_eq(&zero()) &&
+        self.cr(1, 2).approx_eq(&zero()) &&
+
+        self.cr(2, 0).approx_eq(&zero()) &&
+        self.cr(2, 1).approx_eq(&zero())
+    }
+
+    fn is_symmetric(&self) -> bool {
+        self.cr(0, 1).approx_eq(self.cr(1, 0)) &&
+        self.cr(0, 2).approx_eq(self.cr(2, 0)) &&
+
+        self.cr(1, 0).approx_eq(self.cr(0, 1)) &&
+        self.cr(1, 2).approx_eq(self.cr(2, 1)) &&
+
+        self.cr(2, 0).approx_eq(self.cr(0, 2)) &&
+        self.cr(2, 1).approx_eq(self.cr(1, 2))
+    }
 }
 
 impl<S: Clone + Float>
@@ -357,7 +406,6 @@ for Mat4<S>
                   self.cr(0, 3).clone(), self.cr(1, 3).clone(), self.cr(2, 3).clone(), self.cr(3, 3).clone())
     }
 
-    #[inline]
     fn transpose_self(&mut self) {
         self.swap_cr((0, 1), (1, 0));
         self.swap_cr((0, 2), (2, 0));
@@ -435,6 +483,42 @@ for Mat4<S>
             None
         }
     }
+
+    fn is_diagonal(&self) -> bool {
+        self.cr(0, 1).approx_eq(&zero()) &&
+        self.cr(0, 2).approx_eq(&zero()) &&
+        self.cr(0, 3).approx_eq(&zero()) &&
+
+        self.cr(1, 0).approx_eq(&zero()) &&
+        self.cr(1, 2).approx_eq(&zero()) &&
+        self.cr(1, 3).approx_eq(&zero()) &&
+
+        self.cr(2, 0).approx_eq(&zero()) &&
+        self.cr(2, 1).approx_eq(&zero()) &&
+        self.cr(2, 3).approx_eq(&zero()) &&
+
+        self.cr(3, 0).approx_eq(&zero()) &&
+        self.cr(3, 1).approx_eq(&zero()) &&
+        self.cr(3, 2).approx_eq(&zero())
+    }
+
+    fn is_symmetric(&self) -> bool {
+        self.cr(0, 1).approx_eq(self.cr(1, 0)) &&
+        self.cr(0, 2).approx_eq(self.cr(2, 0)) &&
+        self.cr(0, 3).approx_eq(self.cr(3, 0)) &&
+
+        self.cr(1, 0).approx_eq(self.cr(0, 1)) &&
+        self.cr(1, 2).approx_eq(self.cr(2, 1)) &&
+        self.cr(1, 3).approx_eq(self.cr(3, 1)) &&
+
+        self.cr(2, 0).approx_eq(self.cr(0, 2)) &&
+        self.cr(2, 1).approx_eq(self.cr(1, 2)) &&
+        self.cr(2, 3).approx_eq(self.cr(3, 2)) &&
+
+        self.cr(3, 0).approx_eq(self.cr(0, 3)) &&
+        self.cr(3, 1).approx_eq(self.cr(1, 3)) &&
+        self.cr(3, 2).approx_eq(self.cr(2, 3))
+    }
 }
 
 impl<S:Clone + Float> ToQuat<S> for Mat3<S> {
diff --git a/src/cgmath/point.rs b/src/cgmath/point.rs
index 3994a43..4cb3612 100644
--- a/src/cgmath/point.rs
+++ b/src/cgmath/point.rs
@@ -30,6 +30,9 @@ struct Point2<S> { x: S, y: S }
 #[deriving(Eq, Zero, Clone)]
 struct Point3<S> { x: S, y: S, z: S }
 
+approx_eq!(impl<S> Point2<S>)
+approx_eq!(impl<S> Point3<S>)
+
 impl<S: Num> Point2<S> {
     #[inline]
     pub fn new(x: S, y: S) -> Point2<S> {
diff --git a/src/cgmath/vector.rs b/src/cgmath/vector.rs
index 9fb0c24..0d4e13f 100644
--- a/src/cgmath/vector.rs
+++ b/src/cgmath/vector.rs
@@ -35,6 +35,10 @@ pub trait ToVec2<S: Clone + Num> { fn to_vec2(&self) -> Vec2<S>; }
 pub trait ToVec3<S: Clone + Num> { fn to_vec3(&self) -> Vec3<S>; }
 pub trait ToVec4<S: Clone + Num> { fn to_vec4(&self) -> Vec4<S>; }
 
+approx_eq!(impl<S> Vec2<S>)
+approx_eq!(impl<S> Vec3<S>)
+approx_eq!(impl<S> Vec4<S>)
+
 // Utility macro for generating associated functions for the vectors
 macro_rules! vec(
     (impl $Self:ident <$S:ident> { $($field:ident),+ }) => (
@@ -166,6 +170,7 @@ pub trait EuclideanVector
     Slice
 >
 :   Vector<S, Slice>
++   ApproxEq<S>
 {
     /// Returns `true` if the vector is perpendicular (at right angles to)
     /// the other vector.