Merge pull request #5288 from AndrewMeadows/arsenic

fix ObjectActionOffset logic

libraries/physics/src/EntityMotionState.cpp

@@ -248,7 +248,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
         btTransform xform = _body->getWorldTransform();
         _serverPosition = bulletToGLM(xform.getOrigin());
         _serverRotation = bulletToGLM(xform.getRotation());
-        _serverVelocity = bulletToGLM(_body->getLinearVelocity());
+        _serverVelocity = getBodyLinearVelocity();
         _serverAngularVelocity = bulletToGLM(_body->getAngularVelocity());
         _lastStep = simulationStep;
         _serverActionData = _entity->getActionData();
@@ -536,7 +536,7 @@ void EntityMotionState::bump(quint8 priority) {
 void EntityMotionState::resetMeasuredBodyAcceleration() {
     _lastMeasureStep = ObjectMotionState::getWorldSimulationStep();
     if (_body) {
-        _lastVelocity = bulletToGLM(_body->getLinearVelocity());
+        _lastVelocity = getBodyLinearVelocity();
     } else {
         _lastVelocity = glm::vec3(0.0f);
     }
@@ -555,7 +555,7 @@ void EntityMotionState::measureBodyAcceleration() {
 
         // Note: the integration equation for velocity uses damping:   v1 = (v0 + a * dt) * (1 - D)^dt
         // hence the equation for acceleration is: a = (v1 / (1 - D)^dt - v0) / dt
-        glm::vec3 velocity = bulletToGLM(_body->getLinearVelocity());
+        glm::vec3 velocity = getBodyLinearVelocity();
         _measuredAcceleration = (velocity / powf(1.0f - _body->getLinearDamping(), dt) - _lastVelocity) * invDt;
         _lastVelocity = velocity;
         if (numSubsteps > PHYSICS_ENGINE_MAX_NUM_SUBSTEPS) {

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.0f),
+    _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;
 }
 

libraries/physics/src/ObjectActionOffset.h

@@ -36,6 +36,7 @@ public:
     float _linearDistance;
     float _linearTimeScale;
     bool _positionalTargetSet;
+    bool _needsActivation = true;
 };
 
 #endif // hifi_ObjectActionOffset_h

libraries/physics/src/ObjectMotionState.cpp

@@ -80,8 +80,19 @@ void ObjectMotionState::setBodyGravity(const glm::vec3& gravity) const {
 }
 
 glm::vec3 ObjectMotionState::getBodyLinearVelocity() const {
-    return bulletToGLM(_body->getLinearVelocity());
+    // returns the body's velocity unless it is moving too slow in which case returns zero
+    btVector3 velocity = _body->getLinearVelocity();
+
+    // NOTE: the threshold to use here relates to the linear displacement threshold (dX) for sending updates
+    // to objects that are tracked server-side (e.g. entities which use dX = 2mm).  Hence an object moving 
+    // just under this velocity threshold would trigger an update about V/dX times per second.
+    const float MIN_LINEAR_SPEED_SQUARED = 0.0036f; // 6 mm/sec
+    if (velocity.length2() < MIN_LINEAR_SPEED_SQUARED) {
+        velocity *= 0.0f;
+    }
+    return bulletToGLM(velocity);
 }
+
 glm::vec3 ObjectMotionState::getObjectLinearVelocityChange() const {
     return glm::vec3(0.0f);  // Subclasses override where meaningful.
 }