Updated SimpleMath

src/SimpleMath/SimpleMath.h

@@ -7,26 +7,4 @@
 #include "SimpleMathQR.h"
 #include "SimpleMathCommaInitializer.h"
 
-namespace SimpleMath {
-
-typedef SimpleMath::Fixed::Matrix<int, 3, 1> Vector3i;
-
-typedef SimpleMath::Fixed::Matrix<double, 3, 1> Vector3d;
-typedef SimpleMath::Fixed::Matrix<double, 3, 3> Matrix33d;
-
-typedef SimpleMath::Fixed::Matrix<double, 4, 1> Vector4d;
-
-typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f;
-typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f;
-typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f;
-typedef SimpleMath::Fixed::Matrix<float, 4, 4> Matrix44f;
-
-typedef SimpleMath::Dynamic::Matrix<double> VectorNd;
-typedef SimpleMath::Dynamic::Matrix<double> MatrixNd;
-
-typedef SimpleMath::Dynamic::Matrix<float> VectorNf;
-typedef SimpleMath::Dynamic::Matrix<float> MatrixNNf;
-
-}
-
 #endif /* _SIMPLEMATH_H */

src/SimpleMath/SimpleMathGL.h

@@ -6,6 +6,12 @@
 
 namespace SimpleMath {
 
+typedef SimpleMath::Fixed::Matrix<float, 3, 1> Vector3f;
+typedef SimpleMath::Fixed::Matrix<float, 3, 3> Matrix33f;
+
+typedef SimpleMath::Fixed::Matrix<float, 4, 1> Vector4f;
+typedef SimpleMath::Fixed::Matrix<float, 4, 4> Matrix44f;
+
 namespace GL {
 
 inline Matrix33f RotateMat33 (float rot_deg, float x, float y, float z) {
@@ -87,18 +93,18 @@ class Quaternion : public Vector4f {
 		/** This function is equivalent to multiplicate their corresponding rotation matrices */
 		Quaternion operator* (const Quaternion &q) const {
 			return Quaternion (
-					q[3] * (*this)[0] + q[0] * (*this)[3] + q[1] * (*this)[2] - q[2] * (*this)[1],
+					(*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
-					q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2],
+					(*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
-					q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0],
+					(*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
-					q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2]
+					(*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
 					);
 		}
 		Quaternion& operator*=(const Quaternion &q) {
 			set (
-					q[3] * (*this)[0] + q[0] * (*this)[3] + q[1] * (*this)[2] - q[2] * (*this)[1],
+					(*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1],
-					q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2],
+					(*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2],
-					q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0],
+					(*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0],
-					q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2]
+					(*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2]
 					);
 			return *this;
 		}
@@ -185,10 +191,33 @@ class Quaternion : public Vector4f {
 					w);
 		}
 
-		static Quaternion fromEulerZYX (const Vector3f &zyx_euler) {
+		static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) {
-			return Quaternion::fromGLRotate (zyx_euler[0] * 180.f / M_PI, 0.f, 0.f, 1.f)
+			double d = axis.norm();
-				* Quaternion::fromGLRotate (zyx_euler[1] * 180.f / M_PI, 0.f, 1.f, 0.f)
+			double s2 = std::sin (angle_rad * 0.5) / d;
-				* Quaternion::fromGLRotate (zyx_euler[2] * 180.f / M_PI, 1.f, 0.f, 0.f);
+			return Quaternion (
+					axis[0] * s2,
+					axis[1] * s2,
+					axis[2] * s2,
+					std::cos(angle_rad * 0.5)
+					);
+		}
+
+		static Quaternion fromEulerZYX (const Vector3f &zyx_angles) {
+			return Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), zyx_angles[0])
+				* Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), zyx_angles[1])
+				* Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), zyx_angles[2]); 
+		}
+
+		static Quaternion fromEulerYXZ (const Vector3f &yxz_angles) {
+			return Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), yxz_angles[0])
+				* Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), yxz_angles[1])
+				* Quaternion::fromAxisAngle (Vector3f (0., 0., 1.), yxz_angles[2]);
+		}
+
+		static Quaternion fromEulerXYZ (const Vector3f &xyz_angles) {
+			return Quaternion::fromAxisAngle (Vector3f (0., 0., 01.), xyz_angles[2]) 
+				* Quaternion::fromAxisAngle (Vector3f (0., 1., 0.), xyz_angles[1])
+				* Quaternion::fromAxisAngle (Vector3f (1., 0., 0.), xyz_angles[0]);
 		}
  
 		Vector3f toEulerZYX () const {
@@ -204,13 +233,6 @@ class Quaternion : public Vector4f {
 					);
 		}
 
-
-		static Quaternion fromEulerYXZ (const Vector3f &yxz_euler) {
-			return Quaternion::fromGLRotate (yxz_euler[0] * 180.f / M_PI, 0.f, 1.f, 0.f)
-				* Quaternion::fromGLRotate (yxz_euler[1] * 180.f / M_PI, 1.f, 0.f, 0.f)
-				* Quaternion::fromGLRotate (yxz_euler[2] * 180.f / M_PI, 0.f, 0.f, 1.f);
-		}
-
 		Vector3f toEulerYXZ() const {
 			return Vector3f (
 					atan2 (-2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1],
@@ -256,8 +278,8 @@ class Quaternion : public Vector4f {
 			Vector3f vn (vec);
 			Quaternion vec_quat (vn[0], vn[1], vn[2], 0.f), res_quat;
 
-			res_quat = vec_quat * (*this);
+			res_quat = (*this) * vec_quat;
-			res_quat = conjugate() * res_quat;
+			res_quat = res_quat * conjugate();
 
 			return Vector3f (res_quat[0], res_quat[1], res_quat[2]);
 		}

src/SimpleMath/SimpleMathQR.h

@@ -310,7 +310,7 @@ namespace SimpleMath {
 				value_type abs_threshold = fabs(mR(0,0)) * mThreshold;
 
 				for (unsigned int i = 1; i < mR.cols(); i++) {
-					if (fabs(mR(i,i) < abs_threshold))
+					if (fabs(mR(i,i)) < abs_threshold)
 						return i;
 				}