Merge pull request #202 from hrydgard/fast_math

Fast math mini-library

Android.mk

@@ -54,6 +54,8 @@ LOCAL_SRC_FILES :=\
     i18n/i18n.cpp \
     input/gesture_detector.cpp \
     input/input_state.cpp \
+    math/fast/fast_math.c \
+    math/fast/fast_matrix.c \
     math/math_util.cpp \
     math/curves.cpp \
     math/expression_parser.cpp \
@@ -107,10 +109,15 @@ LOCAL_C_INCLUDES := $(LOCAL_PATH)/ext $(LOCAL_PATH)/ext/libzip
 #by ($(target_arch_ABI),arquitecture (armeabi-v7a , armeabi , x86 , MIPS)
 ifeq ($(TARGET_ARCH_ABI), armeabi-v7a)
 LOCAL_CFLAGS := $(LOCAL_CFLAGS) -DARM -DARMEABI_V7A -DARMV7
+LOCAL_SRC_FILES := $(LOCAL_SRC_FILES) \
+    math/fast/fast_matrix_neon.S.neon
+
 else ifeq ($(TARGET_ARCH_ABI),armeabi)
 LOCAL_CFLAGS := $(LOCAL_CFLAGS) -DARM -DARMEABI -march=armv6
 else ifeq ($(TARGET_ARCH_ABI),x86)
 LOCAL_CFLAGS := $(LOCAL_CFLAGS) -D_M_IX86
+LOCAL_SRC_FILES := $(LOCAL_SRC_FILES) \
+    math/fast/fast_matrix_sse.c
 endif
 
 include $(BUILD_STATIC_LIBRARY)

math/fast/fast_math.c

@@ -0,0 +1,10 @@
+#include "fast_math.h"
+#include "fast_matrix.h"
+
+void InitFastMath(int enableNEON) {
+#if !defined(_M_IX86) && !defined(_M_X64) && defined(ARMEABI_V7A)
+	if (enableNEON) {
+		fast_matrix_mul_4x4 = &fast_matrix_mul_4x4_neon;
+	}
+#endif
+}

math/fast/fast_math.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Fast Math
+// A mini library of math kernels. These should be large enough to be worth calling
+// as functions, and generic enough to fit in the "native" library (not PSP specific stuff).
+
+// NEON versions are dynamically selected at runtime, when you call InitFastMath.
+
+// SSE versions are hard linked at compile time.
+
+// See fast_matrix.h for the first set of functions.
+
+void InitFastMath(int enableNEON);
+
+#ifdef __cplusplus
+}
+#endif

math/fast/fast_matrix.c

@@ -0,0 +1,45 @@
+#include "fast_math.h"
+#include "fast_matrix.h"
+
+#define xx 0
+#define xy 1
+#define xz 2
+#define xw 3
+#define yx 4
+#define yy 5
+#define yz 6
+#define yw 7
+#define zx 8
+#define zy 9
+#define zz 10
+#define zw 11
+#define wx 12
+#define wy 13
+#define wz 14
+#define ww 15
+
+void fast_matrix_mul_4x4_c(float *dest, const float *a, const float *b) {
+	dest[xx] = b[xx] * a[xx] + b[xy] * a[yx] + b[xz] * a[zx] + b[xw] * a[wx];
+	dest[xy] = b[xx] * a[xy] + b[xy] * a[yy] + b[xz] * a[zy] + b[xw] * a[wy];
+	dest[xz] = b[xx] * a[xz] + b[xy] * a[yz] + b[xz] * a[zz] + b[xw] * a[wz];
+	dest[xw] = b[xx] * a[xw] + b[xy] * a[yw] + b[xz] * a[zw] + b[xw] * a[ww];
+
+	dest[yx] = b[yx] * a[xx] + b[yy] * a[yx] + b[yz] * a[zx] + b[yw] * a[wx];
+	dest[yy] = b[yx] * a[xy] + b[yy] * a[yy] + b[yz] * a[zy] + b[yw] * a[wy];
+	dest[yz] = b[yx] * a[xz] + b[yy] * a[yz] + b[yz] * a[zz] + b[yw] * a[wz];
+	dest[yw] = b[yx] * a[xw] + b[yy] * a[yw] + b[yz] * a[zw] + b[yw] * a[ww];
+
+	dest[zx] = b[zx] * a[xx] + b[zy] * a[yx] + b[zz] * a[zx] + b[zw] * a[wx];
+	dest[zy] = b[zx] * a[xy] + b[zy] * a[yy] + b[zz] * a[zy] + b[zw] * a[wy];
+	dest[zz] = b[zx] * a[xz] + b[zy] * a[yz] + b[zz] * a[zz] + b[zw] * a[wz];
+	dest[zw] = b[zx] * a[xw] + b[zy] * a[yw] + b[zz] * a[zw] + b[zw] * a[ww];
+
+	dest[wx] = b[wx] * a[xx] + b[wy] * a[yx] + b[wz] * a[zx] + b[ww] * a[wx];
+	dest[wy] = b[wx] * a[xy] + b[wy] * a[yy] + b[wz] * a[zy] + b[ww] * a[wy];
+	dest[wz] = b[wx] * a[xz] + b[wy] * a[yz] + b[wz] * a[zz] + b[ww] * a[wz];
+	dest[ww] = b[wx] * a[xw] + b[wy] * a[yw] + b[wz] * a[zw] + b[ww] * a[ww];
+}
+
+#ifndef fast_matrix_mul_4x4
+fptr_fast_matrix_mul_4x4 fast_matrix_mul_4x4 = &fast_matrix_mul_4x4_c;
+#endif

