Merge pull request #5302 from AndrewMeadows/astatine

cleanup of ObjectActionSpring and AvatarActionHold

interface/src/avatar/AvatarActionHold.cpp

@@ -70,30 +70,14 @@ void AvatarActionHold::updateActionWorker(float deltaTimeStep) {
         return;
     }
 
-    // check for NaNs
-    if (position.x != position.x ||
-        position.y != position.y ||
-        position.z != position.z) {
-        qDebug() << "AvatarActionHold::updateActionWorker -- target position includes NaN";
-        return;
-    }
-    if (rotation.x != rotation.x ||
-        rotation.y != rotation.y ||
-        rotation.z != rotation.z ||
-        rotation.w != rotation.w) {
-        qDebug() << "AvatarActionHold::updateActionWorker -- target rotation includes NaN";
-        return;
-    }
-
     if (_positionalTarget != position || _rotationalTarget != rotation) {
         auto ownerEntity = _ownerEntity.lock();
         if (ownerEntity) {
             ownerEntity->setActionDataDirty(true);
         }
-    }
-
         _positionalTarget = position;
         _rotationalTarget = rotation;
+    }
     unlock();
 
     ObjectActionSpring::updateActionWorker(deltaTimeStep);
@@ -101,59 +85,51 @@ void AvatarActionHold::updateActionWorker(float deltaTimeStep) {
 
 
 bool AvatarActionHold::updateArguments(QVariantMap arguments) {
-    bool rPOk = true;
+    bool ok = true;
     glm::vec3 relativePosition =
-        EntityActionInterface::extractVec3Argument("hold", arguments, "relativePosition", rPOk, false);
+        EntityActionInterface::extractVec3Argument("hold", arguments, "relativePosition", ok, false);
-    bool rROk = true;
+    if (!ok) {
+        relativePosition = _relativePosition;
+    }
+
+    ok = true;
     glm::quat relativeRotation =
-        EntityActionInterface::extractQuatArgument("hold", arguments, "relativeRotation", rROk, false);
+        EntityActionInterface::extractQuatArgument("hold", arguments, "relativeRotation", ok, false);
-    bool tSOk = true;
+    if (!ok) {
+        relativeRotation = _relativeRotation;
+    }
+    
+    ok = true;
     float timeScale =
-        EntityActionInterface::extractFloatArgument("hold", arguments, "timeScale", tSOk, false);
+        EntityActionInterface::extractFloatArgument("hold", arguments, "timeScale", ok, false);
-    bool hOk = true;
+    if (!ok) {
+        timeScale = _linearTimeScale;
+    }
+
+    ok = true;
     QString hand =
-        EntityActionInterface::extractStringArgument("hold", arguments, "hand", hOk, false);
+        EntityActionInterface::extractStringArgument("hold", arguments, "hand", ok, false);
+    if (!ok || !(hand == "left" || hand == "right")) {
+        hand = _hand;
+    }
 
+    if (relativePosition != _relativePosition
+            || relativeRotation != _relativeRotation
+            || timeScale != _linearTimeScale
+            || hand != _hand) {
         lockForWrite();
-    if (rPOk) {
         _relativePosition = relativePosition;
-    } else {
-        _relativePosition = glm::vec3(0.0f, 0.0f, 1.0f);
-    }
-
-    if (rROk) {
         _relativeRotation = relativeRotation;
-    } else {
+        const float MIN_TIMESCALE = 0.1f;
-        _relativeRotation = glm::quat(0.0f, 0.0f, 0.0f, 1.0f);
+        _linearTimeScale = glm::min(MIN_TIMESCALE, timeScale);
-    }
+        _angularTimeScale = _linearTimeScale;
-
+        _hand = hand;
-    if (tSOk) {
-        _linearTimeScale = timeScale;
-        _angularTimeScale = timeScale;
-    } else {
-        _linearTimeScale = 0.2f;
-        _angularTimeScale = 0.2f;
-    }
-
-    if (hOk) {
-        hand = hand.toLower();
-        if (hand == "left") {
-            _hand = "left";
-        } else if (hand == "right") {
-            _hand = "right";
-        } else {
-            qDebug() << "hold action -- invalid hand argument:" << hand;
-            _hand = "right";
-        }
-    } else {
-        _hand = "right";
-    }
 
         _mine = true;
-    _positionalTargetSet = true;
-    _rotationalTargetSet = true;
         _active = true;
+        activateBody();
         unlock();
+    }
     return true;
 }
 

libraries/entities/src/EntityActionInterface.cpp

@@ -127,21 +127,21 @@ glm::vec3 EntityActionInterface::extractVec3Argument(QString objectName, QVarian
             qDebug() << objectName << "requires argument:" << argumentName;
         }
         ok = false;
-        return glm::vec3();
+        return glm::vec3(0.0f);
     }
 
     QVariant resultV = arguments[argumentName];
     if (resultV.type() != (QVariant::Type) QMetaType::QVariantMap) {
         qDebug() << objectName << "argument" << argumentName << "must be a map";
         ok = false;
-        return glm::vec3();
+        return glm::vec3(0.0f);
     }
 
     QVariantMap resultVM = resultV.toMap();
     if (!resultVM.contains("x") || !resultVM.contains("y") || !resultVM.contains("z")) {
-        qDebug() << objectName << "argument" << argumentName << "must be a map with keys of x, y, z";
+        qDebug() << objectName << "argument" << argumentName << "must be a map with keys: x, y, z";
         ok = false;
-        return glm::vec3();
+        return glm::vec3(0.0f);
     }
 
     QVariant xV = resultVM["x"];
