Refactors neon optimized math functions.

Adds MathUtil.h, MathUtil.inl and MathUtilNeon.inl to gameplay/src

Provides neon implementation for:
Matrix::add(float, Matrix*)
Vector3::cross(const Vector3&, const Vector3&, Vector3*)

Changes Vector3::cross(const Vector3&) to just call Vector3::cross(const Vector3&, const Vector3&, Vector3*)

For the gameplay project, added the -mfpu=neon for Debug, Coverage, Profile, and Release configurations.

gameplay/.cproject

@@ -29,8 +29,8 @@
 									<listOptionValue builtIn="false" value="&quot;../../external-deps/oggvorbis/include&quot;"/>
 									<listOptionValue builtIn="false" value="${QNX_TARGET}/../target-override/usr/include"/>
 								</option>
-								<option id="com.qnx.qcc.option.compiler.qccoptions.1968057343" name="QCC Options" superClass="com.qnx.qcc.option.compiler.qccoptions" valueType="stringList"/>
-								<option id="com.qnx.qcc.option.compiler.ccoptions.1078137668" superClass="com.qnx.qcc.option.compiler.ccoptions" valueType="stringList">
+								<option id="com.qnx.qcc.option.compiler.qccoptions.1968057343" name="QCC Options" superClass="com.qnx.qcc.option.compiler.qccoptions"/>
+								<option id="com.qnx.qcc.option.compiler.ccoptions.1078137668" name="Compiler Options (-Wc,)" superClass="com.qnx.qcc.option.compiler.ccoptions" valueType="stringList">
 									<listOptionValue builtIn="false" value="-mfpu=neon"/>
 								</option>
 								<inputType id="com.qnx.qcc.inputType.compiler.997142816" superClass="com.qnx.qcc.inputType.compiler"/>
@@ -86,6 +86,9 @@
 									<listOptionValue builtIn="false" value="&quot;../../external-deps/oggvorbis/include&quot;"/>
 									<listOptionValue builtIn="false" value="${QNX_TARGET}/../target-override/usr/include"/>
 								</option>
+								<option id="com.qnx.qcc.option.compiler.ccoptions.1122311163" name="Compiler Options (-Wc,)" superClass="com.qnx.qcc.option.compiler.ccoptions" valueType="stringList">
+									<listOptionValue builtIn="false" value="-mfpu=neon"/>
+								</option>
 								<inputType id="com.qnx.qcc.inputType.compiler.1380846613" superClass="com.qnx.qcc.inputType.compiler"/>
 							</tool>
 							<tool id="com.qnx.qcc.tool.assembler.855139060" name="QCC Assembler" superClass="com.qnx.qcc.tool.assembler">
@@ -138,6 +141,9 @@
 									<listOptionValue builtIn="false" value="&quot;../../external-deps/oggvorbis/include&quot;"/>
 									<listOptionValue builtIn="false" value="${QNX_TARGET}/../target-override/usr/include"/>
 								</option>
+								<option id="com.qnx.qcc.option.compiler.ccoptions.1956270067" name="Compiler Options (-Wc,)" superClass="com.qnx.qcc.option.compiler.ccoptions" valueType="stringList">
+									<listOptionValue builtIn="false" value="-mfpu=neon"/>
+								</option>
 								<inputType id="com.qnx.qcc.inputType.compiler.81809638" superClass="com.qnx.qcc.inputType.compiler"/>
 							</tool>
 							<tool id="com.qnx.qcc.tool.assembler.2145279747" name="QCC Assembler" superClass="com.qnx.qcc.tool.assembler">
@@ -193,6 +199,9 @@
 									<listOptionValue builtIn="false" value="&quot;../../external-deps/oggvorbis/include&quot;"/>
 									<listOptionValue builtIn="false" value="${QNX_TARGET}/../target-override/usr/include"/>
 								</option>
+								<option id="com.qnx.qcc.option.compiler.ccoptions.47607907" name="Compiler Options (-Wc,)" superClass="com.qnx.qcc.option.compiler.ccoptions" valueType="stringList">
+									<listOptionValue builtIn="false" value="-mfpu=neon"/>
+								</option>
 								<inputType id="com.qnx.qcc.inputType.compiler.2007171407" superClass="com.qnx.qcc.inputType.compiler"/>
 							</tool>
 							<tool id="com.qnx.qcc.tool.assembler.1537562121" name="QCC Assembler" superClass="com.qnx.qcc.tool.assembler">
@@ -246,6 +255,7 @@
 									<listOptionValue builtIn="false" value="&quot;../../external-deps/oggvorbis/include&quot;"/>
 									<listOptionValue builtIn="false" value="${QNX_TARGET}/../target-override/usr/include"/>
 								</option>
+								<option id="com.qnx.qcc.option.compiler.ccoptions.1432778691" name="Compiler Options (-Wc,)" superClass="com.qnx.qcc.option.compiler.ccoptions" valueType="stringList"/>
 								<inputType id="com.qnx.qcc.inputType.compiler.1038720310" superClass="com.qnx.qcc.inputType.compiler"/>
 							</tool>
 							<tool id="com.qnx.qcc.tool.assembler.521146732" name="QCC Assembler" superClass="com.qnx.qcc.tool.assembler">
@@ -299,6 +309,7 @@
 									<listOptionValue builtIn="false" value="&quot;../../external-deps/oggvorbis/include&quot;"/>
 									<listOptionValue builtIn="false" value="${QNX_TARGET}/../target-override/usr/include"/>
 								</option>
+								<option id="com.qnx.qcc.option.compiler.ccoptions.663337616" name="Compiler Options (-Wc,)" superClass="com.qnx.qcc.option.compiler.ccoptions" valueType="stringList"/>
 								<inputType id="com.qnx.qcc.inputType.compiler.1961855927" superClass="com.qnx.qcc.inputType.compiler"/>
 							</tool>
 							<tool id="com.qnx.qcc.tool.assembler.1089440729" name="QCC Assembler" superClass="com.qnx.qcc.tool.assembler">
@@ -353,6 +364,7 @@
 									<listOptionValue builtIn="false" value="&quot;../../external-deps/oggvorbis/include&quot;"/>
 									<listOptionValue builtIn="false" value="${QNX_TARGET}/../target-override/usr/include"/>
 								</option>
+								<option id="com.qnx.qcc.option.compiler.ccoptions.346770186" name="Compiler Options (-Wc,)" superClass="com.qnx.qcc.option.compiler.ccoptions" valueType="stringList"/>
 								<inputType id="com.qnx.qcc.inputType.compiler.1658185881" superClass="com.qnx.qcc.inputType.compiler"/>
 							</tool>
 							<tool id="com.qnx.qcc.tool.assembler.746786008" name="QCC Assembler" superClass="com.qnx.qcc.tool.assembler">
@@ -411,4 +423,5 @@
 		<resource resourceType="PROJECT" workspacePath="/gameplay"/>
 	</storageModule>
 	<storageModule moduleId="com.qnx.tools.ide.qde.core.QNXProjectProperties"/>
+	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"/>
 </cproject>

gameplay/gameplay.vcxproj

@@ -144,6 +144,7 @@
     <ClInclude Include="src\Layout.h" />
     <ClInclude Include="src\Light.h" />
     <ClInclude Include="src\Material.h" />
+    <ClInclude Include="src\MathUtil.h" />
     <ClInclude Include="src\MeshBatch.h" />
     <ClInclude Include="src\Mouse.h" />
     <ClInclude Include="src\Pass.h" />
@@ -232,6 +233,8 @@
     <None Include="src\gameplay-main-ios.mm" />
     <None Include="src\gameplay-main-macosx.mm" />
     <None Include="src\Image.inl" />
+    <None Include="src\MathUtil.inl" />
+    <None Include="src\MathUtilNeon.inl" />
     <None Include="src\Matrix.inl" />
     <None Include="src\MeshBatch.inl" />
     <None Include="src\Plane.inl" />

gameplay/gameplay.vcxproj.filters

