check dirty flags when harvesting physics results

libraries/entities/src/EntityItem.cpp

@@ -1610,6 +1610,37 @@ void EntityItem::setPosition(const glm::vec3& value) {
     }
 }
 
+void EntityItem::setWorldTransformAndVelocitiesUnlessDirtyFlags(
+        const glm::vec3& position,
+        const glm::quat& orientation,
+        const glm::vec3& linearVelocity,
+        const glm::vec3& angularVelocity) {
+    // only ever call this for harvesting results of physics simulation
+    // if a dirty bit is set then an update arrived (via script or network) overriding the physics simulation
+    uint32_t flags = _dirtyFlags & (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES);
+    if (!flags) {
+        // flags are clear
+        setWorldTransform(position, orientation);
+        setWorldVelocity(linearVelocity);
+        setWorldAngularVelocity(angularVelocity);
+        setLastSimulated(usecTimestampNow());
+    } else {
+        // only set properties NOT flagged
+        if (!(flags & Simulation::DIRTY_TRANSFORM)) {
+            setWorldTransform(position, orientation);
+        }
+        if (!(flags & Simulation::DIRTY_LINEAR_VELOCITY)) {
+            setWorldVelocity(linearVelocity);
+        }
+        if (!(flags & Simulation::DIRTY_ANGULAR_VELOCITY)) {
+            setWorldAngularVelocity(angularVelocity);
+        }
+        if (flags != (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES)) {
+            setLastSimulated(usecTimestampNow());
+        }
+    }
+}
+
 void EntityItem::setParentID(const QUuid& value) {
     QUuid oldParentID = getParentID();
     if (oldParentID != value) {
@@ -1739,6 +1770,22 @@ void EntityItem::setVelocity(const glm::vec3& value) {
     }
 }
 
+void EntityItem::zeroAllVelocitiesUnlessDirtyFlags() {
+    uint32_t flags = _dirtyFlags & (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES);
+    if (!flags) {
+        setWorldVelocity(glm::vec3(0.0f));
+        setWorldAngularVelocity(glm::vec3(0.0f));
+    } else {
+        if (!(flags & Simulation::DIRTY_LINEAR_VELOCITY)) {
+            setWorldVelocity(glm::vec3(0.0f));
+        }
+        if (!(flags & Simulation::DIRTY_ANGULAR_VELOCITY)) {
+            setWorldAngularVelocity(glm::vec3(0.0f));
+        }
+    }
+    _acceleration = glm::vec3(0.0f);
+}
+
 void EntityItem::setDamping(float value) {
     auto clampedDamping = glm::clamp(value, 0.0f, 1.0f);
     withWriteLock([&] {

libraries/entities/src/EntityItem.h

@@ -355,6 +355,7 @@ public:
 
     void setRotation(glm::quat orientation);
     void setVelocity(const glm::vec3& velocity);
+    void zeroAllVelocitiesUnlessDirtyFlags();
 
     uint32_t getDirtyFlags() const;
     void markDirtyFlags(uint32_t mask);
@@ -368,6 +369,13 @@ public:
     void* getPhysicsInfo() const { return _physicsInfo; }
 
     void setPhysicsInfo(void* data) { _physicsInfo = data; }
+
+    void setWorldTransformAndVelocitiesUnlessDirtyFlags(
+        const glm::vec3& position,
+        const glm::quat& orientation,
+        const glm::vec3& linearVelocity,
+        const glm::vec3& angularVelocity);
+
     EntityTreeElementPointer getElement() const { return _element; }
     EntityTreePointer getTree() const;
     virtual SpatialParentTree* getParentTree() const override;

libraries/physics/src/EntityMotionState.cpp

@@ -256,25 +256,12 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
     assert(_entity);
     assert(entityTreeIsLocked());
     measureBodyAcceleration();
-    bool positionSuccess;
+
-    _entity->setWorldPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset(), positionSuccess, false);
+    _entity->setWorldTransformAndVelocitiesUnlessDirtyFlags(
-    if (!positionSuccess) {
+            bulletToGLM(worldTrans.getOrigin()),
-        static QString repeatedMessage =
+            bulletToGLM(worldTrans.getRotation()),
-            LogHandler::getInstance().addRepeatedMessageRegex("EntityMotionState::setWorldTransform "
+            getBodyLinearVelocity(),
-                                                              "setPosition failed.*");
+            getBodyAngularVelocity());
-        qCDebug(physics) << "EntityMotionState::setWorldTransform setPosition failed" << _entity->getID();
-    }
-    bool orientationSuccess;
-    _entity->setWorldOrientation(bulletToGLM(worldTrans.getRotation()), orientationSuccess, false);
-    if (!orientationSuccess) {
-        static QString repeatedMessage =
-            LogHandler::getInstance().addRepeatedMessageRegex("EntityMotionState::setWorldTransform "
-                                                              "setOrientation failed.*");
-        qCDebug(physics) << "EntityMotionState::setWorldTransform setOrientation failed" << _entity->getID();
-    }
-    _entity->setVelocity(getBodyLinearVelocity());
-    _entity->setAngularVelocity(getBodyAngularVelocity());
-    _entity->setLastSimulated(usecTimestampNow());
 
     if (_entity->getSimulatorID().isNull()) {
         _loopsWithoutOwner++;
@@ -530,9 +517,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
 
     if (!_body->isActive()) {
         // make sure all derivatives are zero
-        _entity->setVelocity(Vectors::ZERO);
+        _entity->zeroAllVelocitiesUnlessDirtyFlags();
-        _entity->setAngularVelocity(Vectors::ZERO);
-        _entity->setAcceleration(Vectors::ZERO);
         _numInactiveUpdates++;
     } else {
         glm::vec3 gravity = _entity->getGravity();
@@ -559,9 +544,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
             if (movingSlowly) {
                 // velocities might not be zero, but we'll fake them as such, which will hopefully help convince
                 // other simulating observers to deactivate their own copies
-                glm::vec3 zero(0.0f);
+                _entity->zeroAllVelocitiesUnlessDirtyFlags();
-                _entity->setVelocity(zero);
-                _entity->setAngularVelocity(zero);
             }
         }
         _numInactiveUpdates = 0;

libraries/shared/src/GLMHelpers.h

@@ -253,6 +253,7 @@ glm::vec2 getFacingDir2D(const glm::mat4& m);
 
 inline bool isNaN(const glm::vec3& value) { return isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }
 inline bool isNaN(const glm::quat& value) { return isNaN(value.w) || isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }
+inline bool isNaN(const glm::mat3& value) { return isNaN(value * glm::vec3(1.0f)); }
 
 glm::mat4 orthoInverse(const glm::mat4& m);
 

libraries/shared/src/SpatiallyNestable.cpp

@@ -464,6 +464,36 @@ glm::vec3 SpatiallyNestable::localToWorldDimensions(const glm::vec3& dimensions,
     return dimensions;
 }
 
+void SpatiallyNestable::setWorldTransform(const glm::vec3& position, const glm::quat& orientation) {
+    // guard against introducing NaN into the transform
+    if (isNaN(orientation) || isNaN(position)) {
+        return;
+    }
+
+    bool changed = false;
+    bool success = true;
+    Transform parentTransform = getParentTransform(success);
+    _transformLock.withWriteLock([&] {
+        Transform myWorldTransform;
+        Transform::mult(myWorldTransform, parentTransform, _transform);
+        if (myWorldTransform.getRotation() != orientation) {
+            changed = true;
+            myWorldTransform.setRotation(orientation);
+        }
+        if (myWorldTransform.getTranslation() != position) {
+            changed = true;
+            myWorldTransform.setTranslation(position);
+        }
+        if (changed) {
+            Transform::inverseMult(_transform, parentTransform, myWorldTransform);
+            _translationChanged = usecTimestampNow();
+        }
+    });
+    if (success && changed) {
+        locationChanged(false);
+    }
+}
+
 glm::vec3 SpatiallyNestable::getWorldPosition(bool& success) const {
     return getTransform(success).getTranslation();
 }

libraries/shared/src/SpatiallyNestable.h

@@ -87,6 +87,7 @@ public:
 
     virtual Transform getParentTransform(bool& success, int depth = 0) const;
 
+    void setWorldTransform(const glm::vec3& position, const glm::quat& orientation);
     virtual glm::vec3 getWorldPosition(bool& success) const;
     virtual glm::vec3 getWorldPosition() const;
     virtual void setWorldPosition(const glm::vec3& position, bool& success, bool tellPhysics = true);