math/fast/fast_matrix.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// A mini library of matrix math kernels.
+
+// TODO: Really need to wrap this block in a macro or something, will get repetitive.
+
+typedef void(*fptr_fast_matrix_mul_4x4)(float *dest, const float *a, const float *b);
+extern void fast_matrix_mul_4x4_c(float *dest, const float *a, const float *b);
+extern void fast_matrix_mul_4x4_neon(float *dest, const float *a, const float *b);
+extern void fast_matrix_mul_4x4_sse(float *dest, const float *a, const float *b);
+
+#if defined(_M_IX86) || defined(_M_X64)  // Hard link to SSE implementations on x86
+#define fast_matrix_mul_4x4 fast_matrix_mul_4x4_sse
+#else
+extern fptr_fast_matrix_mul_4x4 fast_matrix_mul_4x4;
+#endif
+
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

math/fast/fast_matrix_neon.S

@@ -0,0 +1,48 @@
+	.syntax unified		// Allow both ARM and Thumb-2 instructions
+	.text
+	.align 2		// Align the function code to a 4-byte (2^n) word boundary.
+	.arm			// Use ARM instructions instead of Thumb.
+
+@ From ARM samples
+@
+@ matrix_mul_float:
+@ Calculate 4x4 (matrix 0) * (matrix 1) and store to result 4x4 matrix.
+@  matrix 0, matrix 1 and result pointers can be the same,
+@  ie. my_matrix = my_matrix * my_matrix is possible.
+@
+@ r0 = pointer to 4x4 result matrix, single precision floats, column major order
+@ r1 = pointer to 4x4 matrix 0, single precision floats, column major order
+@ r2 = pointer to 4x4 matrix 1, single precision floats, column major order
+@
+
+	.globl fast_matrix_mul_4x4_neon
+fast_matrix_mul_4x4_neon:
+    vld1.32     {d16-d19}, [r1]!            @ load first eight elements of matrix 0
+    vld1.32     {d20-d23}, [r1]!            @ load second eight elements of matrix 0
+    vld1.32     {d0-d3}, [r2]!              @ load first eight elements of matrix 1
+    vld1.32     {d4-d7}, [r2]!              @ load second eight elements of matrix 1
+
+    vmul.f32    q12, q8, d0[0]              @ rslt col0  = (mat0 col0) * (mat1 col0 elt0)
+    vmul.f32    q13, q8, d2[0]              @ rslt col1  = (mat0 col0) * (mat1 col1 elt0)
+    vmul.f32    q14, q8, d4[0]              @ rslt col2  = (mat0 col0) * (mat1 col2 elt0)
+    vmul.f32    q15, q8, d6[0]              @ rslt col3  = (mat0 col0) * (mat1 col3 elt0)
+
+    vmla.f32    q12, q9, d0[1]              @ rslt col0 += (mat0 col1) * (mat1 col0 elt1)
+    vmla.f32    q13, q9, d2[1]              @ rslt col1 += (mat0 col1) * (mat1 col1 elt1)
+    vmla.f32    q14, q9, d4[1]              @ rslt col2 += (mat0 col1) * (mat1 col2 elt1)
+    vmla.f32    q15, q9, d6[1]              @ rslt col3 += (mat0 col1) * (mat1 col3 elt1)
+
+    vmla.f32    q12, q10, d1[0]             @ rslt col0 += (mat0 col2) * (mat1 col0 elt2)
+    vmla.f32    q13, q10, d3[0]             @ rslt col1 += (mat0 col2) * (mat1 col1 elt2)
+    vmla.f32    q14, q10, d5[0]             @ rslt col2 += (mat0 col2) * (mat1 col2 elt2)
+    vmla.f32    q15, q10, d7[0]             @ rslt col3 += (mat0 col2) * (mat1 col2 elt2)
+
+    vmla.f32    q12, q11, d1[1]             @ rslt col0 += (mat0 col3) * (mat1 col0 elt3)
+    vmla.f32    q13, q11, d3[1]             @ rslt col1 += (mat0 col3) * (mat1 col1 elt3)
+    vmla.f32    q14, q11, d5[1]             @ rslt col2 += (mat0 col3) * (mat1 col2 elt3)
+    vmla.f32    q15, q11, d7[1]             @ rslt col3 += (mat0 col3) * (mat1 col3 elt3)
+
+    vst1.32     {d24-d27}, [r0]!            @ store first eight elements of result
+    vst1.32     {d28-d31}, [r0]!            @ store second eight elements of result
+	bx lr
+

