Mirrors/bx
1
0
Fork 0
mirror of https://github.com/bkaradzic/bx.git synced 2026-02-18 04:53:06 +01:00
Code Issues Packages Projects Releases Wiki Activity

Added quaternion functions.

Browse Source
This commit is contained in:
Branimir Karadžić
2014-11-02 00:05:05 -07:00
parent 7e15347858
commit 92529967be
3 changed files with 271 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

197
include/bx/fpumath.h
View File

@@ -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;

1
include/bx/mutex.h
View File

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

76
tests/fpumath.cpp Normal file
View File

@@ -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) );
}