From 74827fc4c802ea3c60a4031818dc76502d80d007 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 1 Jun 2017 10:05:26 -0700
Subject: [PATCH] recently moved static objects added to changelist

since static objects are not active this will make the interface
release ownership of recently changed static objects
---
 libraries/physics/src/PhysicsEngine.cpp       | 43 +++++++++++--------
 libraries/physics/src/PhysicsEngine.h         |  3 +-
 .../physics/src/ThreadSafeDynamicsWorld.h     |  2 +
 3 files changed, 28 insertions(+), 20 deletions(-)

diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 3a02e95e7c..1c6faada1d 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -129,6 +129,9 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
             }
             body->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
             body->updateInertiaTensor();
+            if (motionState->isLocallyOwned()) {
+                _activeStaticBodies.insert(body);
+            }
             break;
         }
     }
@@ -174,19 +177,9 @@ void PhysicsEngine::removeObjects(const VectorOfMotionStates& objects) {
         // frame (because the framerate is faster than our physics simulation rate).  When this happens we must scan
         // _activeStaticBodies for objects that were recently deleted so we don't try to access a dangling pointer.
         for (auto object : objects) {
-            btRigidBody* body = object->getRigidBody();
-
-            std::vector<btRigidBody*>::reverse_iterator itr = _activeStaticBodies.rbegin();
-            while (itr != _activeStaticBodies.rend()) {
-                if (body == *itr) {
-                    if (*itr != *(_activeStaticBodies.rbegin())) {
-                        // swap with rbegin
-                        *itr = *(_activeStaticBodies.rbegin());
-                    }
-                    _activeStaticBodies.pop_back();
-                    break;
-                }
-                ++itr;
+            std::set<btRigidBody*>::iterator itr = _activeStaticBodies.find(object->getRigidBody());
+            if (itr != _activeStaticBodies.end()) {
+                _activeStaticBodies.erase(itr);
             }
         }
     }
@@ -245,14 +238,16 @@ VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& ob
             object->clearIncomingDirtyFlags();
         }
         if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) {
-            _activeStaticBodies.push_back(object->getRigidBody());
+            _activeStaticBodies.insert(object->getRigidBody());
         }
     }
     // active static bodies have changed (in an Easy way) and need their Aabbs updated
     // but we've configured Bullet to NOT update them automatically (for improved performance)
     // so we must do it ourselves
-    for (size_t i = 0; i < _activeStaticBodies.size(); ++i) {
-        _dynamicsWorld->updateSingleAabb(_activeStaticBodies[i]);
+    std::set<btRigidBody*>::const_iterator itr = _activeStaticBodies.begin();
+    while (itr != _activeStaticBodies.end()) {
+        _dynamicsWorld->updateSingleAabb(*itr);
+        ++itr;
     }
     return stillNeedChange;
 }
@@ -496,13 +491,23 @@ const CollisionEvents& PhysicsEngine::getCollisionEvents() {
 
 const VectorOfMotionStates& PhysicsEngine::getChangedMotionStates() {
     BT_PROFILE("copyOutgoingChanges");
+
+    _dynamicsWorld->synchronizeMotionStates();
+
     // Bullet will not deactivate static objects (it doesn't expect them to be active)
     // so we must deactivate them ourselves
-    for (size_t i = 0; i < _activeStaticBodies.size(); ++i) {
-        _activeStaticBodies[i]->forceActivationState(ISLAND_SLEEPING);
+    std::set<btRigidBody*>::const_iterator itr = _activeStaticBodies.begin();
+    while (itr != _activeStaticBodies.end()) {
+        btRigidBody* body = *itr;
+        body->forceActivationState(ISLAND_SLEEPING);
+        ObjectMotionState* motionState = static_cast<ObjectMotionState*>(body->getUserPointer());
+        if (motionState) {
+            _dynamicsWorld->addChangedMotionState(motionState);
+        }
+        ++itr;
     }
     _activeStaticBodies.clear();
-    _dynamicsWorld->synchronizeMotionStates();
+
     _hasOutgoingChanges = false;
     return _dynamicsWorld->getChangedMotionStates();
 }
diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h
index e9b29a43a4..b20d73f0e6 100644
--- a/libraries/physics/src/PhysicsEngine.h
+++ b/libraries/physics/src/PhysicsEngine.h
@@ -13,6 +13,7 @@
 #define hifi_PhysicsEngine_h
 
 #include <stdint.h>
+#include <set>
 #include <vector>
 
 #include <QUuid>
@@ -114,7 +115,7 @@ private:
     CollisionEvents _collisionEvents;
     QHash<QUuid, EntityDynamicPointer> _objectDynamics;
     QHash<btRigidBody*, QSet<QUuid>> _objectDynamicsByBody;
-    std::vector<btRigidBody*> _activeStaticBodies;
+    std::set<btRigidBody*> _activeStaticBodies;
 
     glm::vec3 _originOffset;
 
diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.h b/libraries/physics/src/ThreadSafeDynamicsWorld.h
index b4fcca8cdb..54c3ddb756 100644
--- a/libraries/physics/src/ThreadSafeDynamicsWorld.h
+++ b/libraries/physics/src/ThreadSafeDynamicsWorld.h
@@ -51,6 +51,8 @@ public:
     const VectorOfMotionStates& getChangedMotionStates() const { return _changedMotionStates; }
     const VectorOfMotionStates& getDeactivatedMotionStates() const { return _deactivatedStates; }
 
+    void addChangedMotionState(ObjectMotionState* motionState) { _changedMotionStates.push_back(motionState); }
+
 private:
     // call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState()
     void synchronizeMotionState(btRigidBody* body);