math/fast/fast_matrix_sse.c

@@ -0,0 +1,19 @@
+#include <emmintrin.h>
+
+#include "fast_matrix.h"
+
+void fast_matrix_mul_4x4_sse(float *dest, const float *a, const float *b) {
+	int i;
+	for (i = 0; i < 16; i += 4) {
+		__m128 a_col_1 = _mm_loadu_ps(a);
+		__m128 a_col_2 = _mm_loadu_ps(&a[4]);
+		__m128 a_col_3 = _mm_loadu_ps(&a[8]);
+		__m128 a_col_4 = _mm_loadu_ps(&a[12]);
+
+		__m128 r_col = _mm_mul_ps(a_col_1, _mm_set1_ps(b[i]));
+		r_col = _mm_add_ps(r_col, _mm_mul_ps(a_col_2, _mm_set1_ps(b[i + 1])));
+		r_col = _mm_add_ps(r_col, _mm_mul_ps(a_col_3, _mm_set1_ps(b[i + 2])));
+		r_col = _mm_add_ps(r_col, _mm_mul_ps(a_col_4, _mm_set1_ps(b[i + 3])));
+		_mm_storeu_ps(&dest[i], r_col);
+	}
+}

math/lin/matrix4x4.cpp

@@ -4,6 +4,7 @@
 
 #include "math/lin/vec3.h"
 #include "math/lin/quat.h"
+#include "math/fast/fast_matrix.h"
 
 #ifdef _WIN32
 #undef far
@@ -13,28 +14,6 @@
 // See http://code.google.com/p/oolongengine/source/browse/trunk/Oolong+Engine2/Math/neonmath/neon_matrix_impl.cpp?spec=svn143&r=143	when we need speed
 // no wait. http://code.google.com/p/math-neon/
 
