math: replace mat3::rotate(quat) with an explicit constructor, and add

more unit tests for the quaternion to 3×3 matrix conversion.

src/lol/math/vector.h

@@ -1565,6 +1565,8 @@ template <typename T> struct Mat3
         v1(mat[1].xyz),
         v2(mat[2].xyz) {}
 
+    explicit Mat3(Quat<T> const &q);
+
     inline Vec3<T>& operator[](size_t n) { return (&v0)[n]; }
     inline Vec3<T> const& operator[](size_t n) const { return (&v0)[n]; }
 
@@ -1573,7 +1575,6 @@ template <typename T> struct Mat3
     static Mat3<T> scale(Vec3<T> v);
     static Mat3<T> rotate(T angle, T x, T y, T z);
     static Mat3<T> rotate(T angle, Vec3<T> v);
-    static Mat3<T> rotate(Quat<T> q);
     static Mat3<T> fromeuler(T x, T y, T z);
     static Mat3<T> fromeuler(Vec3<T> const &v);
 
@@ -1675,6 +1676,8 @@ template <typename T> struct Mat4
         v2(mat[2], (T)0),
         v3((T)0, (T)0, (T)0, val) {}
 
+    explicit Mat4(Quat<T> const &q);
+
     inline Vec4<T>& operator[](size_t n) { return (&v0)[n]; }
     inline Vec4<T> const& operator[](size_t n) const { return (&v0)[n]; }
 
@@ -1707,11 +1710,6 @@ template <typename T> struct Mat4
         return Mat4<T>(Mat3<T>::rotate(angle, v), (T)1);
     }
 
-    static inline Mat4<T> rotate(Quat<T> q)
-    {
-        return Mat4<T>(Mat3<T>::rotate(q), (T)1);
-    }
-
     static inline Mat4<T> rotate(Mat4<T> &mat, T angle, Vec3<T> v)
     {
         return rotate(angle, v) * mat;

src/math/vector.cpp

@@ -361,7 +361,7 @@ template<> mat3 mat3::rotate(float angle, float x, float y, float z)
     float st = std::sin(angle);
     float ct = std::cos(angle);
 
-    float len = sqrtf(x * x + y * y + z * z);
+    float len = std::sqrt(x * x + y * y + z * z);
     float invlen = len ? 1.0f / len : 0.0f;
     x *= invlen;
     y *= invlen;
@@ -393,38 +393,45 @@ template<> mat3 mat3::rotate(float angle, vec3 v)
     return rotate(angle, v.x, v.y, v.z);
 }
 
-template<> mat3 mat3::rotate(quat q)
+template<> mat3::Mat3(quat const &q)
 {
     float n = norm(q);
 
     if (!n)
-        return mat3(1.0f);
+    {
+        for (int j = 0; j < 3; j++)
+            for (int i = 0; i < 3; i++)
+                (*this)[i][j] = (i == j) ? 1.f : 0.f;
+        return;
+    }
 
-    mat3 ret;
     float s = 2.0f / n;
 
-    ret[0][0] = 1.0f - s * (q.y * q.y + q.z * q.z);
-    ret[0][1] = s * (q.x * q.y - q.z * q.w);
-    ret[0][2] = s * (q.x * q.z + q.y * q.w);
+    v0[0] = 1.0f - s * (q.y * q.y + q.z * q.z);
+    v0[1] = s * (q.x * q.y + q.z * q.w);
+    v0[2] = s * (q.x * q.z - q.y * q.w);
 
-    ret[1][0] = s * (q.x * q.y + q.z * q.w);
-    ret[1][1] = 1.0f - s * (q.z * q.z + q.x * q.x);
-    ret[1][2] = s * (q.y * q.z - q.x * q.w);
+    v1[0] = s * (q.x * q.y - q.z * q.w);
+    v1[1] = 1.0f - s * (q.z * q.z + q.x * q.x);
+    v1[2] = s * (q.y * q.z + q.x * q.w);
 
-    ret[2][0] = s * (q.x * q.z - q.y * q.w);
-    ret[2][1] = s * (q.y * q.z + q.x * q.w);
-    ret[2][2] = 1.0f - s * (q.x * q.x + q.y * q.y);
+    v2[0] = s * (q.x * q.z + q.y * q.w);
+    v2[1] = s * (q.y * q.z - q.x * q.w);
+    v2[2] = 1.0f - s * (q.x * q.x + q.y * q.y);
+}
 
