From a30e06d5949d36bc6742200d9042fa0496f5dfc4 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 14 May 2018 09:58:16 -0700
Subject: [PATCH] don't blend avatar position when far from target

---
 interface/src/avatar/AvatarMotionState.cpp | 16 ++++++----------
 interface/src/avatar/AvatarMotionState.h   |  2 +-
 2 files changed, 7 insertions(+), 11 deletions(-)

diff --git a/interface/src/avatar/AvatarMotionState.cpp b/interface/src/avatar/AvatarMotionState.cpp
index ac52a6e485..beb7e34439 100644
--- a/interface/src/avatar/AvatarMotionState.cpp
+++ b/interface/src/avatar/AvatarMotionState.cpp
@@ -59,7 +59,7 @@ const btCollisionShape* AvatarMotionState::computeNewShape() {
     std::static_pointer_cast<Avatar>(_avatar)->computeShapeInfo(shapeInfo);
     glm::vec3 halfExtents = shapeInfo.getHalfExtents();
     halfExtents.y = 0.0f;
-    _radius2 = glm::length2(halfExtents);
+    _diameter = 2.0f * glm::length(halfExtents);
     return getShapeManager()->getShape(shapeInfo);
 }
 
@@ -80,25 +80,21 @@ void AvatarMotionState::getWorldTransform(btTransform& worldTrans) const {
 
 // virtual
 void AvatarMotionState::setWorldTransform(const btTransform& worldTrans) {
-    // HACK: The PhysicsEngine does not actually move OTHER avatars -- instead it slaves their local RigidBody to the transform
-    // as specified by a remote simulation.  However, to give the remote simulation time to respond to our own objects we tie
-    // the other avatar's body to its true position with a simple spring. This is a HACK that will have to be improved later.
     const float SPRING_TIMESCALE = 0.5f;
     float tau = PHYSICS_ENGINE_FIXED_SUBSTEP / SPRING_TIMESCALE;
     btVector3 currentPosition = worldTrans.getOrigin();
     btVector3 offsetToTarget = glmToBullet(getObjectPosition()) - currentPosition;
-    float distance2 = offsetToTarget.length2();
-    const float TWO_SQUARED = 4.0f;
-    if (distance2 > TWO_SQUARED * _radius2) {
-        // reduce the offsetToTarget by slamming the position
+    float distance = offsetToTarget.length();
+    if ((1.0f - tau) * distance > _diameter) {
+        // the avatar body is far from its target --> slam position
         btTransform newTransform;
-        newTransform.setOrigin(currentPosition + tau * offsetToTarget);
+        newTransform.setOrigin(currentPosition + offsetToTarget);
         newTransform.setRotation(glmToBullet(getObjectRotation()));
         _body->setWorldTransform(newTransform);
         _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
         _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
     } else {
-        // reduce the offsetToTarget by slamming the velocity
+        // the avatar body is near its target --> slam velocity
         btVector3 velocity = glmToBullet(getObjectLinearVelocity()) + (1.0f / SPRING_TIMESCALE) * offsetToTarget;
         _body->setLinearVelocity(velocity);
         _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
diff --git a/interface/src/avatar/AvatarMotionState.h b/interface/src/avatar/AvatarMotionState.h
index cfddab90ce..b0f7f29e37 100644
--- a/interface/src/avatar/AvatarMotionState.h
+++ b/interface/src/avatar/AvatarMotionState.h
@@ -81,7 +81,7 @@ protected:
     const btCollisionShape* computeNewShape() override;
 
     AvatarSharedPointer _avatar;
-    float _radius2 { 0.0f };
+    float _diameter { 0.0f };
 
     uint32_t _dirtyFlags;
 };