diff --git a/src/math/vector.cpp b/src/math/vector.cpp index e2625f9c..c2c683a1 100644 --- a/src/math/vector.cpp +++ b/src/math/vector.cpp @@ -631,13 +631,10 @@ static inline quat quat_fromeuler_generic(vec3 const &v, int i, int j, int k) } else { - vec4 v1(c0 * c1 * c2, s0 * c1 * c2, c0 * s1 * c2, c0 * c1 * s2); - vec4 v2(s0 * s1 * s2, -c0 * s1 * s2, s0 * c1 * s2, -s0 * s1 * c2); - - ret[0] = v1[0] + sign * v2[0]; - ret[1 + i] = v1[1] + sign * v2[1]; - ret[1 + j] = v1[2] + sign * v2[2]; - ret[1 + k] = v1[3] + sign * v2[3]; + ret[0] = c0 * c1 * c2 - sign * (s0 * s1 * s2); + ret[1 + i] = s0 * c1 * c2 + sign * (c0 * s1 * s2); + ret[1 + j] = c0 * s1 * c2 - sign * (s0 * c1 * s2); + ret[1 + k] = c0 * c1 * s2 + sign * (s0 * s1 * c2); } return ret; diff --git a/test/unit/quat.cpp b/test/unit/quat.cpp index de5ac87a..a89ac70d 100644 --- a/test/unit/quat.cpp +++ b/test/unit/quat.cpp @@ -188,6 +188,73 @@ LOLUNIT_FIXTURE(QuaternionTest) LOLUNIT_ASSERT_DOUBLES_EQUAL(d.y, a.y / ratio, 1e-5); LOLUNIT_ASSERT_DOUBLES_EQUAL(d.z, a.z / ratio, 1e-5); } + + LOLUNIT_TEST(TaitBryanAngles) + { + for (int i = 0; i < 10; ++i) + { + /* Pick a random point and a random quaternion. We’re going + * to check whether going to Tait-Bryan angles and back to + * quaternion creates the same transform. */ + vec3 p(rand(1.f, 2.f), rand(1.f, 2.f), rand(1.f, 2.f)); + quat q = normalize(quat(rand(-1.f, 1.f), rand(-1.f, 1.f), + rand(-1.f, 1.f), rand(-1.f, 1.f))); + vec3 p0 = q.transform(p); + + /* x-y-z */ + quat q1 = quat::fromeuler_xyz(vec3::toeuler_xyz(q)); + vec3 p1 = q1.transform(p); + + LOLUNIT_ASSERT_DOUBLES_EQUAL(p1.x, p0.x, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p1.y, p0.y, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p1.z, p0.z, 1e-4); + + /* y-z-x */ + quat q2 = quat::fromeuler_yzx(vec3::toeuler_yzx(q)); + vec3 p2 = q2.transform(p); + + LOLUNIT_ASSERT_DOUBLES_EQUAL(p2.x, p0.x, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p2.y, p0.y, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p2.z, p0.z, 1e-4); + + /* z-x-y */ + quat q3 = quat::fromeuler_zxy(vec3::toeuler_zxy(q)); + vec3 p3 = q3.transform(p); + + LOLUNIT_ASSERT_DOUBLES_EQUAL(p3.x, p0.x, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p3.y, p0.y, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p3.z, p0.z, 1e-4); + + /* x-z-y */ + quat q4 = quat::fromeuler_xzy(vec3::toeuler_xzy(q)); + vec3 p4 = q4.transform(p); + + LOLUNIT_ASSERT_DOUBLES_EQUAL(p4.x, p0.x, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p4.y, p0.y, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p4.z, p0.z, 1e-4); + + /* z-y-x */ + quat q5 = quat::fromeuler_zyx(vec3::toeuler_zyx(q)); + vec3 p5 = q5.transform(p); + + LOLUNIT_ASSERT_DOUBLES_EQUAL(p5.x, p0.x, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p5.y, p0.y, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p5.z, p0.z, 1e-4); + + /* y-x-z */ + quat q6 = quat::fromeuler_yxz(vec3::toeuler_yxz(q)); + vec3 p6 = q6.transform(p); + + LOLUNIT_ASSERT_DOUBLES_EQUAL(p6.x, p0.x, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p6.y, p0.y, 1e-4); + LOLUNIT_ASSERT_DOUBLES_EQUAL(p6.z, p0.z, 1e-4); + } + } + + LOLUNIT_TEST(EulerAngles) + { + + } }; } /* namespace lol */