@@ -560,6 +560,9 @@
     <ClInclude Include="src\ScrollLayout.h">
       <Filter>src</Filter>
     </ClInclude>
+    <ClInclude Include="src\MathUtil.h">
+      <Filter>src</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <None Include="res\shaders\bumped-specular.vsh">
@@ -658,6 +661,12 @@
     <None Include="res\logo_white.png">
       <Filter>res</Filter>
     </None>
+    <None Include="src\MathUtil.inl">
+      <Filter>src</Filter>
+    </None>
+    <None Include="src\MathUtilNeon.inl">
+      <Filter>src</Filter>
+    </None>
   </ItemGroup>
   <ItemGroup>
     <None Include="src\PhysicsFixedConstraint.inl">

gameplay/src/MathUtil.h

@@ -0,0 +1,36 @@
+#ifndef MATHUTIL_H_
+#define MATHUTIL_H_
+
+namespace gameplay
+{
+class MathUtil
+{
+	//friend class Matrix;
+	//friend class Vector3;
+public:
+
+	/** Matrix **/
+	inline static void addMatrix(const float* m, float scalar, float* dst);
+	inline static void addMatrix(const float* m1, const float* m2, float* dst);
+	inline static void multiplyMatrix(const float* m, float scalar, float* dst);
+	inline static void multiplyMatrix(const float* m1, const float* m2, float* dst);
+	inline static void negateMatrix(const float* m, float* dst);
+	inline static void subtractMatrix(const float* m1, const float* m2, float* dst);
+	inline static void transformVectorMatrix(const float* m, float x, float y, float z, float w, float* dst);
+	inline static void transformVectorMatrix(const float* m, const float* v, float* dst);
+	inline static void transpose(const float* m, float* dst);
+
+	/** Vector3 **/
+	inline static void crossVector3(const float* v1, const float* v2, float* dst);
+private:
+	MathUtil();
+};
+}
+
+#ifdef USE_NEON
+#include "MathUtilNeon.inl"
+#else
+#include "MathUtil.inl"
+#endif
+
+#endif /* MATHUTIL_H_ */

gameplay/src/MathUtil.inl

@@ -0,0 +1,173 @@
+#define MATRIX_SIZE ( sizeof(float) * 16)
+
+namespace gameplay
+{
+
+inline void MathUtil::addMatrix(const float* m, float scalar, float* dst)
+{
+	dst[0]  = m[0]  + scalar;
+	dst[1]  = m[1]  + scalar;
+	dst[2]  = m[2]  + scalar;
+	dst[3]  = m[3]  + scalar;
+	dst[4]  = m[4]  + scalar;
+	dst[5]  = m[5]  + scalar;
+	dst[6]  = m[6]  + scalar;
+	dst[7]  = m[7]  + scalar;
+	dst[8]  = m[8]  + scalar;
+	dst[9]  = m[9]  + scalar;
+	dst[10] = m[10] + scalar;
+	dst[11] = m[11] + scalar;
+	dst[12] = m[12] + scalar;
+	dst[13] = m[13] + scalar;
+	dst[14] = m[14] + scalar;
+	dst[15] = m[15] + scalar;
+}
+
+inline void MathUtil::addMatrix(const float* m1, const float* m2, float* dst)
+{
+	dst[0]  = m1[0]  + m2[0];
+	dst[1]  = m1[1]  + m2[1];
+	dst[2]  = m1[2]  + m2[2];
+	dst[3]  = m1[3]  + m2[3];
+	dst[4]  = m1[4]  + m2[4];
+	dst[5]  = m1[5]  + m2[5];
+	dst[6]  = m1[6]  + m2[6];
+	dst[7]  = m1[7]  + m2[7];
+	dst[8]  = m1[8]  + m2[8];
+	dst[9]  = m1[9]  + m2[9];
+	dst[10] = m1[10] + m2[10];
+	dst[11] = m1[11] + m2[11];
+	dst[12] = m1[12] + m2[12];
+	dst[13] = m1[13] + m2[13];
+	dst[14] = m1[14] + m2[14];
+	dst[15] = m1[15] + m2[15];
+}
+
+inline void MathUtil::multiplyMatrix(const float* m, float scalar, float* dst)
+{
+	dst[0]  = m[0]  * scalar;
+	dst[1]  = m[1]  * scalar;
+	dst[2]  = m[2]  * scalar;
+	dst[3]  = m[3]  * scalar;
+	dst[4]  = m[4]  * scalar;
+	dst[5]  = m[5]  * scalar;
+	dst[6]  = m[6]  * scalar;
+	dst[7]  = m[7]  * scalar;
+	dst[8]  = m[8]  * scalar;
+	dst[9]  = m[9]  * scalar;
+	dst[10] = m[10] * scalar;
+	dst[11] = m[11] * scalar;
+	dst[12] = m[12] * scalar;
+	dst[13] = m[13] * scalar;
+	dst[14] = m[14] * scalar;
+	dst[15] = m[15] * scalar;
+}
+
+inline void MathUtil::multiplyMatrix(const float* m1, const float* m2, float* dst)
+{
+	// Support the case where m1 or m2 is the same array as dst.
+	float product[16];
+
+	product[0]  = m1[0] * m2[0]  + m1[4] * m2[1] + m1[8]   * m2[2]  + m1[12] * m2[3];
+	product[1]  = m1[1] * m2[0]  + m1[5] * m2[1] + m1[9]   * m2[2]  + m1[13] * m2[3];
+	product[2]  = m1[2] * m2[0]  + m1[6] * m2[1] + m1[10]  * m2[2]  + m1[14] * m2[3];
+	product[3]  = m1[3] * m2[0]  + m1[7] * m2[1] + m1[11]  * m2[2]  + m1[15] * m2[3];
+
+	product[4]  = m1[0] * m2[4]  + m1[4] * m2[5] + m1[8]   * m2[6]  + m1[12] * m2[7];
+	product[5]  = m1[1] * m2[4]  + m1[5] * m2[5] + m1[9]   * m2[6]  + m1[13] * m2[7];
+	product[6]  = m1[2] * m2[4]  + m1[6] * m2[5] + m1[10]  * m2[6]  + m1[14] * m2[7];
+	product[7]  = m1[3] * m2[4]  + m1[7] * m2[5] + m1[11]  * m2[6]  + m1[15] * m2[7];
+
+	product[8]  = m1[0] * m2[8]  + m1[4] * m2[9] + m1[8]   * m2[10] + m1[12] * m2[11];
+	product[9]  = m1[1] * m2[8]  + m1[5] * m2[9] + m1[9]   * m2[10] + m1[13] * m2[11];
+	product[10] = m1[2] * m2[8]  + m1[6] * m2[9] + m1[10]  * m2[10] + m1[14] * m2[11];
+	product[11] = m1[3] * m2[8]  + m1[7] * m2[9] + m1[11]  * m2[10] + m1[15] * m2[11];
+
+	product[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8]  * m2[14] + m1[12] * m2[15];
+	product[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9]  * m2[14] + m1[13] * m2[15];
+	product[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15];
+	product[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15];
+
+	memcpy(dst, product, MATRIX_SIZE);
+}
+
+inline void MathUtil::negateMatrix(const float* m, float* dst)
+{
+	dst[0]  = -m[0];
+	dst[1]  = -m[1];
+	dst[2]  = -m[2];
+	dst[3]  = -m[3];
+	dst[4]  = -m[4];
+	dst[5]  = -m[5];
+	dst[6]  = -m[6];
+	dst[7]  = -m[7];
+	dst[8]  = -m[8];
+	dst[9]  = -m[9];
+	dst[10] = -m[10];
+	dst[11] = -m[11];
+	dst[12] = -m[12];
+	dst[13] = -m[13];
+	dst[14] = -m[14];
+	dst[15] = -m[15];
+}
+
+inline void MathUtil::subtractMatrix(const float* m1, const float* m2, float* dst)
+{
+	dst[0]  = m1[0]  - m2[0];
+	dst[1]  = m1[1]  - m2[1];
+	dst[2]  = m1[2]  - m2[2];
+	dst[3]  = m1[3]  - m2[3];
+	dst[4]  = m1[4]  - m2[4];
+	dst[5]  = m1[5]  - m2[5];
+	dst[6]  = m1[6]  - m2[6];
+	dst[7]  = m1[7]  - m2[7];
+	dst[8]  = m1[8]  - m2[8];
+	dst[9]  = m1[9]  - m2[9];
+	dst[10] = m1[10] - m2[10];
+	dst[11] = m1[11] - m2[11];
+	dst[12] = m1[12] - m2[12];
+	dst[13] = m1[13] - m2[13];
+	dst[14] = m1[14] - m2[14];
+	dst[15] = m1[15] - m2[15];
+}
+
+inline void MathUtil::transformVectorMatrix(const float* m, float x, float y, float z, float w, float* dst)
+{
+	dst[0] = x * m[0] + y * m[4] + z * m[8] + w * m[12];
+	dst[1] = x * m[1] + y * m[5] + z * m[9] + w * m[13];
+	dst[2] = x * m[2] + y * m[6] + z * m[10] + w * m[14];
+}
+
+inline void MathUtil::transformVectorMatrix(const float* m, const float* v, float* dst)
+{
+	dst[0] = v[0] * m[0] + v[1] * m[4] + v[2] * m[8] + v[3] * m[12];
+	dst[1] = v[0] * m[1] + v[1] * m[5] + v[2] * m[9] + v[3] * m[13];
+	dst[2] = v[0] * m[2] + v[1] * m[6] + v[2] * m[10] + v[3] * m[14];
+	dst[3] = v[0] * m[3] + v[1] * m[7] + v[2] * m[11] + v[3] * m[15];
+}
+
+inline void MathUtil::transpose(const float* m, float* dst)
+{
+	float t[16] = {
+		m[0], m[4], m[8], m[12],
+		m[1], m[5], m[9], m[13],
+		m[2], m[6], m[10], m[14],
+		m[3], m[7], m[11], m[15]
+	};
+	memcpy(dst, t, MATRIX_SIZE);
+}
+
+inline void MathUtil::crossVector3(const float* v1, const float* v2, float* dst)
+{
+	float x = (v1[1] * v2[2]) - (v1[2] * v2[1]);
+	float y = (v1[2] * v2[0]) - (v1[0] * v2[2]);
+	float z = (v1[0] * v2[1]) - (v1[1] * v2[0]);
+
+	dst[0] = x;
+	dst[1] = y;
+	dst[2] = z;
+}
+
+}
+
+

