From adda7774d377ef3e635b81b55394e489efcabdf9 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Wed, 28 Aug 2019 14:08:21 -0700
Subject: [PATCH] WIP checkpoint addAbsolute still broken

---
 libraries/animation/src/AnimBlendLinear.cpp | 32 ++++++++++-----
 libraries/animation/src/AnimClip.cpp        | 44 ++++++++++++++++++---
 2 files changed, 62 insertions(+), 14 deletions(-)

diff --git a/libraries/animation/src/AnimBlendLinear.cpp b/libraries/animation/src/AnimBlendLinear.cpp
index c740de72cb..65329dd167 100644
--- a/libraries/animation/src/AnimBlendLinear.cpp
+++ b/libraries/animation/src/AnimBlendLinear.cpp
@@ -85,17 +85,31 @@ void AnimBlendLinear::evaluateAndBlendChildren(const AnimVariantMap& animVars, c
             } else if (_blendType == AnimBlendType_AddRelative) {
                 ::blendAdd(_poses.size(), &prevPoses[0], &nextPoses[0], alpha, &_poses[0]);
             } else if (_blendType == AnimBlendType_AddAbsolute) {
-                // onvert from relative to absolute
-                AnimPoseVec prev = prevPoses;
-                _skeleton->convertRelativePosesToAbsolute(prev);
-                AnimPoseVec next = nextPoses;
-                _skeleton->convertRelativePosesToAbsolute(next);
+                // convert prev from relative to absolute
+                AnimPoseVec absPrev = prevPoses;
+                _skeleton->convertRelativePosesToAbsolute(absPrev);
+
+                // rotate the offset rotations from next into the parent relative frame of each joint.
+                AnimPoseVec relOffsetPoses;
+                relOffsetPoses.reserve(nextPoses.size());
+                for (size_t i = 0; i < nextPoses.size(); ++i) {
+                    // copy translation and scale from nextPoses
+                    AnimPose pose = nextPoses[i];
+
+                    // AJT: HACK nuke trans
+                    pose.trans() = glm::vec3(0.0f);
+
+                    int parentIndex = _skeleton->getParentIndex(i);
+                    if (parentIndex >= 0) {
+                        // but transform nextPoses rot into absPrev parent frame.
+                        pose.rot() = glm::inverse(absPrev[parentIndex].rot()) * nextPoses[i].rot();
+                    }
+
+                    relOffsetPoses.push_back(pose);
+                }
 
                 // then blend
-                ::blendAdd(_poses.size(), &prevPoses[0], &nextPoses[0], alpha, &_poses[0]);
-
-                // convert result back into relative
-                _skeleton->convertAbsolutePosesToRelative(_poses);
+                ::blendAdd(_poses.size(), &prevPoses[0], &relOffsetPoses[0], alpha, &_poses[0]);
             }
         }
     }
diff --git a/libraries/animation/src/AnimClip.cpp b/libraries/animation/src/AnimClip.cpp
index d58c3511cd..3f386f2804 100644
--- a/libraries/animation/src/AnimClip.cpp
+++ b/libraries/animation/src/AnimClip.cpp
@@ -41,7 +41,7 @@ static std::vector<int> buildJointIndexMap(const AnimSkeleton& dstSkeleton, cons
     return jointIndexMap;
 }
 
-static void bakeRelativeAnim(std::vector<AnimPoseVec>& anim, const AnimPoseVec& basePoses) {
+static void bakeRelativeDeltaAnim(std::vector<AnimPoseVec>& anim, const AnimPoseVec& basePoses) {
 
     // invert all the basePoses
     AnimPoseVec invBasePoses = basePoses;
@@ -55,13 +55,42 @@ static void bakeRelativeAnim(std::vector<AnimPoseVec>& anim, const AnimPoseVec&
 
         // for each joint in animPoses
         for (size_t i = 0; i < animPoses.size(); ++i) {
-
-            // convert this AnimPose into a relative animation.
+            // convert this relative AnimPose into a delta animation.
             animPoses[i] = animPoses[i] * invBasePoses[i];
         }
     }
 }
 
+void bakeAbsoluteDeltaAnim(std::vector<AnimPoseVec>& anim, const AnimPoseVec& basePoses, AnimSkeleton::ConstPointer skeleton) {
+
+    // invert all the basePoses
+    AnimPoseVec invBasePoses = basePoses;
+    for (auto&& invBasePose : invBasePoses) {
+        invBasePose = invBasePose.inverse();
+    }
+
+    AnimPoseVec absBasePoses = basePoses;
+    skeleton->convertRelativePosesToAbsolute(absBasePoses);
+
+    // for each frame of the animation
+    for (auto&& animPoses : anim) {
+        ASSERT(animPoses.size() == basePoses.size());
+
+        // for each joint in animPoses
+        for (size_t i = 0; i < animPoses.size(); ++i) {
+
+            // scale and translation are relative frame
+            animPoses[i] = animPoses[i] * invBasePoses[i];
+
+            // but transform the rotation delta into the absolute frame.
+            int parentIndex = skeleton->getParentIndex(i);
+            if (parentIndex >= 0) {
+                animPoses[i].rot() = absBasePoses[parentIndex].rot() * animPoses[i].rot();
+            }
+        }
+    }
+}
+
 static std::vector<AnimPoseVec> copyAndRetargetFromNetworkAnim(AnimationPointer networkAnim, AnimSkeleton::ConstPointer avatarSkeleton) {
     ASSERT(networkAnim && networkAnim->isLoaded() && avatarSkeleton);
     std::vector<AnimPoseVec> anim;
@@ -264,8 +293,13 @@ const AnimPoseVec& AnimClip::evaluate(const AnimVariantMap& animVars, const Anim
             // copy & retarget baseAnim!
             auto baseAnim = copyAndRetargetFromNetworkAnim(_baseNetworkAnim, _skeleton);
 
-            // make _anim relative to the baseAnim reference frame.
-            bakeRelativeAnim(_anim, baseAnim[(int)_baseFrame]);
+//#define BLEND_ADD_ABSOLUTE
+
+#ifdef BLEND_ADD_ABSOLUTE
+            bakeAbsoluteDeltaAnim(_anim, baseAnim[(int)_baseFrame], _skeleton);
+#else
+            bakeRelativeDeltaAnim(_anim, baseAnim[(int)_baseFrame]);
+#endif
         }
     }