diff --git a/src/lol/math/vector.h b/src/lol/math/vector.h index b5940e79..840cb946 100644 --- a/src/lol/math/vector.h +++ b/src/lol/math/vector.h @@ -909,21 +909,24 @@ template struct Vec4 : BVec4 template struct Quat { inline Quat() {} - inline Quat(T X) : x(0), y(0), z(0), w(X) {} - inline Quat(T X, T Y, T Z, T W) : x(X), y(Y), z(Z), w(W) {} + inline Quat(T W) : x(0), y(0), z(0), w(W) {} + inline Quat(T W, T X, T Y, T Z) : x(X), y(Y), z(Z), w(W) {} Quat(Mat3 const &m); Quat(Mat4 const &m); DECLARE_MEMBER_OPS(Quat) + static Quat rotate(T angle, T x, T y, T z); + static Quat rotate(T angle, Vec3 const &v); + inline Quat operator *(Quat const &val) const { Quat ret; Vec3 v1(x, y, z); Vec3 v2(val.x, val.y, val.z); Vec3 v3 = cross(v1, v2) + w * v2 + val.w * v1; - return Quat(v3.x, v3.y, v3.z, w * val.w - dot(v1, v2)); + return Quat(w * val.w - dot(v1, v2), v3.x, v3.y, v3.z); } inline Quat operator *=(Quat const &val) @@ -933,7 +936,7 @@ template struct Quat inline Quat operator ~() const { - return Quat(-x, -y, -z, w); + return Quat(w, -x, -y, -z); } #if !defined __ANDROID__ @@ -941,6 +944,7 @@ template struct Quat friend std::ostream &operator<<(std::ostream &stream, Quat const &v); #endif + /* Storage order is still xyzw because operator[] uses &this->x */ T x, y, z, w; }; diff --git a/src/math/vector.cpp b/src/math/vector.cpp index 7d81cbd3..0c24448d 100644 --- a/src/math/vector.cpp +++ b/src/math/vector.cpp @@ -203,7 +203,7 @@ template<> void ivec4::printf() const template<> void quat::printf() const { - Log::Debug("[ %6.6f %6.6f %6.6f %6.6f ]\n", x, y, z, w); + Log::Debug("[ %6.6f %6.6f %6.6f %6.6f ]\n", w, x, y, z); } template<> void mat2::printf() const @@ -340,8 +340,8 @@ template<> mat2 mat2::rotate(float angle) { angle *= (M_PI / 180.0f); - float st = sinf(angle); - float ct = cosf(angle); + float st = std::sin(angle); + float ct = std::cos(angle); mat2 ret; @@ -358,8 +358,8 @@ template<> mat3 mat3::rotate(float angle, float x, float y, float z) { angle *= (M_PI / 180.0f); - float st = sinf(angle); - float ct = cosf(angle); + float st = std::sin(angle); + float ct = std::cos(angle); float len = sqrtf(x * x + y * y + z * z); float invlen = len ? 1.0f / len : 0.0f; @@ -456,6 +456,20 @@ template<> quat::Quat(mat4 const &m) } } +template<> quat quat::rotate(float angle, vec3 const &v) +{ + angle *= (M_PI / 360.0f); + + vec3 tmp = normalize(v) * std::sin(angle); + + return quat(tmp.x, tmp.y, tmp.z, std::cos(angle)); +} + +template<> quat quat::rotate(float angle, float x, float y, float z) +{ + return quat::rotate(angle, vec3(x, y, z)); +} + template<> mat4 mat4::lookat(vec3 eye, vec3 center, vec3 up) { vec3 v3 = normalize(eye - center); diff --git a/test/unit/quat.cpp b/test/unit/quat.cpp index ee6cbb90..5e42af16 100644 --- a/test/unit/quat.cpp +++ b/test/unit/quat.cpp @@ -57,7 +57,7 @@ LOLUNIT_FIXTURE(QuaternionTest) LOLUNIT_TEST(Conjugate) { quat a(1.0f, 3.0f, 2.0f, 4.0f); - quat b(-1.0f, -3.0f, -2.0f, 4.0f); + quat b(1.0f, -3.0f, -2.0f, -4.0f); LOLUNIT_ASSERT_EQUAL(a, ~b); LOLUNIT_ASSERT_EQUAL(~a, b); @@ -70,7 +70,7 @@ LOLUNIT_FIXTURE(QuaternionTest) LOLUNIT_ASSERT_EQUAL(norm(a), 81.0f); quat b = a * ~a; - quat c(0.0f, 0.0f, 0.0f, norm(a)); + quat c(norm(a), 0.0f, 0.0f, 0.0f); LOLUNIT_ASSERT_EQUAL(b, c); @@ -81,10 +81,10 @@ LOLUNIT_FIXTURE(QuaternionTest) LOLUNIT_TEST(Base) { - quat i(1.0f, 0.0f, 0.0f, 0.0f); - quat j(0.0f, 1.0f, 0.0f, 0.0f); - quat k(0.0f, 0.0f, 1.0f, 0.0f); - quat one(0.0f, 0.0f, 0.0f, 1.0f); + quat i(0.0f, 1.0f, 0.0f, 0.0f); + quat j(0.0f, 0.0f, 1.0f, 0.0f); + quat k(0.0f, 0.0f, 0.0f, 1.0f); + quat one(1.0f, 0.0f, 0.0f, 0.0f); LOLUNIT_ASSERT_EQUAL(norm(i), 1.0f); LOLUNIT_ASSERT_EQUAL(norm(j), 1.0f); @@ -129,6 +129,18 @@ LOLUNIT_FIXTURE(QuaternionTest) LOLUNIT_ASSERT_DOUBLES_EQUAL(m1.z, 0.0, 1e-8); LOLUNIT_ASSERT_DOUBLES_EQUAL(m1.w, 1.0, 1e-8); } + + LOLUNIT_TEST(Rotation) + { + quat a = quat::rotate(10.0f, vec3(1, 0, 0)); + quat b = quat::rotate(20.0f, vec3(1, 0, 0)); + quat c = a * a; + + LOLUNIT_ASSERT_DOUBLES_EQUAL(a.w, a.w, 1e-8); + LOLUNIT_ASSERT_DOUBLES_EQUAL(a.x, a.x, 1e-8); + LOLUNIT_ASSERT_DOUBLES_EQUAL(a.y, a.y, 1e-8); + LOLUNIT_ASSERT_DOUBLES_EQUAL(a.z, a.z, 1e-8); + } }; } /* namespace lol */