gameplay/src/MathUtilNeon.inl

@@ -0,0 +1,228 @@
+namespace gameplay
+{
+
+inline void MathUtil::addMatrix(const float* m, float scalar, float* dst)
+{
+	asm volatile(
+		"vld1.32 {q0, q1}, [%1]! 	\n\t" // M[m0-m7]
+		"vld1.32 {q2, q3}, [%1] 	\n\t" // M[m8-m15]
+		"vld1.32 {d8[0]},  [%2] 	\n\t" // s
+		"vmov.f32 s17, s16          \n\t" // s
+		"vmov.f32 s18, s16          \n\t" // s
+		"vmov.f32 s19, s16          \n\t" // s
+
+		"vadd.f32 q8, q0, q4  		\n\t" // DST->M[m0-m3] = M[m0-m3] + s
+		"vadd.f32 q9, q1, q4 		\n\t" // DST->M[m4-m7] = M[m4-m7] + s
+		"vadd.f32 q10, q2, q4 		\n\t" // DST->M[m8-m11] = M[m8-m11] + s
+		"vadd.f32 q11, q3, q4 		\n\t" // DST->M[m12-m15] = M[m12-m15] + s
+
+		"vst1.32 {q8, q9}, [%0]!  	\n\t" // DST->M[m0-m7]
+		"vst1.32 {q10, q11}, [%0]   \n\t" // DST->M[m8-m15]
+		:
+		: "r"(dst), "r"(m), "r"(&scalar)
+		: "q0", "q1", "q2", "q3", "q4", "q8", "q9", "q10", "q11", "memory"
+	);
+}
+
+inline void MathUtil::addMatrix(const float* m1, const float* m2, float* dst)
+{
+	asm volatile(
+		"vld1.32 	{q0, q1}, 	[%1]! 	\n\t" // M1[m0-m7]
+		"vld1.32 	{q2, q3}, 	[%1] 	\n\t" // M1[m8-m15]
+		"vld1.32 	{q8, q9}, 	[%2]! 	\n\t" // M2[m0-m7]
+		"vld1.32 	{q10, q11}, [%2]  	\n\t" // M2[m8-m15]
+
+		"vadd.f32   q12, q0, q8 		\n\t" // DST->M[m0-m3] = M1[m0-m3] + M2[m0-m3]
+		"vadd.f32   q13, q1, q9			\n\t" // DST->M[m4-m7] = M1[m4-m7] + M2[m4-m7]
+		"vadd.f32   q14, q2, q10		\n\t" // DST->M[m8-m11] = M1[m8-m11] + M2[m8-m11]
+		"vadd.f32   q15, q3, q11		\n\t" // DST->M[m12-m15] = M1[m12-m15] + M2[m12-m15]
+
+		"vst1.32    {q12, q13}, [%0]!   \n\t" // DST->M[m0-m7]
+		"vst1.32    {q14, q15}, [%0]    \n\t" // DST->M[m8-m15]
+		:
+		: "r"(dst), "r"(m1), "r"(m2)
+		: "q0", "q1", "q2", "q3", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15", "memory"
+	);
+}
+
+inline void MathUtil::multiplyMatrix(const float* m, float scalar, float* dst)
+{
+	asm volatile(
+		"vld1.32 	{d0[0]},	 	[%2]     	\n\t" // M[m0-m7]
+		"vld1.32	{q4-q5},  		[%1]!    	\n\t" // M[m8-m15]
+		"vld1.32	{q6-q7},  		[%1]		\n\t" // s
+
+		"vmul.f32 	q8, q4, d0[0]    			\n\t" // DST->M[m0-m3] = M[m0-m3] * s
+		"vmul.f32 	q9, q5, d0[0]    			\n\t" // DST->M[m4-m7] = M[m4-m7] * s
+		"vmul.f32 	q10, q6, d0[0]    			\n\t" // DST->M[m8-m11] = M[m8-m11] * s
+		"vmul.f32 	q11, q7, d0[0]   		 	\n\t" // DST->M[m12-m15] = M[m12-m15] * s
+
+		"vst1.32 	{q8-q9},   		[%0]! 		\n\t" // DST->M[m0-m7]
+		"vst1.32 	{q10-q11}, 		[%0]		\n\t" // DST->M[m8-m15]
+		:
+		: "r"(dst), "r"(m), "r"(&scalar)
+		: "q0", "q4", "q5", "q6", "q7", "q8", "q9", "q10", "q11", "memory"
+	);
+}
+
+inline void MathUtil::multiplyMatrix(const float* m1, const float* m2, float* dst)
+{
+	asm volatile(
+		"vld1.32	 {d16 - d19}, [%1]!	  \n\t"       // M1[m0-m7]
+		"vld1.32     {d20 - d23}, [%1]    \n\t"       // M1[m8-m15]
+		"vld1.32     {d0 - d3}, [%2]!     \n\t"       // M2[m0-m7]
+		"vld1.32     {d4 - d7}, [%2]      \n\t"       // M2[m8-m15]
+
+		"vmul.f32    q12, q8, d0[0]     \n\t"         // DST->M[m0-m3] = M1[m0-m3] * M2[m0]
+		"vmul.f32    q13, q8, d2[0]     \n\t"         // DST->M[m4-m7] = M1[m4-m7] * M2[m4]
+		"vmul.f32    q14, q8, d4[0]     \n\t"         // DST->M[m8-m11] = M1[m8-m11] * M2[m8]
+		"vmul.f32    q15, q8, d6[0]     \n\t"         // DST->M[m12-m15] = M1[m12-m15] * M2[m12]
+
+		"vmla.f32    q12, q9, d0[1]     \n\t"         // DST->M[m0-m3] += M1[m0-m3] * M2[m1]
+		"vmla.f32    q13, q9, d2[1]     \n\t"         // DST->M[m4-m7] += M1[m4-m7] * M2[m5]
+		"vmla.f32    q14, q9, d4[1]     \n\t"         // DST->M[m8-m11] += M1[m8-m11] * M2[m9]
+		"vmla.f32    q15, q9, d6[1]     \n\t"         // DST->M[m12-m15] += M1[m12-m15] * M2[m13]
+
+		"vmla.f32    q12, q10, d1[0]    \n\t"         // DST->M[m0-m3] += M1[m0-m3] * M2[m2]
+		"vmla.f32    q13, q10, d3[0]    \n\t"         // DST->M[m4-m7] += M1[m4-m7] * M2[m6]
+		"vmla.f32    q14, q10, d5[0]    \n\t"         // DST->M[m8-m11] += M1[m8-m11] * M2[m10]
+		"vmla.f32    q15, q10, d7[0]    \n\t"         // DST->M[m12-m15] += M1[m12-m15] * M2[m14]
+
+		"vmla.f32    q12, q11, d1[1]    \n\t"         // DST->M[m0-m3] += M1[m0-m3] * M2[m3]
+		"vmla.f32    q13, q11, d3[1]    \n\t"         // DST->M[m4-m7] += M1[m4-m7] * M2[m7]
+		"vmla.f32    q14, q11, d5[1]    \n\t"         // DST->M[m8-m11] += M1[m8-m11] * M2[m11]
+		"vmla.f32    q15, q11, d7[1]    \n\t"         // DST->M[m12-m15] += M1[m12-m15] * M2[m15]
+
+		"vst1.32    {d24 - d27}, [%0]!    \n\t"       // DST->M[m0-m7]
+		"vst1.32    {d28 - d31}, [%0]     \n\t"       // DST->M[m8-m15]
+
+		: // output
+		: "r"(dst), "r"(m1), "r"(m2) // input - note *value* of pointer doesn't change.
+		: "memory", "q0", "q1", "q2", "q3", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15"
+	);
+}
+
+inline void MathUtil::negateMatrix(const float* m, float* dst)
+{
+	asm volatile(
+		"vld1.32 	{q0-q1},  [%1]! 	\n\t" // load m0-m7
+		"vld1.32 	{q2-q3},  [%1]   	\n\t" // load m8-m15
+
+		"vneg.f32 	q4, q0 				\n\t" // negate m0-m3
+		"vneg.f32 	q5, q1 				\n\t" // negate m4-m7
+		"vneg.f32 	q6, q2 				\n\t" // negate m8-m15
+		"vneg.f32 	q7, q3 				\n\t" // negate m8-m15
+
+		"vst1.32 	{q4-q5},  [%0]!		\n\t" // store m0-m7
+		"vst1.32 	{q6-q7},  [%0]		\n\t" // store m8-m15
+		:
+		: "r"(dst), "r"(m)
+		: "q0", "q1", "q2", "q3", "q4", "q5", "q6", "q7", "memory"
+	);
+}
+
+inline void MathUtil::subtractMatrix(const float* m1, const float* m2, float* dst)
+{
+	asm volatile(
+		"vld1.32 	{q0, q1}, 	[%1]! 	\n\t" // M1[m0-m7]
+		"vld1.32 	{q2, q3}, 	[%1] 	\n\t" // M1[m8-m15]
+		"vld1.32 	{q8, q9}, 	[%2]! 	\n\t" // M2[m0-m7]
+		"vld1.32 	{q10, q11}, [%2] 	\n\t" // M2[m8-m15]
+
+		"vsub.f32   q12, q0, q8 		\n\t" // DST->M[m0-m3] = M1[m0-m3] - M2[m0-m3]
+		"vsub.f32   q13, q1, q9			\n\t" // DST->M[m4-m7] = M1[m4-m7] - M2[m4-m7]
+		"vsub.f32   q14, q2, q10		\n\t" // DST->M[m8-m11] = M1[m8-m11] - M2[m8-m11]
+		"vsub.f32   q15, q3, q11		\n\t" // DST->M[m12-m15] = M1[m12-m15] - M2[m12-m15]
+
+		"vst1.32    {q12, q13}, [%0]!   \n\t" // DST->M[m0-m7]
+		"vst1.32    {q14, q15}, [%0]    \n\t" // DST->M[m8-m15]
+		:
+		: "r"(dst), "r"(m1), "r"(m2)
+		: "q0", "q1", "q2", "q3", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15", "memory"
+	);
+}
+
+inline void MathUtil::transformVectorMatrix(const float* m, float x, float y, float z, float w, float* dst)
+{
+	asm volatile(
+		"vld1.32	{d0[0]},		[%1]	\n\t"	// V[x]
+		"vld1.32	{d0[1]},    	[%2]	\n\t"	// V[y]
+		"vld1.32	{d1[0]},		[%3]	\n\t"	// V[z]
+		"vld1.32	{d1[1]},		[%4]	\n\t"	// V[w]
+		"vld1.32	{d18 - d21},	[%5]!	\n\t"	// M[m0-m7]
+		"vld1.32	{d22 - d25},	[%5]	\n\t"	// M[m8-m15]
+
+		"vmul.f32 q13,  q9, d0[0]			\n\t"	// DST->V = M[m0-m3] * V[x]
+		"vmla.f32 q13, q10, d0[1]      		\n\t"	// DST->V += M[m4-m7] * V[y]
+		"vmla.f32 q13, q11, d1[0]      		\n\t"	// DST->V += M[m8-m11] * V[z]
+		"vmla.f32 q13, q12, d1[1]      		\n\t"	// DST->V += M[m12-m15] * V[w]
+
+		"vst1.32 {d26}, [%0]!        		\n\t"	// DST->V[x, y]
+		"vst1.32 {d27[0]}, [%0]        		\n\t"	// DST->V[z]
+		:
+		: "r"(dst), "r"(&x), "r"(&y), "r"(&z), "r"(&w), "r"(m)
+		: "q0", "q9", "q10","q11", "q12", "q13", "memory"
+	);
+}
+
+inline void MathUtil::transformVectorMatrix(const float* m, const float* v, float* dst)
+{
+	asm volatile
+	(
+		"vld1.32	{d0, d1}, [%1]		\n\t"   // V[x, y, z, w]
+		"vld1.32    {d18 - d21}, [%2]!  \n\t"   // M[m0-m7]
+		"vld1.32    {d22 - d25}, [%2]  \n\t"    // M[m8-m15]
+
+		"vmul.f32   q13, q9, d0[0]      \n\t"   // DST->V = M[m0-m3] * V[x]
+		"vmla.f32   q13, q10, d0[1]     \n\t"   // DST->V = M[m4-m7] * V[y]
+		"vmla.f32   q13, q11, d1[0]     \n\t"   // DST->V = M[m8-m11] * V[z]
+		"vmla.f32   q13, q12, d1[1]     \n\t"   // DST->V = M[m12-m15] * V[w]
+
+		"vst1.32    {d26, d27}, [%0]    \n\t"   // DST->V
+		:
+		: "r"(dst), "r"(v), "r"(m)
+		: "q0", "q9", "q10","q11", "q12", "q13", "memory"
+	);
+}
+
+inline void MathUtil::transpose(const float* m, float* dst)
+{
+	asm volatile(
+		"vld4.32 {d0[0], d2[0], d4[0], d6[0]}, [%1]! 	\n\t" // DST->M[m0, m4, m8, m12] = M[m0-m3]
+		"vld4.32 {d0[1], d2[1], d4[1], d6[1]}, [%1]!	\n\t" // DST->M[m1, m5, m9, m12] = M[m4-m7]
+		"vld4.32 {d1[0], d3[0], d5[0], d7[0]}, [%1]!	\n\t" // DST->M[m2, m6, m10, m12] = M[m8-m11]
+		"vld4.32 {d1[1], d3[1], d5[1], d7[1]}, [%1] 	\n\t" // DST->M[m3, m7, m11, m12] = M[m12-m15]
+
+		"vst1.32 {q0-q1}, [%0]! 						\n\t" // DST->M[m0-m7]
+		"vst1.32 {q2-q3}, [%0] 							\n\t"  // DST->M[m8-m15]
+		:
+		: "r"(dst), "r"(m)
+		: "q0", "q1", "q2", "q3", "memory"
+	);
+}
+
+inline void MathUtil::crossVector3(const float* v1, const float* v2, float* dst)
+{
+	asm volatile(
+		"vld1.32 {d1[1]},  [%1] 		\n\t" //
+		"vld1.32 {d0},     [%2]         \n\t" //
+		"vmov.f32 s2, s1                \n\t" // q0 = (v1y, v1z, v1z, v1x)
+
+		"vld1.32 {d2[1]},  [%3]	    	\n\t" //
+		"vld1.32 {d3},     [%4]         \n\t" //
+		"vmov.f32 s4, s7          		\n\t" // q1 = (v2z, v2x, v2y, v2z)
+
+		"vmul.f32 d4, d0, d2  			\n\t" // x = v1y * v2z, y = v1z * v2x
+		"vmls.f32 d4, d1, d3  			\n\t" // x -= v1z * v2y, y-= v1x - v2z
+
+		"vmul.f32 d5, d3, d1[1]			\n\t" // z = v1x * v2y
+		"vmls.f32 d5, d0, d2[1]         \n\t" // z-= v1y * vx
+
+		"vst1.32 {d4}, 	  [%0]!    		\n\t" // V[x, y]
+		"vst1.32 {d5[0]}, [%0]     		\n\t" // V[z]
+		:
+		: "r"(dst), "r"(v1), "r"((v1+1)), "r"(v2), "r"((v2+1))
+		: "q0", "q1", "q2", "memory"
+	);
+}
+}

