//// PhysicsEngine.cpp// libraries/physics/src//// Created by Andrew Meadows 2014.10.29// Copyright 2014 High Fidelity, Inc.//// Distributed under the Apache License, Version 2.0.// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include "PhysicsEngine.h" #include <functional> #include <QFile> #include <PerfStat.h>#include <PhysicsCollisionGroups.h>#include <Profile.h>#include <BulletCollision/CollisionShapes/btTriangleShape.h> #include "CharacterController.h"#include "ObjectMotionState.h"#include "PhysicsHelpers.h"#include "PhysicsDebugDraw.h"#include "ThreadSafeDynamicsWorld.h"#include "PhysicsLogging.h" PhysicsEngine::PhysicsEngine(const glm::vec3& offset) : _originOffset(offset), _myAvatarController(nullptr) {} PhysicsEngine::~PhysicsEngine() { _myAvatarController = nullptr; delete _collisionConfig; delete _collisionDispatcher; delete _broadphaseFilter; delete _constraintSolver; delete _dynamicsWorld; delete _ghostPairCallback;} void PhysicsEngine::init() { if (!_dynamicsWorld) { _collisionConfig = new btDefaultCollisionConfiguration(); _collisionDispatcher = new btCollisionDispatcher(_collisionConfig); _broadphaseFilter = new btDbvtBroadphase(); _constraintSolver = new btSequentialImpulseConstraintSolver; _dynamicsWorld = new ThreadSafeDynamicsWorld(_collisionDispatcher, _broadphaseFilter, _constraintSolver, _collisionConfig); _physicsDebugDraw.reset(new PhysicsDebugDraw()); // hook up debug draw renderer _dynamicsWorld->setDebugDrawer(_physicsDebugDraw.get()); _ghostPairCallback = new btGhostPairCallback(); _dynamicsWorld->getPairCache()->setInternalGhostPairCallback(_ghostPairCallback); // 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)); // By default Bullet will update the Aabb's of all objects every frame, even statics. // This can waste CPU cycles so we configure Bullet to only update ACTIVE objects here. // However, this means when a static object is moved we must manually update its Aabb // in order for its broadphase collision queries to work correctly. Look at how we use // _activeStaticBodies to track and update the Aabb's of moved static objects. _dynamicsWorld->setForceUpdateAllAabbs(false); }} uint32_t PhysicsEngine::getNumSubsteps() const { return _dynamicsWorld->getNumSubsteps();} int32_t PhysicsEngine::getNumCollisionObjects() const { return _dynamicsWorld ? _dynamicsWorld->getNumCollisionObjects() : 0;} // privatevoid PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { assert(motionState); btVector3 inertia(0.0f, 0.0f, 0.0f); float mass = 0.0f; // NOTE: the body may or may not already exist, depending on whether this corresponds to a reinsertion, or a new insertion. btRigidBody* body = motionState->getRigidBody(); PhysicsMotionType motionType = motionState->computePhysicsMotionType(); motionState->setMotionType(motionType); switch(motionType) { case MOTION_TYPE_KINEMATIC: { if (!body) { btCollisionShape* shape = const_cast<btCollisionShape*>(motionState->getShape()); assert(shape); body = new btRigidBody(mass, motionState, shape, inertia); motionState->setRigidBody(body); } else { body->setMassProps(mass, inertia); } body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); body->updateInertiaTensor(); motionState->updateBodyVelocities(); motionState->updateLastKinematicStep(); body->setSleepingThresholds(KINEMATIC_LINEAR_SPEED_THRESHOLD, KINEMATIC_ANGULAR_SPEED_THRESHOLD); motionState->clearInternalKinematicChanges(); break; } case MOTION_TYPE_DYNAMIC: { mass = motionState->getMass(); const float MIN_DYNAMIC_MASS = 0.01f; if (mass != mass || mass < MIN_DYNAMIC_MASS) { mass = MIN_DYNAMIC_MASS; } btCollisionShape* shape = const_cast<btCollisionShape*>(motionState->getShape()); assert(shape); shape->calculateLocalInertia(mass, inertia); if (!body) { body = new btRigidBody(mass, motionState, shape, inertia); motionState->setRigidBody(body); } else { body->setMassProps(mass, inertia); } body->setCollisionFlags(body->getCollisionFlags() & ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT)); body->updateInertiaTensor(); motionState->updateBodyVelocities(); // NOTE: Bullet will deactivate any object whose velocity is below these thresholds for longer than 2 seconds. // (the 2 seconds is determined by: static btRigidBody::gDeactivationTime body->setSleepingThresholds(DYNAMIC_LINEAR_SPEED_THRESHOLD, DYNAMIC_ANGULAR_SPEED_THRESHOLD); if (!motionState->isMoving()) { // try to initialize this object as inactive body->forceActivationState(ISLAND_SLEEPING); } break; } case MOTION_TYPE_STATIC: default: { if (!body) { assert(motionState->getShape()); body = new btRigidBody(mass, motionState, const_cast<btCollisionShape*>(motionState->getShape()), inertia); motionState->setRigidBody(body); } else { body->setMassProps(mass, inertia); } body->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT); body->updateInertiaTensor(); if (motionState->isLocallyOwned()) { _activeStaticBodies.insert(body); } break; } } body->setFlags(BT_DISABLE_WORLD_GRAVITY); motionState->updateBodyMaterialProperties(); int32_t group, mask; motionState->computeCollisionGroupAndMask(group, mask); _dynamicsWorld->addRigidBody(body, group, mask);} QList<EntityDynamicPointer> PhysicsEngine::removeDynamicsForBody(btRigidBody* body) { // remove dynamics that are attached to this body QList<EntityDynamicPointer> removedDynamics; QMutableSetIterator<QUuid> i(_objectDynamicsByBody[body]); while (i.hasNext()) { QUuid dynamicID = i.next(); if (dynamicID.isNull()) { continue; } EntityDynamicPointer dynamic = _objectDynamics[dynamicID]; if (!dynamic) { continue; } removeDynamic(dynamicID); removedDynamics += dynamic; } return removedDynamics;} void PhysicsEngine::removeObjects(const VectorOfMotionStates& objects) { // bump and prune contacts for all objects in the list for (auto object : objects) { bumpAndPruneContacts(object); } if (_activeStaticBodies.size() > 0) { // _activeStaticBodies was not cleared last frame. // The only way to get here is if a static object were moved but we did not actually step the simulation last // 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) { std::set<btRigidBody*>::iterator itr = _activeStaticBodies.find(object->getRigidBody()); if (itr != _activeStaticBodies.end()) { _activeStaticBodies.erase(itr); } } } // remove bodies for (auto object : objects) { btRigidBody* body = object->getRigidBody(); if (body) { removeDynamicsForBody(body); _dynamicsWorld->removeRigidBody(body); // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. object->setRigidBody(nullptr); body->setMotionState(nullptr); delete body; } }} // Same as above, but takes a Set instead of a Vector. Should only be called during teardown.void PhysicsEngine::removeSetOfObjects(const SetOfMotionStates& objects) { _contactMap.clear(); for (auto object : objects) { btRigidBody* body = object->getRigidBody(); if (body) { removeDynamicsForBody(body); _dynamicsWorld->removeRigidBody(body); // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. object->setRigidBody(nullptr); body->setMotionState(nullptr); delete body; } object->clearIncomingDirtyFlags(); } _activeStaticBodies.clear();} void PhysicsEngine::addObjects(const VectorOfMotionStates& objects) { for (auto object : objects) { addObjectToDynamicsWorld(object); }} void PhysicsEngine::reinsertObject(ObjectMotionState* object) { // remove object from DynamicsWorld bumpAndPruneContacts(object); btRigidBody* body = object->getRigidBody(); if (body) { _dynamicsWorld->removeRigidBody(body); // add it back addObjectToDynamicsWorld(object); }} void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction) { // removes for (auto object : transaction.objectsToRemove) { bumpAndPruneContacts(object); btRigidBody* body = object->getRigidBody(); if (body) { if (body->isStaticObject() && _activeStaticBodies.size() > 0) { std::set<btRigidBody*>::iterator itr = _activeStaticBodies.find(body); if (itr != _activeStaticBodies.end()) { _activeStaticBodies.erase(itr); } } removeDynamicsForBody(body); _dynamicsWorld->removeRigidBody(body); // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. object->setRigidBody(nullptr); body->setMotionState(nullptr); delete body; } } // adds for (auto object : transaction.objectsToAdd) { addObjectToDynamicsWorld(object); } // reinserts for (auto object : transaction.objectsToReinsert) { reinsertObject(object); } for (auto object : transaction.activeStaticObjects) { btRigidBody* body = object->getRigidBody(); _dynamicsWorld->updateSingleAabb(body); _activeStaticBodies.insert(body); }} void PhysicsEngine::removeContacts(ObjectMotionState* motionState) { // trigger events for new/existing/old contacts ContactMap::iterator contactItr = _contactMap.begin(); while (contactItr != _contactMap.end()) { if (contactItr->first._a == motionState || contactItr->first._b == motionState) { contactItr = _contactMap.erase(contactItr); } else { ++contactItr; } }} void PhysicsEngine::stepSimulation() { #ifndef BT_NO_PROFILE CProfileManager::Reset(); BT_PROFILE("stepSimulation"); #endif // NOTE: the grand order of operations is: // (1) pull incoming changes // (2) step simulation // (3) synchronize outgoing motion states // (4) send outgoing packets const float MAX_TIMESTEP = (float)PHYSICS_ENGINE_MAX_NUM_SUBSTEPS * PHYSICS_ENGINE_FIXED_SUBSTEP; float dt = 1.0e-6f * (float)(_clock.getTimeMicroseconds()); _clock.reset(); float timeStep = btMin(dt, MAX_TIMESTEP); auto onSubStep = [this]() { this->updateContactMap(); this->doOwnershipInfectionForConstraints(); }; int numSubsteps = _dynamicsWorld->stepSimulationWithSubstepCallback(timeStep, PHYSICS_ENGINE_MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP, onSubStep); if (numSubsteps > 0) { _hasOutgoingChanges = true; if (_physicsDebugDraw->getDebugMode()) { BT_PROFILE("debugDrawWorld"); _dynamicsWorld->debugDrawWorld(); } }}#ifndef BT_NO_PROFILEclass CProfileOperator {public: CProfileOperator() {} void recurse(CProfileIterator* itr, QString context) { // The context string will get too long if we accumulate it properly //QString newContext = context + QString("/") + itr->Get_Current_Parent_Name(); // so we use this four-character indentation QString newContext = context + QString(".../"); process(itr, newContext); // count the children int32_t numChildren = 0; itr->First(); while (!itr->Is_Done()) { itr->Next(); ++numChildren; } // recurse the children if (numChildren > 0) { // recurse the children for (int32_t i = 0; i < numChildren; ++i) { itr->Enter_Child(i); recurse(itr, newContext); } } // retreat back to parent itr->Enter_Parent(); } virtual void process(CProfileIterator*, QString context) = 0;}; class StatsHarvester : public CProfileOperator {public: StatsHarvester() {} void process(CProfileIterator* itr, QString context) override { QString name = context + itr->Get_Current_Parent_Name(); uint64_t time = (uint64_t)((btScalar)MSECS_PER_SECOND * itr->Get_Current_Parent_Total_Time()); PerformanceTimer::addTimerRecord(name, time); };}; class StatsWriter : public CProfileOperator {public: StatsWriter(QString filename) : _file(filename) { _file.open(QFile::WriteOnly); if (_file.error() != QFileDevice::NoError) { qCDebug(physics) << "unable to open file " << _file.fileName() << " to save stepSimulation() stats"; } } ~StatsWriter() { _file.close(); } void process(CProfileIterator* itr, QString context) override { QString name = context + itr->Get_Current_Parent_Name(); float time = (btScalar)MSECS_PER_SECOND * itr->Get_Current_Parent_Total_Time(); if (_file.error() == QFileDevice::NoError) { QTextStream s(&_file); s << name << " = " << time << "\n"; } }protected: QFile _file;};#endif void PhysicsEngine::harvestPerformanceStats() { #ifndef BT_NO_PROFILE // unfortunately the full context names get too long for our stats presentation format //QString contextName = PerformanceTimer::getContextName(); // TODO: how to show full context name? QString contextName("..."); CProfileIterator* itr = CProfileManager::Get_Iterator(); if (itr) { // hunt for stepSimulation context itr->First(); for (int32_t childIndex = 0; !itr->Is_Done(); ++childIndex) { if (QString(itr->Get_Current_Name()) == "stepSimulation") { itr->Enter_Child(childIndex); StatsHarvester harvester; harvester.recurse(itr, "physics/"); break; } itr->Next(); } } #endif} void PhysicsEngine::printPerformanceStatsToFile(const QString& filename) { #ifndef BT_NO_PROFILE CProfileIterator* itr = CProfileManager::Get_Iterator(); if (itr) { // hunt for stepSimulation context itr->First(); for (int32_t childIndex = 0; !itr->Is_Done(); ++childIndex) { if (QString(itr->Get_Current_Name()) == "stepSimulation") { itr->Enter_Child(childIndex); StatsWriter writer(filename); writer.recurse(itr, ""); break; } itr->Next(); } } #endif} void PhysicsEngine::doOwnershipInfection(const btCollisionObject* objectA, const btCollisionObject* objectB) { BT_PROFILE("ownershipInfection"); const btCollisionObject* characterObject = _myAvatarController ? _myAvatarController->getCollisionObject() : nullptr; ObjectMotionState* motionStateA = static_cast<ObjectMotionState*>(objectA->getUserPointer()); ObjectMotionState* motionStateB = static_cast<ObjectMotionState*>(objectB->getUserPointer()); if (motionStateB && ((motionStateA && motionStateA->getSimulatorID() == Physics::getSessionUUID() && !objectA->isStaticObject()) || (objectA == characterObject))) { // NOTE: we might own the simulation of a kinematic object (A) // but we don't claim ownership of kinematic objects (B) based on collisions here. if (!objectB->isStaticOrKinematicObject() && motionStateB->getSimulatorID() != Physics::getSessionUUID()) { uint8_t priorityA = motionStateA ? motionStateA->getSimulationPriority() : PERSONAL_SIMULATION_PRIORITY; motionStateB->bump(priorityA); } } else if (motionStateA && ((motionStateB && motionStateB->getSimulatorID() == Physics::getSessionUUID() && !objectB->isStaticObject()) || (objectB == characterObject))) { // SIMILARLY: we might own the simulation of a kinematic object (B) // but we don't claim ownership of kinematic objects (A) based on collisions here. if (!objectA->isStaticOrKinematicObject() && motionStateA->getSimulatorID() != Physics::getSessionUUID()) { uint8_t priorityB = motionStateB ? motionStateB->getSimulationPriority() : PERSONAL_SIMULATION_PRIORITY; motionStateA->bump(priorityB); } }} void PhysicsEngine::updateContactMap() { DETAILED_PROFILE_RANGE(simulation_physics, "updateContactMap"); BT_PROFILE("updateContactMap"); ++_numContactFrames; // update all contacts every frame int numManifolds = _collisionDispatcher->getNumManifolds(); for (int i = 0; i < numManifolds; ++i) { btPersistentManifold* contactManifold = _collisionDispatcher->getManifoldByIndexInternal(i); if (contactManifold->getNumContacts() > 0) { // TODO: require scripts to register interest in callbacks for specific objects // so we can filter out most collision events right here. const btCollisionObject* objectA = static_cast<const btCollisionObject*>(contactManifold->getBody0()); const btCollisionObject* objectB = static_cast<const btCollisionObject*>(contactManifold->getBody1()); if (!(objectA->isActive() || objectB->isActive())) { // both objects are inactive so stop tracking this contact, // which will eventually trigger a CONTACT_EVENT_TYPE_END continue; } ObjectMotionState* a = static_cast<ObjectMotionState*>(objectA->getUserPointer()); ObjectMotionState* b = static_cast<ObjectMotionState*>(objectB->getUserPointer()); if (a || b) { // the manifold has up to 4 distinct points, but only extract info from the firstLines 1–500 of 933. Load more
// // PhysicsEngine.cpp // libraries/physics/src // // Created by Andrew Meadows 2014.10.29 // Copyright 2014 High Fidelity, Inc. // // Distributed under the Apache License, Version 2.0. // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // #include "PhysicsEngine.h" #include <functional> #include <QFile> #include <PerfStat.h> #include <PhysicsCollisionGroups.h> #include <Profile.h> #include <BulletCollision/CollisionShapes/btTriangleShape.h> #include "CharacterController.h" #include "ObjectMotionState.h" #include "PhysicsHelpers.h" #include "PhysicsDebugDraw.h" #include "ThreadSafeDynamicsWorld.h" #include "PhysicsLogging.h" PhysicsEngine::PhysicsEngine(const glm::vec3& offset) : _originOffset(offset), _myAvatarController(nullptr) { } PhysicsEngine::~PhysicsEngine() { _myAvatarController = nullptr; delete _collisionConfig; delete _collisionDispatcher; delete _broadphaseFilter; delete _constraintSolver; delete _dynamicsWorld; delete _ghostPairCallback; } void PhysicsEngine::init() { if (!_dynamicsWorld) { _collisionConfig = new btDefaultCollisionConfiguration(); _collisionDispatcher = new btCollisionDispatcher(_collisionConfig); _broadphaseFilter = new btDbvtBroadphase(); _constraintSolver = new btSequentialImpulseConstraintSolver; _dynamicsWorld = new ThreadSafeDynamicsWorld(_collisionDispatcher, _broadphaseFilter, _constraintSolver, _collisionConfig); _physicsDebugDraw.reset(new PhysicsDebugDraw()); // hook up debug draw renderer _dynamicsWorld->setDebugDrawer(_physicsDebugDraw.get()); _ghostPairCallback = new btGhostPairCallback(); _dynamicsWorld->getPairCache()->setInternalGhostPairCallback(_ghostPairCallback); // 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)); // By default Bullet will update the Aabb's of all objects every frame, even statics. // This can waste CPU cycles so we configure Bullet to only update ACTIVE objects here. // However, this means when a static object is moved we must manually update its Aabb // in order for its broadphase collision queries to work correctly. Look at how we use // _activeStaticBodies to track and update the Aabb's of moved static objects. _dynamicsWorld->setForceUpdateAllAabbs(false); } } uint32_t PhysicsEngine::getNumSubsteps() const { return _dynamicsWorld->getNumSubsteps(); } int32_t PhysicsEngine::getNumCollisionObjects() const { return _dynamicsWorld ? _dynamicsWorld->getNumCollisionObjects() : 0; } // private void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { assert(motionState); btVector3 inertia(0.0f, 0.0f, 0.0f); float mass = 0.0f; // NOTE: the body may or may not already exist, depending on whether this corresponds to a reinsertion, or a new insertion. btRigidBody* body = motionState->getRigidBody(); PhysicsMotionType motionType = motionState->computePhysicsMotionType(); motionState->setMotionType(motionType); switch(motionType) { case MOTION_TYPE_KINEMATIC: { if (!body) { btCollisionShape* shape = const_cast<btCollisionShape*>(motionState->getShape()); assert(shape); body = new btRigidBody(mass, motionState, shape, inertia); motionState->setRigidBody(body); } else { body->setMassProps(mass, inertia); } body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); body->updateInertiaTensor(); motionState->updateBodyVelocities(); motionState->updateLastKinematicStep(); body->setSleepingThresholds(KINEMATIC_LINEAR_SPEED_THRESHOLD, KINEMATIC_ANGULAR_SPEED_THRESHOLD); motionState->clearInternalKinematicChanges(); break; } case MOTION_TYPE_DYNAMIC: { mass = motionState->getMass(); const float MIN_DYNAMIC_MASS = 0.01f; if (mass != mass || mass < MIN_DYNAMIC_MASS) { mass = MIN_DYNAMIC_MASS; } btCollisionShape* shape = const_cast<btCollisionShape*>(motionState->getShape()); assert(shape); shape->calculateLocalInertia(mass, inertia); if (!body) { body = new btRigidBody(mass, motionState, shape, inertia); motionState->setRigidBody(body); } else { body->setMassProps(mass, inertia); } body->setCollisionFlags(body->getCollisionFlags() & ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT)); body->updateInertiaTensor(); motionState->updateBodyVelocities(); // NOTE: Bullet will deactivate any object whose velocity is below these thresholds for longer than 2 seconds. // (the 2 seconds is determined by: static btRigidBody::gDeactivationTime body->setSleepingThresholds(DYNAMIC_LINEAR_SPEED_THRESHOLD, DYNAMIC_ANGULAR_SPEED_THRESHOLD); if (!motionState->isMoving()) { // try to initialize this object as inactive body->forceActivationState(ISLAND_SLEEPING); } break; } case MOTION_TYPE_STATIC: default: { if (!body) { assert(motionState->getShape()); body = new btRigidBody(mass, motionState, const_cast<btCollisionShape*>(motionState->getShape()), inertia); motionState->setRigidBody(body); } else { body->setMassProps(mass, inertia); } body->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT); body->updateInertiaTensor(); if (motionState->isLocallyOwned()) { _activeStaticBodies.insert(body); } break; } } body->setFlags(BT_DISABLE_WORLD_GRAVITY); motionState->updateBodyMaterialProperties(); int32_t group, mask; motionState->computeCollisionGroupAndMask(group, mask); _dynamicsWorld->addRigidBody(body, group, mask); } QList<EntityDynamicPointer> PhysicsEngine::removeDynamicsForBody(btRigidBody* body) { // remove dynamics that are attached to this body QList<EntityDynamicPointer> removedDynamics; QMutableSetIterator<QUuid> i(_objectDynamicsByBody[body]); while (i.hasNext()) { QUuid dynamicID = i.next(); if (dynamicID.isNull()) { continue; } EntityDynamicPointer dynamic = _objectDynamics[dynamicID]; if (!dynamic) { continue; } removeDynamic(dynamicID); removedDynamics += dynamic; } return removedDynamics; } void PhysicsEngine::removeObjects(const VectorOfMotionStates& objects) { // bump and prune contacts for all objects in the list for (auto object : objects) { bumpAndPruneContacts(object); } if (_activeStaticBodies.size() > 0) { // _activeStaticBodies was not cleared last frame. // The only way to get here is if a static object were moved but we did not actually step the simulation last // 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) { std::set<btRigidBody*>::iterator itr = _activeStaticBodies.find(object->getRigidBody()); if (itr != _activeStaticBodies.end()) { _activeStaticBodies.erase(itr); } } } // remove bodies for (auto object : objects) { btRigidBody* body = object->getRigidBody(); if (body) { removeDynamicsForBody(body); _dynamicsWorld->removeRigidBody(body); // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. object->setRigidBody(nullptr); body->setMotionState(nullptr); delete body; } } } // Same as above, but takes a Set instead of a Vector. Should only be called during teardown. void PhysicsEngine::removeSetOfObjects(const SetOfMotionStates& objects) { _contactMap.clear(); for (auto object : objects) { btRigidBody* body = object->getRigidBody(); if (body) { removeDynamicsForBody(body); _dynamicsWorld->removeRigidBody(body); // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. object->setRigidBody(nullptr); body->setMotionState(nullptr); delete body; } object->clearIncomingDirtyFlags(); } _activeStaticBodies.clear(); } void PhysicsEngine::addObjects(const VectorOfMotionStates& objects) { for (auto object : objects) { addObjectToDynamicsWorld(object); } } void PhysicsEngine::reinsertObject(ObjectMotionState* object) { // remove object from DynamicsWorld bumpAndPruneContacts(object); btRigidBody* body = object->getRigidBody(); if (body) { _dynamicsWorld->removeRigidBody(body); // add it back addObjectToDynamicsWorld(object); } } void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction) { // removes for (auto object : transaction.objectsToRemove) { bumpAndPruneContacts(object); btRigidBody* body = object->getRigidBody(); if (body) { if (body->isStaticObject() && _activeStaticBodies.size() > 0) { std::set<btRigidBody*>::iterator itr = _activeStaticBodies.find(body); if (itr != _activeStaticBodies.end()) { _activeStaticBodies.erase(itr); } } removeDynamicsForBody(body); _dynamicsWorld->removeRigidBody(body); // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. object->setRigidBody(nullptr); body->setMotionState(nullptr); delete body; } } // adds for (auto object : transaction.objectsToAdd) { addObjectToDynamicsWorld(object); } // reinserts for (auto object : transaction.objectsToReinsert) { reinsertObject(object); } for (auto object : transaction.activeStaticObjects) { btRigidBody* body = object->getRigidBody(); _dynamicsWorld->updateSingleAabb(body); _activeStaticBodies.insert(body); } } void PhysicsEngine::removeContacts(ObjectMotionState* motionState) { // trigger events for new/existing/old contacts ContactMap::iterator contactItr = _contactMap.begin(); while (contactItr != _contactMap.end()) { if (contactItr->first._a == motionState || contactItr->first._b == motionState) { contactItr = _contactMap.erase(contactItr); } else { ++contactItr; } } } void PhysicsEngine::stepSimulation() { #ifndef BT_NO_PROFILE CProfileManager::Reset(); BT_PROFILE("stepSimulation"); #endif // NOTE: the grand order of operations is: // (1) pull incoming changes // (2) step simulation // (3) synchronize outgoing motion states // (4) send outgoing packets const float MAX_TIMESTEP = (float)PHYSICS_ENGINE_MAX_NUM_SUBSTEPS * PHYSICS_ENGINE_FIXED_SUBSTEP; float dt = 1.0e-6f * (float)(_clock.getTimeMicroseconds()); _clock.reset(); float timeStep = btMin(dt, MAX_TIMESTEP); auto onSubStep = [this]() { this->updateContactMap(); this->doOwnershipInfectionForConstraints(); }; int numSubsteps = _dynamicsWorld->stepSimulationWithSubstepCallback(timeStep, PHYSICS_ENGINE_MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP, onSubStep); if (numSubsteps > 0) { _hasOutgoingChanges = true; if (_physicsDebugDraw->getDebugMode()) { BT_PROFILE("debugDrawWorld"); _dynamicsWorld->debugDrawWorld(); } } } #ifndef BT_NO_PROFILE class CProfileOperator { public: CProfileOperator() {} void recurse(CProfileIterator* itr, QString context) { // The context string will get too long if we accumulate it properly //QString newContext = context + QString("/") + itr->Get_Current_Parent_Name(); // so we use this four-character indentation QString newContext = context + QString(".../"); process(itr, newContext); // count the children int32_t numChildren = 0; itr->First(); while (!itr->Is_Done()) { itr->Next(); ++numChildren; } // recurse the children if (numChildren > 0) { // recurse the children for (int32_t i = 0; i < numChildren; ++i) { itr->Enter_Child(i); recurse(itr, newContext); } } // retreat back to parent itr->Enter_Parent(); } virtual void process(CProfileIterator*, QString context) = 0; }; class StatsHarvester : public CProfileOperator { public: StatsHarvester() {} void process(CProfileIterator* itr, QString context) override { QString name = context + itr->Get_Current_Parent_Name(); uint64_t time = (uint64_t)((btScalar)MSECS_PER_SECOND * itr->Get_Current_Parent_Total_Time()); PerformanceTimer::addTimerRecord(name, time); }; }; class StatsWriter : public CProfileOperator { public: StatsWriter(QString filename) : _file(filename) { _file.open(QFile::WriteOnly); if (_file.error() != QFileDevice::NoError) { qCDebug(physics) << "unable to open file " << _file.fileName() << " to save stepSimulation() stats"; } } ~StatsWriter() { _file.close(); } void process(CProfileIterator* itr, QString context) override { QString name = context + itr->Get_Current_Parent_Name(); float time = (btScalar)MSECS_PER_SECOND * itr->Get_Current_Parent_Total_Time(); if (_file.error() == QFileDevice::NoError) { QTextStream s(&_file); s << name << " = " << time << "\n"; } } protected: QFile _file; }; #endif void PhysicsEngine::harvestPerformanceStats() { #ifndef BT_NO_PROFILE // unfortunately the full context names get too long for our stats presentation format //QString contextName = PerformanceTimer::getContextName(); // TODO: how to show full context name? QString contextName("..."); CProfileIterator* itr = CProfileManager::Get_Iterator(); if (itr) { // hunt for stepSimulation context itr->First(); for (int32_t childIndex = 0; !itr->Is_Done(); ++childIndex) { if (QString(itr->Get_Current_Name()) == "stepSimulation") { itr->Enter_Child(childIndex); StatsHarvester harvester; harvester.recurse(itr, "physics/"); break; } itr->Next(); } } #endif } void PhysicsEngine::printPerformanceStatsToFile(const QString& filename) { #ifndef BT_NO_PROFILE CProfileIterator* itr = CProfileManager::Get_Iterator(); if (itr) { // hunt for stepSimulation context itr->First(); for (int32_t childIndex = 0; !itr->Is_Done(); ++childIndex) { if (QString(itr->Get_Current_Name()) == "stepSimulation") { itr->Enter_Child(childIndex); StatsWriter writer(filename); writer.recurse(itr, ""); break; } itr->Next(); } } #endif } void PhysicsEngine::doOwnershipInfection(const btCollisionObject* objectA, const btCollisionObject* objectB) { BT_PROFILE("ownershipInfection"); const btCollisionObject* characterObject = _myAvatarController ? _myAvatarController->getCollisionObject() : nullptr; ObjectMotionState* motionStateA = static_cast<ObjectMotionState*>(objectA->getUserPointer()); ObjectMotionState* motionStateB = static_cast<ObjectMotionState*>(objectB->getUserPointer()); if (motionStateB && ((motionStateA && motionStateA->getSimulatorID() == Physics::getSessionUUID() && !objectA->isStaticObject()) || (objectA == characterObject))) { // NOTE: we might own the simulation of a kinematic object (A) // but we don't claim ownership of kinematic objects (B) based on collisions here. if (!objectB->isStaticOrKinematicObject() && motionStateB->getSimulatorID() != Physics::getSessionUUID()) { uint8_t priorityA = motionStateA ? motionStateA->getSimulationPriority() : PERSONAL_SIMULATION_PRIORITY; motionStateB->bump(priorityA); } } else if (motionStateA && ((motionStateB && motionStateB->getSimulatorID() == Physics::getSessionUUID() && !objectB->isStaticObject()) || (objectB == characterObject))) { // SIMILARLY: we might own the simulation of a kinematic object (B) // but we don't claim ownership of kinematic objects (A) based on collisions here. if (!objectA->isStaticOrKinematicObject() && motionStateA->getSimulatorID() != Physics::getSessionUUID()) { uint8_t priorityB = motionStateB ? motionStateB->getSimulationPriority() : PERSONAL_SIMULATION_PRIORITY; motionStateA->bump(priorityB); } } } void PhysicsEngine::updateContactMap() { DETAILED_PROFILE_RANGE(simulation_physics, "updateContactMap"); BT_PROFILE("updateContactMap"); ++_numContactFrames; // update all contacts every frame int numManifolds = _collisionDispatcher->getNumManifolds(); for (int i = 0; i < numManifolds; ++i) { btPersistentManifold* contactManifold = _collisionDispatcher->getManifoldByIndexInternal(i); if (contactManifold->getNumContacts() > 0) { // TODO: require scripts to register interest in callbacks for specific objects // so we can filter out most collision events right here. const btCollisionObject* objectA = static_cast<const btCollisionObject*>(contactManifold->getBody0()); const btCollisionObject* objectB = static_cast<const btCollisionObject*>(contactManifold->getBody1()); if (!(objectA->isActive() || objectB->isActive())) { // both objects are inactive so stop tracking this contact, // which will eventually trigger a CONTACT_EVENT_TYPE_END continue; } ObjectMotionState* a = static_cast<ObjectMotionState*>(objectA->getUserPointer()); ObjectMotionState* b = static_cast<ObjectMotionState*>(objectB->getUserPointer()); if (a || b) { // the manifold has up to 4 distinct points, but only extract info from the first