From 744fb78662c61d44a54d033f5c293becd35f66a8 Mon Sep 17 00:00:00 2001
From: derekburch <derekaburch@gmail.com>
Date: Tue, 22 Dec 2015 13:00:20 -0800
Subject: [PATCH] Fix conversion from Matrix3 to Quaternion

---
 src/matrix.rs   | 30 +++++++++++++++---------------
 tests/matrix.rs | 38 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 53 insertions(+), 15 deletions(-)

diff --git a/src/matrix.rs b/src/matrix.rs
index 8970614..a4e0431 100644
--- a/src/matrix.rs
+++ b/src/matrix.rs
@@ -1066,28 +1066,28 @@ impl<S: BaseFloat> From<Matrix3<S>> for Quaternion<S> {
             let z = (mat[0][1] - mat[1][0]) * s;
             Quaternion::new(w, x, y, z)
         } else if (mat[0][0] > mat[1][1]) && (mat[0][0] > mat[2][2]) {
-            let s = (half + (mat[0][0] - mat[1][1] - mat[2][2])).sqrt();
-            let w = half * s;
+            let s = ((mat[0][0] - mat[1][1] - mat[2][2]) + S::one()).sqrt();
+            let x = half * s;
             let s = half / s;
-            let x = (mat[0][1] - mat[1][0]) * s;
-            let y = (mat[2][0] - mat[0][2]) * s;
-            let z = (mat[1][2] - mat[2][1]) * s;
+            let y = (mat[1][0] + mat[0][1]) * s;
+            let z = (mat[0][2] + mat[2][0]) * s;
+            let w = (mat[1][2] - mat[2][1]) * s;
             Quaternion::new(w, x, y, z)
         } else if mat[1][1] > mat[2][2] {
-            let s = (half + (mat[1][1] - mat[0][0] - mat[2][2])).sqrt();
-            let w = half * s;
+            let s = ((mat[1][1] - mat[0][0] - mat[2][2]) + S::one()).sqrt();
+            let y = half * s;
             let s = half / s;
-            let x = (mat[0][1] - mat[1][0]) * s;
-            let y = (mat[1][2] - mat[2][1]) * s;
-            let z = (mat[2][0] - mat[0][2]) * s;
+            let z = (mat[2][1] + mat[1][2]) * s;
+            let x = (mat[1][0] + mat[0][1]) * s;
+            let w = (mat[2][0] - mat[0][2]) * s;
             Quaternion::new(w, x, y, z)
         } else {
-            let s = (half + (mat[2][2] - mat[0][0] - mat[1][1])).sqrt();
-            let w = half * s;
+            let s = ((mat[2][2] - mat[0][0] - mat[1][1]) + S::one()).sqrt();
+            let z = half * s;
             let s = half / s;
-            let x = (mat[2][0] - mat[0][2]) * s;
-            let y = (mat[1][2] - mat[2][1]) * s;
-            let z = (mat[0][1] - mat[1][0]) * s;
+            let x = (mat[0][2] + mat[2][0]) * s;
+            let y = (mat[2][1] + mat[1][2]) * s;
+            let w = (mat[0][1] - mat[1][0]) * s;
             Quaternion::new(w, x, y, z)
         }
     }
diff --git a/tests/matrix.rs b/tests/matrix.rs
index dbc3432..71c4998 100644
--- a/tests/matrix.rs
+++ b/tests/matrix.rs
@@ -13,6 +13,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#[macro_use]
 extern crate cgmath;
 
 use cgmath::*;
@@ -365,3 +366,40 @@ fn test_from_angle() {
     let rot3: Matrix2<f64> = Matrix2::from_angle(rad(f64::consts::PI));
     assert!((rot3 * Vector2::new(1.0, 1.0)).approx_eq(&Vector2::new(-1.0, -1.0)));
 }
+
+fn test_matrix3_into_quaternion_for_angles(x: Rad<f32>, y: Rad<f32>, z: Rad<f32>) {
+    let matrix3: Matrix3<f32> = Matrix3::from_euler(x, y, z);
+    let quaternion: Quaternion<f32> = matrix3.into();
+    let quaternion_matrix3: Matrix3<f32> = quaternion.into();
+    assert_approx_eq!(matrix3, quaternion_matrix3);
+}
+
+// The next 4 tests trigger each of the cases in the impl for
+// conversion from Matrix3 to Quaternion
+#[test]
+fn test_matrix3_into_quaternion_positive_trace()
+{
+    // trigger the case: trace >= S::zero()
+    test_matrix3_into_quaternion_for_angles(rad(0.0f32), rad(0.0), rad(0.0f32));
+}
+
+#[test]
+fn test_matrix3_into_quaternion_xx_maximum()
+{
+    // trigger the case: (mat[0][0] > mat[1][1]) && (mat[0][0] > mat[2][2])
+    test_matrix3_into_quaternion_for_angles(rad(2.0f32), rad(1.0), rad(-1.2f32));
+}
+
+#[test]
+fn test_matrix3_into_quaternion_yy_maximum()
+{
+    // trigger the case: mat[1][1] > mat[2][2]
+    test_matrix3_into_quaternion_for_angles(rad(2.0f32), rad(1.0), rad(3.0f32));
+}
+
+#[test]
+fn test_matrix3_into_quaternion_zz_maximum()
+{
+    // trigger the else case
+    test_matrix3_into_quaternion_for_angles(rad(1.0f32), rad(1.0), rad(3.0f32));
+}