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);
+        motionState->dirtyInternalKinematicChanges();
-        ownerEntity->setRotation(_rotationalTarget);
 
         _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 _expires { 0 }; // in seconds since epoch
-    quint64 localTimeToServerTime(quint64 timeValue) const;
+    bool _active { false };
-    quint64 serverTimeToLocalTime(quint64 timeValue) const;
 
 private:
     int getEntityServerClockSkew() const;

libraries/physics/src/ObjectMotionState.cpp

@@ -20,17 +20,17 @@
 // origin of physics simulation in world-frame
 glm::vec3 _worldOffset(0.0f);
 
-// static 
+// static
 void ObjectMotionState::setWorldOffset(const glm::vec3& offset) {
     _worldOffset = offset;
 }
 
-// static 
+// static
 const glm::vec3& ObjectMotionState::getWorldOffset() {
     return _worldOffset;
 }
 
-// static 
+// static
 uint32_t worldSimulationStep = 0;
 void ObjectMotionState::setWorldSimulationStep(uint32_t step) {
     assert(step > worldSimulationStep);
@@ -41,7 +41,7 @@ uint32_t ObjectMotionState::getWorldSimulationStep() {
     return worldSimulationStep;
 }
 
-// static 
+// static
 ShapeManager* shapeManager = nullptr;
 void ObjectMotionState::setShapeManager(ShapeManager* manager) {
     assert(manager);
@@ -85,7 +85,7 @@ glm::vec3 ObjectMotionState::getBodyLinearVelocity() const {
 
 glm::vec3 ObjectMotionState::getBodyLinearVelocityGTSigma() const {
     // 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 
+    // 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
 

libraries/physics/src/ObjectMotionState.h

@@ -44,12 +44,12 @@ enum MotionStateType {
     MOTIONSTATE_TYPE_AVATAR
 };
 
-// The update flags trigger two varieties of updates: "hard" which require the body to be pulled 
+// The update flags trigger two varieties of updates: "hard" which require the body to be pulled
 // and re-added to the physics engine and "easy" which just updates the body properties.
-const uint32_t HARD_DIRTY_PHYSICS_FLAGS = (uint32_t)(Simulation::DIRTY_MOTION_TYPE | Simulation::DIRTY_SHAPE | 
+const uint32_t HARD_DIRTY_PHYSICS_FLAGS = (uint32_t)(Simulation::DIRTY_MOTION_TYPE | Simulation::DIRTY_SHAPE |
                                                      Simulation::DIRTY_COLLISION_GROUP);
 const uint32_t EASY_DIRTY_PHYSICS_FLAGS = (uint32_t)(Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES |
-                                                     Simulation::DIRTY_MASS | Simulation::DIRTY_MATERIAL | 
+                                                     Simulation::DIRTY_MASS | Simulation::DIRTY_MATERIAL |
                                                      Simulation::DIRTY_SIMULATOR_ID | Simulation::DIRTY_SIMULATOR_OWNERSHIP);
 
 // These are the set of incoming flags that the PhysicsEngine needs to hear about:
@@ -66,7 +66,7 @@ class PhysicsEngine;
 class ObjectMotionState : public btMotionState {
 public:
     // These poroperties of the PhysicsEngine are "global" within the context of all ObjectMotionStates
-    // (assuming just one PhysicsEngine).  They are cached as statics for fast calculations in the 
+    // (assuming just one PhysicsEngine).  They are cached as statics for fast calculations in the
     // ObjectMotionState context.
     static void setWorldOffset(const glm::vec3& offset);
     static const glm::vec3& getWorldOffset();
@@ -121,7 +121,7 @@ public:
     virtual float getObjectFriction() const = 0;
     virtual float getObjectLinearDamping() const = 0;
     virtual float getObjectAngularDamping() const = 0;
-    
+
     virtual glm::vec3 getObjectPosition() const = 0;
     virtual glm::quat getObjectRotation() const = 0;
     virtual glm::vec3 getObjectLinearVelocity() const = 0;
@@ -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

@@ -4,8 +4,8 @@
  *
  * This software is provided 'as-is', without any express or implied warranty.
  * In no event will the authors be held liable for any damages arising from the use of this software.
- * Permission is granted to anyone to use this software for any purpose, 
+ * Permission is granted to anyone to use this software for any purpose,
- * including commercial applications, and to alter it and redistribute it freely, 
+ * including commercial applications, and to alter it and redistribute it freely,
  * subject to the following restrictions:
  *
  * 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -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,22 +124,22 @@ 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()));
                 }
             }
         }
-    } else  {       
+    } else  {
         //iterate over all active rigid bodies
         for (int i=0;i<m_nonStaticRigidBodies.size();i++) {
             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

@@ -4,8 +4,8 @@
  *
  * This software is provided 'as-is', without any express or implied warranty.
  * In no event will the authors be held liable for any damages arising from the use of this software.
- * Permission is granted to anyone to use this software for any purpose, 
+ * Permission is granted to anyone to use this software for any purpose,
- * including commercial applications, and to alter it and redistribute it freely, 
+ * including commercial applications, and to alter it and redistribute it freely,
  * subject to the following restrictions:
  *
  * 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
@@ -43,13 +43,16 @@ public:
     void synchronizeMotionStates();
 
     // btDiscreteDynamicsWorld::m_localTime is the portion of real-time that has not yet been simulated
-    // but is used for MotionState::setWorldTransform() extrapolation (a feature that Bullet uses to provide 
+    // but is used for MotionState::setWorldTransform() extrapolation (a feature that Bullet uses to provide
     // smoother rendering of objects when the physics simulation loop is ansynchronous to the render loop).
     float getLocalTimeAccumulation() const { return m_localTime; }
 
     VectorOfMotionStates& getChangedMotionStates() { return _changedMotionStates; }
 
 private:
+    // call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState()
+    void synchronizeMotionState(btRigidBody* body);
+
     VectorOfMotionStates _changedMotionStates;
 };