move conversions Quaternion <-> Matrix3x3 from Matrix3x3.h into Quaternion.h

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@339 92aac97c-a6ce-11dd-a772-7fcde58d38e6

sources/reactphysics3d/mathematics/Matrix3x3.cpp

@@ -46,40 +46,6 @@ Matrix3x3::Matrix3x3(const Matrix3x3& matrix2) {
                  matrix2.array[2][0], matrix2.array[2][1], matrix2.array[2][2]);
 }
 
-// Create a Matrix3x3 from a quaternion (the quaternion can be non-unit)
-Matrix3x3::Matrix3x3(const Quaternion& quaternion) {
-    double x = quaternion.getX();
-    double y = quaternion.getY();
-    double z = quaternion.getZ();
-    double w = quaternion.getW();
-
-    double nQ = x*x + y*y + z*z + w*w;
-    double s = 0.0;
-
-    if (nQ > 0.0) {
-        s = 2.0/nQ;
-    }
-
-    // Computations used for optimization (less multiplications)
-    double xs = x*s;
-    double ys = y*s;
-    double zs = z*s;
-    double wxs = w*xs;
-    double wys = w*ys;
-    double wzs = w*zs;
-    double xxs = x*xs;
-    double xys = x*ys;
-    double xzs = x*zs;
-    double yys = y*ys;
-    double yzs = y*zs;
-    double zzs = z*zs;
-
-    // Create the matrix corresponding to the quaternion
-    setAllValues(1.0-yys-zzs, xys-wzs, xzs + wys,
-                 xys + wzs, 1.0-xxs-zzs, yzs-wxs,
-                 xzs-wys, yzs + wxs, 1.0-xxs-yys);
-}
-
 // Destructor
 Matrix3x3::~Matrix3x3() {
 
@@ -109,46 +75,6 @@ Matrix3x3 Matrix3x3::getInverse() const throw(MathematicsException) {
     }
 }
 
