From 5c7e81584c204a3556e65073abb82f342b655540 Mon Sep 17 00:00:00 2001
From: Anthony Thibault <tony@highfidelity.io>
Date: Tue, 5 Feb 2019 18:23:58 -0800
Subject: [PATCH] AnimPose::inverse() optimization

---
 libraries/animation/src/AnimPose.cpp |  5 ++-
 tests/animation/src/AnimTests.cpp    | 54 ++++++++++++++++++++++++++++
 tests/animation/src/AnimTests.h      |  1 +
 3 files changed, 59 insertions(+), 1 deletion(-)

diff --git a/libraries/animation/src/AnimPose.cpp b/libraries/animation/src/AnimPose.cpp
index 7b80e96ed5..366d863c3d 100644
--- a/libraries/animation/src/AnimPose.cpp
+++ b/libraries/animation/src/AnimPose.cpp
@@ -51,7 +51,10 @@ AnimPose AnimPose::operator*(const AnimPose& rhs) const {
 }
 
 AnimPose AnimPose::inverse() const {
-    return AnimPose(glm::inverse(static_cast<glm::mat4>(*this)));
+    float invScale = 1.0f / _scale;
+    glm::quat invRot = glm::inverse(_rot);
+    glm::vec3 invTrans = invScale * (invRot * -_trans);
+    return AnimPose(invScale, invRot, invTrans);
 }
 
 // mirror about x-axis without applying negative scale.
diff --git a/tests/animation/src/AnimTests.cpp b/tests/animation/src/AnimTests.cpp
index 2a49846b6b..b1926efb71 100644
--- a/tests/animation/src/AnimTests.cpp
+++ b/tests/animation/src/AnimTests.cpp
@@ -526,6 +526,60 @@ void AnimTests::testAnimPoseMultiply() {
     }
 }
 
+void AnimTests::testAnimPoseInverse() {
+    const float PI = (float)M_PI;
+    const glm::quat ROT_X_90 = glm::angleAxis(PI / 2.0f, glm::vec3(1.0f, 0.0f, 0.0f));
+    const glm::quat ROT_Y_180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0, 0.0f));
+    const glm::quat ROT_Z_30 = glm::angleAxis(PI / 6.0f, glm::vec3(1.0f, 0.0f, 0.0f));
+
+    std::vector<float> scaleVec = {
+        1.0f,
+        2.0f,
+        0.5f
+    };
+
+    std::vector<glm::quat> rotVec = {
+        glm::quat(),
+        ROT_X_90,
+        ROT_Y_180,
+        ROT_Z_30,
+        ROT_X_90 * ROT_Y_180 * ROT_Z_30,
+        -ROT_Y_180
+    };
+
+    std::vector<glm::vec3> transVec = {
+        glm::vec3(),
+        glm::vec3(10.0f, 0.0f, 0.0f),
+        glm::vec3(0.0f, 5.0f, 0.0f),
+        glm::vec3(0.0f, 0.0f, 7.5f),
+        glm::vec3(10.0f, 5.0f, 7.5f),
+        glm::vec3(-10.0f, 5.0f, 7.5f),
+        glm::vec3(10.0f, -5.0f, 7.5f),
+        glm::vec3(10.0f, 5.0f, -7.5f)
+    };
+
+    const float TEST_EPSILON = 0.001f;
+
+    for (auto& scale : scaleVec) {
+        for (auto& rot : rotVec) {
+            for (auto& trans : transVec) {
+
+                // build a matrix the old fashioned way.
+                glm::mat4 scaleMat = glm::scale(glm::mat4(), glm::vec3(scale));
+                glm::mat4 rotTransMat = createMatFromQuatAndPos(rot, trans);
+                glm::mat4 rawMat = glm::inverse(rotTransMat * scaleMat);
+
+                // use an anim pose to build a matrix by parts.
+                AnimPose pose(scale, rot, trans);
+                glm::mat4 poseMat = pose.inverse();
+
+                QCOMPARE_WITH_ABS_ERROR(rawMat, poseMat, TEST_EPSILON);
+            }
+        }
+    }
+}
+
+
 void AnimTests::testExpressionTokenizer() {
     QString str = "(10 +  x) >= 20.1 && (y != !z)";
     AnimExpression e("x");
diff --git a/tests/animation/src/AnimTests.h b/tests/animation/src/AnimTests.h
index 637611e8c4..326545b0a9 100644
--- a/tests/animation/src/AnimTests.h
+++ b/tests/animation/src/AnimTests.h
@@ -28,6 +28,7 @@ private slots:
     void testAccumulateTime();
     void testAnimPose();
     void testAnimPoseMultiply();
+    void testAnimPoseInverse();
     void testExpressionTokenizer();
     void testExpressionParser();
     void testExpressionEvaluator();