matrix4f, vector4f multiply simd x86 ok

Author: rusoleal

inc/vector_math/matrix4f.hpp

@@ -48,6 +48,26 @@ namespace systems::leal::vector_math
     Matrix4f Matrix4f::operator*(const Matrix4f &rhs) const {
         #ifdef __VECTOR_MATH_ARCH_X86_X64
             Matrix4f toReturn;
+            __m128 row1 = _mm_load_ps(&rhs.data[0]);
+            __m128 row2 = _mm_load_ps(&rhs.data[4]);
+            __m128 row3 = _mm_load_ps(&rhs.data[8]);
+            __m128 row4 = _mm_load_ps(&rhs.data[12]);
+            for(int i=0; i<4; i++) {
+                __m128 brod1 = _mm_set1_ps(this->data[4*i + 0]);
+                __m128 brod2 = _mm_set1_ps(this->data[4*i + 1]);
+                __m128 brod3 = _mm_set1_ps(this->data[4*i + 2]);
+                __m128 brod4 = _mm_set1_ps(this->data[4*i + 3]);
+                __m128 row = _mm_add_ps(
+                            _mm_add_ps(
+                                _mm_mul_ps(brod1, row1),
+                                _mm_mul_ps(brod2, row2)),
+                            _mm_add_ps(
+                                _mm_mul_ps(brod3, row3),
+                                _mm_mul_ps(brod4, row4)));
+                _mm_store_ps(&toReturn.data[4*i], row);
+            }
+            return toReturn;        
+            /*Matrix4f toReturn;
             auto transposed = rhs.transpose();
             alignas(float) float result[4];
 
@@ -70,7 +90,8 @@ namespace systems::leal::vector_math
                 }
                 plhs += 4;
             }
-            return toReturn;
+            return toReturn;*/
+
         #elif defined(__VECTOR_MATH_ARCH_ARM)
             auto toReturn = ((Matrix4<float> *)this)->operator*(rhs);
             return *(Matrix4f *)&toReturn;
@@ -80,6 +101,38 @@ namespace systems::leal::vector_math
     Vector4f Matrix4f::operator*(const Vector4f &rhs) const {
         #ifdef __VECTOR_MATH_ARCH_X86_X64
             Vector4f toReturn;
+            __m128 row1 = _mm_load_ps(&this->data[0]);
+            __m128 row2 = _mm_load_ps(&this->data[4]);
+            __m128 row3 = _mm_load_ps(&this->data[8]);
+            __m128 row4 = _mm_load_ps(&this->data[12]);
+            __m128 vector = _mm_load_ps(rhs.data);
+            __m128 r1 = _mm_mul_ps(row1, vector);
+            __m128 r2 = _mm_mul_ps(row2, vector);
+            __m128 r3 = _mm_mul_ps(row3, vector);
+            __m128 r4 = _mm_mul_ps(row4, vector);
+            __m128 result = _mm_hadd_ps(
+                _mm_hadd_ps(r1,r2),
+                _mm_hadd_ps(r3,r4)
+            );
+            _mm_store_ps(toReturn.data, result);
+
+            /*__m128 brod1 = _mm_set1_ps(rhs.data[0]);
+            __m128 brod2 = _mm_set1_ps(rhs.data[1]);
+            __m128 brod3 = _mm_set1_ps(rhs.data[2]);
+            __m128 brod4 = _mm_set1_ps(rhs.data[3]);
+            __m128 row = _mm_add_ps(
+                        _mm_add_ps(
+                            _mm_mul_ps(brod1, row1),
+                            _mm_mul_ps(brod2, row2)),
+                        _mm_add_ps(
+                            _mm_mul_ps(brod3, row3),
+                            _mm_mul_ps(brod4, row4)));
+            _mm_store_ps(toReturn.data, row);
+            printf("vector: %f %f %f %f\n", toReturn.data[0], toReturn.data[1], toReturn.data[2], toReturn.data[3]);
+            */
+            return toReturn;        
+
+            /*Vector4f toReturn;
             alignas(float) float result[4];
 
             const float *plhs = this->data;
@@ -98,7 +151,7 @@ namespace systems::leal::vector_math
 
                 plhs += 4;
             }
-            return toReturn;
+            return toReturn;*/
         #elif defined(__VECTOR_MATH_ARCH_ARM)
             auto toReturn = ((Matrix4<float> *)this)->operator*(rhs);
             return *(Vector4f *)&toReturn;

inc/vector_math/vector_math.hpp

@@ -2,6 +2,7 @@
 
 #include <string>
 #include <vector_math/matrix4.hpp>
+#include <vector_math/matrix4f.hpp>
 
 namespace systems::leal::vector_math {
 

test/main.cpp

@@ -40,7 +40,16 @@ TEST(Matrix4Float, sum) {
 }
 
 TEST(Matrix4Float, mul) {
-    EXPECT_EQ(Matrix4<float>::identity()*Matrix4<float>(1), Matrix4<float>(1));
+    float m1[16] = {1,2,3,4,0,1,2,3,5,6,7,8,10,11,12,13};
+    float m2[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
+    float m3[16] = {80,90,100,110,56,62,68,74,176,202,228,254,296,342,388,434};
+
+    EXPECT_EQ(Matrix4<float>(m1)*Matrix4<float>(m2), Matrix4<float>(m3));
+    EXPECT_EQ(Matrix4f(m1)*Matrix4f(m2), Matrix4f(m3));
+
+    EXPECT_EQ(Matrix4<float>(m1)*Vector4<float>(2,6,10,14), Vector4<float>(100,68,228,388));
+    EXPECT_EQ(Matrix4f(m1)*Vector4f(2,6,10,14), Vector4f(100,68,228,388));
+
 }
 
 TEST(Matrix4Float, transpose) {