Added quaternion functions.

include/bx/fpumath.h

@@ -18,7 +18,7 @@ namespace bx
 
 	inline float toRad(float _deg)
 	{
-		return _deg * pi / 180.0f;;
+		return _deg * pi / 180.0f;
 	}
 
 	inline float toDeg(float _rad)
@@ -91,6 +91,21 @@ namespace bx
 		return _a - floorf(_a);
 	}
 
+	inline bool fequal(float _a, float _b, float _epsilon)
+	{
+		return fabsolute(_a - _b) <= _epsilon;
+	}
+
+	inline bool fequal(const float* __restrict _a, const float* __restrict _b, uint32_t _num, float _epsilon)
+	{
+		bool equal = fequal(_a[0], _b[0], _epsilon);
+		for (uint32_t ii = 1; equal && ii < _num; ++ii)
+		{
+			equal = fequal(_a[ii], _b[ii], _epsilon);
+		}
+		return equal;
+	}
+
 	inline void vec3Move(float* __restrict _result, const float* __restrict _a)
 	{
 		_result[0] = _a[0];
@@ -167,6 +182,123 @@ namespace bx
 		return len;
 	}
 
+	inline void quatIdentity(float* _result)
+	{
+		_result[0] = 0.0f;
+		_result[1] = 0.0f;
+		_result[2] = 0.0f;
+		_result[3] = 1.0f;
+	}
+
+	inline void quatMulXYZ(float* __restrict _result, const float* __restrict _qa, const float* __restrict _qb)
+	{
+		const float ax = _qa[0];
+		const float ay = _qa[1];
+		const float az = _qa[2];
+		const float aw = _qa[3];
+
+		const float bx = _qb[0];
+		const float by = _qb[1];
+		const float bz = _qb[2];
+		const float bw = _qb[3];
+
+		_result[0] = aw * bx + ax * bw + ay * bz - az * by;
+		_result[1] = aw * by - ax * bz + ay * bw + az * bx;
+		_result[2] = aw * bz + ax * by - ay * bx + az * bw;
+	}
+
+	inline void quatMul(float* __restrict _result, const float* __restrict _qa, const float* __restrict _qb)
+	{
+		const float ax = _qa[0];
+		const float ay = _qa[1];
+		const float az = _qa[2];
+		const float aw = _qa[3];
+
+		const float bx = _qb[0];
+		const float by = _qb[1];
+		const float bz = _qb[2];
+		const float bw = _qb[3];
+
+		_result[0] = aw * bx + ax * bw + ay * bz - az * by;
+		_result[1] = aw * by - ax * bz + ay * bw + az * bx;
+		_result[2] = aw * bz + ax * by - ay * bx + az * bw;
+		_result[3] = aw * bw - ax * bx - ay * by - az * bz;
+	}
+
+	inline void quatInvert(float* __restrict _result, const float* __restrict _quat)
+	{
+		_result[0] = -_quat[0];
+		_result[1] = -_quat[1];
+		_result[2] = -_quat[2];
+		_result[3] =  _quat[3];
+	}
+
+	inline void quatToEuler(float* __restrict _result, const float* __restrict _quat)
+	{
+		const float x = _quat[0];
+		const float y = _quat[1];
+		const float z = _quat[2];
+		const float w = _quat[3];
+
+		const float yy = y * y;
+		const float zz = z * z;
+
+		const float xx = x * x;
+		_result[0] = atan2f(2.0f * (x * w - y * z), 1.0f - 2.0f * (xx + zz) );
+		_result[1] = atan2f(2.0f * (y * w + x * z), 1.0f - 2.0f * (yy + zz) );
+		_result[2] = asinf (2.0f * (x * y + z * w) );
+	}
+
+	inline void quatRotateX(float* _result, float _ax)
+	{
+		const float hx = _ax * 0.5f;
+		const float cx = cosf(hx);
+		const float sx = sinf(hx);
+		_result[0] = sx;
+		_result[1] = 0.0f;
+		_result[2] = 0.0f;
+		_result[3] = cx;
+	}
+
+	inline void quatRotateY(float* _result, float _ay)
+	{
+		const float hy = _ay * 0.5f;
+		const float cy = cosf(hy);
+		const float sy = sinf(hy);
+		_result[0] = 0.0f;
+		_result[1] = sy;
+		_result[2] = 0.0f;
+		_result[3] = cy;
+	}
+
+	inline void quatRotateZ(float* _result, float _az)
+	{
+		const float hz = _az * 0.5f;
+		const float cz = cosf(hz);
+		const float sz = sinf(hz);
+		_result[0] = 0.0f;
+		_result[1] = 0.0f;
+		_result[2] = sz;
+		_result[3] = cz;
+	}
+
+	inline void vec3MulQuat(float* __restrict _result, const float* __restrict _vec, const float* __restrict _quat)
+	{
+		float tmp0[4];
+		quatInvert(tmp0, _quat);
+
+		float qv[4];
+		qv[0] = _vec[0];
+		qv[1] = _vec[1];
+		qv[2] = _vec[2];
+		qv[3] = 0.0f;
+
+		float tmp1[4];
+		quatMul(tmp1, tmp0, qv);
+
+		quatMulXYZ(_result, tmp1, _quat);
+	}
+
 	inline void mtxIdentity(float* _result)
 	{
 		memset(_result, 0, sizeof(float)*16);
@@ -190,6 +322,65 @@ namespace bx
 		_result[15] = 1.0f;
 	}
 
