From f88168bdfb34e2db081795f183272de294caa63a Mon Sep 17 00:00:00 2001
From: Simon Walton <simon@highfidelity.io>
Date: Tue, 6 Nov 2018 17:29:02 -0800
Subject: [PATCH] Send avatar-joint translations at world scale

---
 libraries/animation/src/Rig.cpp                | 12 ++++++++----
 libraries/networking/src/udt/PacketHeaders.cpp |  2 +-
 libraries/networking/src/udt/PacketHeaders.h   |  3 ++-
 3 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index c2f909dd24..223305cf47 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1927,6 +1927,7 @@ AnimPose Rig::getJointPose(int jointIndex) const {
 void Rig::copyJointsIntoJointData(QVector<JointData>& jointDataVec) const {
 
     const AnimPose geometryToRigPose(_geometryToRigTransform);
+    const glm::vec3 geometryToRigScale(geometryToRigPose.scale());
 
     jointDataVec.resize((int)getJointStateCount());
     for (auto i = 0; i < jointDataVec.size(); i++) {
@@ -1939,9 +1940,10 @@ void Rig::copyJointsIntoJointData(QVector<JointData>& jointDataVec) const {
 
             // translations are in relative frame but scaled so that they are in meters,
             // instead of model units.
-            glm::vec3 defaultRelTrans = _geometryOffset.scale() * _animSkeleton->getRelativeDefaultPose(i).trans();
-            data.translation = _geometryOffset.scale() * (!_sendNetworkNode ? _internalPoseSet._relativePoses[i].trans() : _networkPoseSet._relativePoses[i].trans());
-            data.translationIsDefaultPose = isEqual(data.translation, defaultRelTrans);
+            glm::vec3 defaultRelTrans = _animSkeleton->getRelativeDefaultPose(i).trans();
+            glm::vec3 currentRelTrans = _sendNetworkNode ? _networkPoseSet._relativePoses[i].trans() : _internalPoseSet._relativePoses[i].trans();
+            data.translation = geometryToRigScale * currentRelTrans;
+            data.translationIsDefaultPose = isEqual(currentRelTrans, defaultRelTrans);
         } else {
             data.translationIsDefaultPose = true;
             data.rotationIsDefaultPose = true;
@@ -1967,6 +1969,8 @@ void Rig::copyJointsFromJointData(const QVector<JointData>& jointDataVec) {
     std::vector<glm::quat> rotations;
     rotations.reserve(numJoints);
     const glm::quat rigToGeometryRot(glmExtractRotation(_rigToGeometryTransform));
+    const glm::vec3 rigToGeometryScale(extractScale(_rigToGeometryTransform));
+
     for (int i = 0; i < numJoints; i++) {
         const JointData& data = jointDataVec.at(i);
         if (data.rotationIsDefaultPose) {
@@ -1992,7 +1996,7 @@ void Rig::copyJointsFromJointData(const QVector<JointData>& jointDataVec) {
             _internalPoseSet._relativePoses[i].trans() = relativeDefaultPoses[i].trans();
         } else {
             // JointData translations are in scaled relative-frame so we scale back to regular relative-frame
-            _internalPoseSet._relativePoses[i].trans() = _invGeometryOffset.scale() * data.translation;
+            _internalPoseSet._relativePoses[i].trans() = rigToGeometryScale * data.translation;
         }
     }
 }
diff --git a/libraries/networking/src/udt/PacketHeaders.cpp b/libraries/networking/src/udt/PacketHeaders.cpp
index 9eed463d2d..18e439aa77 100644
--- a/libraries/networking/src/udt/PacketHeaders.cpp
+++ b/libraries/networking/src/udt/PacketHeaders.cpp
@@ -40,7 +40,7 @@ PacketVersion versionForPacketType(PacketType packetType) {
         case PacketType::AvatarData:
         case PacketType::BulkAvatarData:
         case PacketType::KillAvatar:
-            return static_cast<PacketVersion>(AvatarMixerPacketVersion::FarGrabJointsRedux);
+            return static_cast<PacketVersion>(AvatarMixerPacketVersion::JointTransScaled);
         case PacketType::MessagesData:
             return static_cast<PacketVersion>(MessageDataVersion::TextOrBinaryData);
         // ICE packets
diff --git a/libraries/networking/src/udt/PacketHeaders.h b/libraries/networking/src/udt/PacketHeaders.h
index 37a4b32940..8d734c1043 100644
--- a/libraries/networking/src/udt/PacketHeaders.h
+++ b/libraries/networking/src/udt/PacketHeaders.h
@@ -297,7 +297,8 @@ enum class AvatarMixerPacketVersion : PacketVersion {
     FarGrabJoints,
     MigrateSkeletonURLToTraits,
     MigrateAvatarEntitiesToTraits,
-    FarGrabJointsRedux
+    FarGrabJointsRedux,
+    JointTransScaled
 };
 
 enum class DomainConnectRequestVersion : PacketVersion {