diff --git a/src/lol/math/vector.h b/src/lol/math/vector.h index a68bb804..e192ccf0 100644 --- a/src/lol/math/vector.h +++ b/src/lol/math/vector.h @@ -1565,6 +1565,8 @@ template struct Mat3 v1(mat[1].xyz), v2(mat[2].xyz) {} + explicit Mat3(Quat const &q); + inline Vec3& operator[](size_t n) { return (&v0)[n]; } inline Vec3 const& operator[](size_t n) const { return (&v0)[n]; } @@ -1573,7 +1575,6 @@ template struct Mat3 static Mat3 scale(Vec3 v); static Mat3 rotate(T angle, T x, T y, T z); static Mat3 rotate(T angle, Vec3 v); - static Mat3 rotate(Quat q); static Mat3 fromeuler(T x, T y, T z); static Mat3 fromeuler(Vec3 const &v); @@ -1675,6 +1676,8 @@ template struct Mat4 v2(mat[2], (T)0), v3((T)0, (T)0, (T)0, val) {} + explicit Mat4(Quat const &q); + inline Vec4& operator[](size_t n) { return (&v0)[n]; } inline Vec4 const& operator[](size_t n) const { return (&v0)[n]; } @@ -1707,11 +1710,6 @@ template struct Mat4 return Mat4(Mat3::rotate(angle, v), (T)1); } - static inline Mat4 rotate(Quat q) - { - return Mat4(Mat3::rotate(q), (T)1); - } - static inline Mat4 rotate(Mat4 &mat, T angle, Vec3 v) { return rotate(angle, v) * mat; diff --git a/src/math/vector.cpp b/src/math/vector.cpp index fc60e16d..ef9ddf13 100644 --- a/src/math/vector.cpp +++ b/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]); diff --git a/test/unit/rotation.cpp b/test/unit/rotation.cpp index 499aeaa5..e9020f0e 100644 --- a/test/unit/rotation.cpp +++ b/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 */