flag kinematic objs when changed in stepSimulation()

interface/src/avatar/AvatarActionHold.cpp

@@ -119,8 +119,7 @@ void AvatarActionHold::doKinematicUpdate(float deltaTimeStep) {
         worldTrans.setRotation(glmToBullet(_rotationalTarget));
         rigidBody->setWorldTransform(worldTrans);
 
-        ownerEntity->setPosition(_positionalTarget);
-        ownerEntity->setRotation(_rotationalTarget);
+        motionState->dirtyInternalKinematicChanges();
 
         _previousPositionalTarget = _positionalTarget;
         _previousRotationalTarget = _rotationalTarget;

libraries/physics/src/ObjectAction.cpp

@@ -16,7 +16,6 @@
 ObjectAction::ObjectAction(EntityActionType type, const QUuid& id, EntityItemPointer ownerEntity) :
     btActionInterface(),
     EntityActionInterface(type, id),
-    _active(false),
     _ownerEntity(ownerEntity) {
 }
 

libraries/physics/src/ObjectAction.h

@@ -50,6 +50,8 @@ public:
     virtual quint64 getExpires() { return _expires; }
 
 protected:
+    quint64 localTimeToServerTime(quint64 timeValue) const;
+    quint64 serverTimeToLocalTime(quint64 timeValue) const;
 
     virtual btRigidBody* getRigidBody();
     virtual glm::vec3 getPosition();
@@ -62,14 +64,10 @@ protected:
     virtual void setAngularVelocity(glm::vec3 angularVelocity);
     virtual void activateBody();
 
-    bool _active;
     EntityItemWeakPointer _ownerEntity;
-
-    quint64 _expires; // in seconds since epoch
     QString _tag;
-
-    quint64 localTimeToServerTime(quint64 timeValue) const;
-    quint64 serverTimeToLocalTime(quint64 timeValue) const;
+    quint64 _expires { 0 }; // in seconds since epoch
+    bool _active { false };
 
 private:
     int getEntityServerClockSkew() const;

libraries/physics/src/ObjectMotionState.h

@@ -140,6 +140,11 @@ public:
 
     bool isActive() const { return _body ? _body->isActive() : false; }
 
+    bool hasInternalKinematicChanges() const { return _hasInternalKinematicChanges; }
+
+    void dirtyInternalKinematicChanges() { _hasInternalKinematicChanges = true; }
+    void clearInternalKinematicChanges() { _hasInternalKinematicChanges = false; }
+
     friend class PhysicsEngine;
 
 protected:
@@ -160,6 +165,7 @@ protected:
     float _mass;
 
     uint32_t _lastKinematicStep;
+    bool _hasInternalKinematicChanges { false };
 };
 
 typedef QSet<ObjectMotionState*> SetOfMotionStates;

libraries/physics/src/ThreadSafeDynamicsWorld.cpp

@@ -75,7 +75,7 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep
         }
     }
 
-    // NOTE: We do NOT call synchronizeMotionState() after each substep (to avoid multiple locks on the
+    // NOTE: We do NOT call synchronizeMotionStates() after each substep (to avoid multiple locks on the
     // object data outside of the physics engine).  A consequence of this is that the transforms of the
     // external objects only ever update at the end of the full step.
 
@@ -87,6 +87,33 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep
     return subSteps;
 }
 
+// call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState()
+void ThreadSafeDynamicsWorld::synchronizeMotionState(btRigidBody* body) {
+    btAssert(body);
+    if (body->getMotionState() && !body->isStaticObject()) {
+        //we need to call the update at least once, even for sleeping objects
+        //otherwise the 'graphics' transform never updates properly
+        ///@todo: add 'dirty' flag
+        //if (body->getActivationState() != ISLAND_SLEEPING)
+        {
+            if (body->isKinematicObject()) {
+                ObjectMotionState* objectMotionState = static_cast<ObjectMotionState*>(body->getMotionState());
+                if (!objectMotionState->hasInternalKinematicChanges()) {
+                    return;
+                } else {
+                    objectMotionState->clearInternalKinematicChanges();
+                }
+            }
+            btTransform interpolatedTransform;
+            btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
+                body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
+                (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
+                interpolatedTransform);
+            body->getMotionState()->setWorldTransform(interpolatedTransform);
+        }
+    }
+}
+
 void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
     _changedMotionStates.clear();
     BT_PROFILE("synchronizeMotionStates");
@@ -97,7 +124,7 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
             btRigidBody* body = btRigidBody::upcast(colObj);
             if (body) {
                 if (body->getMotionState()) {
-                    synchronizeSingleMotionState(body);
+                    synchronizeMotionState(body);
                     _changedMotionStates.push_back(static_cast<ObjectMotionState*>(body->getMotionState()));
                 }
             }
@@ -108,7 +135,7 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
             btRigidBody* body = m_nonStaticRigidBodies[i];
             if (body->isActive()) {
                 if (body->getMotionState()) {
-                    synchronizeSingleMotionState(body);
+                    synchronizeMotionState(body);
                     _changedMotionStates.push_back(static_cast<ObjectMotionState*>(body->getMotionState()));
                 }
             }

libraries/physics/src/ThreadSafeDynamicsWorld.h

@@ -50,6 +50,9 @@ public:
     VectorOfMotionStates& getChangedMotionStates() { return _changedMotionStates; }
 
 private:
+    // call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState()
+    void synchronizeMotionState(btRigidBody* body);
+
     VectorOfMotionStates _changedMotionStates;
 };