From ecda7cd5698f9040747599583f6f2f77806245c2 Mon Sep 17 00:00:00 2001
From: Sam Hocevar <sam@hocevar.net>
Date: Mon, 30 Apr 2012 18:40:52 +0000
Subject: [PATCH] =?UTF-8?q?math:=20replace=20mat3::rotate(quat)=20with=20a?=
=?UTF-8?q?n=20explicit=20constructor,=20and=20add=20more=20unit=20tests?=
=?UTF-8?q?=20for=20the=20quaternion=20to=203=C3=973=20matrix=20conversion?=
=?UTF-8?q?.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/lol/math/vector.h | 10 +++---
src/math/vector.cpp | 45 ++++++++++++++-----------
test/unit/rotation.cpp | 75 ++++++++++++++++++++++++++++++++++++++----
3 files changed, 99 insertions(+), 31 deletions(-)
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 <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;
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 */