diff --git a/libraries/animation/src/AnimPose.cpp b/libraries/animation/src/AnimPose.cpp index d77514e691..8649db8233 100644 --- a/libraries/animation/src/AnimPose.cpp +++ b/libraries/animation/src/AnimPose.cpp @@ -11,7 +11,6 @@ #include "AnimPose.h" #include #include -#include #include "AnimUtil.h" const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f), @@ -19,16 +18,29 @@ const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f), glm::vec3(0.0f)); AnimPose::AnimPose(const glm::mat4& mat) { - static const float EPSILON = 0.0001f; - _scale = extractScale(mat); - // quat_cast doesn't work so well with scaled matrices, so cancel it out. - glm::mat4 tmp = glm::scale(mat, 1.0f / _scale); + glm::mat3 m(mat); + _scale = glm::vec3(glm::length(m[0]), glm::length(m[1]), glm::length(m[2])); + float det = glm::determinant(m); + + glm::mat3 tmp; + if (det < 0.0f) { + _scale *= -1.0f; + } + + // quat_cast doesn't work so well with scaled matrices, so cancel out scale. + // also, as a side effect, multiply mirrored matrices by -1 to get the right rotation out. + tmp[0] = m[0] * (1.0f / _scale[0]); + tmp[1] = m[1] * (1.0f / _scale[1]); + tmp[2] = m[2] * (1.0f / _scale[2]); _rot = glm::quat_cast(tmp); + + // normalize quat if necessary float lengthSquared = glm::length2(_rot); if (glm::abs(lengthSquared - 1.0f) > EPSILON) { float oneOverLength = 1.0f / sqrtf(lengthSquared); _rot = glm::quat(_rot.w * oneOverLength, _rot.x * oneOverLength, _rot.y * oneOverLength, _rot.z * oneOverLength); } + _trans = extractTranslation(mat); } diff --git a/libraries/animation/src/AnimTwoBoneIK.cpp b/libraries/animation/src/AnimTwoBoneIK.cpp index 8960b15940..c91518d5db 100644 --- a/libraries/animation/src/AnimTwoBoneIK.cpp +++ b/libraries/animation/src/AnimTwoBoneIK.cpp @@ -156,7 +156,7 @@ const AnimPoseVec& AnimTwoBoneIK::evaluate(const AnimVariantMap& animVars, const glm::quat relMidRot = glm::angleAxis(midAngle, _midHingeAxis); // insert new relative pose into the chain and rebuild it. - ikChain.setRelativePoseAtJointIndex(_midJointIndex, AnimPose(relMidRot, underPoses[_midJointIndex].trans())); + ikChain.setRelativePoseAtJointIndex(_midJointIndex, AnimPose(underPoses[_midJointIndex].scale(), relMidRot, underPoses[_midJointIndex].trans())); ikChain.buildDirtyAbsolutePoses(); // recompute tip pose after mid joint has been rotated @@ -180,7 +180,7 @@ const AnimPoseVec& AnimTwoBoneIK::evaluate(const AnimVariantMap& animVars, const // transform result back into parent relative frame. glm::quat relBaseRot = glm::inverse(baseParentPose.rot()) * absRot; - ikChain.setRelativePoseAtJointIndex(_baseJointIndex, AnimPose(relBaseRot, underPoses[_baseJointIndex].trans())); + ikChain.setRelativePoseAtJointIndex(_baseJointIndex, AnimPose(underPoses[_baseJointIndex].scale(), relBaseRot, underPoses[_baseJointIndex].trans())); } // recompute midJoint pose after base has been rotated. @@ -189,7 +189,7 @@ const AnimPoseVec& AnimTwoBoneIK::evaluate(const AnimVariantMap& animVars, const // transform target rotation in to parent relative frame. glm::quat relTipRot = glm::inverse(midJointPose.rot()) * targetPose.rot(); - ikChain.setRelativePoseAtJointIndex(_tipJointIndex, AnimPose(relTipRot, underPoses[_tipJointIndex].trans())); + ikChain.setRelativePoseAtJointIndex(_tipJointIndex, AnimPose(underPoses[_tipJointIndex].scale(), relTipRot, underPoses[_tipJointIndex].trans())); // blend with the underChain ikChain.blend(underChain, alpha); diff --git a/tests/animation/CMakeLists.txt b/tests/animation/CMakeLists.txt index 2af4d5f2cd..e378750425 100644 --- a/tests/animation/CMakeLists.txt +++ b/tests/animation/CMakeLists.txt @@ -1,7 +1,7 @@ # Declare dependencies macro (setup_testcase_dependencies) # link in the shared libraries - link_hifi_libraries(shared animation gpu fbx hfm graphics networking test-utils) + link_hifi_libraries(shared animation gpu fbx hfm graphics networking test-utils image) package_libraries_for_deployment() endmacro () diff --git a/tests/animation/src/AnimTests.cpp b/tests/animation/src/AnimTests.cpp index 0cd9571e22..a14ffcf967 100644 --- a/tests/animation/src/AnimTests.cpp +++ b/tests/animation/src/AnimTests.cpp @@ -443,6 +443,28 @@ void AnimTests::testAnimPose() { } } } + + + // test matrix that has a negative determiant. + glm::vec4 col0(-9.91782e-05f, -5.40349e-05f, 0.000724383f, 0.0f); + glm::vec4 col1(-0.000155237f, 0.00071579f, 3.21398e-05f, 0.0f); + glm::vec4 col2(0.000709614f, 0.000149036f, 0.000108273f, 0.0f); + glm::vec4 col3(0.117922f, 0.250457f, 0.102155f, 1.0f); + glm::mat4 m(col0, col1, col2, col3); + AnimPose p(m); + + glm::vec3 resultTrans = glm::vec3(col3); + glm::quat resultRot = glm::quat(0.0530394f, 0.751549f, 0.0949531f, -0.650649f); + glm::vec3 resultScale = glm::vec3(-0.000733135f, -0.000733135f, -0.000733135f); + + const float TEST_EPSILON2 = 0.00001f; + QCOMPARE_WITH_ABS_ERROR(p.trans(), resultTrans, TEST_EPSILON2); + + if (glm::dot(p.rot(), resultRot) < 0.0f) { + resultRot = -resultRot; + } + QCOMPARE_WITH_ABS_ERROR(p.rot(), resultRot, TEST_EPSILON2); + QCOMPARE_WITH_ABS_ERROR(p.scale(), resultScale, TEST_EPSILON2); } void AnimTests::testExpressionTokenizer() {