diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index dd8b3abb37..e4c1652b3f 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -299,6 +299,7 @@ public: void* getPhysicsInfo() const { return _physicsInfo; } + void setPhysicsInfo(void* data) { _physicsInfo = data; } EntityTreeElement* getElement() const { return _element; } EntitySimulation* getSimulation() const { return _simulation; } @@ -311,7 +312,6 @@ public: glm::vec3 entityToWorld(const glm::vec3& point) const; protected: - void setPhysicsInfo(void* data) { _physicsInfo = data; } static bool _sendPhysicsUpdates; EntityTypes::EntityType _type; diff --git a/libraries/entities/src/EntitySimulation.cpp b/libraries/entities/src/EntitySimulation.cpp index bd1dbcc63e..1cf3ba42e6 100644 --- a/libraries/entities/src/EntitySimulation.cpp +++ b/libraries/entities/src/EntitySimulation.cpp @@ -56,7 +56,7 @@ void EntitySimulation::getEntitiesToDelete(SetOfEntities& entitiesToDelete) { } } -// private +// protected void EntitySimulation::expireMortalEntities(const quint64& now) { if (now > _nextExpiry) { // only search for expired entities if we expect to find one @@ -82,7 +82,7 @@ void EntitySimulation::expireMortalEntities(const quint64& now) { } } -// private +// protected void EntitySimulation::callUpdateOnEntitiesThatNeedIt(const quint64& now) { PerformanceTimer perfTimer("updatingEntities"); SetOfEntities::iterator itemItr = _entitiesToUpdate.begin(); @@ -99,7 +99,7 @@ void EntitySimulation::callUpdateOnEntitiesThatNeedIt(const quint64& now) { } } -// private +// protected void EntitySimulation::sortEntitiesThatMoved() { // NOTE: this is only for entities that have been moved by THIS EntitySimulation. // External changes to entity position/shape are expected to be sorted outside of the EntitySimulation. diff --git a/libraries/entities/src/EntitySimulation.h b/libraries/entities/src/EntitySimulation.h index 509c13a5a2..69bfe2622a 100644 --- a/libraries/entities/src/EntitySimulation.h +++ b/libraries/entities/src/EntitySimulation.h @@ -77,6 +77,8 @@ signals: protected: + void clearEntitySimulation(EntityItem* entity) { entity->_simulation = nullptr; } + // These pure virtual methods are protected because they are not to be called will-nilly. The base class // calls them in the right places. diff --git a/libraries/entities/src/SimpleEntitySimulation.cpp b/libraries/entities/src/SimpleEntitySimulation.cpp index 14e0374ea7..43d187796a 100644 --- a/libraries/entities/src/SimpleEntitySimulation.cpp +++ b/libraries/entities/src/SimpleEntitySimulation.cpp @@ -65,15 +65,15 @@ void SimpleEntitySimulation::removeEntityInternal(EntityItem* entity) { _movingEntities.remove(entity); _movableButStoppedEntities.remove(entity); _hasSimulationOwnerEntities.remove(entity); - entity->_simulation = nullptr; + clearEntitySimulation(entity); } void SimpleEntitySimulation::deleteEntityInternal(EntityItem* entity) { _movingEntities.remove(entity); _movableButStoppedEntities.remove(entity); _hasSimulationOwnerEntities.remove(entity); - entity->_simulation = nullptr; - if (!entity->_tree) { + clearEntitySimulation(entity); + if (!entity->getElement()) { // we held the last reference to this entity, so delete it delete entity; } diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index fddb89c399..fd1a53a0ce 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -54,21 +54,6 @@ MotionType EntityMotionState::computeObjectMotionType() const { return _entity->isMoving() ? MOTION_TYPE_KINEMATIC : MOTION_TYPE_STATIC; } -void EntityMotionState::updateKinematicState(uint32_t substep) { - setKinematic(_entity->isMoving(), substep); -} - -void EntityMotionState::stepKinematicSimulation(quint64 now) { - assert(_isKinematic); - // NOTE: this is non-physical kinematic motion which steps to real run-time (now) - // which is different from physical kinematic motion (inside getWorldTransform()) - // which steps in physics simulation time. - _entity->simulate(now); - // TODO: we can't use measureBodyAcceleration() here because the entity - // has no RigidBody and the timestep is a little bit out of sync with the physics simulation anyway. - // Hence we must manually measure kinematic velocity and acceleration. -} - bool EntityMotionState::isMoving() const { return _entity->isMoving(); } @@ -79,18 +64,18 @@ bool EntityMotionState::isMoving() const { // (2) at the beginning of each simulation step for KINEMATIC RigidBody's -- // it is an opportunity for outside code to update the object's simulation position void EntityMotionState::getWorldTransform(btTransform& worldTrans) const { - if (_isKinematic) { + if (_motionType == MOTION_TYPE_KINEMATIC) { // This is physical kinematic motion which steps strictly by the subframe count // of the physics simulation. - uint32_t substep = PhysicsEngine::getNumSubsteps(); - float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP; + uint32_t thisStep = ObjectMotionState::getWorldSimulationStep(); + float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP; _entity->simulateKinematicMotion(dt); _entity->setLastSimulated(usecTimestampNow()); - // bypass const-ness so we can remember the substep - const_cast(this)->_lastKinematicSubstep = substep; + // bypass const-ness so we can remember the step + const_cast(this)->_lastKinematicStep = thisStep; } - worldTrans.setOrigin(glmToBullet(_entity->getPosition() - ObjectMotionState::getWorldOffset())); + worldTrans.setOrigin(glmToBullet(getObjectPosition())); worldTrans.setRotation(glmToBullet(_entity->getRotation())); } @@ -101,13 +86,8 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { _entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset()); _entity->setRotation(bulletToGLM(worldTrans.getRotation())); - glm::vec3 v; - getVelocity(v); - _entity->setVelocity(v); - - glm::vec3 av; - getAngularVelocity(av); - _entity->setAngularVelocity(av); + _entity->setVelocity(getBodyLinearVelocity()); + _entity->setAngularVelocity(getBodyAngularVelocity()); _entity->setLastSimulated(usecTimestampNow()); @@ -405,3 +385,32 @@ uint32_t EntityMotionState::getIncomingDirtyFlags() const { return dirtyFlags; */ } + +void EntityMotionState::resetMeasuredBodyAcceleration() { + _lastMeasureStep = ObjectMotionState::getWorldSimulationStep(); + _lastVelocity = bulletToGLM(_body->getLinearVelocity()); + _measuredAcceleration = glm::vec3(0.0f); +} + +void EntityMotionState::measureBodyAcceleration() { + // try to manually measure the true acceleration of the object + uint32_t thisStep = ObjectMotionState::getWorldSimulationStep(); + uint32_t numSubsteps = thisStep - _lastMeasureStep; + if (numSubsteps > 0) { + float dt = ((float)numSubsteps * PHYSICS_ENGINE_FIXED_SUBSTEP); + float invDt = 1.0f / dt; + _lastMeasureStep = thisStep; + + // Note: the integration equation for velocity uses damping: v1 = (v0 + a * dt) * (1 - D)^dt + // hence the equation for acceleration is: a = (v1 / (1 - D)^dt - v0) / dt + glm::vec3 velocity = bulletToGLM(_body->getLinearVelocity()); + _measuredAcceleration = (velocity / powf(1.0f - _body->getLinearDamping(), dt) - _lastVelocity) * invDt; + _lastVelocity = velocity; + } +} + +// virtual +void EntityMotionState::setMotionType(MotionType motionType) { + ObjectMotionState::setMotionType(motionType); + resetMeasuredBodyAcceleration(); +} diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index 9e8cf16c4f..db3ced229b 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/libraries/physics/src/EntityMotionState.h @@ -31,9 +31,6 @@ public: /// \return MOTION_TYPE_DYNAMIC or MOTION_TYPE_STATIC based on params set in EntityItem virtual MotionType computeObjectMotionType() const; - virtual void updateKinematicState(uint32_t substep); - virtual void stepKinematicSimulation(quint64 now); - virtual bool isMoving() const; // this relays incoming position/rotation to the RigidBody @@ -65,15 +62,20 @@ public: virtual float getObjectLinearDamping() const { return _entity->getDamping(); } virtual float getObjectAngularDamping() const { return _entity->getAngularDamping(); } - virtual const glm::vec3& getObjectPosition() const { return _entity->getPosition(); } + virtual glm::vec3 getObjectPosition() const { return _entity->getPosition() - ObjectMotionState::getWorldOffset(); } virtual const glm::quat& getObjectRotation() const { return _entity->getRotation(); } virtual const glm::vec3& getObjectLinearVelocity() const { return _entity->getVelocity(); } virtual const glm::vec3& getObjectAngularVelocity() const { return _entity->getAngularVelocity(); } virtual const glm::vec3& getObjectGravity() const { return _entity->getGravity(); } - EntityItem* getEntityItem() const { return _entityItem; } + EntityItem* getEntity() const { return _entity; } + + void resetMeasuredBodyAcceleration(); + void measureBodyAcceleration(); protected: + virtual void setMotionType(MotionType motionType); + EntityItem* _entity; bool _sentMoving; // true if last update was moving @@ -87,6 +89,10 @@ protected: glm::vec3 _sentGravity; glm::vec3 _sentAcceleration; + uint32_t _lastMeasureStep; + glm::vec3 _lastVelocity; + glm::vec3 _measuredAcceleration; + quint8 _accelerationNearlyGravityCount; bool _shouldClaimSimulationOwnership; quint32 _movingStepsWithoutSimulationOwner; diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index a49555a0d7..e5bb80512c 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -36,22 +36,26 @@ const glm::vec3& ObjectMotionState::getWorldOffset() { } // static -uint32_t _worldSimulationStep = 0; +uint32_t worldSimulationStep = 0; void ObjectMotionState::setWorldSimulationStep(uint32_t step) { - assert(step > _worldSimulationStep); - _worldSimulationStep = step; + assert(step > worldSimulationStep); + worldSimulationStep = step; +} + +uint32_t ObjectMotionState::getWorldSimulationStep() { + return worldSimulationStep; } // static -ShapeManager* _shapeManager = nullptr; -void ObjectMotionState::setShapeManager(ShapeManager* shapeManager) { - assert(shapeManager); - _shapeManager = shapeManager; +ShapeManager* shapeManager = nullptr; +void ObjectMotionState::setShapeManager(ShapeManager* manager) { + assert(manager); + shapeManager = manager; } ShapeManager* ObjectMotionState::getShapeManager() { - assert(_shapeManager); // you must properly set _shapeManager before calling getShapeManager() - return _shapeManager; + assert(shapeManager); // you must properly set shapeManager before calling getShapeManager() + return shapeManager; } ObjectMotionState::ObjectMotionState(btCollisionShape* shape) : @@ -59,43 +63,8 @@ ObjectMotionState::ObjectMotionState(btCollisionShape* shape) : _shape(shape), _body(nullptr), _mass(0.0f), - _lastSimulationStep(0), - _lastVelocity(0.0f), - _measuredAcceleration(0.0f) + _lastKinematicStep(worldSimulationStep) { - assert(_shape); -} - -ObjectMotionState::~ObjectMotionState() { - // NOTE: you MUST remove this MotionState from the world before you call the dtor. - assert(_body == nullptr); - if (_shape) { - // the ObjectMotionState owns a reference to its shape in the ShapeManager - // se we must release it - getShapeManager()->releaseShape(_shape); - _shape = nullptr; - } -} - -void ObjectMotionState::measureBodyAcceleration() { - // try to manually measure the true acceleration of the object - uint32_t numSubsteps = _worldSimulationStep - _lastSimulationStep; - if (numSubsteps > 0) { - float dt = ((float)numSubsteps * PHYSICS_ENGINE_FIXED_SUBSTEP); - float invDt = 1.0f / dt; - _lastSimulationStep = _worldSimulationStep; - - // Note: the integration equation for velocity uses damping: v1 = (v0 + a * dt) * (1 - D)^dt - // hence the equation for acceleration is: a = (v1 / (1 - D)^dt - v0) / dt - glm::vec3 velocity = bulletToGLM(_body->getLinearVelocity()); - _measuredAcceleration = (velocity / powf(1.0f - _body->getLinearDamping(), dt) - _lastVelocity) * invDt; - _lastVelocity = velocity; - } -} - -void ObjectMotionState::resetMeasuredBodyAcceleration() { - _lastSimulationStep = _worldSimulationStep; - _lastVelocity = bulletToGLM(_body->getLinearVelocity()); } void ObjectMotionState::setBodyLinearVelocity(const glm::vec3& velocity) const { @@ -110,7 +79,7 @@ void ObjectMotionState::setBodyGravity(const glm::vec3& gravity) const { _body->setGravity(glmToBullet(gravity)); } -glm::vec3 ObjectMotionState::getBodyVelocity() const { +glm::vec3 ObjectMotionState::getBodyLinearVelocity() const { return bulletToGLM(_body->getLinearVelocity()); } @@ -118,6 +87,17 @@ glm::vec3 ObjectMotionState::getBodyAngularVelocity() const { return bulletToGLM(_body->getAngularVelocity()); } +void ObjectMotionState::releaseShape() { + if (_shape) { + shapeManager->releaseShape(_shape); + _shape = nullptr; + } +} + +void ObjectMotionState::setMotionType(MotionType motionType) { + _motionType = motionType; +} + void ObjectMotionState::setRigidBody(btRigidBody* body) { // give the body a (void*) back-pointer to this ObjectMotionState if (_body != body) { diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index 83046d9cca..a96bf1de13 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/libraries/physics/src/ObjectMotionState.h @@ -18,7 +18,7 @@ #include #include "ContactInfo.h" -#include "ShapeInfo.h" +#include "ShapeManager.h" enum MotionType { MOTION_TYPE_STATIC, // no motion @@ -46,6 +46,7 @@ const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = EntityItem::DIRTY_POSITION | Entit class OctreeEditPacketSender; +class PhysicsEngine; extern const int MAX_NUM_NON_MOVING_UPDATES; @@ -56,18 +57,18 @@ public: // ObjectMotionState context. static void setWorldOffset(const glm::vec3& offset); static const glm::vec3& getWorldOffset(); + static void setWorldSimulationStep(uint32_t step); - static void setShapeManager(ShapeManager* shapeManager); + static uint32_t getWorldSimulationStep(); + + static void setShapeManager(ShapeManager* manager); static ShapeManager* getShapeManager(); ObjectMotionState(btCollisionShape* shape); ~ObjectMotionState(); - void measureBodyAcceleration(); - void resetMeasuredBodyAcceleration(); - - void handleEasyChanges(); - void handleHardAndEasyChanges(); + void handleEasyChanges(uint32_t flags); + void handleHardAndEasyChanges(uint32_t flags, PhysicsEngine* engine); virtual void updateBodyMaterialProperties(); virtual void updateBodyVelocities(); @@ -82,7 +83,7 @@ public: void setBodyAngularVelocity(const glm::vec3& velocity) const; void setBodyGravity(const glm::vec3& gravity) const; - glm::vec3 getBodyVelocity() const; + glm::vec3 getBodyLinearVelocity() const; glm::vec3 getBodyAngularVelocity() const; virtual uint32_t getIncomingDirtyFlags() const = 0; @@ -93,6 +94,8 @@ public: btCollisionShape* getShape() const { return _shape; } btRigidBody* getRigidBody() const { return _body; } + void releaseShape(); + virtual bool isMoving() const = 0; // These pure virtual methods must be implemented for each MotionState type @@ -103,7 +106,7 @@ public: virtual float getObjectLinearDamping() const = 0; virtual float getObjectAngularDamping() const = 0; - virtual const glm::vec3& getObjectPosition() const = 0; + virtual glm::vec3 getObjectPosition() const = 0; virtual const glm::quat& getObjectRotation() const = 0; virtual const glm::vec3& getObjectLinearVelocity() const = 0; virtual const glm::vec3& getObjectAngularVelocity() const = 0; @@ -112,6 +115,7 @@ public: friend class PhysicsEngine; protected: + virtual void setMotionType(MotionType motionType); void setRigidBody(btRigidBody* body); MotionStateType _type = MOTION_STATE_TYPE_UNKNOWN; // type of MotionState @@ -121,9 +125,7 @@ protected: btRigidBody* _body; float _mass; - uint32_t _lastSimulationStep; - glm::vec3 _lastVelocity; - glm::vec3 _measuredAcceleration; + uint32_t _lastKinematicStep; }; #endif // hifi_ObjectMotionState_h diff --git a/libraries/physics/src/PhysicalEntitySimulation.cpp b/libraries/physics/src/PhysicalEntitySimulation.cpp index 08d899bb6f..9e6716cd38 100644 --- a/libraries/physics/src/PhysicalEntitySimulation.cpp +++ b/libraries/physics/src/PhysicalEntitySimulation.cpp @@ -9,13 +9,15 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // +#include "EntityMotionState.h" #include "PhysicalEntitySimulation.h" #include "PhysicsEngine.h" #include "PhysicsHelpers.h" #include "PhysicsLogging.h" #include "ShapeInfoUtil.h" +#include "ShapeManager.h" -PhysicalEntitySimulation::PhysicalEntitySimulation() : +PhysicalEntitySimulation::PhysicalEntitySimulation() { } PhysicalEntitySimulation::~PhysicalEntitySimulation() { @@ -29,10 +31,10 @@ void PhysicalEntitySimulation::init( assert(tree); setEntityTree(tree); - assert(*physicsEngine); + assert(physicsEngine); _physicsEngine = physicsEngine; - assert(*shapeManager); + assert(shapeManager); _shapeManager = shapeManager; assert(packetSender); @@ -44,26 +46,6 @@ void PhysicalEntitySimulation::updateEntitiesInternal(const quint64& now) { // TODO: add back non-physical kinematic objects and step them forward here } -void PhysicalEntitySimulation::sendOutgoingPackets() { - uint32_t numSubsteps = _physicsEngine->getNumSubsteps(); - if (_lastNumSubstepsAtUpdateInternal != numSubsteps) { - _lastNumSubstepsAtUpdateInternal = numSubsteps; - - SetOfMotionStates::iterator stateItr = _outgoingChanges.begin(); - while (stateItr != _outgoingChanges.end()) { - EntityMotionState* state = *stateItr; - if (state->doesNotNeedToSendUpdate()) { - stateItr = _outgoingChanges.erase(stateItr); - } else if (state->shouldSendUpdate(numSubsteps)) { - state->sendUpdate(_entityPacketSender, numSubsteps); - ++stateItr; - } else { - ++stateItr; - } - } - } -} - void PhysicalEntitySimulation::addEntityInternal(EntityItem* entity) { assert(entity); void* physicsInfo = entity->getPhysicsInfo(); @@ -76,10 +58,9 @@ void PhysicalEntitySimulation::removeEntityInternal(EntityItem* entity) { assert(entity); void* physicsInfo = entity->getPhysicsInfo(); if (physicsInfo) { - ObjectMotionState* motionState = static_cast(physicsInfo); - _pendingRemoves.insert(motionState); + _pendingRemoves.insert(entity); } else { - entity->_simulation = nullptr; + clearEntitySimulation(entity); } } @@ -87,10 +68,9 @@ void PhysicalEntitySimulation::deleteEntityInternal(EntityItem* entity) { assert(entity); void* physicsInfo = entity->getPhysicsInfo(); if (physicsInfo) { - ObjectMotionState* motionState = static_cast(physicsInfo); - _pendingRemoves.insert(motionState); + _pendingRemoves.insert(entity); } else { - entity->_simulation = nullptr; + clearEntitySimulation(entity); delete entity; } } @@ -100,10 +80,9 @@ void PhysicalEntitySimulation::entityChangedInternal(EntityItem* entity) { assert(entity); void* physicsInfo = entity->getPhysicsInfo(); if (physicsInfo) { - ObjectMotionState* motionState = static_cast(physicsInfo); - _pendingChanges.insert(motionState); + _pendingChanges.insert(entity); } else { - if (!entity->ignoreForCollisions()) { + if (!entity->getIgnoreForCollisions()) { // The intent is for this object to be in the PhysicsEngine. // Perhaps it's shape has changed and it can now be added? _pendingAdds.insert(entity); @@ -130,14 +109,16 @@ void PhysicalEntitySimulation::clearEntitiesInternal() { // TODO: we should probably wait to lock the _physicsEngine so we don't mess up data structures // while it is in the middle of a simulation step. As it is, we're probably in shutdown mode // anyway, so maybe the simulation was already properly shutdown? Cross our fingers... - SetOfMotionStates::const_iterator stateItr = _physicalEntities.begin(); + + _physicsEngine->deleteObjects(_physicalEntities); + for (auto stateItr : _physicalEntities) { - EntityMotionState motionState = static_cast(*stateItr); - _physicsEngine->removeObjectFromBullet(motionState); - EntityItem* entity = motionState->_entity; - _entity->setPhysicsInfo(nullptr); + EntityMotionState* motionState = static_cast(&(*stateItr)); + EntityItem* entity = motionState->getEntity(); + entity->setPhysicsInfo(nullptr); delete motionState; } + _physicalEntities.clear(); _pendingRemoves.clear(); _pendingAdds.clear(); @@ -149,13 +130,13 @@ void PhysicalEntitySimulation::clearEntitiesInternal() { VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToRemove() { _tempVector.clear(); for (auto entityItr : _pendingRemoves) { - EntityItem* entity = *entityItr; - _physicalEntities.remove(entity); + EntityItem* entity = &(*entityItr); _pendingAdds.remove(entity); _pendingChanges.remove(entity); - ObjectMotionState* motionState = static_cast(entity->getPhysicsInfo()); + EntityMotionState* motionState = static_cast(entity->getPhysicsInfo()); if (motionState) { _tempVector.push_back(motionState); + _physicalEntities.remove(motionState); } } _pendingRemoves.clear(); @@ -176,15 +157,14 @@ VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToAdd() { if (shape) { EntityMotionState* motionState = new EntityMotionState(shape, entity); entity->setPhysicsInfo(static_cast(motionState)); + motionState->setMass(entity->computeMass()); _physicalEntities.insert(motionState); + _tempVector.push_back(motionState); entityItr = _pendingAdds.erase(entityItr); - // TODO: figure out how to get shapeInfo (or relevant info) to PhysicsEngine during add - //_physicsEngine->addObject(shapeInfo, motionState); } else { ++entityItr; } } - _tempVector.push_back(motionState); } return _tempVector; } @@ -192,7 +172,7 @@ VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToAdd() { VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToChange() { _tempVector.clear(); for (auto entityItr : _pendingChanges) { - EntityItem* entity = *entityItr; + EntityItem* entity = &(*entityItr); ObjectMotionState* motionState = static_cast(entity->getPhysicsInfo()); if (motionState) { _tempVector.push_back(motionState); @@ -205,14 +185,32 @@ VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToChange() { void PhysicalEntitySimulation::handleOutgoingChanges(VectorOfMotionStates& motionStates) { // walk the motionStates looking for those that correspond to entities for (auto stateItr : motionStates) { - ObjectMotionState* state = *stateItr; + ObjectMotionState* state = &(*stateItr); if (state->getType() == MOTION_STATE_TYPE_ENTITY) { EntityMotionState* entityState = static_cast(state); _outgoingChanges.insert(entityState); - _entitiesToSort.insert(entityState->getEntityItem()); + _entitiesToSort.insert(entityState->getEntity()); + } + } + + // send outgoing packets + uint32_t numSubsteps = _physicsEngine->getNumSubsteps(); + if (_lastStepSendPackets != numSubsteps) { + _lastStepSendPackets = numSubsteps; + + QSet::iterator stateItr = _outgoingChanges.begin(); + while (stateItr != _outgoingChanges.end()) { + EntityMotionState* state = *stateItr; + if (state->doesNotNeedToSendUpdate()) { + stateItr = _outgoingChanges.erase(stateItr); + } else if (state->shouldSendUpdate(numSubsteps)) { + state->sendUpdate(_entityPacketSender, numSubsteps); + ++stateItr; + } else { + ++stateItr; + } } } - sendOutgoingPackets(); } void PhysicalEntitySimulation::bump(EntityItem* bumpEntity) { diff --git a/libraries/physics/src/PhysicalEntitySimulation.h b/libraries/physics/src/PhysicalEntitySimulation.h index ece564639c..2d49aa5d8a 100644 --- a/libraries/physics/src/PhysicalEntitySimulation.h +++ b/libraries/physics/src/PhysicalEntitySimulation.h @@ -22,14 +22,16 @@ #include "PhysicsTypedefs.h" +class EntityMotionState; class PhysicsEngine; +class ShapeManager; class PhysicalEntitySimulation :public EntitySimulation { public: PhysicalEntitySimulation(); ~PhysicalEntitySimulation(); - void init(EntityTree* tree, PhysicsEngine* engine, EntityEditPacketSender* packetSender); + void init(EntityTree* tree, PhysicsEngine* engine, ShapeManager* shapeManager, EntityEditPacketSender* packetSender); // overrides for EntitySimulation void updateEntitiesInternal(const quint64& now); @@ -64,7 +66,7 @@ private: PhysicsEngine* _physicsEngine = nullptr; EntityEditPacketSender* _entityPacketSender = nullptr; - uint32_t _lastNumSubstepsAtUpdateInternal = 0; + uint32_t _lastStepSendPackets = 0; }; #endif // hifi_PhysicalEntitySimulation_h diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index e6a256ca1e..af4ec42df8 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -68,11 +68,26 @@ void PhysicsEngine::removeObjects(VectorOfMotionStates& objects) { btRigidBody* body = object->getRigidBody(); _dynamicsWorld->removeRigidBody(body); + removeContacts(object); // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. object->setRigidBody(nullptr); delete body; + object->releaseShape(); + } +} + +void PhysicsEngine::deleteObjects(SetOfMotionStates& objects) { + for (auto object : objects) { + assert(object); + + btRigidBody* body = object->getRigidBody(); + _dynamicsWorld->removeRigidBody(body); removeContacts(object); + + // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it. + object->setRigidBody(nullptr); + delete body; object->releaseShape(); } } @@ -151,6 +166,7 @@ void PhysicsEngine::stepSimulation() { } void PhysicsEngine::doOwnershipInfection(const btCollisionObject* objectA, const btCollisionObject* objectB) { + /* TODO: Andrew to make this work for ObjectMotionState BT_PROFILE("ownershipInfection"); assert(objectA); assert(objectB); @@ -184,6 +200,7 @@ void PhysicsEngine::doOwnershipInfection(const btCollisionObject* objectA, const a->setShouldClaimSimulationOwnership(true); } } + */ } void PhysicsEngine::computeCollisionEvents() { @@ -215,7 +232,13 @@ void PhysicsEngine::computeCollisionEvents() { doOwnershipInfection(objectA, objectB); } } - + + fireCollisionEvents(); + ++_numContactFrames; +} + +void PhysicsEngine::fireCollisionEvents() { + /* TODO: Andrew to make this work for ObjectMotionStates const uint32_t CONTINUE_EVENT_FILTER_FREQUENCY = 10; // scan known contacts and trigger events @@ -253,14 +276,14 @@ void PhysicsEngine::computeCollisionEvents() { ++contactItr; } } - ++_numContactFrames; + */ } VectorOfMotionStates& PhysicsEngine::getOutgoingChanges() { BT_PROFILE("copyOutgoingChanges"); - _dynamicsWorld.synchronizeMotionStates(); + _dynamicsWorld->synchronizeMotionStates(); _hasOutgoingChanges = false; - return _dynamicsWorld.getChangedMotionStates(); + return _dynamicsWorld->getChangedMotionStates(); } void PhysicsEngine::dumpStatsIfNecessary() { @@ -287,7 +310,9 @@ void PhysicsEngine::addObject(ObjectMotionState* motionState) { btVector3 inertia(0.0f, 0.0f, 0.0f); float mass = 0.0f; btRigidBody* body = nullptr; - switch(motionState->computeObjectMotionType()) { + MotionType motionType = motionState->computeObjectMotionType(); + motionState->setMotionType(motionType); + switch(motionType) { case MOTION_TYPE_KINEMATIC: { body = new btRigidBody(mass, motionState, shape, inertia); body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); @@ -330,11 +355,50 @@ void PhysicsEngine::addObject(ObjectMotionState* motionState) { motionState->updateBodyMaterialProperties(); _dynamicsWorld->addRigidBody(body); - motionState->resetMeasuredBodyAcceleration(); } -/* TODO: convert bump() to take an ObjectMotionState. -* Expose SimulationID to ObjectMotionState*/ +void PhysicsEngine::bump(ObjectMotionState* object) { + /* TODO: Andrew to implement this + // If this node is doing something like deleting an entity, scan for contacts involving the + // entity. For each found, flag the other entity involved as being simulated by this node. + int numManifolds = _collisionDispatcher->getNumManifolds(); + for (int i = 0; i < numManifolds; ++i) { + btPersistentManifold* contactManifold = _collisionDispatcher->getManifoldByIndexInternal(i); + if (contactManifold->getNumContacts() > 0) { + const btCollisionObject* objectA = static_cast(contactManifold->getBody0()); + const btCollisionObject* objectB = static_cast(contactManifold->getBody1()); + if (objectA && objectB) { + void* a = objectA->getUserPointer(); + void* b = objectB->getUserPointer(); + if (a && b) { + EntityMotionState* entityMotionStateA = static_cast(a); + EntityMotionState* entityMotionStateB = static_cast(b); + EntityItem* entityA = entityMotionStateA ? entityMotionStateA->getEntity() : nullptr; + EntityItem* entityB = entityMotionStateB ? entityMotionStateB->getEntity() : nullptr; + if (entityA && entityB) { + if (entityA == bumpEntity) { + entityMotionStateB->setShouldClaimSimulationOwnership(true); + if (!objectB->isActive()) { + objectB->setActivationState(ACTIVE_TAG); + } + } + if (entityB == bumpEntity) { + entityMotionStateA->setShouldClaimSimulationOwnership(true); + if (!objectA->isActive()) { + objectA->setActivationState(ACTIVE_TAG); + } + } + } + } + } + } + } + */ +} + +/* +// TODO: convert bump() to take an ObjectMotionState. +// Expose SimulationID to ObjectMotionState void PhysicsEngine::bump(EntityItem* bumpEntity) { // If this node is doing something like deleting an entity, scan for contacts involving the // entity. For each found, flag the other entity involved as being simulated by this node. @@ -371,58 +435,13 @@ void PhysicsEngine::bump(EntityItem* bumpEntity) { } } } +*/ void PhysicsEngine::removeRigidBody(btRigidBody* body) { // pull body out of physics engine _dynamicsWorld->removeRigidBody(body); } -void PhysicsEngine::addRigidBody(btRigidBody* body, MotionType motionType, float mass) { - switch (motionType) { - case MOTION_TYPE_KINEMATIC: { - int collisionFlags = body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT; - collisionFlags &= ~(btCollisionObject::CF_STATIC_OBJECT); - body->setCollisionFlags(collisionFlags); - body->forceActivationState(DISABLE_DEACTIVATION); - - body->setMassProps(0.0f, btVector3(0.0f, 0.0f, 0.0f)); - body->updateInertiaTensor(); - break; - } - case MOTION_TYPE_DYNAMIC: { - int collisionFlags = body->getCollisionFlags() & ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT); - body->setCollisionFlags(collisionFlags); - if (! (flags & EntityItem::DIRTY_MASS)) { - // always update mass properties when going dynamic (unless it's already been done above) - btVector3 inertia(0.0f, 0.0f, 0.0f); - body->getCollisionShape()->calculateLocalInertia(mass, inertia); - body->setMassProps(mass, inertia); - body->updateInertiaTensor(); - } - body->forceActivationState(ACTIVE_TAG); - break; - } - default: { - // MOTION_TYPE_STATIC - int collisionFlags = body->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT; - collisionFlags &= ~(btCollisionObject::CF_KINEMATIC_OBJECT); - body->setCollisionFlags(collisionFlags); - body->forceActivationState(DISABLE_SIMULATION); - - body->setMassProps(0.0f, btVector3(0.0f, 0.0f, 0.0f)); - body->updateInertiaTensor(); - - body->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f)); - body->setAngularVelocity(btVector3(0.0f, 0.0f, 0.0f)); - break; - } - } - - _dynamicsWorld->addRigidBody(body); - - body->activate(); -} - void PhysicsEngine::setCharacterController(DynamicCharacterController* character) { if (_characterController != character) { if (_characterController) { diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 3b3467a2c9..4625df605e 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -53,11 +53,13 @@ public: /// process queue of changed from external sources void removeObjects(VectorOfMotionStates& objects); + void deleteObjects(SetOfMotionStates& objects); void addObjects(VectorOfMotionStates& objects); void changeObjects(VectorOfMotionStates& objects); void stepSimulation(); void computeCollisionEvents(); + void fireCollisionEvents(); bool hasOutgoingChanges() const { return _hasOutgoingChanges; } VectorOfMotionStates& getOutgoingChanges(); @@ -71,7 +73,11 @@ public: /// \param motionState pointer to Object's MotionState /// \return true if Object added - void addObject(const ShapeInfo& shapeInfo, btCollisionShape* shape, ObjectMotionState* motionState); + void addObject(ObjectMotionState* motionState); + + void bump(ObjectMotionState* motionState); + + void removeRigidBody(btRigidBody* body); void setCharacterController(DynamicCharacterController* character); diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp index 4ea4f2e170..7774f277b6 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp @@ -17,6 +17,7 @@ #include +#include "ObjectMotionState.h" #include "ThreadSafeDynamicsWorld.h" ThreadSafeDynamicsWorld::ThreadSafeDynamicsWorld( @@ -95,7 +96,7 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() { btRigidBody* body = btRigidBody::upcast(colObj); if (body) { synchronizeSingleMotionState(body); - _changedMotionStates.push_back(body->getMotionState()); + _changedMotionStates.push_back(static_cast(body->getMotionState())); } } } else { @@ -104,7 +105,7 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() { btRigidBody* body = m_nonStaticRigidBodies[i]; if (body->isActive()) { synchronizeSingleMotionState(body); - _changedMotionStates.push_back(body->getMotionState()); + _changedMotionStates.push_back(static_cast(body->getMotionState())); } } } diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.h b/libraries/physics/src/ThreadSafeDynamicsWorld.h index 6f8ddaa4b8..4a96eae311 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.h +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.h @@ -18,6 +18,7 @@ #ifndef hifi_ThreadSafeDynamicsWorld_h #define hifi_ThreadSafeDynamicsWorld_h +#include #include #include "PhysicsTypedefs.h" @@ -34,13 +35,14 @@ public: // virtual overrides from btDiscreteDynamicsWorld int stepSimulation( btScalar timeStep, int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + void synchronizeMotionStates(); // btDiscreteDynamicsWorld::m_localTime is the portion of real-time that has not yet been simulated // but is used for MotionState::setWorldTransform() extrapolation (a feature that Bullet uses to provide // smoother rendering of objects when the physics simulation loop is ansynchronous to the render loop). float getLocalTimeAccumulation() const { return m_localTime; } - VectorOfMotionStates& getChangedMotionStates() const { return _changedMotionStates; } + VectorOfMotionStates& getChangedMotionStates() { return _changedMotionStates; } private: VectorOfMotionStates _changedMotionStates;