Merge pull request #15119 from hyperlogic/feature/improve-anim-pose-negative-scale

Improve AnimPose support for negative scale

libraries/animation/src/AnimPose.cpp

@@ -11,7 +11,6 @@
 #include "AnimPose.h"
 #include <GLMHelpers.h>
 #include <algorithm>
-#include <glm/gtc/matrix_transform.hpp>
 #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);
 }
 

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

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 ()

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() {