gameplay/src/Matrix.cpp

@@ -1,6 +1,7 @@
 #include "Base.h"
 #include "Matrix.h"
 #include "Quaternion.h"
+#include "MathUtil.h"
 
 #define MATRIX_SIZE     ( sizeof(float) * 16 )
 
@@ -360,6 +361,29 @@ void Matrix::add(float scalar)
 void Matrix::add(float scalar, Matrix* dst)
 {
     GP_ASSERT(dst);
+/*
+#ifdef USE_NEON
+
+    asm volatile(
+    	"vld1.32 {q0, q1}, [%1]! 	\n\t" // M[m0-m7]
+    	"vld1.32 {q2, q3}, [%1] 	\n\t" // M[m8-m15]
+    	"vld1.32 {d8[0]},  [%2] 	\n\t" // s
+    	"vmov.f32 s17, s16          \n\t" // s
+    	"vmov.f32 s18, s16          \n\t" // s
+    	"vmov.f32 s19, s16          \n\t" // s
+    	"vadd.f32 q8, q0, q4  	\n\t" // DST->M[m0-m3] = M[m0-m3] + s
+    	"vadd.f32 q9, q1, q4 	\n\t" // DST->M[m4-m7] = M[m4-m7] + s
+    	"vadd.f32 q10, q2, q4 	\n\t" // DST->M[m8-m11] = M[m8-m11] + s
+    	"vadd.f32 q11, q3, q4 	\n\t" // DST->M[m12-m15] = M[m12-m15] + s
+
+    	"vst1.32 {q8, q9}, [%0]!  \n\t" // DST->M[m0-m7]
+    	"vst1.32 {q10, q11}, [%0]   \n\t" // DST->M[m8-m15]
+    	:
+    	: "r"(dst->m), "r"(m), "r"(&scalar)
+    	: "q0", "q1", "q2", "q3", "q4", "q8", "q9", "q10", "q11", "memory"
+    );
+
+#else
 
     dst->m[0]  = m[0]  + scalar;
     dst->m[1]  = m[1]  + scalar;
@@ -377,6 +401,10 @@ void Matrix::add(float scalar, Matrix* dst)
     dst->m[13] = m[13] + scalar;
     dst->m[14] = m[14] + scalar;
     dst->m[15] = m[15] + scalar;
+
+#endif*/
+
+    MathUtil::addMatrix(m, scalar, dst->m);
 }
 
 void Matrix::add(const Matrix& m)
