From 92529967be3d863b2bc188d2124ba9bb6f4838c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Branimir=20Karad=C5=BEi=C4=87?= Date: Sun, 2 Nov 2014 00:05:05 -0700 Subject: [PATCH] Added quaternion functions. --- include/bx/fpumath.h | 197 ++++++++++++++++++++++++++++++++++++++++++- include/bx/mutex.h | 1 + tests/fpumath.cpp | 76 +++++++++++++++++ 3 files changed, 271 insertions(+), 3 deletions(-) create mode 100644 tests/fpumath.cpp diff --git a/include/bx/fpumath.h b/include/bx/fpumath.h index d877d73..baff90d 100644 --- a/include/bx/fpumath.h +++ b/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; diff --git a/include/bx/mutex.h b/include/bx/mutex.h index bb09059..8fb4942 100644 --- a/include/bx/mutex.h +++ b/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 diff --git a/tests/fpumath.cpp b/tests/fpumath.cpp new file mode 100644 index 0000000..15b294e --- /dev/null +++ b/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 + +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) ); +}