quaternions and tests, matrices can be multiplied by quaternions and

vice versa

Main.cpp

@@ -12,6 +12,7 @@
 #include "tests/MatrixStackTests.h"
 #include "tests/PlaneTests.h"
 #include "tests/FrustumTests.h"
+#include "tests/QuaternionTests.h"
 
 int main() {
     ArrayTests::test();
@@ -28,5 +29,6 @@ int main() {
     MatrixStackTests::test();
     PlaneTests::test();
     FrustumTests::test();
+    QuaternionTests::test();
     return 0;
 }

math/Matrix.cpp

@@ -105,4 +105,34 @@ Matrix& Matrix::rotateY(float degrees) {
 
 Matrix& Matrix::rotateZ(float degrees) {
     return rotate(degrees, 0, 1);
+}
+
+Matrix& Matrix::operator*=(const Quaternion& q) {
+    const float* mp = getValues();
+    Vector3 a = Vector3(mp[0], mp[1], mp[2]) * q;
+    Vector3 b = Vector3(mp[4], mp[5], mp[6]) * q;
+    Vector3 c = Vector3(mp[8], mp[9], mp[10]) * q;
+    set(0, Vector4(a[0], a[1], a[2], mp[3]));
+    set(1, Vector4(b[0], b[1], b[2], mp[7]));
+    set(2, Vector4(c[0], c[1], c[2], mp[11]));
+    return *this;
+}
+
+Matrix Matrix::operator*(const Quaternion& q) const {
+    Matrix m(*this);
+    m *= q;
+    return m;
+}
+
+Matrix operator*(const Quaternion& q, const Matrix& m) {
+    const float* mp = m.getValues();
+    Vector3 a = q * Vector3(mp[0], mp[4], mp[8]);
+    Vector3 b = q * Vector3(mp[1], mp[5], mp[9]);
+    Vector3 c = q * Vector3(mp[2], mp[6], mp[10]);
+    Vector3 d = q * Vector3(mp[3], mp[7], mp[11]);
+    Matrix r(m);
+    r.set(0, Vector4(a[0], b[0], c[0], d[0]));
+    r.set(1, Vector4(a[1], b[1], c[1], d[1]));
+    r.set(2, Vector4(a[2], b[2], c[2], d[2]));
+    return r;
 }

math/Matrix.h

@@ -1,7 +1,7 @@
 #ifndef MATRIX_H
 #define MATRIX_H
 
-#include "math/Vector.h"
+#include "math/Quaternion.h"
 #include "utils/StringBuffer.h"
 
 class Matrix final {
@@ -34,7 +34,10 @@ public:
     Matrix& rotateX(float degrees);
     Matrix& rotateY(float degrees);
     Matrix& rotateZ(float degrees);
-
+    
+    Matrix& operator*=(const Quaternion& q);
+    Matrix operator*(const Quaternion& q) const;
+    
     template<int L>
     void toString(StringBuffer<L>& s) const {
         s.append('[').append(data[0]).append(", ");
@@ -44,4 +47,6 @@ public:
     }
 };
 
+Matrix operator*(const Quaternion& q, const Matrix& m);
+
 #endif

math/Quaternion.cpp

@@ -0,0 +1,48 @@
+#include <cmath>
+
+#include "math/Quaternion.h"
+
+Quaternion::Quaternion() : w(1.0f) {
+}
+
+Quaternion::Quaternion(const Vector3& axis, float angle) : xyz(axis) {
+    xyz.normalize();
+    float sin;
+    sincosf(angle * (M_PI / 360.0f), &sin, &w);
+    xyz *= sin;
+}
+
+Quaternion Quaternion::lerp(float f, const Quaternion& other) const {
+    Quaternion q;
+    q.xyz = xyz * (1.0f - f) + other.xyz * f;
+    q.w = w * (1.0f - f) + other.w * f;
+    float iLength = 1.0f / sqrtf(q.xyz.squareLength() + q.w * q.w);
+    q.xyz *= iLength;
+    q.w *= iLength;
+    return q;
+}
+
+Quaternion& Quaternion::operator*=(const Quaternion& other) {
+    float dot = xyz.dot(other.xyz);
+    xyz = other.xyz * w + xyz * other.w + xyz.cross(other.xyz);
+    w = w * other.w - dot;
+    return *this;
+}
+
+Quaternion Quaternion::operator*(const Quaternion& other) const {
+    Quaternion q(*this);
+    q *= other;
+    return q;
+}
+
+Vector3 Quaternion::operator*(const Vector3& v) const {
+    Vector3 qv = v * w + xyz.cross(v);
+    Vector3 qvq = xyz * xyz.dot(v) + qv * w - qv.cross(xyz);
+    return qvq;
+}
+
+Vector3 operator*(const Vector3& v, const Quaternion& q) {
+    Vector3 qv = v * q.w - q.xyz.cross(v);
+    Vector3 qvq = q.xyz * q.xyz.dot(v) + qv * q.w + qv.cross(q.xyz);
+    return qvq;
+}

math/Quaternion.h

@@ -0,0 +1,34 @@
+#ifndef QUATERNION_H
+#define QUATERNION_H
+
+#include "math/Vector.h"
+#include "utils/StringBuffer.h"
+
+class Quaternion {
+    Vector3 xyz;
+    float w;
+
+    friend Vector3 operator*(const Vector3& v, const Quaternion& q);
+
+public:
+    Quaternion();
+    Quaternion(const Vector3& axis, float angle);
+
+    Quaternion lerp(float f, const Quaternion& other) const;
+    Quaternion& operator*=(const Quaternion& other);
+    Quaternion operator*(const Quaternion& other) const;
+    Vector3 operator*(const Vector3& v) const;
+
+    template<int L>
+    void toString(StringBuffer<L>& s) const {
+        s.append("(");
+        s.append(xyz[0]).append(" i + ");
+        s.append(xyz[1]).append(" j + ");
+        s.append(xyz[2]).append(" k + ");
+        s.append(w).append(')');
+    }
+};
+
+Vector3 operator*(const Vector3& v, const Quaternion& q);
+
+#endif

meson.build

@@ -1,6 +1,6 @@
 project('gaming core tests', 'cpp')
 
-sources = ['Main.cpp', 'tests/Test.cpp', 'tests/ArrayTests.cpp', 'tests/HashMapTests.cpp', 'tests/ListTests.cpp', 'tests/BitArrayTests.cpp', 'tests/StringBufferTests.cpp', 'tests/RandomTests.cpp', 'utils/Random.cpp', 'tests/RingBufferTests.cpp', 'tests/SplitStringTests.cpp', 'tests/VectorTests.cpp', 'math/Vector.cpp', 'math/Matrix.cpp', 'tests/MatrixTests.cpp', 'tests/StackTests.cpp', 'tests/MatrixStackTests.cpp', 'tests/PlaneTests.cpp', 'math/Plane.cpp', 'tests/FrustumTests.cpp', 'math/Frustum.cpp', 'utils/Size.cpp']
+sources = ['Main.cpp', 'tests/Test.cpp', 'tests/ArrayTests.cpp', 'tests/HashMapTests.cpp', 'tests/ListTests.cpp', 'tests/BitArrayTests.cpp', 'tests/StringBufferTests.cpp', 'tests/RandomTests.cpp', 'utils/Random.cpp', 'tests/RingBufferTests.cpp', 'tests/SplitStringTests.cpp', 'tests/VectorTests.cpp', 'math/Vector.cpp', 'math/Matrix.cpp', 'tests/MatrixTests.cpp', 'tests/StackTests.cpp', 'tests/MatrixStackTests.cpp', 'tests/PlaneTests.cpp', 'math/Plane.cpp', 'tests/FrustumTests.cpp', 'math/Frustum.cpp', 'utils/Size.cpp', 'tests/QuaternionTests.cpp', 'math/Quaternion.cpp']
 
 executable('tests', 
     sources: sources,

tests/MatrixTests.cpp

@@ -148,6 +148,74 @@ static void testToString(Test& test) {
             "[9.00, 10.00, 11.00, 12.00], [13.00, 14.00, 15.00, 16.00]]"), s, "to string");
 }
 
+static void testQuaternionMatrix(Test& test) {
+    Quaternion q1(Vector3(1.0f, 0.0f, 0.0f), 48.0f);
+    Quaternion q2(Vector3(0.0f, 1.0f, 0.0f), 52.0f);
+    Quaternion q3(Vector3(0.0f, 0.0f, 1.0f), 60.0f);
+
+    Matrix m;
+    m.translate(Vector3(1.0f, 2.0f, 3.0f));
+    m = q1 * m;
+    m = q2 * m;
+    m = q3 * m;
+    m.translate(Vector3(1.0f, 2.0f, 3.0f));
+
+    Matrix check;
+    check.translate(Vector3(1.0f, 2.0f, 3.0f));
+    check.rotateX(48.0f);
+    check.rotateY(52.0f);
+    check.rotateZ(60.0f);
+    check.translate(Vector3(1.0f, 2.0f, 3.0f));
+
+    for(int i = 0; i < 16; i++) {
+        test.checkFloat(check.getValues()[i], m.getValues()[i], eps, "mul matrix");
+    }
+}
+
+static void testMulSetQuaternion(Test& test) {
+    Quaternion q1(Vector3(1.0f, 0.0f, 0.0f), 48.0f);
+    Quaternion q2(Vector3(0.0f, 1.0f, 0.0f), 52.0f);
+    Quaternion q3(Vector3(0.0f, 0.0f, 1.0f), 60.0f);
+
+    Matrix m;
+    m *= q1;
+    m *= q2;
+    m *= q3;
+    m.translate(Vector3(1.0f, 2.0f, 3.0f));
+
+    Matrix check;
+    check.rotateZ(60.0f);
+    check.rotateY(52.0f);
+    check.rotateX(48.0f);
+    check.translate(Vector3(1.0f, 2.0f, 3.0f));
+
+    for(int i = 0; i < 16; i++) {
+        test.checkFloat(check.getValues()[i], m.getValues()[i], eps, "mul matrix");
+    }
+}
+
+static void testMatrixQuaternion(Test& test) {
+    Quaternion q1(Vector3(1.0f, 0.0f, 0.0f), 48.0f);
+    Quaternion q2(Vector3(0.0f, 1.0f, 0.0f), 52.0f);
+    Quaternion q3(Vector3(0.0f, 0.0f, 1.0f), 60.0f);
+
+    Matrix m;
+    m = m * q1;
+    m = m * q2;
+    m = m * q3;
+    m.translate(Vector3(1.0f, 2.0f, 3.0f));
+
+    Matrix check;
+    check.rotateZ(60.0f);
+    check.rotateY(52.0f);
+    check.rotateX(48.0f);
+    check.translate(Vector3(1.0f, 2.0f, 3.0f));
+
+    for(int i = 0; i < 16; i++) {
+        test.checkFloat(check.getValues()[i], m.getValues()[i], eps, "mul matrix");
+    }
+}
+
 void MatrixTests::test() {
     Test test("Matrix");
     testInit(test);
@@ -164,5 +232,8 @@ void MatrixTests::test() {
     testRotateY(test);
     testRotateZ(test);
     testToString(test);
+    testQuaternionMatrix(test);
+    testMulSetQuaternion(test);
+    testMatrixQuaternion(test);
     test.finalize();
 }

tests/QuaternionTests.cpp

@@ -0,0 +1,82 @@
+#include "tests/QuaternionTests.h"
+#include "tests/Test.h"
+#include "math/Quaternion.h"
+#include "utils/StringBuffer.h"
+
+typedef StringBuffer<100> String;
+
+const float eps = 0.0001f;
+
+template<int N>
+static void compareVectors(Test& test, const Vector<N>& wanted, const Vector<N>& actual, const char* text) {
+    for(int i = 0; i < N; i++) {
+        test.checkFloat(wanted[i], actual[i], eps, StringBuffer<50>(text).append(" (").append(i).append(")"));
+    }
+}
+
+static void testInit(Test& test) {
+    Quaternion q;
+    test.checkEqual(String("(0.00 i + 0.00 j + 0.00 k + 1.00)"), String(q), "init");
+}
+
+static void testAxisAndDegreesInit(Test& test) {
+    Quaternion q(Vector3(1.0f, 2.0f, 3.0f), 142.0f);
+    test.checkEqual(String("(0.25 i + 0.51 j + 0.76 k + 0.33)"), String(q), "init with axis and degrees");
+}
+
+static void testLerp(Test& test) {
+    Quaternion q1(Vector3(2.0f, 5.0f, 7.0f), 130.0f);
+    Quaternion q2(Vector3(1.0f, 2.0f, 4.0f), 260.0f);
+    Quaternion q3 = q1.lerp(0.3f, q2);
+    test.checkEqual(String("(0.22 i + 0.53 j + 0.81 k + 0.12)"), String(q3), "lerp");
+}
+
+static void testMulSet(Test& test) {
+    Quaternion q1(Vector3(2.0f, 5.0f, 7.0f), 50.0f);
+    Quaternion q2(Vector3(2.0f, 5.0f, 7.0f), 60.0f);
+    Quaternion q3;
+    q3 *= q1;
+    test.checkEqual(String(q1), String(q3), "mul set 1");
+    q3 *= q2;
+    test.checkEqual(String(Quaternion(Vector3(2.0f, 5.0f, 7.0f), 110.0f)), String(q3), "mul set 2");
+}
+
+static void testMul(Test& test) {
+    Quaternion q1(Vector3(2.0f, 5.0f, 7.0f), 50.0f);
+    Quaternion q2(Vector3(2.0f, 5.0f, 7.0f), 60.0f);
+    Quaternion q3 = q1 * q2;
+    test.checkEqual(String(Quaternion(Vector3(2.0f, 5.0f, 7.0f), 110.0f)), String(q3), "mul");
+}
+
+static void testMulVector(Test& test) {
+    Quaternion q1(Vector3(1.0f, 0.0f, 0.0f), 90.0f);
+    Quaternion q2(Vector3(0.0f, 1.0f, 0.0f), 90.0f);
+    Quaternion q3(Vector3(0.0f, 0.0f, 1.0f), 90.0f);
+
+    Vector3 v1(1.0f, 0.0f, 0.0f);
+    Vector3 v2(0.0f, 1.0f, 0.0f);
+    Vector3 v3(0.0f, 0.0f, 1.0f);
+
+    compareVectors(test, Vector3(1.0f, 0.0f, 0.0f), q1 * v1, "mul with vector 1");
+    compareVectors(test, Vector3(0.0f, 0.0f, 1.0f), q1 * v2, "mul with vector 2");
+    compareVectors(test, Vector3(0.0f, -1.0f, 0.0f), q1 * v3, "mul with vector 3");
+
+    compareVectors(test, Vector3(0.0f, 0.0f, -1.0f), q2 * v1, "mul with vector 4");
+    compareVectors(test, Vector3(0.0f, 1.0f, 0.0f), q2 * v2, "mul with vector 5");
+    compareVectors(test, Vector3(1.0f, 0.0f, 0.0f), q2 * v3, "mul with vector 6");
+
+    compareVectors(test, Vector3(0.0f, 1.0f, 0.0f), q3 * v1, "mul with vector 7");
+    compareVectors(test, Vector3(-1.0f, 0.0f, 0.0f), q3 * v2, "mul with vector 8");
+    compareVectors(test, Vector3(0.0f, 0.0f, 1.0f), q3 * v3, "mul with vector 9");
+}
+
+void QuaternionTests::test() {
+    Test test("Quaternion");
+    testInit(test);
+    testAxisAndDegreesInit(test);
+    testLerp(test);
+    testMulSet(test);
+    testMul(test);
+    testMulVector(test);
+    test.finalize();
+}

tests/QuaternionTests.h

@@ -0,0 +1,8 @@
+#ifndef QUATERNIONTESTS_H
+#define QUATERNIONTESTS_H
+
+namespace QuaternionTests {
+    void test();
+}
+
+#endif