@@ -387,20 +415,22 @@ void Matrix::add(const Matrix& m)
 void Matrix::add(const Matrix& m1, const Matrix& m2, Matrix* dst)
 {
     GP_ASSERT(dst);
-
+/*
 #ifdef USE_NEON
 
     asm volatile(
-    	"vld1.32 	{q0, q1}, 	[%1]! 	\n\t"
-		"vld1.32 	{q2, q3}, 	[%1]! 	\n\t"
-    	"vld1.32 	{q8, q9}, 	[%2]! 	\n\t"
-		"vld1.32 	{q10, q11}, [%2]! 	\n\t"
-		"vadd.f32   q12, q0, q8 		\n\t"
-    	"vadd.f32   q13, q1, q9			\n\t"
-    	"vadd.f32   q14, q2, q10		\n\t"
-    	"vadd.f32   q15, q3, q11		\n\t"
-    	"vst1.32    {q12, q13}, [%0]!   \n\t"
-		"vst1.32    {q14, q15}, [%0]!   \n\t"
+    	"vld1.32 	{q0, q1}, 	[%1]! 	\n\t" // M1[m0-m7]
+		"vld1.32 	{q2, q3}, 	[%1] 	\n\t" // M1[m8-m15]
+    	"vld1.32 	{q8, q9}, 	[%2]! 	\n\t" // M2[m0-m7]
+		"vld1.32 	{q10, q11}, [%2]  	\n\t" // M2[m8-m15]
+
+    	"vadd.f32   q12, q0, q8 		\n\t" // DST->M[m0-m3] = M1[m0-m3] + M2[m0-m3]
+    	"vadd.f32   q13, q1, q9			\n\t" // DST->M[m4-m7] = M1[m4-m7] + M2[m4-m7]
+    	"vadd.f32   q14, q2, q10		\n\t" // DST->M[m8-m11] = M1[m8-m11] + M2[m8-m11]
+    	"vadd.f32   q15, q3, q11		\n\t" // DST->M[m12-m15] = M1[m12-m15] + M2[m12-m15]
+
+    	"vst1.32    {q12, q13}, [%0]!   \n\t" // DST->M[m0-m7]
+		"vst1.32    {q14, q15}, [%0]    \n\t" // DST->M[m8-m15]
 		:
         : "r"(dst->m), "r"(m1.m), "r"(m2.m)
         : "q0", "q1", "q2", "q3", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15", "memory"
@@ -425,7 +455,9 @@ void Matrix::add(const Matrix& m1, const Matrix& m2, Matrix* dst)
     dst->m[14] = m1.m[14] + m2.m[14];
     dst->m[15] = m1.m[15] + m2.m[15];
 
-#endif
+#endif*/
+
+    MathUtil::addMatrix(m1.m, m2.m, dst->m);
 }
 
 bool Matrix::decompose(Vector3* scale, Quaternion* rotation, Vector3* translation) const