-    return ret;
+template<> mat4::Mat4(quat const &q)
+{
+    *this = mat4(mat3(q), 1.f);
 }
 
-static void MatrixToQuat(quat &that, mat3 const &m)
+static inline void MatrixToQuat(quat &that, mat3 const &m)
 {
     /* See http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/christian.htm for a version with no branches */
     float t = m[0][0] + m[1][1] + m[2][2];
     if (t > 0)
     {
-        that.w = 0.5f * sqrtf(1.0f + t);
+        that.w = 0.5f * std::sqrt(1.0f + t);
         float s = 0.25f / that.w;
         that.x = s * (m[1][2] - m[2][1]);
         that.y = s * (m[2][0] - m[0][2]);
@@ -432,7 +439,7 @@ static void MatrixToQuat(quat &that, mat3 const &m)
     }
     else if (m[0][0] > m[1][1] && m[0][0] > m[2][2])
     {
-        that.x = 0.5f * sqrtf(1.0f + m[0][0] - m[1][1] - m[2][2]);
+        that.x = 0.5f * std::sqrt(1.0f + m[0][0] - m[1][1] - m[2][2]);
         float s = 0.25f / that.x;
         that.y = s * (m[0][1] + m[1][0]);
         that.z = s * (m[2][0] + m[0][2]);
@@ -440,7 +447,7 @@ static void MatrixToQuat(quat &that, mat3 const &m)
     }
     else if (m[1][1] > m[2][2])
     {
-        that.y = 0.5f * sqrtf(1.0f - m[0][0] + m[1][1] - m[2][2]);
+        that.y = 0.5f * std::sqrt(1.0f - m[0][0] + m[1][1] - m[2][2]);
         float s = 0.25f / that.y;
         that.x = s * (m[0][1] + m[1][0]);
         that.z = s * (m[1][2] + m[2][1]);
@@ -448,7 +455,7 @@ static void MatrixToQuat(quat &that, mat3 const &m)
     }
     else
     {
-        that.z = 0.5f * sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]);
+        that.z = 0.5f * std::sqrt(1.0f - m[0][0] - m[1][1] + m[2][2]);
         float s = 0.25f / that.z;
         that.x = s * (m[2][0] + m[0][2]);
         that.y = s * (m[1][2] + m[2][1]);

test/unit/rotation.cpp

@@ -26,7 +26,7 @@ LOLUNIT_FIXTURE(RotationTest)
 
     LOLUNIT_TEST(Rotate2D)
     {
-        /* Check that rotation is CCW */
+        /* Rotations must be CCW */
         mat2 m90 = mat2::rotate(90.f);
 
         vec2 a(2.f, 3.f);
@@ -38,20 +38,21 @@ LOLUNIT_FIXTURE(RotationTest)
 
     LOLUNIT_TEST(Compose2D)
     {
-        /* Check that rotating 20 degrees twice means rotating 40 degrees */
+        /* Rotating 20 degrees twice must equal rotating 40 degrees */
         mat2 m20 = mat2::rotate(20.f);
         mat2 m40 = mat2::rotate(40.f);
         mat2 m20x20 = m20 * m20;
 
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[0][0], m40[0][0], 1e-5);
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[1][0], m40[1][0], 1e-5);
+
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[0][1], m40[0][1], 1e-5);
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[1][1], m40[1][1], 1e-5);
     }
 
     LOLUNIT_TEST(Rotate3D)
     {
-        /* Check that rotation is CCW around each axis */
+        /* Rotations must be CCW along each axis */
         mat3 m90x = mat3::rotate(90.f, 1.f, 0.f, 0.f);
         mat3 m90y = mat3::rotate(90.f, 0.f, 1.f, 0.f);
         mat3 m90z = mat3::rotate(90.f, 0.f, 0.f, 1.f);
@@ -76,7 +77,7 @@ LOLUNIT_FIXTURE(RotationTest)
 
     LOLUNIT_TEST(Compose3D)
     {
-        /* Check that rotating 20 degrees twice means rotating 40 degrees */
+        /* Rotating 20 degrees twice must equal rotating 40 degrees */
         mat3 m20 = mat3::rotate(20.f, 1.f, 2.f, 3.f);
         mat3 m40 = mat3::rotate(40.f, 1.f, 2.f, 3.f);
         mat3 m20x20 = m20 * m20;
@@ -84,9 +85,11 @@ LOLUNIT_FIXTURE(RotationTest)
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[0][0], m40[0][0], 1e-5);
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[1][0], m40[1][0], 1e-5);
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[2][0], m40[2][0], 1e-5);
+
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[0][1], m40[0][1], 1e-5);
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[1][1], m40[1][1], 1e-5);
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[2][1], m40[2][1], 1e-5);
+
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[0][2], m40[0][2], 1e-5);
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[1][2], m40[1][2], 1e-5);
         LOLUNIT_ASSERT_DOUBLES_EQUAL(m20x20[2][2], m40[2][2], 1e-5);
