diff --git a/include/bx/inline/math.inl b/include/bx/inline/math.inl index 6699a73..4c45114 100644 --- a/include/bx/inline/math.inl +++ b/include/bx/inline/math.inl @@ -690,6 +690,14 @@ namespace bx }; } + inline BX_CONSTEXPR_FUNC bool equal(const Vec3 _a, const Vec3 _b, float _epsilon) + { + return equal(_a.x, _b.x, _epsilon) + && equal(_a.y, _b.y, _epsilon) + && equal(_a.z, _b.z, _epsilon) + ; + } + inline void calcTangentFrame(Vec3& _outT, Vec3& _outB, const Vec3 _n) { const float nx = _n.x; @@ -891,6 +899,24 @@ namespace bx return normalize(qq); } + inline BX_CONST_FUNC Quaternion fromEuler(const Vec3 _euler) + { + const float sx = sin(_euler.x * 0.5f); + const float cx = cos(_euler.x * 0.5f); + const float sy = sin(_euler.y * 0.5f); + const float cy = cos(_euler.y * 0.5f); + const float sz = sin(_euler.z * 0.5f); + const float cz = cos(_euler.z * 0.5f); + + return + { + sx * cy * cz - cx * sy * sz, + cx * sy * cz + sx * cy * sz, + cx * cy * sz - sx * sy * cz, + cx * cy * cz + sx * sy * sz, + }; + } + inline BX_CONST_FUNC Vec3 toEuler(const Quaternion _a) { const float xx = _a.x; @@ -960,7 +986,7 @@ namespace bx }; } - inline BX_CONST_FUNC Quaternion rotateAxis(const Vec3 _axis, float _angle) + inline BX_CONST_FUNC Quaternion fromAxisAngle(const Vec3 _axis, float _angle) { const float ha = _angle * 0.5f; const float sa = sin(ha); @@ -974,6 +1000,24 @@ namespace bx }; } + inline void toAxisAngle(Vec3& _outAxis, float& _outAngle, const Quaternion _a) + { + const float ww = _a.w; + const float sa = sqrt(1.0f - square(ww) ); + + _outAngle = 2.0f * acos(ww); + + if (0.001f > sa) + { + _outAxis = { _a.x, _a.y, _a.z }; + return; + } + + const float invSa = 1.0f/sa; + + _outAxis = { _a.x * invSa, _a.y * invSa, _a.z * invSa }; + } + inline BX_CONST_FUNC Quaternion rotateX(float _ax) { const float hx = _ax * 0.5f; @@ -1013,6 +1057,15 @@ namespace bx }; } + inline BX_CONSTEXPR_FUNC bool equal(const Quaternion _a, const Quaternion _b, float _epsilon) + { + return equal(_a.x, _b.x, _epsilon) + && equal(_a.y, _b.y, _epsilon) + && equal(_a.z, _b.z, _epsilon) + && equal(_a.w, _b.w, _epsilon) + ; + } + inline void mtxIdentity(float* _result) { memSet(_result, 0, sizeof(float)*16); @@ -1079,12 +1132,12 @@ namespace bx _result[15] = 1.0f; } - inline void mtxQuat(float* _result, const Quaternion& _quat) + inline void mtxFromQuaternion(float* _result, const Quaternion& _rotation) { - const float qx = _quat.x; - const float qy = _quat.y; - const float qz = _quat.z; - const float qw = _quat.w; + const float qx = _rotation.x; + const float qy = _rotation.y; + const float qz = _rotation.z; + const float qw = _rotation.w; const float x2 = qx + qx; const float y2 = qy + qy; @@ -1120,24 +1173,12 @@ namespace bx _result[15] = 1.0f; } - inline void mtxQuatTranslation(float* _result, const Quaternion& _quat, const Vec3& _translation) + inline void mtxFromQuaternion(float* _result, const Quaternion& _rotation, const Vec3& _translation) { - mtxQuat(_result, _quat); + mtxFromQuaternion(_result, _rotation); store(&_result[12], neg(mulXyz0(_translation, _result) ) ); } - inline void mtxQuatTranslationHMD(float* _result, const Quaternion& _quat, const Vec3& _translation) - { - const Quaternion quat = - { - -_quat.x, - -_quat.y, - _quat.z, - _quat.w, - }; - mtxQuatTranslation(_result, quat, _translation); - } - inline Vec3 mul(const Vec3& _vec, const float* _mat) { Vec3 result(init::None); diff --git a/include/bx/math.h b/include/bx/math.h index ee4aa29..ba39abf 100644 --- a/include/bx/math.h +++ b/include/bx/math.h @@ -447,6 +447,9 @@ namespace bx /// BX_CONSTEXPR_FUNC Vec3 rcp(const Vec3 _a); + /// + BX_CONSTEXPR_FUNC bool equal(const Vec3 _a, const Vec3 _b, float _epsilon); + /// void calcTangentFrame(Vec3& _outT, Vec3& _outB, const Vec3 _n); @@ -489,6 +492,9 @@ namespace bx /// BX_CONSTEXPR_FUNC Quaternion lerp(const Quaternion _a, const Quaternion _b, float _t); + /// + BX_CONST_FUNC Quaternion fromEuler(const Vec3 _euler); + /// BX_CONST_FUNC Vec3 toEuler(const Quaternion _a); @@ -502,7 +508,10 @@ namespace bx BX_CONST_FUNC Vec3 toZAxis(const Quaternion _a); /// - BX_CONST_FUNC Quaternion rotateAxis(const Vec3 _axis, float _angle); + BX_CONST_FUNC Quaternion fromAxisAngle(const Vec3 _axis, float _angle); + + /// + void toAxisAngle(Vec3& _outAxis, float& _outAngle, const Quaternion _a); /// BX_CONST_FUNC Quaternion rotateX(float _ax); @@ -513,6 +522,9 @@ namespace bx /// BX_CONST_FUNC Quaternion rotateZ(float _az); + /// + BX_CONSTEXPR_FUNC bool equal(const Quaternion _a, const Quaternion _b, float _epsilon); + /// void mtxIdentity(float* _result); @@ -543,13 +555,10 @@ namespace bx ); /// - void mtxQuat(float* _result, const Quaternion& _quat); + void mtxFromQuaternion(float* _result, const Quaternion& _rotation); /// - void mtxQuatTranslation(float* _result, const Quaternion& _quat, const Vec3& _translation); - - /// - void mtxQuatTranslationHMD(float* _result, const Quaternion& _quat, const Vec3& _translation); + void mtxFromQuaternion(float* _result, const Quaternion& _rotation, const Vec3& _translation); /// void mtxLookAt( diff --git a/tests/math_test.cpp b/tests/math_test.cpp index fa3a00d..b049194 100644 --- a/tests/math_test.cpp +++ b/tests/math_test.cpp @@ -228,7 +228,7 @@ void mtxCheck(const float* _a, const float* _b) , _b[12], _b[13], _b[14], _b[15] ); - CHECK(false); + REQUIRE(false); } } @@ -238,7 +238,13 @@ TEST_CASE("quaternion", "") float mtx[16]; bx::Quaternion quat = bx::init::Identity; - bx::mtxQuat(mtxQ, quat); + bx::Quaternion q2 = bx::init::None; + + bx::Vec3 axis = bx::init::None; + bx::Vec3 euler = bx::init::None; + float angle; + + bx::mtxFromQuaternion(mtxQ, quat); bx::mtxIdentity(mtx); mtxCheck(mtxQ, mtx); @@ -246,27 +252,51 @@ TEST_CASE("quaternion", "") float ay = bx::kPi/13.0f; float az = bx::kPi/7.0f; - quat = bx::rotateX(ax); - bx::mtxQuat(mtxQ, quat); - bx::mtxRotateX(mtx, ax); - mtxCheck(mtxQ, mtx); + { // x + quat = bx::rotateX(ax); + bx::mtxFromQuaternion(mtxQ, quat); + bx::mtxRotateX(mtx, ax); + mtxCheck(mtxQ, mtx); - bx::Vec3 euler = bx::toEuler(quat); - CHECK(bx::equal(euler.x, ax, 0.001f) ); + bx::toAxisAngle(axis, angle, quat); + REQUIRE(bx::equal(axis, bx::Vec3{1.0f, 0.0f, 0.0f}, 0.01f) ); + REQUIRE(bx::equal(angle, ax, 0.01f) ); - quat = bx::rotateY(ay); - bx::mtxQuat(mtxQ, quat); - bx::mtxRotateY(mtx, ay); - mtxCheck(mtxQ, mtx); + euler = bx::toEuler(quat); + REQUIRE(bx::equal(euler.x, ax, 0.001f) ); + q2 = bx::fromEuler(euler); + REQUIRE(bx::equal(quat, q2, 0.001f) ); + } - euler = bx::toEuler(quat); - CHECK(bx::equal(euler.y, ay, 0.001f) ); + { // y + quat = bx::rotateY(ay); + bx::mtxFromQuaternion(mtxQ, quat); + bx::mtxRotateY(mtx, ay); + mtxCheck(mtxQ, mtx); - quat = bx::rotateZ(az); - bx::mtxQuat(mtxQ, quat); - bx::mtxRotateZ(mtx, az); - mtxCheck(mtxQ, mtx); + bx::toAxisAngle(axis, angle, quat); + REQUIRE(bx::equal(axis, bx::Vec3{0.0f, 1.0f, 0.0f}, 0.01f) ); + REQUIRE(bx::equal(angle, ay, 0.01f) ); + euler = bx::toEuler(quat); + REQUIRE(bx::equal(euler.y, ay, 0.001f) ); + q2 = bx::fromEuler(euler); + REQUIRE(bx::equal(quat, q2, 0.001f) ); - euler = bx::toEuler(quat); - CHECK(bx::equal(euler.z, az, 0.001f) ); + } + + { // z + quat = bx::rotateZ(az); + bx::mtxFromQuaternion(mtxQ, quat); + bx::mtxRotateZ(mtx, az); + mtxCheck(mtxQ, mtx); + + bx::toAxisAngle(axis, angle, quat); + REQUIRE(bx::equal(axis, bx::Vec3{0.0f, 0.0f, 1.0f}, 0.01f) ); + REQUIRE(bx::equal(angle, az, 0.01f) ); + + euler = bx::toEuler(quat); + REQUIRE(bx::equal(euler.z, az, 0.001f) ); + q2 = bx::fromEuler(euler); + REQUIRE(bx::equal(quat, q2, 0.001f) ); + } } diff --git a/tools/bin/linux/bin2c b/tools/bin/linux/bin2c index 39015b9..2b8fca8 100755 Binary files a/tools/bin/linux/bin2c and b/tools/bin/linux/bin2c differ diff --git a/tools/bin/linux/lemon b/tools/bin/linux/lemon index be869ab..6c5e3bd 100755 Binary files a/tools/bin/linux/lemon and b/tools/bin/linux/lemon differ