CollisionWorld only updates _active_ Aabbs

we manually set/clear active state of static objects
  that need their Aabbs updated
also fixing a bug when starting kinematic motion

libraries/physics/src/EntityMotionState.cpp

@@ -94,7 +94,7 @@ void EntityMotionState::updateServerPhysicsVariables() {
 }
 
 // virtual
-bool EntityMotionState::handleEasyChanges(uint32_t& flags) {
+void EntityMotionState::handleEasyChanges(uint32_t& flags) {
     assert(entityTreeIsLocked());
     updateServerPhysicsVariables();
     ObjectMotionState::handleEasyChanges(flags);
@@ -137,8 +137,6 @@ bool EntityMotionState::handleEasyChanges(uint32_t& flags) {
     if ((flags & Simulation::DIRTY_PHYSICS_ACTIVATION) && !_body->isActive()) {
         _body->activate();
     }
-
-    return true;
 }
 
 
@@ -422,19 +420,18 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, const Q
         const float ACCELERATION_EQUIVALENT_EPSILON_RATIO = 0.1f;
         if (accVsGravity < ACCELERATION_EQUIVALENT_EPSILON_RATIO * gravityLength) {
             // acceleration measured during the most recent simulation step was close to gravity.
-            if (getAccelerationNearlyGravityCount() < STEPS_TO_DECIDE_BALLISTIC) {
-                // only increment this if we haven't reached the threshold yet.  this is to avoid
-                // overflowing the counter.
-                incrementAccelerationNearlyGravityCount();
+            if (_accelerationNearlyGravityCount < STEPS_TO_DECIDE_BALLISTIC) {
+                // only increment this if we haven't reached the threshold yet, to avoid overflowing the counter
+                ++_accelerationNearlyGravityCount;
             }
         } else {
-            // acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter
-            resetAccelerationNearlyGravityCount();
+            // acceleration wasn't similar to this entity's gravity, reset the counter
+            _accelerationNearlyGravityCount = 0;
         }
 
         // if this entity has been accelerated at close to gravity for a certain number of simulation-steps, let
         // the entity server's estimates include gravity.
-        if (getAccelerationNearlyGravityCount() >= STEPS_TO_DECIDE_BALLISTIC) {
+        if (_accelerationNearlyGravityCount >= STEPS_TO_DECIDE_BALLISTIC) {
             _entity->setAcceleration(_entity->getGravity());
         } else {
             _entity->setAcceleration(glm::vec3(0.0f));

libraries/physics/src/EntityMotionState.h

@@ -29,7 +29,7 @@ public:
     virtual ~EntityMotionState();
 
     void updateServerPhysicsVariables();
-    virtual bool handleEasyChanges(uint32_t& flags) override;
+    virtual void handleEasyChanges(uint32_t& flags) override;
     virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
 
     /// \return PhysicsMotionType based on params set in EntityItem
@@ -51,10 +51,6 @@ public:
     virtual uint32_t getIncomingDirtyFlags() override;
     virtual void clearIncomingDirtyFlags() override;
 
-    void incrementAccelerationNearlyGravityCount() { _accelerationNearlyGravityCount++; }
-    void resetAccelerationNearlyGravityCount() { _accelerationNearlyGravityCount = 0; }
-    uint8_t getAccelerationNearlyGravityCount() { return _accelerationNearlyGravityCount; }
-
     virtual float getObjectRestitution() const override { return _entity->getRestitution(); }
     virtual float getObjectFriction() const override { return _entity->getFriction(); }
     virtual float getObjectLinearDamping() const override { return _entity->getDamping(); }

libraries/physics/src/ObjectMotionState.cpp

@@ -39,7 +39,7 @@ const glm::vec3& ObjectMotionState::getWorldOffset() {
 }
 
 // static
-uint32_t worldSimulationStep = 0;
+uint32_t worldSimulationStep = 1;
 void ObjectMotionState::setWorldSimulationStep(uint32_t step) {
     assert(step > worldSimulationStep);
     worldSimulationStep = step;
@@ -164,7 +164,7 @@ void ObjectMotionState::setRigidBody(btRigidBody* body) {
     }
 }
 
-bool ObjectMotionState::handleEasyChanges(uint32_t& flags) {
+void ObjectMotionState::handleEasyChanges(uint32_t& flags) {
     if (flags & Simulation::DIRTY_POSITION) {
         btTransform worldTrans = _body->getWorldTransform();
         btVector3 newPosition = glmToBullet(getObjectPosition());
@@ -183,6 +183,10 @@ bool ObjectMotionState::handleEasyChanges(uint32_t& flags) {
             worldTrans.setRotation(newRotation);
         }
         _body->setWorldTransform(worldTrans);
+        if (!(flags & HARD_DIRTY_PHYSICS_FLAGS) && _body->isStaticObject()) {
+            // force activate static body so its Aabb is updated later
+            _body->activate(true);
+        }
     } else if (flags & Simulation::DIRTY_ROTATION) {
         btTransform worldTrans = _body->getWorldTransform();
         btQuaternion newRotation = glmToBullet(getObjectRotation());
@@ -192,6 +196,10 @@ bool ObjectMotionState::handleEasyChanges(uint32_t& flags) {
         }
         worldTrans.setRotation(newRotation);
         _body->setWorldTransform(worldTrans);
+        if (!(flags & HARD_DIRTY_PHYSICS_FLAGS) && _body->isStaticObject()) {
+            // force activate static body so its Aabb is updated later
+            _body->activate(true);
+        }
     }
 
     if (flags & Simulation::DIRTY_LINEAR_VELOCITY) {
@@ -232,8 +240,6 @@ bool ObjectMotionState::handleEasyChanges(uint32_t& flags) {
     if (flags & Simulation::DIRTY_MASS) {
         updateBodyMassProperties();
     }
-
-    return true;
 }
 
 bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) {
@@ -292,6 +298,10 @@ void ObjectMotionState::updateBodyVelocities() {
     _body->setActivationState(ACTIVE_TAG);
 }
 
+void ObjectMotionState::updateLastKinematicStep() {
+    _lastKinematicStep = ObjectMotionState::getWorldSimulationStep() - 1;
+}
+
 void ObjectMotionState::updateBodyMassProperties() {
     float mass = getMass();
     btVector3 inertia(0.0f, 0.0f, 0.0f);

libraries/physics/src/ObjectMotionState.h

@@ -50,11 +50,12 @@ const uint32_t HARD_DIRTY_PHYSICS_FLAGS = (uint32_t)(Simulation::DIRTY_MOTION_TY
                                                      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_SIMULATOR_ID | Simulation::DIRTY_SIMULATION_OWNERSHIP_PRIORITY);
+                                                     Simulation::DIRTY_SIMULATOR_ID | Simulation::DIRTY_SIMULATION_OWNERSHIP_PRIORITY |
+                                                     Simulation::DIRTY_PHYSICS_ACTIVATION);
+
 
 // These are the set of incoming flags that the PhysicsEngine needs to hear about:
-const uint32_t DIRTY_PHYSICS_FLAGS = (uint32_t)(HARD_DIRTY_PHYSICS_FLAGS | EASY_DIRTY_PHYSICS_FLAGS |
-                                                Simulation::DIRTY_PHYSICS_ACTIVATION);
+const uint32_t DIRTY_PHYSICS_FLAGS = (uint32_t)(HARD_DIRTY_PHYSICS_FLAGS | EASY_DIRTY_PHYSICS_FLAGS);
 
 // These are the outgoing flags that the PhysicsEngine can affect:
 const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES;
@@ -80,11 +81,12 @@ public:
     ObjectMotionState(btCollisionShape* shape);
     ~ObjectMotionState();
 
-    virtual bool handleEasyChanges(uint32_t& flags);
+    virtual void handleEasyChanges(uint32_t& flags);
     virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine);
 
     void updateBodyMaterialProperties();
     void updateBodyVelocities();
+    void updateLastKinematicStep();
     virtual void updateBodyMassProperties();
 
     MotionStateType getType() const { return _type; }

libraries/physics/src/PhysicsEngine.cpp

@@ -50,6 +50,7 @@ void PhysicsEngine::init() {
         // default gravity of the world is zero, so each object must specify its own gravity
         // TODO: set up gravity zones
         _dynamicsWorld->setGravity(btVector3(0.0f, 0.0f, 0.0f));
+        _dynamicsWorld->setForceUpdateAllAabbs(false);
     }
 }
 
@@ -80,6 +81,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);
@@ -189,12 +191,18 @@ VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& ob
                 stillNeedChange.push_back(object);
             }
         } else if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
-            if (object->handleEasyChanges(flags)) {
-                object->clearIncomingDirtyFlags();
-            } else {
-                stillNeedChange.push_back(object);
-            }
+            object->handleEasyChanges(flags);
+            object->clearIncomingDirtyFlags();
         }
+        if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) {
+            _activeStaticBodies.push_back(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 outselves
+    for (int i = 0; i < _activeStaticBodies.size(); ++i) {
+        _dynamicsWorld->updateSingleAabb(_activeStaticBodies[i]);
     }
     return stillNeedChange;
 }
@@ -389,6 +397,12 @@ const CollisionEvents& PhysicsEngine::getCollisionEvents() {
 
 const VectorOfMotionStates& PhysicsEngine::getOutgoingChanges() {
     BT_PROFILE("copyOutgoingChanges");
+    // Bullet will not deactivate static objects (it doesn't expect them to be active)
+    // so we must deactivate them ourselves
+    for (int i = 0; i < _activeStaticBodies.size(); ++i) {
+        _activeStaticBodies[i]->forceActivationState(ISLAND_SLEEPING);
+    }
+    _activeStaticBodies.clear();
     _dynamicsWorld->synchronizeMotionStates();
     _hasOutgoingChanges = false;
     return _dynamicsWorld->getChangedMotionStates();

libraries/physics/src/PhysicsEngine.h

@@ -13,9 +13,9 @@
 #define hifi_PhysicsEngine_h
 
 #include <stdint.h>
+#include <vector>
 
 #include <QUuid>
-#include <QVector>
 #include <btBulletDynamicsCommon.h>
 #include <BulletCollision/CollisionDispatch/btGhostObject.h>
 
@@ -41,7 +41,7 @@ public:
 };
 
 typedef std::map<ContactKey, ContactInfo> ContactMap;
-typedef QVector<Collision> CollisionEvents;
+typedef std::vector<Collision> CollisionEvents;
 
 class PhysicsEngine {
 public:
@@ -110,6 +110,7 @@ private:
     ContactMap _contactMap;
     CollisionEvents _collisionEvents;
     QHash<QUuid, EntityActionPointer> _objectActions;
+    std::vector<btRigidBody*> _activeStaticBodies;
 
     glm::vec3 _originOffset;
     QUuid _sessionID;
@@ -121,7 +122,6 @@ private:
 
     bool _dumpNextStats = false;
     bool _hasOutgoingChanges = false;
-
 };
 
 typedef std::shared_ptr<PhysicsEngine> PhysicsEnginePointer;