@@ -696,21 +728,21 @@ void Matrix::multiply(float scalar, Matrix* dst) const
 void Matrix::multiply(const Matrix& m, float scalar, Matrix* dst)
 {
     GP_ASSERT(dst);
-
+/*
 #ifdef USE_NEON
 
     asm volatile(
-    	"vld1.32 	{d0[0]},	 	[%0]     	\n\t"
-    	"vld1.32	{q4-q5},  		[%1]!    	\n\t"
-		"vld1.32	{q6-q7},  		[%1]!		\n\t"
+    	"vld1.32 	{d0[0]},	 	[%0]     	\n\t" // M[m0-m7]
+    	"vld1.32	{q4-q5},  		[%1]!    	\n\t" // M[m8-m15]
+		"vld1.32	{q6-q7},  		[%1]		\n\t" // s
 
-    	"vmul.f32 	q8, q4, d0[0]    			\n\t"
-    	"vmul.f32 	q9, q5, d0[0]    			\n\t"
-		"vmul.f32 	q10, q6, d0[0]    			\n\t"
-		"vmul.f32 	q11, q7, d0[0]   		 	\n\t"
+    	"vmul.f32 	q8, q4, d0[0]    			\n\t" // DST->M[m0-m3] = M[m0-m3] * s
+    	"vmul.f32 	q9, q5, d0[0]    			\n\t" // DST->M[m4-m7] = M[m4-m7] * s
+		"vmul.f32 	q10, q6, d0[0]    			\n\t" // DST->M[m8-m11] = M[m8-m11] * s
+		"vmul.f32 	q11, q7, d0[0]   		 	\n\t" // DST->M[m12-m15] = M[m12-m15] * s
 
-    	"vst1.32 	{q8-q9},   		[%2]! 		\n\t"
-		"vst1.32 	{q10-q11}, 		[%2]!		\n\t"
+    	"vst1.32 	{q8-q9},   		[%2]! 		\n\t" // DST->M[m0-m7]
+		"vst1.32 	{q10-q11}, 		[%2]		\n\t" // DST->M[m8-m15]
 		:
 		: "r"(&scalar), "r"(m.m), "r"(dst->m)
 		: "q0", "q4", "q5", "q6", "q7", "q8", "q9", "q10", "q11", "memory"
@@ -735,7 +767,8 @@ void Matrix::multiply(const Matrix& m, float scalar, Matrix* dst)
     dst->m[14] = m.m[14] * scalar;
     dst->m[15] = m.m[15] * scalar;
 
-#endif
+#endif*/
+    MathUtil::multiplyMatrix(m.m, scalar, dst->m);
 }
 
 void Matrix::multiply(const Matrix& m)
@@ -746,38 +779,38 @@ void Matrix::multiply(const Matrix& m)
 void Matrix::multiply(const Matrix& m1, const Matrix& m2, Matrix* dst)
 {
 	GP_ASSERT(dst);
-
+/*
 #ifdef USE_NEON // if set, neon unit is present.
 
     asm volatile
     (
-        "vld1.32	 {d16 - d19}, [%1]!	\n\t"         // load first eight elements of matrix 0
-		"vld1.32     {d20 - d23}, [%1]!   \n\t"         // load second eight elements of matrix 0
-		"vld1.32     {d0 - d3}, [%2]!     \n\t"         // load first eight elements of matrix 1
-		"vld1.32     {d4 - d7}, [%2]!     \n\t"         // load second eight elements of matrix 1
-
-		"vmul.f32    q12, q8, d0[0]     \n\t"         // rslt col0  = (mat0 col0) * (mat1 col0 elt0)
-		"vmul.f32    q13, q8, d2[0]     \n\t"         // rslt col1  = (mat0 col0) * (mat1 col1 elt0)
-		"vmul.f32    q14, q8, d4[0]     \n\t"         // rslt col2  = (mat0 col0) * (mat1 col2 elt0)
-		"vmul.f32    q15, q8, d6[0]     \n\t"         // rslt col3  = (mat0 col0) * (mat1 col3 elt0)
-
-		"vmla.f32    q12, q9, d0[1]     \n\t"         // rslt col0 += (mat0 col1) * (mat1 col0 elt1)
-		"vmla.f32    q13, q9, d2[1]     \n\t"         // rslt col1 += (mat0 col1) * (mat1 col1 elt1)
-		"vmla.f32    q14, q9, d4[1]     \n\t"         // rslt col2 += (mat0 col1) * (mat1 col2 elt1)
-		"vmla.f32    q15, q9, d6[1]     \n\t"         // rslt col3 += (mat0 col1) * (mat1 col3 elt1)
-
-		"vmla.f32    q12, q10, d1[0]    \n\t"         // rslt col0 += (mat0 col2) * (mat1 col0 elt2)
-		"vmla.f32    q13, q10, d3[0]    \n\t"         // rslt col1 += (mat0 col2) * (mat1 col1 elt2)
-		"vmla.f32    q14, q10, d5[0]    \n\t"         // rslt col2 += (mat0 col2) * (mat1 col2 elt2)
-		"vmla.f32    q15, q10, d7[0]    \n\t"         // rslt col3 += (mat0 col2) * (mat1 col2 elt2)
-
-		"vmla.f32    q12, q11, d1[1]    \n\t"         // rslt col0 += (mat0 col3) * (mat1 col0 elt3)
-		"vmla.f32    q13, q11, d3[1]    \n\t"         // rslt col1 += (mat0 col3) * (mat1 col1 elt3)
-		"vmla.f32    q14, q11, d5[1]    \n\t"         // rslt col2 += (mat0 col3) * (mat1 col2 elt3)
-		"vmla.f32    q15, q11, d7[1]    \n\t"         // rslt col3 += (mat0 col3) * (mat1 col3 elt3)
-
-		"vst1.32    {d24 - d27}, [%0]!    \n\t"         // store first eight elements of result
-		"vst1.32    {d28 - d31}, [%0]!    \n\t"         // store second eight elements of result
+        "vld1.32	 {d16 - d19}, [%1]!	  \n\t"       // M1[m0-m7]
+		"vld1.32     {d20 - d23}, [%1]    \n\t"       // M1[m8-m15]
+		"vld1.32     {d0 - d3}, [%2]!     \n\t"       // M2[m0-m7]
+		"vld1.32     {d4 - d7}, [%2]      \n\t"       // M2[m8-m15]
+
+		"vmul.f32    q12, q8, d0[0]     \n\t"         // DST->M[m0-m3] = M1[m0-m3] * M2[m0]
+		"vmul.f32    q13, q8, d2[0]     \n\t"         // DST->M[m4-m7] = M1[m4-m7] * M2[m4]
+		"vmul.f32    q14, q8, d4[0]     \n\t"         // DST->M[m8-m11] = M1[m8-m11] * M2[m8]
+		"vmul.f32    q15, q8, d6[0]     \n\t"         // DST->M[m12-m15] = M1[m12-m15] * M2[m12]
+
+		"vmla.f32    q12, q9, d0[1]     \n\t"         // DST->M[m0-m3] += M1[m0-m3] * M2[m1]
+		"vmla.f32    q13, q9, d2[1]     \n\t"         // DST->M[m4-m7] += M1[m4-m7] * M2[m5]
+		"vmla.f32    q14, q9, d4[1]     \n\t"         // DST->M[m8-m11] += M1[m8-m11] * M2[m9]
+		"vmla.f32    q15, q9, d6[1]     \n\t"         // DST->M[m12-m15] += M1[m12-m15] * M2[m13]
+
+		"vmla.f32    q12, q10, d1[0]    \n\t"         // DST->M[m0-m3] += M1[m0-m3] * M2[m2]
+		"vmla.f32    q13, q10, d3[0]    \n\t"         // DST->M[m4-m7] += M1[m4-m7] * M2[m6]
+		"vmla.f32    q14, q10, d5[0]    \n\t"         // DST->M[m8-m11] += M1[m8-m11] * M2[m10]
+		"vmla.f32    q15, q10, d7[0]    \n\t"         // DST->M[m12-m15] += M1[m12-m15] * M2[m14]
+
+		"vmla.f32    q12, q11, d1[1]    \n\t"         // DST->M[m0-m3] += M1[m0-m3] * M2[m3]
+		"vmla.f32    q13, q11, d3[1]    \n\t"         // DST->M[m4-m7] += M1[m4-m7] * M2[m7]
+		"vmla.f32    q14, q11, d5[1]    \n\t"         // DST->M[m8-m11] += M1[m8-m11] * M2[m11]
+		"vmla.f32    q15, q11, d7[1]    \n\t"         // DST->M[m12-m15] += M1[m12-m15] * M2[m15]
+
+		"vst1.32    {d24 - d27}, [%0]!    \n\t"       // DST->M[m0-m7]
+		"vst1.32    {d28 - d31}, [%0]     \n\t"       // DST->M[m8-m15]
         
         : // output
         : "r"(dst->m), "r"(m1.m), "r"(m2.m) // input - note *value* of pointer doesn't change.
@@ -811,7 +844,9 @@ void Matrix::multiply(const Matrix& m1, const Matrix& m2, Matrix* dst)
 
     memcpy(dst->m, product, MATRIX_SIZE);
 
-#endif
+#endif*/
+
+	MathUtil::multiplyMatrix(m1.m, m2.m, dst->m);
 }
 
 void Matrix::negate()
