efficient matrix rotation using intrinsics

client/math/Matrix.cpp

@@ -1,6 +1,7 @@
 #include <cmath>
 #include <iomanip>
 #include <cstring>
+#include <x86intrin.h>
 
 #include "client/math/Matrix.h"
 
@@ -33,7 +34,7 @@ Matrix& Matrix::setToIdentity() {
     return *this;
 }
 
-Matrix& Matrix::set(unsigned int index, float f) {
+Matrix& Matrix::set(uint index, float f) {
     data[index] = f;
     return *this;
 }
@@ -85,11 +86,7 @@ Matrix& Matrix::scale(float s) {
 }
 
 Matrix& Matrix::translate(float tx, float ty, float tz) {
-    data[12] += data[0] * tx + data[4] * ty + data[8] * tz;
-    data[13] += data[1] * tx + data[5] * ty + data[9] * tz;
-    data[14] += data[2] * tx + data[6] * ty + data[10] * tz;
-    data[15] += data[3] * tx + data[7] * ty + data[11] * tz;
-    return *this;
+    return translateX(tx).translateY(ty).translateZ(tz);
 }
 
 Matrix& Matrix::translateX(float tx) {
@@ -136,85 +133,35 @@ Matrix& Matrix::translateTo(float tx, float ty, float tz) {
     return *this;
 }
 
-Matrix& Matrix::rotateX(float degrees) {
-    degrees *= M_PI / 180.0f;
-    float sin = sinf(degrees);
-    float cos = cosf(degrees);
-
-    float a = data[4];
-    float b = data[8];
-    data[4] = a * cos + b * sin;
-    data[8] = a * -sin + b * cos;
-
-    a = data[5];
-    b = data[9];
-    data[5] = a * cos + b * sin;
-    data[9] = a * -sin + b * cos;
-
-    a = data[6];
-    b = data[10];
-    data[6] = a * cos + b * sin;
-    data[10] = a * -sin + b * cos;
-
-    a = data[7];
-    b = data[11];
-    data[7] = a * cos + b * sin;
-    data[11] = a * -sin + b * cos;
+Matrix& Matrix::rotate(float degrees, uint indexA, uint indexB) {
+    degrees *= M_PIf32 / 180.0f;
+    float sin;
+    float cos;
+    sincosf(degrees, &sin, &cos);
+
+    __m128 va = _mm_load_ps(data + indexA);
+    __m128 vb = _mm_load_ps(data + indexB);
+
+    __m128 vcos = _mm_set1_ps(cos);
+
+    __m128 vresult1 = _mm_add_ps(_mm_mul_ps(va, vcos), _mm_mul_ps(vb, _mm_set1_ps(sin)));
+    __m128 vresult2 = _mm_add_ps(_mm_mul_ps(va, _mm_set1_ps(-sin)), _mm_mul_ps(vb, vcos));
+
+    _mm_store_ps(data + indexA, vresult1);
+    _mm_store_ps(data + indexB, vresult2);
     return *this;
 }
 
+Matrix& Matrix::rotateX(float degrees) {
+    return rotate(degrees, 4, 8);
+}
+
 Matrix& Matrix::rotateY(float degrees) {
-    degrees *= M_PI / 180.0f;
-    float sin = sinf(degrees);
-    float cos = cosf(degrees);
-
-    float a = data[0];
-    float b = data[8];
-    data[0] = a * cos + b * -sin;
-    data[8] = a * sin + b * cos;
-
-    a = data[1];
-    b = data[9];
-    data[1] = a * cos + b * -sin;
-    data[9] = a * sin + b * cos;
-
-    a = data[2];
-    b = data[10];
-    data[2] = a * cos + b * -sin;
-    data[10] = a * sin + b * cos;
-
-    a = data[3];
-    b = data[11];
-    data[3] = a * cos + b * -sin;
-    data[11] = a * sin + b * cos;
-    return *this;
+    return rotate(degrees, 0, 8);
 }
 
 Matrix& Matrix::rotateZ(float degrees) {
-    degrees *= M_PI / 180.0f;
-    float sin = sinf(degrees);
-    float cos = cosf(degrees);
-
-    float a = data[0];
-    float b = data[4];
-    data[0] = a * cos + b * sin;
-    data[4] = a * -sin + b * cos;
-
-    a = data[1];
-    b = data[5];
-    data[1] = a * cos + b * sin;
-    data[5] = a * -sin + b * cos;
-
-    a = data[2];
-    b = data[6];
-    data[2] = a * cos + b * sin;
-    data[6] = a * -sin + b * cos;
-
-    a = data[3];
-    b = data[7];
-    data[3] = a * cos + b * sin;
-    data[7] = a * -sin + b * cos;
-    return *this;
+    return rotate(degrees, 0, 4);
 }
 
 std::ostream& operator<<(std::ostream& os, const Matrix& m) {

client/math/Matrix.h

@@ -9,7 +9,7 @@ public:
 
     Matrix& set(const Matrix& other);
     Matrix& setToIdentity();
-    Matrix& set(unsigned int index, float f);
+    Matrix& set(uint index, float f);
 
     const float* getValues() const;
 
@@ -29,7 +29,9 @@ public:
     Matrix& rotateZ(float degrees);
 
 private:
-    float data[16];
+    Matrix& rotate(float degrees, uint indexA, uint indexB);
+
+    alignas(16) float data[16];
 };
 
 std::ostream& operator<<(std::ostream& os, const Matrix& m);

meson.build

@@ -8,7 +8,7 @@ sourcesServer = ['server/Main.cpp', 'server/network/Server.cpp', 'server/GameSer
 
 sourcesClient = ['client/Main.cpp', 'client/rendering/WindowSize.cpp', 'client/math/Frustum.cpp', 'client/math/Ray.cpp', 'client/rendering/Framebuffers.cpp', 'client/rendering/wrapper/GLFWWrapper.cpp', 'client/rendering/wrapper/Window.cpp', 'client/rendering/Engine.cpp', 'client/input/Keys.cpp', 'client/rendering/wrapper/Shader.cpp', 'client/rendering/Shaders.cpp', 'client/utils/Utils.cpp', 'client/rendering/Mesh.cpp', 'client/math/Matrix.cpp', 'client/math/MatrixStack.cpp', 'client/math/Vector.cpp', 'client/math/Camera.cpp', 'client/math/Plane.cpp', 'client/Game.cpp', 'client/input/MouseButtons.cpp', 'client/rendering/FileTexture.cpp', 'client/rendering/FontRenderer.cpp', 'client/rendering/wrapper/Framebuffer.cpp', 'client/rendering/NoiseTexture.cpp', 'client/utils/Clock.cpp', 'client/input/Control.cpp', 'client/rendering/RenderSettings.cpp', 'client/rendering/wrapper/VertexBuffer.cpp', 'client/rendering/wrapper/StreamBuffer.cpp', 'client/rendering/wrapper/Texture.cpp', 'client/utils/PNGReader.cpp', 'client/rendering/wrapper/GLWrapper.cpp', 'client/rendering/Renderer.cpp']
 
-sourcesTest = ['tests/Main.cpp', 'common/utils/String.cpp', 'common/utils/SplitString.cpp']
+sourcesTest = ['tests/Main.cpp', 'common/utils/String.cpp', 'common/utils/SplitString.cpp', 'client/math/Matrix.cpp', 'client/math/Vector.cpp']
 
 c_compiler = meson.get_compiler('cpp')
 readline = c_compiler.find_library('readline', required: true)

tests/Main.cpp

@@ -3,6 +3,9 @@
 
 #include "common/utils/SplitString.h"
 #include "common/utils/List.h"
+#include "client/math/Vector.h"
+#include "client/math/Matrix.h"
+#include <limits.h>
 
 typedef List<const char*, 16> StringList;
 
@@ -72,7 +75,7 @@ void testCommandParser() {
 
     list.clear().add("test").add("");
     checkCommandParser("test \"\"", list, "command with empty argument");
-    
+
     list.clear();
     checkCommandParser("test \"", list, "command syntax exception 1");
     checkCommandParser("test aaa\"", list, "command syntax exception 2");
@@ -82,7 +85,20 @@ void testCommandParser() {
     finalizeTest();
 }
 
+long getNanos() {
+    struct timespec time;
+    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time);
+    return time.tv_nsec + time.tv_sec * 1000000000;
+}
+
 int main() {
-    testCommandParser();
+    //testCommandParser();
+    //Matrix m;
+    
+    float f;
+    long* l = (long*) (&f);
+    *l = 1016003125;
+    std::cout << f << "\n";
+
     return 0;
 }