diff --git a/libraries/physics/src/PhysicalEntitySimulation.cpp b/libraries/physics/src/PhysicalEntitySimulation.cpp index aaa706b370..629c04ae19 100644 --- a/libraries/physics/src/PhysicalEntitySimulation.cpp +++ b/libraries/physics/src/PhysicalEntitySimulation.cpp @@ -167,12 +167,13 @@ void PhysicalEntitySimulation::getObjectsToRemoveFromPhysics(VectorOfMotionState _entitiesToAddToPhysics.remove(entity); EntityMotionState* motionState = static_cast(entity->getPhysicsInfo()); - assert(motionState); - _pendingChanges.remove(motionState); - _outgoingChanges.remove(motionState); - _physicalObjects.remove(motionState); - result.push_back(motionState); - _entitiesToRelease.insert(entity); + if (motionState) { + _pendingChanges.remove(motionState); + _outgoingChanges.remove(motionState); + _physicalObjects.remove(motionState); + result.push_back(motionState); + _entitiesToRelease.insert(entity); + } if (entity->isSimulated() && entity->isDead()) { _entitiesToDelete.insert(entity); diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 0a7ef606ba..d8108a8aed 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -136,23 +136,19 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { motionState->clearIncomingDirtyFlags(); } -// private -void PhysicsEngine::removeObjectFromDynamicsWorld(ObjectMotionState* object) { - // wake up anything touching this object - bump(object); - removeContacts(object); - - btRigidBody* body = object->getRigidBody(); - assert(body); - _dynamicsWorld->removeRigidBody(body); -} - void PhysicsEngine::removeObjects(const VectorOfMotionStates& objects) { + // first bump and prune contacts for all objects in the list for (auto object : objects) { - removeObjectFromDynamicsWorld(object); + bumpAndPruneContacts(object); + } + + // then remove them + for (auto object : objects) { + btRigidBody* body = object->getRigidBody(); + assert(body); + _dynamicsWorld->removeRigidBody(body); // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. - btRigidBody* body = object->getRigidBody(); object->setRigidBody(nullptr); body->setMotionState(nullptr); delete body; @@ -163,7 +159,8 @@ void PhysicsEngine::removeObjects(const VectorOfMotionStates& objects) { void PhysicsEngine::removeObjects(const SetOfMotionStates& objects) { for (auto object : objects) { btRigidBody* body = object->getRigidBody(); - removeObjectFromDynamicsWorld(object); + assert(body); + _dynamicsWorld->removeRigidBody(body); // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. object->setRigidBody(nullptr); @@ -200,7 +197,13 @@ VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& ob } void PhysicsEngine::reinsertObject(ObjectMotionState* object) { - removeObjectFromDynamicsWorld(object); + // remove object from DynamicsWorld + bumpAndPruneContacts(object); + btRigidBody* body = object->getRigidBody(); + assert(body); + _dynamicsWorld->removeRigidBody(body); + + // add it back addObjectToDynamicsWorld(object); } @@ -402,7 +405,7 @@ void PhysicsEngine::dumpStatsIfNecessary() { // CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing // CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing -void PhysicsEngine::bump(ObjectMotionState* motionState) { +void PhysicsEngine::bumpAndPruneContacts(ObjectMotionState* motionState) { // Find all objects that touch the object corresponding to motionState and flag the other objects // for simulation ownership by the local simulation. @@ -434,6 +437,7 @@ void PhysicsEngine::bump(ObjectMotionState* motionState) { } } } + removeContacts(motionState); } void PhysicsEngine::setCharacterController(CharacterController* character) { diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 16c8456e55..f644d6f5b2 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -78,9 +78,6 @@ public: /// \return position of simulation origin in domain-frame const glm::vec3& getOriginOffset() const { return _originOffset; } - /// \brief call bump on any objects that touch the object corresponding to motionState - void bump(ObjectMotionState* motionState); - void setCharacterController(CharacterController* character); void dumpNextStats() { _dumpNextStats = true; } @@ -94,7 +91,9 @@ public: private: void addObjectToDynamicsWorld(ObjectMotionState* motionState); - void removeObjectFromDynamicsWorld(ObjectMotionState* motionState); + + /// \brief bump any objects that touch this one, then remove contact info + void bumpAndPruneContacts(ObjectMotionState* motionState); void removeContacts(ObjectMotionState* motionState);