-void matrix_mul_4x4(Matrix4x4 &res, const Matrix4x4 &inA, const Matrix4x4 &inB) {
-	res.xx = inA.xx*inB.xx + inA.xy*inB.yx + inA.xz*inB.zx + inA.xw*inB.wx;
-	res.xy = inA.xx*inB.xy + inA.xy*inB.yy + inA.xz*inB.zy + inA.xw*inB.wy;
-	res.xz = inA.xx*inB.xz + inA.xy*inB.yz + inA.xz*inB.zz + inA.xw*inB.wz;
-	res.xw = inA.xx*inB.xw + inA.xy*inB.yw + inA.xz*inB.zw + inA.xw*inB.ww;
-
-	res.yx = inA.yx*inB.xx + inA.yy*inB.yx + inA.yz*inB.zx + inA.yw*inB.wx;
-	res.yy = inA.yx*inB.xy + inA.yy*inB.yy + inA.yz*inB.zy + inA.yw*inB.wy;
-	res.yz = inA.yx*inB.xz + inA.yy*inB.yz + inA.yz*inB.zz + inA.yw*inB.wz;
-	res.yw = inA.yx*inB.xw + inA.yy*inB.yw + inA.yz*inB.zw + inA.yw*inB.ww;
-
-	res.zx = inA.zx*inB.xx + inA.zy*inB.yx + inA.zz*inB.zx + inA.zw*inB.wx;
-	res.zy = inA.zx*inB.xy + inA.zy*inB.yy + inA.zz*inB.zy + inA.zw*inB.wy;
-	res.zz = inA.zx*inB.xz + inA.zy*inB.yz + inA.zz*inB.zz + inA.zw*inB.wz;
-	res.zw = inA.zx*inB.xw + inA.zy*inB.yw + inA.zz*inB.zw + inA.zw*inB.ww;
-
-	res.wx = inA.wx*inB.xx + inA.wy*inB.yx + inA.wz*inB.zx + inA.ww*inB.wx;
-	res.wy = inA.wx*inB.xy + inA.wy*inB.yy + inA.wz*inB.zy + inA.ww*inB.wy;
-	res.wz = inA.wx*inB.xz + inA.wy*inB.yz + inA.wz*inB.zz + inA.ww*inB.wz;
-	res.ww = inA.wx*inB.xw + inA.wy*inB.yw + inA.wz*inB.zw + inA.ww*inB.ww;
-}
-
 Matrix4x4 Matrix4x4::simpleInverse() const {
 	Matrix4x4 out;
 	out.xx = xx;
@@ -73,7 +52,7 @@ Matrix4x4 Matrix4x4::transpose() const
 Matrix4x4 Matrix4x4::operator * (const Matrix4x4 &other) const 
 {
 	Matrix4x4 temp;
-	matrix_mul_4x4(temp, *this, other);
+	fast_matrix_mul_4x4(temp.m, this->m, other.m);
 	return temp;
 }
 

math/lin/matrix4x4.h

@@ -7,10 +7,15 @@ class Quaternion;
 
 class Matrix4x4 {
 public:
-	float xx, xy, xz, xw;
-	float yx, yy, yz, yw;
-	float zx, zy, zz, zw;
-	float wx, wy, wz, ww;
+	union {
+		struct {
+			float xx, xy, xz, xw;
+			float yx, yy, yz, yw;
+			float zx, zy, zz, zw;
+			float wx, wy, wz, ww;
+		};
+		float m[16];
+	};
 
 	const Vec3 right() const {return Vec3(xx, xy, xz);}
 	const Vec3 up()		const {return Vec3(yx, yy, yz);}

native.vcxproj

@@ -180,6 +180,7 @@
     <None Include="README.md" />
     <None Include="tools\CMakeLists.txt" />
     <None Include="ui\CMakeLists.txt" />
+    <None Include="math\fast\fast_matrix_neon.S" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="android\app-android.h" />
@@ -264,6 +265,8 @@
     <ClInclude Include="math\lin\quat.h" />
     <ClInclude Include="math\lin\vec3.h" />
     <ClInclude Include="math\math_util.h" />
+    <ClInclude Include="math\fast\fast_math.h" />
+    <ClInclude Include="math\fast\fast_matrix.h" />
     <ClInclude Include="midi\midi_input.h" />
     <ClInclude Include="net\http_client.h" />
     <ClInclude Include="net\resolve.h" />
@@ -743,6 +746,9 @@
     <ClCompile Include="math\lin\quat.cpp" />
     <ClCompile Include="math\lin\vec3.cpp" />
     <ClCompile Include="math\math_util.cpp" />
+    <ClCompile Include="math\fast\fast_math.c" />
+    <ClCompile Include="math\fast\fast_matrix.c" />
+    <ClCompile Include="math\fast\fast_matrix_sse.c" />
     <ClCompile Include="midi\midi_input.cpp" />
     <ClCompile Include="net\http_client.cpp" />
     <ClCompile Include="net\resolve.cpp" />
@@ -770,4 +776,4 @@
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">
   </ImportGroup>
-</Project>
+</Project>

native.vcxproj.filters

@@ -22,6 +22,9 @@
     <None Include="ext\libzip\Android.mk">
       <Filter>ext\libzip</Filter>
     </None>
+    <None Include="math\fast\fast_matrix_neon.S">
+      <Filter>math\fast</Filter>
+    </None>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="gfx\gl_debug_log.h">
@@ -329,6 +332,12 @@
     <ClInclude Include="gfx_es2\gl3stub.h">
       <Filter>gfx</Filter>
     </ClInclude>
+    <ClInclude Include="math\fast\fast_math.h">
+      <Filter>math\fast</Filter>
+    </ClInclude>
+    <ClInclude Include="math\fast\fast_matrix.h">
+      <Filter>math\fast</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="gfx\gl_debug_log.cpp">
@@ -778,6 +787,15 @@
     <ClCompile Include="gfx_es2\gl3stub.c">
       <Filter>gfx</Filter>
     </ClCompile>
+    <ClCompile Include="math\fast\fast_math.c">
+      <Filter>math\fast</Filter>
+    </ClCompile>
+    <ClCompile Include="math\fast\fast_matrix.c">
+      <Filter>math\fast</Filter>
+    </ClCompile>
+    <ClCompile Include="math\fast\fast_matrix_sse.c">
+      <Filter>math\fast</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <Filter Include="gfx">
@@ -855,5 +873,8 @@
     <Filter Include="ext\vjson">
       <UniqueIdentifier>{c6910d82-73d3-4d97-ac9c-bcc03265b1bc}</UniqueIdentifier>
     </Filter>
+    <Filter Include="math\fast">
+      <UniqueIdentifier>{5ce64c0e-98e4-4411-86cc-aacaf2f60b21}</UniqueIdentifier>
+    </Filter>
   </ItemGroup>
 </Project>