@@ -94,8 +97,7 @@ LOLUNIT_FIXTURE(RotationTest)
 
     LOLUNIT_TEST(QuaternionTransform)
     {
-        /* Check that rotating using a quaternion is the same as rotating
-         * using a matrix */
+        /* Rotating using a quaternion must equal rotating using a matrix */
         mat3 m20 = mat3::rotate(20.f, 1.f, 2.f, 3.f);
         quat q20 = quat::rotate(20.f, 1.f, 2.f, 3.f);
         vec3 a(-2.f, 4.f, 3.f);
@@ -110,6 +112,8 @@ LOLUNIT_FIXTURE(RotationTest)
 
     LOLUNIT_TEST(QuaternionFromMatrix)
     {
+        /* A rotation matrix converted to a quaternion should match the
+         * quaternion built with the same parameters */
         quat q1 = quat::rotate(20.f, 1.f, 2.f, 3.f);
         quat q2 = quat(mat3::rotate(20.f, 1.f, 2.f, 3.f));
 
@@ -118,6 +122,65 @@ LOLUNIT_FIXTURE(RotationTest)
         LOLUNIT_ASSERT_DOUBLES_EQUAL(q2.y, q1.y, 1e-5);
         LOLUNIT_ASSERT_DOUBLES_EQUAL(q2.z, q1.z, 1e-5);
     }
+
+    LOLUNIT_TEST(MatrixFromQuaternion)
+    {
+        /* A quaternion converted to a rotation matrix should match the
+         * rotation matrix built with the same parameters */
+        mat3 m1 = mat3::rotate(60.f, 1.f, -2.f, 3.f);
+        mat3 m2 = mat3(quat::rotate(60.f, 1.f, -2.f, 3.f));
+
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m2[0][0], m1[0][0], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m2[1][0], m1[1][0], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m2[2][0], m1[2][0], 1e-5);
+
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m2[0][1], m1[0][1], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m2[1][1], m1[1][1], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m2[2][1], m1[2][1], 1e-5);
+
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m2[0][2], m1[0][2], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m2[1][2], m1[1][2], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m2[2][2], m1[2][2], 1e-5);
+    }
+
+    LOLUNIT_TEST(MatrixCompositionThroughQuaternions)
+    {
+        /* Combining two rotation matrices should match the matrix created
+         * from the combination of the two equivalent quaternions */
+        mat3 m1 = mat3::rotate(60.f, 1.f, -2.f, 3.f);
+        mat3 m2 = mat3::rotate(20.f, -3.f, 1.f, -3.f);
+
+        mat3 m3 = m2 * m1;
+        mat3 m4(quat(m2) * quat(m1));
+
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m4[0][0], m3[0][0], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m4[1][0], m3[1][0], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m4[2][0], m3[2][0], 1e-5);
+
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m4[0][1], m3[0][1], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m4[1][1], m3[1][1], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m4[2][1], m3[2][1], 1e-5);
+
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m4[0][2], m3[0][2], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m4[1][2], m3[1][2], 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(m4[2][2], m3[2][2], 1e-5);
+    }
+
+    LOLUNIT_TEST(QuaternionCompositionThroughMatrices)
+    {
+        /* Combining two quaternions should match the quaternion created
+         * from the combination of the two equivalent rotation matrices */
+        quat q1 = quat::rotate(60.f, 1.f, -2.f, 3.f);
+        quat q2 = quat::rotate(20.f, -3.f, 1.f, -2.f);
+
+        quat q3 = q2 * q1;
+        quat q4(mat3(q2) * mat3(q1));
+
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(q4.w, q3.w, 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(q4.x, q3.x, 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(q4.y, q3.y, 1e-5);
+        LOLUNIT_ASSERT_DOUBLES_EQUAL(q4.z, q3.z, 1e-5);
+    }
 };
 
 } /* namespace lol */