Quaternions and tests

CMakeLists.txt

@@ -7,6 +7,7 @@ set(SRC
     "src/Buffer.c"
     "src/Logger.c"
     "src/Matrix.c"
+    "src/Quaternion.c"
     "src/Random.c"
     "src/Utility.c"
     "src/Vector.c"
@@ -14,7 +15,6 @@ set(SRC
     #"src/Box.cpp"
     #"src/Frustum.cpp"
     #"src/Plane.cpp"
-    #"src/Quaternion.cpp"
     #"src/SpinLock.cpp"
     #"src/View.cpp"
 )
@@ -24,6 +24,7 @@ set(SRC_TESTS
     "test/Test.c"
     "test/modules/BufferTests.c"
     "test/modules/MatrixTests.c"
+    "test/modules/QuaternionTests.c"
     "test/modules/RandomTests.c"
     "test/modules/UtilityTests.c"
     "test/modules/VectorTests.c"
@@ -36,7 +37,6 @@ set(SRC_TESTS
     #"test/modules/ListTests.cpp"
     #"test/modules/MatrixStackTests.cpp"
     #"test/modules/PlaneTests.cpp"
-    #"test/modules/QuaternionTests.cpp"
     #"test/modules/RingBufferTests.cpp"
     #"test/modules/StackTests.cpp"
     #"test/modules/ThreadTests.cpp"
@@ -158,6 +158,7 @@ target_sources(core PUBLIC
         ./include/core/Check.h
         ./include/core/Logger.h
         ./include/core/Matrix.h
+        ./include/core/Quaternion.h
         ./include/core/Random.h
         ./include/core/Types.h
         ./include/core/Utility.h
@@ -171,7 +172,6 @@ target_sources(core PUBLIC
 #        ./include/core/MatrixStack.hpp
 #        ./include/core/Plane.hpp
 #        ./include/core/ProbingHashMap.hpp
-#        ./include/core/Quaternion.hpp
 #        ./include/core/RingBuffer.hpp
 #        ./include/core/Stack.hpp
 #        ./include/core/Vector.hpp

include/core/Matrix.h

@@ -1,7 +1,7 @@
 #ifndef CORE_MATRIX_H
 #define CORE_MATRIX_H
 
-// #include "core/Quaternion.h"
+#include "core/Quaternion.h"
 #include "core/Vector.h"
 
 typedef struct {
@@ -28,7 +28,7 @@ CoreMatrix* coreTranslateMatrixTo(CoreMatrix* m, const CoreVector3* v);
 CoreMatrix* coreRotateMatrixX(CoreMatrix* m, float degrees);
 CoreMatrix* coreRotateMatrixY(CoreMatrix* m, float degrees);
 CoreMatrix* coreRotateMatrixZ(CoreMatrix* m, float degrees);
-// CoreMatrix* coreRotateMatrix(const Quaternion& q);
+CoreMatrix* coreRotateMatrix(CoreMatrix* m, const CoreQuaternion* q);
 size_t coreToStringMatrix(const CoreMatrix* m, char* buffer, size_t n);
 
 #endif

include/core/Quaternion.h

@@ -0,0 +1,24 @@
+#ifndef CORE_QUATERNION_H
+#define CORE_QUATERNION_H
+
+#include "core/Vector.h"
+
+typedef struct {
+    CoreVector3 xyz;
+    float w;
+} CoreQuaternion;
+
+#define CORE_UNIT_QUATERNION ((CoreQuaternion){{0.0f, 0.0f, 0.0f}, 1.0f})
+
+CoreQuaternion* coreAxisAngleQ(CoreQuaternion* q, const CoreVector3* axis,
+                               float angle);
+CoreQuaternion* coreLerpQ(CoreQuaternion* q, const CoreQuaternion* a, float f,
+                          const CoreQuaternion* b);
+CoreQuaternion* coreMulSetQ(CoreQuaternion* q, const CoreQuaternion* other);
+CoreQuaternion* coreMulQ(CoreQuaternion* q, const CoreQuaternion* a,
+                         const CoreQuaternion* b);
+CoreVector3* coreMulQV3(CoreVector3* r, const CoreQuaternion* q,
+                        const CoreVector3* v);
+size_t coreToStringQ(const CoreQuaternion* q, char* buffer, size_t n);
+
+#endif

src/Matrix.c

@@ -112,17 +112,36 @@ CoreMatrix* coreRotateMatrixZ(CoreMatrix* m, float degrees) {
     return rotate(m, degrees, 0, 1);
 }
 
-// CoreMatrix* coreRotateMatrix(const Quaternion& q){}
-// Core::Matrix& Core::Matrix::rotate(const Quaternion& q) {
-//    Vector3 a = q * Vector3(data[0][0], data[1][0], data[2][0]);
-//    Vector3 b = q * Vector3(data[0][1], data[1][1], data[2][1]);
-//    Vector3 c = q * Vector3(data[0][2], data[1][2], data[2][2]);
-//    Vector3 d = q * Vector3(data[0][3], data[1][3], data[2][3]);
-//    set(0, Vector4(a[0], b[0], c[0], d[0]));
-//    set(1, Vector4(a[1], b[1], c[1], d[1]));
-//    set(2, Vector4(a[2], b[2], c[2], d[2]));
-//    return *this;
-//}
+CoreMatrix* coreRotateMatrix(CoreMatrix* m, const CoreQuaternion* q) {
+    CoreVector3 a;
+    CoreVector3 b;
+    CoreVector3 c;
+    CoreVector3 d;
+    coreMulQV3(&a, q,
+               &(CoreVector3){m->data[0].data[0], m->data[1].data[0],
+                              m->data[2].data[0]});
+    coreMulQV3(&b, q,
+               &(CoreVector3){m->data[0].data[1], m->data[1].data[1],
+                              m->data[2].data[1]});
+    coreMulQV3(&c, q,
+               &(CoreVector3){m->data[0].data[2], m->data[1].data[2],
+                              m->data[2].data[2]});
+    coreMulQV3(&d, q,
+               &(CoreVector3){m->data[0].data[3], m->data[1].data[3],
+                              m->data[2].data[3]});
+    m->data[0] = (CoreVector4){a.data[0], b.data[0], c.data[0], d.data[0]};
+    m->data[1] = (CoreVector4){a.data[1], b.data[1], c.data[1], d.data[1]};
+    m->data[2] = (CoreVector4){a.data[2], b.data[2], c.data[2], d.data[2]};
+
+    //    Vector3 a = q * Vector3(data[0][0], data[1][0], data[2][0]);
+    //    Vector3 b = q * Vector3(data[0][1], data[1][1], data[2][1]);
+    //    Vector3 c = q * Vector3(data[0][2], data[1][2], data[2][2]);
+    //    Vector3 d = q * Vector3(data[0][3], data[1][3], data[2][3]);
+    //    set(0, Vector4(a[0], b[0], c[0], d[0]));
+    //    set(1, Vector4(a[1], b[1], c[1], d[1]));
+    //    set(2, Vector4(a[2], b[2], c[2], d[2]));
+    return m;
+}
 
 static void add(size_t* w, char** buffer, size_t* n, size_t shift) {
     *w += shift;

src/Quaternion.c

@@ -0,0 +1,63 @@
+#include "core/Quaternion.h"
+
+#include <math.h>
+#include <stdio.h>
+
+#include "core/Utility.h"
+
+#define CV30 (&(CoreVector3){0})
+
+CoreQuaternion* coreAxisAngleQ(CoreQuaternion* q, const CoreVector3* axis,
+                               float angle) {
+    q->xyz = *axis;
+    coreNormalizeV3(&q->xyz);
+    angle = coreDegreeToRadian(angle) * 0.5f;
+    q->w = cosf(angle);
+    coreMulSetV3F(&q->xyz, sinf(angle));
+    return q;
+}
+
+CoreQuaternion* coreLerpQ(CoreQuaternion* q, const CoreQuaternion* a, float f,
+                          const CoreQuaternion* b) {
+    coreAddV3(&q->xyz, coreMulV3F(CV30, &a->xyz, 1.0f - f),
+              coreMulV3F(CV30, &b->xyz, f));
+    q->w = a->w * (1.0f - f) + b->w * f;
+    float iLength = 1.0f / sqrtf(coreSquareLengthV3(&q->xyz) + q->w * q->w);
+    coreMulSetV3F(&q->xyz, iLength);
+    q->w *= iLength;
+    return q;
+}
+
+CoreQuaternion* coreMulSetQ(CoreQuaternion* q, const CoreQuaternion* other) {
+    float dot = coreDotV3(&q->xyz, &other->xyz);
+    coreAddV3(&q->xyz, coreMulV3F(CV30, &other->xyz, q->w),
+              coreAddV3(CV30, coreMulV3F(CV30, &q->xyz, other->w),
+                        coreCross(CV30, &q->xyz, &other->xyz)));
+    q->w = q->w * other->w - dot;
+    return q;
+}
+
+CoreQuaternion* coreMulQ(CoreQuaternion* q, const CoreQuaternion* a,
+                         const CoreQuaternion* b) {
+    *q = *a;
+    coreMulSetQ(q, b);
+    return q;
+}
+
+CoreVector3* coreMulQV3(CoreVector3* r, const CoreQuaternion* q,
+                        const CoreVector3* v) {
+    CoreVector3 qv;
+    coreAddV3(&qv, coreMulV3F(CV30, v, q->w), coreCross(CV30, &q->xyz, v));
+
+    coreAddV3(r, coreMulV3F(CV30, &q->xyz, coreDotV3(&q->xyz, v)),
+              coreSubV3(CV30, coreMulV3F(CV30, &qv, q->w),
+                        coreCross(CV30, &qv, &q->xyz)));
+    return r;
+}
+
+size_t coreToStringQ(const CoreQuaternion* q, char* buffer, size_t n) {
+    int w = snprintf(buffer, n, "(%.3f i + %.3f j + %.3f k + %.3f)",
+                     (double)q->xyz.data[0], (double)q->xyz.data[1],
+                     (double)q->xyz.data[2], (double)q->w);
+    return w < 0 ? 0 : (size_t)w;
+}

test/Main.c

@@ -42,7 +42,7 @@ int main(int argAmount, const char** args) {
     // coreTestMatrixStack(light);
     coreTestMatrix();
     // coreTestPlane();
-    // coreTestQuaternion();
+    coreTestQuaternion();
     coreTestRandom(light);
     // coreTestRingBuffer();
     // coreTestStack(light);

test/modules/MatrixTests.c

@@ -160,6 +160,33 @@ static void testRotateZ() {
                  coreMulMatrixV3(CV30, &m, CV3(0.0f, 0.0f, 1.0f)));
 }
 
+static void testQuaternionMatrix() {
+    CoreQuaternion q1 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q1, CV3(1.0f, 0.0f, 0.0f), 48.0f);
+    CoreQuaternion q2 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q2, CV3(0.0f, 1.0f, 0.0f), 52.0f);
+    CoreQuaternion q3 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q3, CV3(0.0f, 0.0f, 1.0f), 60.0f);
+
+    Matrix m = CORE_UNIT_MATRIX;
+    coreTranslateMatrix(&m, CV3(1.0f, 2.0f, 3.0f));
+    coreRotateMatrix(&m, &q1);
+    coreRotateMatrix(&m, &q2);
+    coreRotateMatrix(&m, &q3);
+    coreTranslateMatrix(&m, CV3(1.0f, 2.0f, 3.0f));
+
+    Matrix check = CORE_UNIT_MATRIX;
+    coreTranslateMatrix(&check, CV3(1.0f, 2.0f, 3.0f));
+    coreRotateMatrixX(&check, 48.0f);
+    coreRotateMatrixY(&check, 52.0f);
+    coreRotateMatrixZ(&check, 60.0f);
+    coreTranslateMatrix(&check, CV3(1.0f, 2.0f, 3.0f));
+
+    for(int i = 0; i < 16; i++) {
+        CORE_TEST_FLOAT(((float*)&check)[i], ((float*)&m)[i], 0.0001f);
+    }
+}
+
 static void testToString() {
     Matrix m;
     for(int i = 0; i < 16; i++) {
@@ -178,26 +205,6 @@ static void testToString() {
     CORE_TEST_STRING("[[1.000, 2.000, 3.0", buffer);
 }
 
-/*static void testQuaternionMatrix() {
-    Core::Quaternion q1(V3(1.0f, 0.0f, 0.0f), 48.0f);
-    Core::Quaternion q2(V3(0.0f, 1.0f, 0.0f), 52.0f);
-    Core::Quaternion q3(V3(0.0f, 0.0f, 1.0f), 60.0f);
-
-    Core::Matrix m;
-    m.translate(V3(1.0f, 2.0f, 3.0f));
-    m.rotate(q1).rotate(q2).rotate(q3);
-    m.translate(V3(1.0f, 2.0f, 3.0f));
-
-    Core::Matrix check;
-    check.translate(V3(1.0f, 2.0f, 3.0f));
-    check.rotateX(48.0f).rotateY(52.0f).rotateZ(60.0f);
-    check.translate(V3(1.0f, 2.0f, 3.0f));
-
-    for(int i = 0; i < 16; i++) {
-        CORE_TEST_FLOAT(check.getValues()[i], m.getValues()[i], 0.0001f);
-    }
-}*/
-
 void coreTestMatrix() {
     testInit();
     testTranspose();
@@ -213,6 +220,6 @@ void coreTestMatrix() {
     testRotateX();
     testRotateY();
     testRotateZ();
+    testQuaternionMatrix();
     testToString();
-    // testQuaternionMatrix();
 }

test/modules/QuaternionTests.c

@@ -0,0 +1,109 @@
+#include "../Tests.h"
+#include "core/Quaternion.h"
+
+typedef CoreQuaternion Q;
+#define Q(a, b, c, d) (&(Q){{a, b, c}, d})
+#define CV3(a, b, c) (&(CoreVector3){a, b, c})
+#define CV30 CV3(0.0f, 0.0f, 0.0f)
+
+static void testInit() {
+    Q q = CORE_UNIT_QUATERNION;
+    char buffer[128];
+    coreToStringQ(&q, buffer, sizeof(buffer));
+    CORE_TEST_STRING("(0.000 i + 0.000 j + 0.000 k + 1.000)", buffer);
+}
+
+static void testAxisAndDegreesInit() {
+    Q q = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q, CV3(1.0f, 2.0f, 3.0f), 142.0f);
+    char buffer[128];
+    coreToStringQ(&q, buffer, sizeof(buffer));
+    CORE_TEST_STRING("(0.253 i + 0.505 j + 0.758 k + 0.326)", buffer);
+}
+
+static void testLerp() {
+    Q q1 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), 130.0f);
+    Q q2 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q2, CV3(1.0f, 2.0f, 4.0f), 260.0f);
+    Q q3;
+    coreLerpQ(&q3, &q1, 0.3f, &q2);
+
+    char buffer[128];
+    coreToStringQ(&q3, buffer, sizeof(buffer));
+    CORE_TEST_STRING("(0.223 i + 0.529 j + 0.810 k + 0.119)", buffer);
+}
+
+static void testMulSet() {
+    Q q1 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), 50.0f);
+    Q q2 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q2, CV3(2.0f, 5.0f, 7.0f), 60.0f);
+    Q q3 = CORE_UNIT_QUATERNION;
+    coreMulSetQ(&q3, &q1);
+
+    char bufferQ1[128];
+    coreToStringQ(&q1, bufferQ1, sizeof(bufferQ1));
+    char bufferQ3[128];
+    coreToStringQ(&q3, bufferQ3, sizeof(bufferQ3));
+    CORE_TEST_STRING(bufferQ1, bufferQ3);
+
+    coreMulSetQ(&q3, &q2);
+    coreToStringQ(&q3, bufferQ3, sizeof(bufferQ3));
+
+    coreAxisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), 110.0f);
+    coreToStringQ(&q1, bufferQ1, sizeof(bufferQ1));
+
+    CORE_TEST_STRING(bufferQ1, bufferQ3);
+}
+
+static void testMul() {
+    Q q1 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), 50.0f);
+    Q q2 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q2, CV3(2.0f, 5.0f, 7.0f), 60.0f);
+    Q q3 = CORE_UNIT_QUATERNION;
+    coreMulQ(&q3, &q1, &q2);
+
+    char bufferQ3[128];
+    coreToStringQ(&q3, bufferQ3, sizeof(bufferQ3));
+
+    Q q = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q, CV3(2.0f, 5.0f, 7.0f), 110.0f);
+    char bufferQ[128];
+    coreToStringQ(&q, bufferQ, sizeof(bufferQ));
+
+    CORE_TEST_STRING(bufferQ, bufferQ3);
+}
+
+static void testMulVector() {
+    Q q1 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q1, CV3(1.0f, 0.0f, 0.0f), 90.0f);
+    Q q2 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q2, CV3(0.0f, 1.0f, 0.0f), 90.0f);
+    Q q3 = CORE_UNIT_QUATERNION;
+    coreAxisAngleQ(&q3, CV3(0.0f, 0.0f, 1.0f), 90.0f);
+
+    CoreVector3 v1 = {1.0f, 0.0f, 0.0f};
+    CoreVector3 v2 = {0.0f, 1.0f, 0.0f};
+    CoreVector3 v3 = {0.0f, 0.0f, 1.0f};
+
+    CORE_TEST_V3(CV3(1.0f, 0.0f, 0.0f), coreMulQV3(CV30, &q1, &v1));
+    CORE_TEST_V3(CV3(0.0f, 0.0f, 1.0f), coreMulQV3(CV30, &q1, &v2));
+    CORE_TEST_V3(CV3(0.0f, -1.0f, 0.0f), coreMulQV3(CV30, &q1, &v3));
+    CORE_TEST_V3(CV3(0.0f, 0.0f, -1.0f), coreMulQV3(CV30, &q2, &v1));
+    CORE_TEST_V3(CV3(0.0f, 1.0f, 0.0f), coreMulQV3(CV30, &q2, &v2));
+    CORE_TEST_V3(CV3(1.0f, 0.0f, 0.0f), coreMulQV3(CV30, &q2, &v3));
+    CORE_TEST_V3(CV3(0.0f, 1.0f, 0.0f), coreMulQV3(CV30, &q3, &v1));
+    CORE_TEST_V3(CV3(-1.0f, 0.0f, 0.0f), coreMulQV3(CV30, &q3, &v2));
+    CORE_TEST_V3(CV3(0.0f, 0.0f, 1.0f), coreMulQV3(CV30, &q3, &v3));
+}
+
+void coreTestQuaternion() {
+    testInit();
+    testAxisAndDegreesInit();
+    testLerp();
+    testMulSet();
+    testMul();
+    testMulVector();
+}