From 8cbec06616186f5507d5434ec22eb0827a345b0e Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 18 Apr 2016 13:54:26 -0700
Subject: [PATCH] fix bug where kinematic objs pop on first step

---
 libraries/physics/src/ObjectMotionState.cpp | 7 +++++++
 libraries/physics/src/ObjectMotionState.h   | 2 ++
 libraries/physics/src/PhysicsEngine.cpp     | 1 +
 3 files changed, 10 insertions(+)

diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp
index 0468e02cb8..de435e80da 100644
--- a/libraries/physics/src/ObjectMotionState.cpp
+++ b/libraries/physics/src/ObjectMotionState.cpp
@@ -45,6 +45,7 @@ void ObjectMotionState::setWorldSimulationStep(uint32_t step) {
     worldSimulationStep = step;
 }
 
+// static
 uint32_t ObjectMotionState::getWorldSimulationStep() {
     return worldSimulationStep;
 }
@@ -298,6 +299,12 @@ void ObjectMotionState::updateBodyVelocities() {
     _body->setActivationState(ACTIVE_TAG);
 }
 
+void ObjectMotionState::updateLastKinematicStep() {
+    // NOTE: we init to worldSimulationStep - 1 so that: when any object transitions to kinematic
+    // it will compute a non-zero dt on its first step.
+    _lastKinematicStep = ObjectMotionState::getWorldSimulationStep() - 1;
+}
+
 void ObjectMotionState::updateBodyMassProperties() {
     float mass = getMass();
     btVector3 inertia(0.0f, 0.0f, 0.0f);
diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h
index 4293df6b3f..c0c10c6f71 100644
--- a/libraries/physics/src/ObjectMotionState.h
+++ b/libraries/physics/src/ObjectMotionState.h
@@ -86,6 +86,8 @@ public:
 
     void updateBodyMaterialProperties();
     void updateBodyVelocities();
+    void updateLastKinematicStep();
+
     virtual void updateBodyMassProperties();
 
     MotionStateType getType() const { return _type; }
diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 8edbe67431..73d95f0c7f 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -86,6 +86,7 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
             body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
             body->updateInertiaTensor();
             motionState->updateBodyVelocities();
+            motionState->updateLastKinematicStep();
             const float KINEMATIC_LINEAR_VELOCITY_THRESHOLD = 0.01f;  // 1 cm/sec
             const float KINEMATIC_ANGULAR_VELOCITY_THRESHOLD = 0.01f;  // ~1 deg/sec
             body->setSleepingThresholds(KINEMATIC_LINEAR_VELOCITY_THRESHOLD, KINEMATIC_ANGULAR_VELOCITY_THRESHOLD);