From 612e9649d8ab85c64cbab526f6f0d5849b323df5 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 9 Jul 2015 11:32:30 -0700
Subject: [PATCH] fix update logic of ObjectActionOffset

---
 libraries/physics/src/ObjectActionOffset.cpp | 104 +++++++++++--------
 libraries/physics/src/ObjectActionOffset.h   |   1 +
 2 files changed, 61 insertions(+), 44 deletions(-)

diff --git a/libraries/physics/src/ObjectActionOffset.cpp b/libraries/physics/src/ObjectActionOffset.cpp
index 7cf35b27d7..ff789dfefe 100644
--- a/libraries/physics/src/ObjectActionOffset.cpp
+++ b/libraries/physics/src/ObjectActionOffset.cpp
@@ -16,7 +16,11 @@
 const uint16_t ObjectActionOffset::offsetVersion = 1;
 
 ObjectActionOffset::ObjectActionOffset(const QUuid& id, EntityItemPointer ownerEntity) :
-    ObjectAction(ACTION_TYPE_OFFSET, id, ownerEntity) {
+    ObjectAction(ACTION_TYPE_OFFSET, id, ownerEntity),
+    _pointToOffsetFrom(0.f),
+    _linearDistance(0.0f),
+    _linearTimeScale(FLT_MAX),
+    _positionalTargetSet(false) {
     #if WANT_DEBUG
     qDebug() << "ObjectActionOffset::ObjectActionOffset";
     #endif
@@ -44,6 +48,7 @@ void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) {
         unlock();
         return;
     }
+
     ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
     btRigidBody* rigidBody = motionState->getRigidBody();
     if (!rigidBody) {
@@ -52,21 +57,32 @@ void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) {
         return;
     }
 
-    if (_positionalTargetSet) {
-        glm::vec3 offset = _pointToOffsetFrom - bulletToGLM(rigidBody->getCenterOfMassPosition());
-        float offsetLength = glm::length(offset);
-        float offsetError = _linearDistance - offsetLength;
-
-        // if (glm::abs(offsetError) > IGNORE_POSITION_DELTA) {
-        if (glm::abs(offsetError) > 0.0f) {
-            float offsetErrorAbs = glm::abs(offsetError);
-            float offsetErrorDirection = - offsetError / offsetErrorAbs;
-            glm::vec3 previousVelocity = bulletToGLM(rigidBody->getLinearVelocity());
-
-            glm::vec3 velocityAdjustment = glm::normalize(offset) * offsetErrorDirection * offsetErrorAbs / _linearTimeScale;
-            rigidBody->setLinearVelocity(glmToBullet(previousVelocity + velocityAdjustment));
-            // rigidBody->setLinearVelocity(glmToBullet(velocityAdjustment));
+    const float MAX_LINEAR_TIMESCALE = 600.0f;  // 10 minutes is a long time
+    if (_positionalTargetSet && _linearTimeScale < MAX_LINEAR_TIMESCALE) {
+        if (_needsActivation) {
             rigidBody->activate();
+            _needsActivation = false;
+        }
+        glm::vec3 objectPosition = bulletToGLM(rigidBody->getCenterOfMassPosition());
+        glm::vec3 springAxis = objectPosition - _pointToOffsetFrom; // from anchor to object
+        float distance = glm::length(springAxis);
+        if (distance > FLT_EPSILON) {
+            springAxis /= distance;  // normalize springAxis
+
+            // compute (critically damped) target velocity of spring relaxation
+            glm::vec3 offset = (distance - _linearDistance) * springAxis;
+            glm::vec3 targetVelocity = (-1.0f / _linearTimeScale) * offset;
+
+            // compute current velocity and its parallel component
+            glm::vec3 currentVelocity = bulletToGLM(rigidBody->getLinearVelocity());
+            glm::vec3 parallelVelocity = glm::dot(currentVelocity, springAxis) * springAxis;
+
+            // we blend the parallel component with the spring's target velocity to get the new velocity
+            float blend = deltaTimeStep / _linearTimeScale;
+            if (blend > 1.0f) {
+                blend = 1.0f;
+            }
+            rigidBody->setLinearVelocity(glmToBullet(currentVelocity + blend * (targetVelocity - parallelVelocity));
         }
     }
 
@@ -75,45 +91,45 @@ void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) {
 
 
 bool ObjectActionOffset::updateArguments(QVariantMap arguments) {
-    bool pOk0 = true;
+    bool ok = true;
     glm::vec3 pointToOffsetFrom =
-        EntityActionInterface::extractVec3Argument("offset action", arguments, "pointToOffsetFrom", pOk0, true);
+        EntityActionInterface::extractVec3Argument("offset action", arguments, "pointToOffsetFrom", ok, true);
+    if (!ok) {
+        return false;
+    }
 
-    bool pOk1 = true;
+    ok = true;
     float linearTimeScale =
-        EntityActionInterface::extractFloatArgument("offset action", arguments, "linearTimeScale", pOk1, false);
+        EntityActionInterface::extractFloatArgument("offset action", arguments, "linearTimeScale", ok, false);
+    if (ok) { 
+        if (linearTimeScale <= 0.0f) {
+            qDebug() << "offset action -- linearTimeScale must be greater than zero.";
+            return false;
+        }
+    } else {
+        linearTimeScale = 0.1f;
+    }
 
-    bool pOk2 = true;
+    ok = true;
     float linearDistance =
-        EntityActionInterface::extractFloatArgument("offset action", arguments, "linearDistance", pOk2, false);
-
-    if (!pOk0) {
-        return false;
-    }
-    if (pOk1 && linearTimeScale <= 0.0f) {
-        qDebug() << "offset action -- linearTimeScale must be greater than zero.";
-        return false;
+        EntityActionInterface::extractFloatArgument("offset action", arguments, "linearDistance", ok, false);
+    if (!ok) {
+        linearDistance = 0.0f;
     }
 
-    lockForWrite();
-
-    _pointToOffsetFrom = pointToOffsetFrom;
-    _positionalTargetSet = true;
-
-    if (pOk1) {
+    // only change stuff if something actually changed
+    if (_pointToOffsetFrom != pointToOffsetFrom
+            || _linearTimeScale != linearTimeScale
+            || _linearDistance != linearDistance) {
+        lockForWrite();
+        _pointToOffsetFrom = pointToOffsetFrom;
         _linearTimeScale = linearTimeScale;
-    } else {
-        _linearTimeScale = 0.1f;
-    }
-
-    if (pOk2) {
         _linearDistance = linearDistance;
-    } else {
-        _linearDistance = 1.0f;
+        _positionalTargetSet = true;
+        _active = true;
+        _needsActivation = true;
+        unlock();
     }
-
-    _active = true;
-    unlock();
     return true;
 }
 
diff --git a/libraries/physics/src/ObjectActionOffset.h b/libraries/physics/src/ObjectActionOffset.h
index 1918da6996..a0190f3832 100644
--- a/libraries/physics/src/ObjectActionOffset.h
+++ b/libraries/physics/src/ObjectActionOffset.h
@@ -36,6 +36,7 @@ public:
     float _linearDistance;
     float _linearTimeScale;
     bool _positionalTargetSet;
+    bool _needsActivation = true;
 };
 
 #endif // hifi_ObjectActionOffset_h