@@ -822,12 +857,12 @@ void Matrix::negate()
 void Matrix::negate(Matrix* dst) const
 {
     GP_ASSERT(dst);
-
+/*
 #ifdef USE_NEON
 
     asm volatile(
     	"vld1.32 	{q0-q1},  [%1]! 	\n\t" // load m0-m7
-    	"vld1.32 	{q2-q3},  [%1]! 	\n\t" // load m8-m15
+    	"vld1.32 	{q2-q3},  [%1]   	\n\t" // load m8-m15
 
     	"vneg.f32 	q4, q0 				\n\t" // negate m0-m3
     	"vneg.f32 	q5, q1 				\n\t" // negate m4-m7
@@ -835,7 +870,7 @@ void Matrix::negate(Matrix* dst) const
 		"vneg.f32 	q7, q3 				\n\t" // negate m8-m15
 
     	"vst1.32 	{q4-q5},  [%0]!		\n\t" // store m0-m7
-    	"vst1.32 	{q6-q7},  [%0]!		\n\t" // store m8-m15
+    	"vst1.32 	{q6-q7},  [%0]		\n\t" // store m8-m15
     	:
     	: "r"(dst->m), "r"(m)
     	: "q0", "q1", "q2", "q3", "q4", "q5", "q6", "q7", "memory"
@@ -861,6 +896,8 @@ void Matrix::negate(Matrix* dst) const
     dst->m[15] = -m[15];
 
 #endif
+*/
+    MathUtil::negateMatrix(m, dst->m);
 }
 
 void Matrix::rotate(const Quaternion& q)
@@ -1004,20 +1041,22 @@ void Matrix::subtract(const Matrix& m)
 void Matrix::subtract(const Matrix& m1, const Matrix& m2, Matrix* dst)
 {
     GP_ASSERT(dst);
-
+/*
 #ifdef USE_NEON
 
     asm volatile(
-    	"vld1.32 	{q0, q1}, 	[%1]! 	\n\t"
-		"vld1.32 	{q2, q3}, 	[%1]! 	\n\t"
-    	"vld1.32 	{q8, q9}, 	[%2]! 	\n\t"
-		"vld1.32 	{q10, q11}, [%2]! 	\n\t"
-		"vsub.f32   q12, q0, q8 		\n\t"
-    	"vsub.f32   q13, q1, q9			\n\t"
-    	"vsub.f32   q14, q2, q10		\n\t"
-    	"vsub.f32   q15, q3, q11		\n\t"
-    	"vst1.32    {q12, q13}, [%0]!   \n\t"
-		"vst1.32    {q14, q15}, [%0]!   \n\t"
+    	"vld1.32 	{q0, q1}, 	[%1]! 	\n\t" // M1[m0-m7]
+		"vld1.32 	{q2, q3}, 	[%1] 	\n\t" // M1[m8-m15]
+    	"vld1.32 	{q8, q9}, 	[%2]! 	\n\t" // M2[m0-m7]
+		"vld1.32 	{q10, q11}, [%2] 	\n\t" // M2[m8-m15]
+
+		"vsub.f32   q12, q0, q8 		\n\t" // DST->M[m0-m3] = M1[m0-m3] - M2[m0-m3]
+    	"vsub.f32   q13, q1, q9			\n\t" // DST->M[m4-m7] = M1[m4-m7] - M2[m4-m7]
+    	"vsub.f32   q14, q2, q10		\n\t" // DST->M[m8-m11] = M1[m8-m11] - M2[m8-m11]
+    	"vsub.f32   q15, q3, q11		\n\t" // DST->M[m12-m15] = M1[m12-m15] - M2[m12-m15]
+
+    	"vst1.32    {q12, q13}, [%0]!   \n\t" // DST->M[m0-m7]
+		"vst1.32    {q14, q15}, [%0]    \n\t" // DST->M[m8-m15]
 		:
         : "r"(dst->m), "r"(m1.m), "r"(m2.m)
         : "q0", "q1", "q2", "q3", "q8", "q9", "q10", "q11", "q12", "q13", "q14", "q15", "memory"
@@ -1044,6 +1083,8 @@ void Matrix::subtract(const Matrix& m1, const Matrix& m2, Matrix* dst)
     dst->m[15] = m1.m[15] - m2.m[15];
 
 #endif
+*/
+    MathUtil::subtractMatrix(m1.m, m2.m, dst->m);
 }
 
 void Matrix::transformPoint(Vector3* point) const
@@ -1071,25 +1112,24 @@ void Matrix::transformVector(const Vector3& vector, Vector3* dst) const
 void Matrix::transformVector(float x, float y, float z, float w, Vector3* dst) const
 {
     GP_ASSERT(dst);
-
+/*
 #ifdef USE_NEON
 
     asm volatile(
-    	"vld1.32	{d0[0]},		[%0]	\n\t"	// load x
-		"vld1.32	{d0[1]},    	[%1]	\n\t"	// load y
-		"vld1.32	{d1[0]},		[%2]	\n\t"	// load z
-		"vld1.32	{d1[1]},		[%3]	\n\t"	// load w
-		"vld1.32	{d18 - d21},	[%4]!	\n\t"	// load first 8 elements of matrix m0-m7
-		"vld1.32	{d22 - d25},	[%4]!	\n\t"	// load second 8 elements of matrix m8-m15
-
-    	"vmul.f32 q13,  q9, d0[0]			\n\t"	// Q5 =  (m0-m3)*x
-    	"vmla.f32 q13, q10, d0[1]      		\n\t"	// Q5 +=  (m4-m7)*y
-    	"vmla.f32 q13, q11, d1[0]      		\n\t"	// Q5 +=  (m8-m11)*z
-		"vmla.f32 q13, q12, d1[1]      		\n\t"	// Q5 +=  (m12-m15)*w
-
-    	"vst1.32 {d26[0]}, [%5]!        	\n\t"	// store dst->x
-		"vst1.32 {d26[1]}, [%5]!        	\n\t"	// store dst->y
-		"vst1.32 {d27[0]}, [%5]!        	\n\t"	// store dst->z
+    	"vld1.32	{d0[0]},		[%0]	\n\t"	// V[x]
+		"vld1.32	{d0[1]},    	[%1]	\n\t"	// V[y]
+		"vld1.32	{d1[0]},		[%2]	\n\t"	// V[z]
+		"vld1.32	{d1[1]},		[%3]	\n\t"	// V[w]
+		"vld1.32	{d18 - d21},	[%4]!	\n\t"	// M[m0-m7]
+		"vld1.32	{d22 - d25},	[%4]	\n\t"	// M[m8-m15]
+
+    	"vmul.f32 q13,  q9, d0[0]			\n\t"	// DST->V = M[m0-m3] * V[x]
+    	"vmla.f32 q13, q10, d0[1]      		\n\t"	// DST->V += M[m4-m7] * V[y]
+    	"vmla.f32 q13, q11, d1[0]      		\n\t"	// DST->V += M[m8-m11] * V[z]
+		"vmla.f32 q13, q12, d1[1]      		\n\t"	// DST->V += M[m12-m15] * V[w]
+
+    	"vst1.32 {d26}, [%5]!        		\n\t"	// DST->V[x, y]
+		"vst1.32 {d27[0]}, [%5]        		\n\t"	// DST->V[z]
 		:
     	: "r"(&x), "r"(&y), "r"(&z), "r"(&w), "r"(m), "r"(dst)
         : "q0", "q9", "q10","q11", "q12", "q13", "memory"
@@ -1102,7 +1142,8 @@ void Matrix::transformVector(float x, float y, float z, float w, Vector3* dst) c
         x * m[1] + y * m[5] + z * m[9] + w * m[13],
         x * m[2] + y * m[6] + z * m[10] + w * m[14]);
 
-#endif
+#endif*/
+    MathUtil::transformVectorMatrix(m, x, y, z, w, (float*)dst);
 }
 
 void Matrix::transformVector(Vector4* vector) const