-// Return the quaternion corresponding to the matrix (it returns a unit quaternion)
-Quaternion Matrix3x3::getQuaternion() const {
-
-    // Get the trace of the matrix
-    double trace = getTrace();
-
-    double r;
-    double s;
-
-    if (trace < 0.0) {
-        if (array[1][1] > array[0][0]) {
-            if(array[2][2] > array[1][1]) {
-                r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
-                s = 0.5 / r;
-                return Quaternion((array[2][0] + array[0][2])*s, (array[1][2] + array[2][1])*s, 0.5*r, (array[1][0] - array[0][1])*s);
-            }
-            else {
-                r = sqrt(array[1][1] - array[2][2] - array[0][0] + 1.0);
-                s = 0.5 / r;
-                return Quaternion((array[0][1] + array[1][0])*s, 0.5 * r, (array[1][2] + array[2][1])*s, (array[0][2] - array[2][0])*s);
-            }
-        }
-        else if (array[2][2] > array[0][0]) {
-            r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
-            s = 0.5 / r;
-            return Quaternion((array[2][0] + array[0][2])*s, (array[1][2] + array[2][1])*s, 0.5 * r, (array[1][0] - array[0][1])*s);
-        }
-        else {
-            r = sqrt(array[0][0] - array[1][1] - array[2][2] + 1.0);
-            s = 0.5 / r;
-            return Quaternion(0.5 * r, (array[0][1] + array[1][0])*s, (array[2][0] - array[0][2])*s, (array[2][1] - array[1][2])*s);
-        }
-    }
-    else {
-        r = sqrt(trace + 1.0);
-        s = 0.5/r;
-        return Quaternion((array[2][1]-array[1][2])*s, (array[0][2]-array[2][0])*s, (array[1][0]-array[0][1])*s, 0.5 * r);
-    }
-}
-
 // Return the 3x3 identity matrix
 Matrix3x3 Matrix3x3::identity() {
     // Return the isdentity matrix

sources/reactphysics3d/mathematics/Matrix3x3.h

@@ -23,7 +23,6 @@
 
 // Libraries
 #include "exceptions.h"
-#include "Quaternion.h"
 #include "Vector3D.h"
 
 // ReactPhysics3D namespace
@@ -44,7 +43,6 @@ class Matrix3x3 {
         Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3,
                   double c1, double c2, double c3);                                             // Constructor with arguments
         Matrix3x3(const Matrix3x3& matrix);                                                     // Copy-constructor
-        Matrix3x3(const Quaternion& quaternion);                                                // Create a Matrix3x3 from a Quaternion
         virtual ~Matrix3x3();                                                                   // Destructor
 
         double getValue(int i, int j) const throw(std::invalid_argument);                       // Get a value in the matrix
@@ -55,7 +53,6 @@ class Matrix3x3 {
         double getDeterminant() const;                                                          // Return the determinant of the matrix
         double getTrace() const;                                                                // Return the trace of the matrix
         Matrix3x3 getInverse() const throw(MathematicsException);                               // Return the inverse matrix
-        Quaternion getQuaternion() const;                                                       // Return the unit quaternion corresponding to the matrix
         static Matrix3x3 identity();                                                            // Return the 3x3 identity matrix
 
         // --- Overloaded operators --- //

sources/reactphysics3d/mathematics/Quaternion.cpp

@@ -49,6 +49,78 @@ Quaternion::Quaternion(const Quaternion& quaternion)
 
 }
 
+// Create a unit quaternion from a rotation matrix
+Quaternion::Quaternion(const Matrix3x3& matrix) {
+
+    // Get the trace of the matrix
+    double trace = matrix.getTrace();
+
+    double array[3][3];
+    for (int i=0; i<3; i++) {
+        for (int j=0; j<3; j++) {
+            array[i][j] = matrix.getValue(i, j);
+        }
+    }
+
+    double r;
+    double s;
+
+    if (trace < 0.0) {
+        if (array[1][1] > array[0][0]) {
+            if(array[2][2] > array[1][1]) {
+                r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
+                s = 0.5 / r;
+                
+                // Compute the quaternion
+                x = (array[2][0] + array[0][2])*s;
+                y = (array[1][2] + array[2][1])*s;
+                z = 0.5*r;
+                w = (array[1][0] - array[0][1])*s;
+            }
+            else {
+                r = sqrt(array[1][1] - array[2][2] - array[0][0] + 1.0);
+                s = 0.5 / r;
+
+                // Compute the quaternion
+                x = (array[0][1] + array[1][0])*s;
+                y = 0.5 * r;
+                z = (array[1][2] + array[2][1])*s;
+                w = (array[0][2] - array[2][0])*s;
+            }
+        }
+        else if (array[2][2] > array[0][0]) {
+            r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
+            s = 0.5 / r;
+
+            // Compute the quaternion
+            x = (array[2][0] + array[0][2])*s;
+            y = (array[1][2] + array[2][1])*s;
+            z = 0.5 * r;
+            w = (array[1][0] - array[0][1])*s;
+        }
+        else {
+            r = sqrt(array[0][0] - array[1][1] - array[2][2] + 1.0);
+            s = 0.5 / r;
+
+            // Compute the quaternion
+            x = 0.5 * r;
+            y = (array[0][1] + array[1][0])*s;
+            z = (array[2][0] - array[0][2])*s;
+            w = (array[2][1] - array[1][2])*s;
+        }
+    }
+    else {
+        r = sqrt(trace + 1.0);
+        s = 0.5/r;
+
+        // Compute the quaternion
+        x = (array[2][1]-array[1][2])*s;
+        y = (array[0][2]-array[2][0])*s;
+        z = (array[1][0]-array[0][1])*s;
+        w = 0.5 * r;
+    }
+}
+
 // Destructor
 Quaternion::~Quaternion() {
 
@@ -82,6 +154,36 @@ void Quaternion::getRotationAngleAxis(double& angle, Vector3D& axis) const {
     axis.setAllValues(rotationAxis.getX(), rotationAxis.getY(), rotationAxis.getZ());
 }
 
+// Return the orientation matrix corresponding to this quaternion
+Matrix3x3 Quaternion::getMatrix() const {
+
+    double nQ = x*x + y*y + z*z + w*w;
+    double s = 0.0;
+
+    if (nQ > 0.0) {
+        s = 2.0/nQ;
+    }
+
+    // Computations used for optimization (less multiplications)
+    double xs = x*s;
+    double ys = y*s;
+    double zs = z*s;
+    double wxs = w*xs;
+    double wys = w*ys;
+    double wzs = w*zs;
+    double xxs = x*xs;
+    double xys = x*ys;
+    double xzs = x*zs;
+    double yys = y*ys;
+    double yzs = y*zs;
+    double zzs = z*zs;
+
+    // Create the matrix corresponding to the quaternion
+    return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys,
+                     xys + wzs, 1.0-xxs-zzs, yzs-wxs,
+                     xzs-wys, yzs + wxs, 1.0-xxs-yys);
+}
+
 // Compute the spherical linear interpolation between two quaternions.
 // The t argument has to be such that 0 <= t <= 1. This method is static.
 Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) {

sources/reactphysics3d/mathematics/Quaternion.h

@@ -23,6 +23,7 @@
 // Libraries
 #include <cmath>
 #include "Vector3D.h"
+#include "Matrix3x3.h"
 #include "exceptions.h"
 
 // ReactPhysics3D namespace
@@ -46,6 +47,7 @@ class Quaternion {
         Quaternion(double x, double y, double z, double w);                   // Constructor with arguments
         Quaternion(double w, const Vector3D& v);                              // Constructor with the component w and the vector v=(x y z)
         Quaternion(const Quaternion& quaternion);                             // Copy-constructor
+        Quaternion(const Matrix3x3& matrix);                                  // Create a unit quaternion from a rotation matrix
         ~Quaternion();                                                        // Destructor
         double getX() const;                                                  // Return the component x of the quaternion
         double getY() const;                                                  // Return the component y of the quaternion
@@ -60,6 +62,7 @@ class Quaternion {
         Quaternion getUnit() const throw (MathematicsException);              // Return the unit quaternion
         Quaternion getConjugate() const;                                      // Return the conjugate quaternion
         Quaternion getInverse() const throw (MathematicsException);           // Return the inverse of the quaternion
+        Matrix3x3 getMatrix() const;                                          // Return the orientation matrix corresponding to this quaternion
         double scalarProduct(const Quaternion& quaternion) const;             // Scalar product between two quaternions
         void getRotationAngleAxis(double& angle, Vector3D& axis) const;       // Compute the rotation angle (in radians) and the axis
         static Quaternion slerp(const Quaternion& quaternion1,