+	inline void mtxQuat(float* __restrict _result, const float* __restrict _quat)
+	{
+		const float x = _quat[0];
+		const float y = _quat[1];
+		const float z = _quat[2];
+		const float w = _quat[3];
+
+		const float x2  =  x + x;
+		const float y2  =  y + y;
+		const float z2  =  z + z;
+		const float x2x = x2 * x;
+		const float x2y = x2 * y;
+		const float x2z = x2 * z;
+		const float x2w = x2 * w;
+		const float y2y = y2 * y;
+		const float y2z = y2 * z;
+		const float y2w = y2 * w;
+		const float z2z = z2 * z;
+		const float z2w = z2 * w;
+
+		_result[ 0] = 1.0f - (y2y + z2z);
+		_result[ 1] =         x2y - z2w;
+		_result[ 2] =         x2z + y2w;
+		_result[ 3] = 0.0f;
+
+		_result[ 4] =         x2y + z2w;
+		_result[ 5] = 1.0f - (x2x + z2z);
+		_result[ 6] =         y2z - x2w;
+		_result[ 7] = 0.0f;
+
+		_result[ 8] =         x2z - y2w;
+		_result[ 9] =         y2z + x2w;
+		_result[10] = 1.0f - (x2x + y2y);
+		_result[11] = 0.0f;
+
+		_result[12] = 0.0f;
+		_result[13] = 0.0f;
+		_result[14] = 0.0f;
+		_result[15] = 1.0f;
+	}
+
+	inline void mtxQuatTranslation(float* __restrict _result, const float* __restrict _quat, const float* __restrict _translation)
+	{
+		mtxQuat(_result, _quat);
+		_result[12] = -(_result[0]*_translation[0] + _result[4]*_translation[1] + _result[ 8]*_translation[2]);
+		_result[13] = -(_result[1]*_translation[0] + _result[5]*_translation[1] + _result[ 9]*_translation[2]);
+		_result[14] = -(_result[2]*_translation[0] + _result[6]*_translation[1] + _result[10]*_translation[2]);
+	}
+
+	inline void mtxQuatTranslationHMD(float* __restrict _result, const float* __restrict _quat, const float* __restrict _translation)
+	{
+		float quat[4];
+		quat[0] = -_quat[0];
+		quat[1] = -_quat[1];
+		quat[2] =  _quat[2];
+		quat[3] =  _quat[3];
+		mtxQuatTranslation(_result, quat, _translation);
+	}
+
 	inline void mtxLookAt(float* __restrict _result, const float* __restrict _eye, const float* __restrict _at, const float* __restrict _up = NULL)
 	{
 		float tmp[4];
@@ -278,8 +469,8 @@ namespace bx
 		const float ff = _near / (_near - _far);
 
 		memset(_result, 0, sizeof(float)*16);
-		_result[0] = aa;
-		_result[5] = bb;
+		_result[ 0] = aa;
+		_result[ 5] = bb;
 		_result[10] = cc;
 		_result[12] = dd + _offset;
 		_result[13] = ee;

include/bx/mutex.h

@@ -8,6 +8,7 @@
 
 #include "bx.h"
 #include "cpu.h"
+#include "os.h"
 #include "sem.h"
 
 #if BX_CONFIG_SUPPORTS_THREADING

tests/fpumath.cpp

@@ -0,0 +1,76 @@
+/*
+ * Copyright 2010-2014 Branimir Karadzic. All rights reserved.
+ * License: http://www.opensource.org/licenses/BSD-2-Clause
+ */
+
+#include "test.h"
+#include <bx/fpumath.h>
+
+void mtxCheck(const float* _a, const float* _b)
+{
+	if (!bx::fequal(_a, _b, 16, 0.01f) )
+	{
+		DBG("\n"
+			"A:\n"
+			"%10.4f %10.4f %10.4f %10.4f\n"
+			"%10.4f %10.4f %10.4f %10.4f\n"
+			"%10.4f %10.4f %10.4f %10.4f\n"
+			"%10.4f %10.4f %10.4f %10.4f\n"
+			"B:\n"
+			"%10.4f %10.4f %10.4f %10.4f\n"
+			"%10.4f %10.4f %10.4f %10.4f\n"
+			"%10.4f %10.4f %10.4f %10.4f\n"
+			"%10.4f %10.4f %10.4f %10.4f\n"
+			, _a[ 0], _a[ 1], _a[ 2], _a[ 3]
+			, _a[ 4], _a[ 5], _a[ 6], _a[ 7]
+			, _a[ 8], _a[ 9], _a[10], _a[11]
+			, _a[12], _a[13], _a[14], _a[15]
+			, _b[ 0], _b[ 1], _b[ 2], _b[ 3]
+			, _b[ 4], _b[ 5], _b[ 6], _b[ 7]
+			, _b[ 8], _b[ 9], _b[10], _b[11]
+			, _b[12], _b[13], _b[14], _b[15]
+			);
+
+		CHECK(false);
+	}
+}
+
+TEST(Quaternion)
+{
+	float mtxQ[16];
+	float mtx[16];
+
+	float quat[4] = { 0.0f, 0.0f, 0.0f, 1.0f };
+	bx::mtxQuat(mtxQ, quat);
+	bx::mtxIdentity(mtx);
+	mtxCheck(mtxQ, mtx);
+
+	float ax = bx::pi/27.0f;
+	float ay = bx::pi/13.0f;
+	float az = bx::pi/7.0f;
+
+	bx::quatRotateX(quat, ax);
+	bx::mtxQuat(mtxQ, quat);
+	bx::mtxRotateX(mtx, ax);
+	mtxCheck(mtxQ, mtx);
+
+	float euler[3];
+	bx::quatToEuler(euler, quat);
+	CHECK(bx::fequal(euler[0], ax, 0.001f) );
+
+	bx::quatRotateY(quat, ay);
+	bx::mtxQuat(mtxQ, quat);
+	bx::mtxRotateY(mtx, ay);
+	mtxCheck(mtxQ, mtx);
+
+	bx::quatToEuler(euler, quat);
+	CHECK(bx::fequal(euler[1], ay, 0.001f) );
+
+	bx::quatRotateZ(quat, az);
+	bx::mtxQuat(mtxQ, quat);
+	bx::mtxRotateZ(mtx, az);
+	mtxCheck(mtxQ, mtx);
+
+	bx::quatToEuler(euler, quat);
+	CHECK(bx::fequal(euler[2], az, 0.001f) );
+}