From 9c32a38fb63619f8084f53ba8c61a25b06ec40be Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 2 Jul 2018 15:01:57 -0700
Subject: [PATCH] fix deactivation slow-spinning kinematics

---
 libraries/physics/src/EntityMotionState.cpp       | 5 +++++
 libraries/physics/src/EntityMotionState.h         | 1 +
 libraries/physics/src/ObjectMotionState.cpp       | 6 +++++-
 libraries/physics/src/ObjectMotionState.h         | 1 +
 libraries/physics/src/ThreadSafeDynamicsWorld.cpp | 8 +++++++-
 5 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index e7b9f17739..0db1bdc926 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -834,3 +834,8 @@ void EntityMotionState::clearObjectVelocities() const {
     }
     _entity->setAcceleration(glm::vec3(0.0f));
 }
+
+void EntityMotionState::saveKinematicState(btScalar timeStep) {
+    _body->saveKinematicState(timeStep);
+    _body->setAngularVelocity(glmToBullet(_entity->getWorldAngularVelocity()));
+}
diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h
index c2d4f8d87e..b215537d55 100644
--- a/libraries/physics/src/EntityMotionState.h
+++ b/libraries/physics/src/EntityMotionState.h
@@ -97,6 +97,7 @@ public:
     OwnershipState getOwnershipState() const { return _ownershipState; }
 
     void setRegion(uint8_t region);
+    void saveKinematicState(btScalar timeStep) override;
 
 protected:
     void updateSendVelocities();
diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp
index 161d6bd636..0e6eb2511d 100644
--- a/libraries/physics/src/ObjectMotionState.cpp
+++ b/libraries/physics/src/ObjectMotionState.cpp
@@ -347,7 +347,7 @@ void ObjectMotionState::updateLastKinematicStep() {
 }
 
 void ObjectMotionState::updateBodyMassProperties() {
-    float mass = getMass();
+    btScalar mass = getMass();
     btVector3 inertia(1.0f, 1.0f, 1.0f);
     if (mass > 0.0f) {
         _body->getCollisionShape()->calculateLocalInertia(mass, inertia);
@@ -356,3 +356,7 @@ void ObjectMotionState::updateBodyMassProperties() {
     _body->updateInertiaTensor();
 }
 
+void ObjectMotionState::saveKinematicState(btScalar timeStep) {
+    _body->saveKinematicState(timeStep);
+}
+
diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h
index 4a3b200559..7439c1c38d 100644
--- a/libraries/physics/src/ObjectMotionState.h
+++ b/libraries/physics/src/ObjectMotionState.h
@@ -165,6 +165,7 @@ public:
 
     virtual bool isLocallyOwned() const { return false; }
     virtual bool isLocallyOwnedOrShouldBe() const { return false; } // aka shouldEmitCollisionEvents()
+    virtual void saveKinematicState(btScalar timeStep);
 
     friend class PhysicsEngine;
 
diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp
index 07d5ceb9ac..17a52f7cd9 100644
--- a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp
+++ b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp
@@ -162,7 +162,13 @@ void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) {
     for (int i=0;i<m_nonStaticRigidBodies.size();i++) {
         btRigidBody* body = m_nonStaticRigidBodies[i];
         if (body && body->isKinematicObject() && body->getActivationState() != ISLAND_SLEEPING) {
-            body->saveKinematicState(timeStep);
+            if (body->getMotionState()) {
+                btMotionState* motionState = body->getMotionState();
+                ObjectMotionState* objectMotionState = static_cast<ObjectMotionState*>(motionState);
+                objectMotionState->saveKinematicState(timeStep);
+            } else {
+                body->saveKinematicState(timeStep);
+            }
         }
     }
 }