@@ -155,9 +155,15 @@ glm::vec3 EntityActionInterface::extractVec3Argument(QString objectName, QVarian
     float y = yV.toFloat(&yOk);
     float z = zV.toFloat(&zOk);
     if (!xOk || !yOk || !zOk) {
-        qDebug() << objectName << "argument" << argumentName << "must be a map with keys of x, y, z and values of type float.";
+        qDebug() << objectName << "argument" << argumentName << "must be a map with keys: x, y, and z of type float.";
         ok = false;
-        return glm::vec3();
+        return glm::vec3(0.0f);
+    }
+
+    if (x != x || y != y || z != z) {
+        // at least one of the values is NaN
+        ok = false;
+        return glm::vec3(0.0f);
     }
 
     return glm::vec3(x, y, z);
@@ -181,8 +187,8 @@ glm::quat EntityActionInterface::extractQuatArgument(QString objectName, QVarian
     }
 
     QVariantMap resultVM = resultV.toMap();
-    if (!resultVM.contains("x") || !resultVM.contains("y") || !resultVM.contains("z")) {
+    if (!resultVM.contains("x") || !resultVM.contains("y") || !resultVM.contains("z") || !resultVM.contains("w")) {
-        qDebug() << objectName << "argument" << argumentName << "must be a map with keys of x, y, z";
+        qDebug() << objectName << "argument" << argumentName << "must be a map with keys: x, y, z, and w";
         ok = false;
         return glm::quat();
     }
@@ -202,12 +208,18 @@ glm::quat EntityActionInterface::extractQuatArgument(QString objectName, QVarian
     float w = wV.toFloat(&wOk);
     if (!xOk || !yOk || !zOk || !wOk) {
         qDebug() << objectName << "argument" << argumentName
-                 << "must be a map with keys of x, y, z, w and values of type float.";
+                 << "must be a map with keys: x, y, z, and w of type float.";
         ok = false;
         return glm::quat();
     }
 
-    return glm::quat(w, x, y, z);
+    if (x != x || y != y || z != z || w != w) {
+        // at least one of the components is NaN!
+        ok = false;
+        return glm::quat();
+    }
+
+    return glm::normalize(glm::quat(w, x, y, z));
 }
 
 float EntityActionInterface::extractFloatArgument(QString objectName, QVariantMap arguments,
@@ -224,7 +236,7 @@ float EntityActionInterface::extractFloatArgument(QString objectName, QVariantMa
     bool vOk = true;
     float v = vV.toFloat(&vOk);
 
-    if (!vOk) {
+    if (!vOk || v != v) {
         ok = false;
         return 0.0f;
     }

libraries/entities/src/EntityItem.cpp

@@ -1496,7 +1496,7 @@ bool EntityItem::addAction(EntitySimulation* simulation, EntityActionPointer act
 
     bool result = addActionInternal(simulation, action);
     if (!result) {
-        removeAction(simulation, action->getID());
+        removeActionInternal(action->getID());
     }
 
     unlock();
@@ -1520,6 +1520,7 @@ bool EntityItem::addActionInternal(EntitySimulation* simulation, EntityActionPoi
     QByteArray newDataCache = serializeActions(success);
     if (success) {
         _allActionsDataCache = newDataCache;
+        _dirtyFlags |= EntityItem::DIRTY_PHYSICS_ACTIVATION;
     }
     return success;
 }
@@ -1537,6 +1538,7 @@ bool EntityItem::updateAction(EntitySimulation* simulation, const QUuid& actionI
     bool success = action->updateArguments(arguments);
     if (success) {
         _allActionsDataCache = serializeActions(success);
+        _dirtyFlags |= EntityItem::DIRTY_PHYSICS_ACTIVATION;
     } else {
         qDebug() << "EntityItem::updateAction failed";
     }
@@ -1572,6 +1574,7 @@ bool EntityItem::removeActionInternal(const QUuid& actionID, EntitySimulation* s
 
         bool success = true;
         _allActionsDataCache = serializeActions(success);
+        _dirtyFlags |= EntityItem::DIRTY_PHYSICS_ACTIVATION;
         return success;
     }
     return false;
@@ -1590,6 +1593,7 @@ bool EntityItem::clearActions(EntitySimulation* simulation) {
     // empty _serializedActions means no actions for the EntityItem
     _actionsToRemove.clear();
     _allActionsDataCache.clear();
+    _dirtyFlags |= EntityItem::DIRTY_PHYSICS_ACTIVATION;
     unlock();
     return true;
 }

libraries/entities/src/EntityScriptingInterface.cpp

@@ -543,6 +543,9 @@ bool EntityScriptingInterface::actionWorker(const QUuid& entityID,
     }
 
     bool success = actor(simulation, entity);
+    if (success) {
+        _entityTree->entityChanged(entity);
+    }
     _entityTree->unlock();
 
     // transmit the change

libraries/physics/src/ObjectAction.cpp

@@ -129,3 +129,10 @@ void ObjectAction::setAngularVelocity(glm::vec3 angularVelocity) {
     rigidBody->activate();
 }
 
+void ObjectAction::activateBody() {
+    auto rigidBody = getRigidBody();
+    if (rigidBody) {
+        rigidBody->activate();
+    }
+}
+

libraries/physics/src/ObjectAction.h

@@ -55,6 +55,7 @@ protected:
     virtual void setLinearVelocity(glm::vec3 linearVelocity);
     virtual glm::vec3 getAngularVelocity();
     virtual void setAngularVelocity(glm::vec3 angularVelocity);
+    virtual void activateBody();
 
     void lockForRead() { _lock.lockForRead(); }
     bool tryLockForRead() { return _lock.tryLockForRead(); }

libraries/physics/src/ObjectActionOffset.cpp

@@ -59,10 +59,6 @@ void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) {
 
     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);
@@ -95,26 +91,21 @@ bool ObjectActionOffset::updateArguments(QVariantMap arguments) {
     glm::vec3 pointToOffsetFrom =
         EntityActionInterface::extractVec3Argument("offset action", arguments, "pointToOffsetFrom", ok, true);
     if (!ok) {
-        return false;
+        pointToOffsetFrom = _pointToOffsetFrom;
     }
 
     ok = true;
     float linearTimeScale =
         EntityActionInterface::extractFloatArgument("offset action", arguments, "linearTimeScale", ok, false);
-    if (ok) { 
+    if (!ok) { 
-        if (linearTimeScale <= 0.0f) {
+        linearTimeScale = _linearTimeScale;
-            qDebug() << "offset action -- linearTimeScale must be greater than zero.";
-            return false;
-        }
-    } else {
-        linearTimeScale = 0.1f;
     }
 
     ok = true;
     float linearDistance =
         EntityActionInterface::extractFloatArgument("offset action", arguments, "linearDistance", ok, false);
     if (!ok) {
-        linearDistance = 0.0f;
+        linearDistance = _linearDistance;
     }
 
     // only change stuff if something actually changed
@@ -127,7 +118,7 @@ bool ObjectActionOffset::updateArguments(QVariantMap arguments) {
         _linearDistance = linearDistance;
         _positionalTargetSet = true;
         _active = true;
-        _needsActivation = true;
+        activateBody();
         unlock();
     }
     return true;

libraries/physics/src/ObjectActionOffset.h

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

libraries/physics/src/ObjectActionSpring.cpp

@@ -17,14 +17,15 @@ const float SPRING_MAX_SPEED = 10.0f;
 
 const uint16_t ObjectActionSpring::springVersion = 1;
 
+
 ObjectActionSpring::ObjectActionSpring(const QUuid& id, EntityItemPointer ownerEntity) :
     ObjectAction(ACTION_TYPE_SPRING, id, ownerEntity),
     _positionalTarget(glm::vec3(0.0f)),
-    _linearTimeScale(0.2f),
+    _linearTimeScale(FLT_MAX),
-    _positionalTargetSet(false),
+    _positionalTargetSet(true),
     _rotationalTarget(glm::quat()),
-    _angularTimeScale(0.2f),
+    _angularTimeScale(FLT_MAX),
-    _rotationalTargetSet(false) {
+    _rotationalTargetSet(true) {
     #if WANT_DEBUG
     qDebug() << "ObjectActionSpring::ObjectActionSpring";
     #endif
@@ -61,130 +62,92 @@ void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) {
         return;
     }
 
-    // handle the linear part
+    const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
-    if (_positionalTargetSet) {
+    if (_linearTimeScale < MAX_TIMESCALE) {
-        // check for NaN
+        btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget);
-        if (_positionalTarget.x != _positionalTarget.x ||
+        float offsetLength = offset.length();
-            _positionalTarget.y != _positionalTarget.y ||
+        float speed = (offsetLength > FLT_EPSILON) ? glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED) : 0.0f;
-            _positionalTarget.z != _positionalTarget.z) {
-            qDebug() << "ObjectActionSpring::updateActionWorker -- target position includes NaN";
-            unlock();
-            lockForWrite();
-            _active = false;
-            unlock();
-            return;
-        }
-        glm::vec3 offset = _positionalTarget - bulletToGLM(rigidBody->getCenterOfMassPosition());
-        float offsetLength = glm::length(offset);
-        float speed = offsetLength / _linearTimeScale;
 
-        // cap speed
+        // this action is aggresively critically damped and defeats the current velocity
-        if (speed > SPRING_MAX_SPEED) {
+        rigidBody->setLinearVelocity((- speed / offsetLength) * offset);
-            speed = SPRING_MAX_SPEED;
     }
 
-        if (offsetLength > IGNORE_POSITION_DELTA) {
+    if (_angularTimeScale < MAX_TIMESCALE) {
-            glm::vec3 newVelocity = glm::normalize(offset) * speed;
+        btVector3 targetVelocity(0.0f, 0.0f, 0.0f);
-            rigidBody->setLinearVelocity(glmToBullet(newVelocity));
-            rigidBody->activate();
-        } else {
-            rigidBody->setLinearVelocity(glmToBullet(glm::vec3(0.0f)));
-        }
-    }
 
-    // handle rotation
+        btQuaternion bodyRotation = rigidBody->getOrientation();
-    if (_rotationalTargetSet) {
+        auto alignmentDot = bodyRotation.dot(glmToBullet(_rotationalTarget));
-        if (_rotationalTarget.x != _rotationalTarget.x ||
+        const float ALMOST_ONE = 0.99999f;
-            _rotationalTarget.y != _rotationalTarget.y ||
+        if (glm::abs(alignmentDot) < ALMOST_ONE) {
-            _rotationalTarget.z != _rotationalTarget.z ||
+            btQuaternion target = glmToBullet(_rotationalTarget);
-            _rotationalTarget.w != _rotationalTarget.w) {
+            if (alignmentDot < 0.0f) {
-            qDebug() << "AvatarActionHold::updateActionWorker -- target rotation includes NaN";
-            unlock();
-            lockForWrite();
-            _active = false;
-            unlock();
-            return;
-        }
-
-        glm::quat bodyRotation = bulletToGLM(rigidBody->getOrientation());
-        // if qZero and qOne are too close to each other, we can get NaN for angle.
-        auto alignmentDot = glm::dot(bodyRotation, _rotationalTarget);
-        const float almostOne = 0.99999f;
-        if (glm::abs(alignmentDot) < almostOne) {
-            glm::quat target = _rotationalTarget;
-            if (alignmentDot < 0) {
                 target = -target;
             }
-            glm::quat qZeroInverse = glm::inverse(bodyRotation);
+            // if dQ is the incremental rotation that gets an object from Q0 to Q1 then:
-            glm::quat deltaQ = target * qZeroInverse;
+            //
-            glm::vec3 axis = glm::axis(deltaQ);
+            //      Q1 = dQ * Q0 
-            float angle = glm::angle(deltaQ);
+            //
-            assert(!isNaN(angle));
+            // solving for dQ gives:
-            glm::vec3 newAngularVelocity = (angle / _angularTimeScale) * glm::normalize(axis);
+            //
-            rigidBody->setAngularVelocity(glmToBullet(newAngularVelocity));
+            //      dQ = Q1 * Q0^
-            rigidBody->activate();
+            btQuaternion deltaQ = target * bodyRotation.inverse();
-        } else {
+            float angle = deltaQ.getAngle();
-            rigidBody->setAngularVelocity(glmToBullet(glm::vec3(0.0f)));
+            const float MIN_ANGLE = 1.0e-4;
+            if (angle > MIN_ANGLE) {
+                targetVelocity = (angle / _angularTimeScale) * deltaQ.getAxis();
             }
         }
-
+        // this action is aggresively critically damped and defeats the current velocity
+        rigidBody->setAngularVelocity(targetVelocity);
+    }
     unlock();
 }
 
+const float MIN_TIMESCALE = 0.1f;
 
 bool ObjectActionSpring::updateArguments(QVariantMap arguments) {
     // targets are required, spring-constants are optional
-    bool ptOk = true;
+    bool ok = true;
     glm::vec3 positionalTarget =
-        EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ptOk, false);
+        EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ok, false);
-    bool pscOk = true;
+    if (!ok) {
+        positionalTarget = _positionalTarget;
+    }
+    ok = true;
     float linearTimeScale =
-        EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", pscOk, false);
+        EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", ok, false);
-    if (ptOk && pscOk && linearTimeScale <= 0.0f) {
+    if (!ok || linearTimeScale <= 0.0f) {
-        qDebug() << "spring action -- linearTimeScale must be greater than zero.";
+        linearTimeScale = _linearTimeScale;
-        return false;
     }
 
-    bool rtOk = true;
+    ok = true;
     glm::quat rotationalTarget =
-        EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", rtOk, false);
+        EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", ok, false);
-    bool rscOk = true;
+    if (!ok) {
+        rotationalTarget = _rotationalTarget;
+    }
+
+    ok = true;
     float angularTimeScale =
-        EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", rscOk, false);
+        EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", ok, false);
-
+    if (!ok) {
-    if (!ptOk && !rtOk) {
+        angularTimeScale = _angularTimeScale;
-        qDebug() << "spring action requires at least one of targetPosition or targetRotation argument";
-        return false;
     }
 
+    if (positionalTarget != _positionalTarget
+            || linearTimeScale != _linearTimeScale
+            || rotationalTarget != _rotationalTarget
+            || angularTimeScale != _angularTimeScale) {
+        // something changed
         lockForWrite();
-
-    _positionalTargetSet = _rotationalTargetSet = false;
-
-    if (ptOk) {
         _positionalTarget = positionalTarget;
-        _positionalTargetSet = true;
+        _linearTimeScale = glm::max(MIN_TIMESCALE, glm::abs(linearTimeScale));
-
-        if (pscOk) {
-            _linearTimeScale = linearTimeScale;
-        } else {
-            _linearTimeScale = 0.1f;
-        }
-    }
-
-    if (rtOk) {
         _rotationalTarget = rotationalTarget;
-        _rotationalTargetSet = true;
+        _angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale));
-
-        if (rscOk) {
-            _angularTimeScale = angularTimeScale;
-        } else {
-            _angularTimeScale = 0.1f;
-        }
-    }
-
         _active = true;
+        activateBody();
         unlock();
+    }
     return true;
 }
 
@@ -192,15 +155,11 @@ QVariantMap ObjectActionSpring::getArguments() {
     QVariantMap arguments;
     lockForRead();
 
-    if (_positionalTargetSet) {
     arguments["linearTimeScale"] = _linearTimeScale;
     arguments["targetPosition"] = glmToQMap(_positionalTarget);
-    }
 
-    if (_rotationalTargetSet) {
     arguments["targetRotation"] = glmToQMap(_rotationalTarget);
     arguments["angularTimeScale"] = _angularTimeScale;
-    }
 
     unlock();
     return arguments;
@@ -210,7 +169,7 @@ QByteArray ObjectActionSpring::serialize() const {
     QByteArray serializedActionArguments;
     QDataStream dataStream(&serializedActionArguments, QIODevice::WriteOnly);
 
-    dataStream << getType();
+    dataStream << ACTION_TYPE_SPRING;
     dataStream << getID();
     dataStream << ObjectActionSpring::springVersion;
 
@@ -230,7 +189,7 @@ void ObjectActionSpring::deserialize(QByteArray serializedArguments) {
 
     EntityActionType type;
     dataStream >> type;
-    assert(type == getType());
+    assert(type == ACTION_TYPE_SPRING);
 
     QUuid id;
     dataStream >> id;