@@ -1114,20 +1155,22 @@ void Matrix::transformVector(Vector4* vector) const
 void Matrix::transformVector(const Vector4& vector, Vector4* dst) const
 {
     GP_ASSERT(dst);
-
+/*
 #ifdef USE_NEON
 
     asm volatile
     (
-    		"vld1.32	{d0, d1}, [%1]		\n\t"   //Q0 = v (x, y, z, w)
-    		"vld1.32    {d18 - d21}, [%0]!  \n\t"   //Q1 = M (m0-m7)
-    		"vld1.32    {d22 - d25}, [%0]!  \n\t"   //Q2 = M (m8-m15)
-
-    		"vmul.f32   q13, q9, d0[0]      \n\t"   //Q5 =  Q0*Q0[0]
-    		"vmla.f32   q13, q10, d0[1]     \n\t"   //Q5 += Q1*Q0[1]
-    		"vmla.f32   q13, q11, d1[0]     \n\t"   //Q5 += Q2*Q0[2]
-    		"vmla.f32   q13, q12, d1[1]     \n\t"   //Q5 += Q3*Q0[3]
-    		"vst1.32    {d26, d27}, [%2]    \n\t"   //Q4 = m+12
+    		"vld1.32	{d0, d1}, [%1]		\n\t"   // V[x, y, z, w]
+    		"vld1.32    {d18 - d21}, [%0]!  \n\t"   // M[m0-m7]
+    		"vld1.32    {d22 - d25}, [%0]  \n\t"    // M[m8-m15]
+
+    		"vmul.f32   q13, q9, d0[0]      \n\t"   // DST->V = M[m0-m3] * V[x]
+    		"vmla.f32   q13, q10, d0[1]     \n\t"   // DST->V = M[m4-m7] * V[y]
+    		"vmla.f32   q13, q11, d1[0]     \n\t"   // DST->V = M[m8-m11] * V[z]
+    		"vmla.f32   q13, q12, d1[1]     \n\t"   // DST->V = M[m12-m15] * V[w]
+
+    		"vst1.32    {d26, d27}, [%2]    \n\t"   // DST->V
+
     		:
     		: "r"(m), "r"(&vector), "r"(dst)
     		: "q0", "q9", "q10","q11", "q12", "q13", "memory"
@@ -1138,10 +1181,12 @@ void Matrix::transformVector(const Vector4& vector, Vector4* dst) const
     dst->set(
         vector.x * m[0] + vector.y * m[4] + vector.z * m[8] + vector.w * m[12],
         vector.x * m[1] + vector.y * m[5] + vector.z * m[9] + vector.w * m[13],
-        vector.x * m[2] + vector.y * m[6] + vector.z * m[10] + vector.w * m[14],
+        vector.x * m[2] + vector.y * m[6] + vector.z * m[10] + vector.w * m[14],
         vector.x * m[3] + vector.y * m[7] + vector.z * m[11] + vector.w * m[15]);
 
-#endif
+#endif
+*/
+    MathUtil::transformVectorMatrix(m, (const float*) &vector, (float*)dst);
 }
 
 void Matrix::translate(float x, float y, float z)
@@ -1178,13 +1223,13 @@ void Matrix::transpose(Matrix* dst) const
 #ifdef USE_NEON
     
     asm volatile(
-    	"vld4.32 {d0[0], d2[0], d4[0], d6[0]}, [%0]! \n\t"
-		"vld4.32 {d0[1], d2[1], d4[1], d6[1]}, [%0]! \n\t"
-		"vld4.32 {d1[0], d3[0], d5[0], d7[0]}, [%0]! \n\t"
-		"vld4.32 {d1[1], d3[1], d5[1], d7[1]}, [%0]! \n\t"
+    	"vld4.32 {d0[0], d2[0], d4[0], d6[0]}, [%0]! \n\t" // DST->M[m0, m4, m8, m12] = M[m0-m3]
+		"vld4.32 {d0[1], d2[1], d4[1], d6[1]}, [%0]! \n\t" // DST->M[m1, m5, m9, m12] = M[m4-m7]
+		"vld4.32 {d1[0], d3[0], d5[0], d7[0]}, [%0]! \n\t" // DST->M[m2, m6, m10, m12] = M[m8-m11]
+		"vld4.32 {d1[1], d3[1], d5[1], d7[1]}, [%0]  \n\t" // DST->M[m3, m7, m11, m12] = M[m12-m15]
 
-		"vst1.32 {q0-q1}, [%1]! \n\t"
-		"vst1.32 {q2-q3}, [%1]! \n\t"
+		"vst1.32 {q0-q1}, [%1]! \n\t" // DST->M[m0-m7]
+		"vst1.32 {q2-q3}, [%1] \n\t"  // DST->M[m8-m15]
     	:
     	: "r"(this->m), "r"(dst->m)
     	: "q0", "q1", "q2", "q3", "memory"

gameplay/src/Vector3.cpp

@@ -1,5 +1,6 @@
 #include "Base.h"
 #include "Vector3.h"
+#include "MathUtil.h"
 
 namespace gameplay
 {
@@ -165,17 +166,38 @@ void Vector3::clamp(const Vector3& v, const Vector3& min, const Vector3& max, Ve
 
 void Vector3::cross(const Vector3& v)
 {
-    float tx = (y * v.z) - (z * v.y);
-    float ty = (z * v.x) - (x * v.z);
-    float tz = (x * v.y) - (y * v.x);
-    x = tx;
-    y = ty;
-    z = tz;
+    cross(*this, v, this);
 }
 
 void Vector3::cross(const Vector3& v1, const Vector3& v2, Vector3* dst)
 {
     GP_ASSERT(dst);
+/*
+#ifdef USE_NEON
+
+    asm volatile(
+		"vld1.32 {d1[1]},  [%1] 		\n\t" //
+		"vld1.32 {d0},     [%2]         \n\t" //
+		"vmov.f32 s2, s1                \n\t" // q0 = (v1y, v1z, v1z, v1x)
+
+		"vld1.32 {d2[1]},  [%3]	    	\n\t" //
+		"vld1.32 {d3},     [%4]         \n\t" //
+		"vmov.f32 s4, s7          		\n\t" // q1 = (v2z, v2x, v2y, v2z)
+
+		"vmul.f32 d4, d0, d2  			\n\t" // x = v1y * v2z, y = v1z * v2x
+		"vmls.f32 d4, d1, d3  			\n\t" // x -= v1z * v2y, y-= v1x - v2z
+
+		"vmul.f32 d5, d3, d1[1]			\n\t" // z = v1x * v2y
+		"vmls.f32 d5, d0, d2[1]         \n\t" // z-= v1y * vx
+
+		"vst1.32 {d4}, 	  [%0]!    		\n\t" // V[x, y]
+		"vst1.32 {d5[0]}, [%0]     		\n\t" // V[z]
+		:
+		: "r"(dst), "r"(&v1), "r"(&(v1.y)), "r"(&v2), "r"(&(v2.y))
+		: "q0", "q1", "q2", "memory"
+	);
+
+#else
 
     float x = (v1.y * v2.z) - (v1.z * v2.y);
     float y = (v1.z * v2.x) - (v1.x * v2.z);
@@ -183,6 +205,10 @@ void Vector3::cross(const Vector3& v1, const Vector3& v2, Vector3* dst)
     dst->x = x;
     dst->y = y;
     dst->z = z;
+
+#endif
+*/
+    MathUtil::crossVector3((const float*)&v1, (const float*)&v2, (float*)dst);
 }
 
 float Vector3::distance(const Vector3& v) const