From 8f4c3830c3b516ec2501d0f0daf8f1486e7297fe Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Tue, 27 Dec 2016 23:21:56 +0100 Subject: [PATCH] Updated SimpleMath --- src/SimpleMath/SimpleMath.h | 22 ----------- src/SimpleMath/SimpleMathGL.h | 72 +++++++++++++++++++++++------------ src/SimpleMath/SimpleMathQR.h | 2 +- 3 files changed, 48 insertions(+), 48 deletions(-) diff --git a/src/SimpleMath/SimpleMath.h b/src/SimpleMath/SimpleMath.h index 2e36ff2..22324bd 100644 --- a/src/SimpleMath/SimpleMath.h +++ b/src/SimpleMath/SimpleMath.h @@ -7,26 +7,4 @@ #include "SimpleMathQR.h" #include "SimpleMathCommaInitializer.h" -namespace SimpleMath { - -typedef SimpleMath::Fixed::Matrix Vector3i; - -typedef SimpleMath::Fixed::Matrix Vector3d; -typedef SimpleMath::Fixed::Matrix Matrix33d; - -typedef SimpleMath::Fixed::Matrix Vector4d; - -typedef SimpleMath::Fixed::Matrix Vector3f; -typedef SimpleMath::Fixed::Matrix Vector4f; -typedef SimpleMath::Fixed::Matrix Matrix33f; -typedef SimpleMath::Fixed::Matrix Matrix44f; - -typedef SimpleMath::Dynamic::Matrix VectorNd; -typedef SimpleMath::Dynamic::Matrix MatrixNd; - -typedef SimpleMath::Dynamic::Matrix VectorNf; -typedef SimpleMath::Dynamic::Matrix MatrixNNf; - -} - #endif /* _SIMPLEMATH_H */ diff --git a/src/SimpleMath/SimpleMathGL.h b/src/SimpleMath/SimpleMathGL.h index c2dab43..21b97c9 100644 --- a/src/SimpleMath/SimpleMathGL.h +++ b/src/SimpleMath/SimpleMathGL.h @@ -6,6 +6,12 @@ namespace SimpleMath { +typedef SimpleMath::Fixed::Matrix Vector3f; +typedef SimpleMath::Fixed::Matrix Matrix33f; + +typedef SimpleMath::Fixed::Matrix Vector4f; +typedef SimpleMath::Fixed::Matrix 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], - q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2], - q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0], - q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2] + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*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], - q[3] * (*this)[1] + q[1] * (*this)[3] + q[2] * (*this)[0] - q[0] * (*this)[2], - q[3] * (*this)[2] + q[2] * (*this)[3] + q[0] * (*this)[1] - q[1] * (*this)[0], - q[3] * (*this)[3] - q[0] * (*this)[0] - q[1] * (*this)[1] - q[2] * (*this)[2] + (*this)[3] * q[0] + (*this)[0] * q[3] + (*this)[1] * q[2] - (*this)[2] * q[1], + (*this)[3] * q[1] + (*this)[1] * q[3] + (*this)[2] * q[0] - (*this)[0] * q[2], + (*this)[3] * q[2] + (*this)[2] * q[3] + (*this)[0] * q[1] - (*this)[1] * q[0], + (*this)[3] * q[3] - (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] ); return *this; } @@ -185,39 +191,55 @@ class Quaternion : public Vector4f { w); } - static Quaternion fromEulerZYX (const Vector3f &zyx_euler) { - return Quaternion::fromGLRotate (zyx_euler[0] * 180.f / M_PI, 0.f, 0.f, 1.f) - * Quaternion::fromGLRotate (zyx_euler[1] * 180.f / M_PI, 0.f, 1.f, 0.f) - * Quaternion::fromGLRotate (zyx_euler[2] * 180.f / M_PI, 1.f, 0.f, 0.f); + static Quaternion fromAxisAngle (const Vector3f &axis, double angle_rad) { + double d = axis.norm(); + double s2 = std::sin (angle_rad * 0.5) / d; + 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 { return Vector3f ( - atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2], + atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2], (*this)[0] * (*this)[0] + (*this)[3] * (*this)[3] -(*this)[2] * (*this)[2] - (*this)[1] * (*this)[1]), asin (2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1]), - atan2 (-2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0], + atan2 (-2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0], (*this)[2] * (*this)[2] - (*this)[1] * (*this)[1] -(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3] ) ); } - - 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], + atan2 (-2.f * (*this)[0] * (*this)[2] + 2.f * (*this)[3] * (*this)[1], (*this)[2] * (*this)[2] - (*this)[1] * (*this)[1] -(*this)[0] * (*this)[0] + (*this)[3] * (*this)[3]), asin (2.f * (*this)[1] * (*this)[2] + 2.f * (*this)[3] * (*this)[0]), - atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2], + atan2 (-2.f * (*this)[0] * (*this)[1] + 2.f * (*this)[3] * (*this)[2], (*this)[1] * (*this)[1] - (*this)[2] * (*this)[2] +(*this)[3] * (*this)[3] - (*this)[0] * (*this)[0] ) @@ -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 = conjugate() * res_quat; + res_quat = (*this) * vec_quat; + res_quat = res_quat * conjugate(); return Vector3f (res_quat[0], res_quat[1], res_quat[2]); } diff --git a/src/SimpleMath/SimpleMathQR.h b/src/SimpleMath/SimpleMathQR.h index a1bb3b8..31b0add 100644 --- a/src/SimpleMath/SimpleMathQR.h +++ b/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; }