From 4f13809420704e8bc0ee087b140a216fb8cc8d9d Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 16 Aug 2016 11:05:01 -0400
Subject: [PATCH] Added axis angle accessors, removed == operator.

---
 matrix/AxisAngle.hpp      |  9 +++++++
 matrix/Matrix.hpp         | 54 +++++++++++++++++++++++++--------------
 test/attitude.cpp         |  3 ++-
 test/matrixAssignment.cpp |  2 +-
 test/vector.cpp           |  4 +--
 test/vector3.cpp          |  8 +++---
 6 files changed, 53 insertions(+), 27 deletions(-)

diff --git a/matrix/AxisAngle.hpp b/matrix/AxisAngle.hpp
index d21e893e6d..0c73b4ccd5 100644
--- a/matrix/AxisAngle.hpp
+++ b/matrix/AxisAngle.hpp
@@ -153,6 +153,15 @@ public:
         v(1) = a(1)*angle;
         v(2) = a(2)*angle;
     }
+
+
+    Vector<Type, 3> axis() {
+        return Vector<Type, 3>::unit();
+    }
+
+    Type angle() {
+        return Vector<Type, 3>::norm();
+    }
 };
 
 typedef AxisAngle<float> AxisAnglef;
diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index 3188c553d1..c4e9ec9573 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -134,22 +134,6 @@ public:
         return res;
     }
 
-    bool operator==(const Matrix<Type, M, N> &other) const
-    {
-        const Matrix<Type, M, N> &self = *this;
-        static const Type eps = Type(1e-4);
-
-        for (size_t i = 0; i < M; i++) {
-            for (size_t j = 0; j < N; j++) {
-                if (fabs(self(i , j) - other(i, j)) > eps) {
-                    return false;
-                }
-            }
-        }
-
-        return true;
-    }
-
     Matrix<Type, M, N> operator-(const Matrix<Type, M, N> &other) const
     {
         Matrix<Type, M, N> res;
@@ -469,15 +453,47 @@ Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
 
 template<typename Type, size_t  M, size_t N>
 bool isEqual(const Matrix<Type, M, N> &x,
-             const Matrix<Type, M, N> & y) {
-    if (!(x == y)) {
+             const Matrix<Type, M, N> &y, const Type eps=1e-4f) {
+
+    bool equal = true;
+
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            if (fabs(x(i , j) - y(i, j)) > eps) {
+                equal = false;
+                break;
+            }
+        }
+        if (equal == false) break;
+    }
+
+
+    if (!equal) {
         char buf_x[100];
         char buf_y[100];
         x.write_string(buf_x, 100);
         y.write_string(buf_y, 100);
         printf("not equal\nx:\n%s\ny:\n%s\n", buf_x, buf_y);
     }
-    return x == y;
+    return equal;
+}
+
+bool isEqual(float x,
+             float y, float eps);
+
+bool isEqual(float x,
+             float y, float eps=1e-4f) {
+
+    bool equal = true;
+
+    if (fabs(x - y) > eps) {
+        equal = false;
+    }
+
+    if (!equal) {
+        printf("not equal\nx:\n%g\ny:\n%g\n", x, y);
+    }
+    return equal;
 }
 
 #if defined(SUPPORT_STDIOSTREAM)
diff --git a/test/attitude.cpp b/test/attitude.cpp
index b13bf73c2c..fefd6fe0e1 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -276,7 +276,8 @@ int main()
 
     AxisAnglef aa_axis_angle_init(Vector3f(1.0f, 2.0f, 3.0f), 3.0f);
     TEST(isEqual(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f)));
-
+    TEST(isEqual(aa_axis_angle_init.axis(), Vector3f(0.26726124f,  0.53452248f,  0.80178373f)));
+    TEST(isEqual(aa_axis_angle_init.angle(), 3.0f));
     TEST(isEqual(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))),
                  Quatf(1.0f, 0.0f, 0.0f, 0.0f)));
 };
diff --git a/test/matrixAssignment.cpp b/test/matrixAssignment.cpp
index d6097526f1..8dbbf03bf4 100644
--- a/test/matrixAssignment.cpp
+++ b/test/matrixAssignment.cpp
@@ -30,7 +30,7 @@ int main()
     Matrix3f m3(data_times_2);
 
     TEST(isEqual(m, m2));
-    TEST(!(m == m3));
+    TEST(!(isEqual(m, m3)));
 
     m2 *= 2;
     TEST(isEqual(m2, m3));
diff --git a/test/vector.cpp b/test/vector.cpp
index 38824937de..215985164a 100644
--- a/test/vector.cpp
+++ b/test/vector.cpp
@@ -17,10 +17,10 @@ int main()
     TEST(fabs(v1.dot(v2) - 130.0f) < 1e-5);
     v2.normalize();
     Vector<float, 5> v3(v2);
-    TEST(v2 == v3);
+    TEST(isEqual(v2, v3));
     float data1_sq[] = {1,4,9,16,25};
     Vector<float, 5> v4(data1_sq);
-    TEST(v1 == v4.pow(0.5));
+    TEST(isEqual(v1, v4.pow(0.5)));
     return 0;
 }
 
diff --git a/test/vector3.cpp b/test/vector3.cpp
index a8ccc7325b..8e456fe156 100644
--- a/test/vector3.cpp
+++ b/test/vector3.cpp
@@ -12,15 +12,15 @@ int main()
     Vector3f a(1, 0, 0);
     Vector3f b(0, 1, 0);
     Vector3f c = a.cross(b);
-    TEST (c == Vector3f(0,0,1));
+    TEST(isEqual(c, Vector3f(0,0,1)));
     c = a % b;
-    TEST (c == Vector3f(0,0,1));
+    TEST (isEqual(c, Vector3f(0,0,1)));
     Matrix<float, 3, 1> d(c);
     Vector3f e(d);
-    TEST (e == d);
+    TEST (isEqual(e, d));
     float data[] = {4, 5, 6};
     Vector3f f(data);
-    TEST (f == Vector3f(4, 5, 6));
+    TEST(isEqual(f, Vector3f(4, 5, 6)));
     return 0;
 }