Make AnimPose from mat4 work better for matrices with negative determinants.

Took part of this code from glm::decompose() which references
https://opensource.apple.com/source/WebCore/WebCore-514/platform/graphics/transforms/TransformationMatrix.cpp

libraries/animation/src/AnimPose.cpp

@@ -13,12 +13,16 @@
 #include <algorithm>
 #include <glm/gtc/matrix_transform.hpp>
 #include "AnimUtil.h"
+#include <glm/gtx/matrix_decompose.hpp>
 
 const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f),
                                              glm::quat(),
                                              glm::vec3(0.0f));
 
+#define NEW_VERSION
+
 AnimPose::AnimPose(const glm::mat4& mat) {
+#if defined(ORIGINAL_VERSION)
     static const float EPSILON = 0.0001f;
     _scale = extractScale(mat);
     // quat_cast doesn't work so well with scaled matrices, so cancel it out.
@@ -30,6 +34,52 @@ AnimPose::AnimPose(const glm::mat4& mat) {
         _rot = glm::quat(_rot.w * oneOverLength, _rot.x * oneOverLength, _rot.y * oneOverLength, _rot.z * oneOverLength);
     }
     _trans = extractTranslation(mat);
+#elif defined(DECOMPOSE_VERSION)
+    // glm::decompose code
+    glm::vec3 scale;
+    glm::quat rotation;
+    glm::vec3 translation;
+    glm::vec3 skew;
+    glm::vec4 perspective;
+    bool result = glm::decompose(mat, scale, rotation, translation, skew, perspective);
+    _scale = scale;
+    _rot = rotation;
+    _trans = translation;
+    if (!result) {
+        // hack
+        const float HACK_FACTOR = 1000.0f;
+        glm::mat4 tmp = glm::scale(mat, HACK_FACTOR);
+        glm::decompose(tmp, scale, rotation, translation, skew, perspective);
+        _scale = scale / HACK_FACTOR;
+        _rot = rotation;
+        _trans = translation;
+    }
+#elif defined(NEW_VERSION)
+    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);
+#endif
 }
 
 glm::vec3 AnimPose::operator*(const glm::vec3& rhs) const {

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