diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index 0620ae73aa..c57d3fe2e0 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -16,21 +16,33 @@ #endif // USE_BULLET_PHYSICS #include "EntityMotionState.h" -// TODO: store _cachedWorldOffset in a more central location -- VoxelTree and others also need to know about it +// TODO: store _worldOffset in a more central location -- VoxelTree and others also need to know about it // origin of physics simulation in world frame -glm::vec3 _cachedWorldOffset(0.0f); +glm::vec3 _worldOffset(0.0f); // static void EntityMotionState::setWorldOffset(const glm::vec3& offset) { - _cachedWorldOffset = offset; + _worldOffset = offset; } // static const glm::vec3& getWorldOffset() { - return _cachedWorldOffset; + return _worldOffset; } -EntityMotionState::EntityMotionState(EntityItem* entity) : _entity(entity) { +EntityMotionState::EntityMotionState(EntityItem* entity) + : _entity(entity), + _sentMoving(false), + _notMoving(true), + _recievedNotMoving(false), + _sentFrame(0), + _sentPosition(0.0f), + _sentRotation(), + _sentVelocity(0.0f), + _sentVelocity(0.0f), + _sentAngularVelocity(0.0f), + _sentGravity(0.0f) +{ assert(entity != NULL); _oldBoundingCube = _entity->getMaximumAACube(); } @@ -38,7 +50,7 @@ EntityMotionState::EntityMotionState(EntityItem* entity) : _entity(entity) { EntityMotionState::~EntityMotionState() { } -MotionType EntityMotionState::getMotionType() const { +MotionType EntityMotionState::computeMotionType() const { // HACK: According to EntityTree the meaning of "static" is "not moving" whereas // to Bullet it means "can't move". For demo purposes we temporarily interpret // Entity::weightless to mean Bullet::static. @@ -53,7 +65,7 @@ MotionType EntityMotionState::getMotionType() const { // it is an opportunity for outside code to update the object's simulation position void EntityMotionState::getWorldTransform (btTransform &worldTrans) const { btVector3 pos; - glmToBullet(_entity->getPositionInMeters() - _cachedWorldOffset, pos); + glmToBullet(_entity->getPositionInMeters() - _worldOffset, pos); worldTrans.setOrigin(pos); btQuaternion rot; @@ -70,7 +82,7 @@ void EntityMotionState::setWorldTransform (const btTransform &worldTrans) { if (! (updateFlags & EntityItem::UPDATE_POSITION)) { glm::vec3 pos; bulletToGLM(worldTrans.getOrigin(), pos); - _entity->setPositionInMeters(pos + _cachedWorldOffset); + _entity->setPositionInMeters(pos + _worldOffset); glm::quat rot; bulletToGLM(worldTrans.getRotation(), rot); @@ -116,3 +128,42 @@ void EntityMotionState::getBoundingCubes(AACube& oldCube, AACube& newCube) { _oldBoundingCube = newCube; } +const float FIXED_SUBSTEP = 1.0f / 60.0f; +bool EntityMotionState::shouldSendUpdate(uint32_t simulationFrame, float subStepRemainder) const { + // TODO: Andrew to test this and make sure it works as expected + assert(_body); + float dt = (float)(simulationFrame - _sentFrame) * FIXED_SUBSTEP + subStepRemainder; + const float DEFAULT_UPDATE_PERIOD = 10.0f;j + if (dt > DEFAULT_UPDATE_PERIOD) { + return ! (_notMoving && _recievedNotMoving); + } + if (_sentMoving && _notMoving) { + return true; + } + + // compute position error + glm::vec3 expectedPosition = _sentPosition + dt * (_sentVelocity + (0.5f * dt) * _sentGravity); + + glm::vec3 actualPos; + btTransform worldTrans = _body->getWorldTransform(); + bulletToGLM(worldTrans.getOrigin(), actualPos); + + float dx2 = glm::length2(actualPosition - expectedPosition); + const MAX_POSITION_ERROR_SQUARED = 0.001; // 0.001 m^2 ~~> 0.03 m + if (dx2 > MAX_POSITION_ERROR_SQUARED) { + return true; + } + + // compute rotation error + float spin = glm::length(_sentAngularVelocity); + glm::quat expectedRotation = _sentRotation; + const float MIN_SPIN = 1.0e-4f; + if (spin > MIN_SPIN) { + glm::vec3 axis = _sentAngularVelocity / spin; + expectedRotation = glm::angleAxis(dt * spin, axis) * _sentRotation; + } + const float MIN_ROTATION_DOT = 0.98f; + glm::quat actualRotation; + bulletToGLM(worldTrans.getRotation(), actualRotation); + return (glm::dot(actualRotation, expectedRotation) < MIN_ROTATION_DOT); +} diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index 91186c823c..4f05ea53ec 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/libraries/physics/src/EntityMotionState.h @@ -14,6 +14,8 @@ #include +#include "PhysicsEngineParams.h" + #ifdef USE_BULLET_PHYSICS #include "ObjectMotionState.h" #else // USE_BULLET_PHYSICS @@ -26,20 +28,35 @@ public: class EntityItem; +// From the MotionState's perspective: +// Inside = physics simulation +// Outside = external agents (scripts, user interaction, other simulations) + class EntityMotionState : public ObjectMotionState { public: + // The WorldOffset is used to keep the positions of objects in the simulation near the origin, to + // reduce numerical error when computing vector differences. In other words: The EntityMotionState + // class translates between the simulation-frame and the world-frame as known by the render pipeline, + // various object trees, etc. The EntityMotionState class uses a static "worldOffset" to help in + // the translations. static void setWorldOffset(const glm::vec3& offset); static const glm::vec3& getWorldOffset(); EntityMotionState(EntityItem* item); virtual ~EntityMotionState(); - MotionType getMotionType() const; + /// \return MOTION_TYPE_DYNAMIC or MOTION_TYPE_STATIC based on params set in EntityItem + MotionType computeMotionType() const; #ifdef USE_BULLET_PHYSICS + // this relays incoming position/rotation to the RigidBody void getWorldTransform (btTransform &worldTrans) const; + + // this relays outgoing position/rotation to the EntityItem void setWorldTransform (const btTransform &worldTrans); #endif // USE_BULLET_PHYSICS + + // these relay incoming values to the RigidBody void applyVelocities() const; void applyGravity() const; @@ -47,9 +64,23 @@ public: void getBoundingCubes(AACube& oldCube, AACube& newCube); + bool shouldSendUpdate(uint32_t simulationFrame, float subStepRemainder) const; + protected: EntityItem* _entity; AACube _oldBoundingCube; + + bool _sentMoving; + bool _notMoving; + bool _receivedNotMoving; + // TODO: Andrew to talk to Brad about what to do about lost packets ^^^ + + uint32_t _sentFrame; + glm::vec3 _sentPosition; + glm::quat _sentRotation;; + glm::vec3 _sentVelocity; + glm::vec3 _sentAngularVelocity; + glm::vec3 _sentGravity; }; #endif // hifi_EntityMotionState_h diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 922b33d48c..cb7cd99193 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -25,7 +25,8 @@ PhysicsEngine::PhysicsEngine(const glm::vec3& offset) _constraintSolver(NULL), _dynamicsWorld(NULL), _originOffset(offset), - _voxels() { + _voxels(), + _frameCount(0) { } PhysicsEngine::~PhysicsEngine() { @@ -38,41 +39,85 @@ void PhysicsEngine::setEntityTree(EntityTree* tree) { _entityTree = tree; } -/// \param[out] entitiesToDelete list of entities removed from simulation and should be deleted. -void PhysicsEngine::updateEntities(QSet& entitiesToDelete) { - // relay changes - QSet::iterator item_itr = _changedEntities.begin(); - while (item_itr != _changedEntities.end()) { +void PhysicsEngine::relayIncomingChangesToSimulation() { + // process incoming changes + QSet::iterator item_itr = _incomingPhysics.begin(); + while (item_itr != _incomingPhysics.end()) { EntityItem* entity = *item_itr; void* physicsInfo = entity->getPhysicsInfo(); if (physicsInfo) { ObjectMotionState* motionState = static_cast(physicsInfo); + uint32_t preOutgoingFlags = motionState->getOutgoingUpdateFlags(); updateObject(motionState, entity->getUpdateFlags()); + uint32_t postOutgoingFlags = motionState->getOutgoingUpdateFlags(); + if (preOutgoingFlags && !postOutgoingFlags) { + _outgoingPhysics.remove(entity); + } } entity->clearUpdateFlags(); ++item_itr; } - _changedEntities.clear(); + _incomingPhysics.clear(); +} - // hunt for entities who have expired - // TODO: make EntityItems use an expiry to make this work faster. - item_itr = _mortalEntities.begin(); - while (item_itr != _mortalEntities.end()) { +/// \param[out] entitiesToDelete list of entities removed from simulation and should be deleted. +void PhysicsEngine::updateEntities(QSet& entitiesToDelete) { + // TODO: Andrew to make this work. + EnitySimulation::updateEntities(entitiesToDelete); + + item_itr = _outgoingPhysics.begin(); + uint32_t simulationFrame = getSimulationFrame(); + float subStepRemainder = getSubStepRemainder(); + while (item_itr != _outgoingPhysics.end()) { EntityItem* entity = *item_itr; - // always check to see if the lifetime has expired, for immortal entities this is always false - if (entity->lifetimeHasExpired()) { - qDebug() << "Lifetime has expired for entity:" << entity->getEntityItemID(); - entitiesToDelete.insert(entity); - // remove entity from the list - item_itr = _mortalEntities.erase(item_itr); - _entities.remove(entity); - } else if (entity->isImmortal()) { - // remove entity from the list - item_itr = _mortalEntities.erase(item_itr); - } else { - ++item_itr; + void* physicsInfo = entity->getPhysicsInfo(); + if (physicsInfo) { + ObjectMotionState* motionState = static_cast(physicsInfo); + if (motionState->shouldSendUpdate(simulationFrame, subStepRemainder)) { + EntityItemProperties properties = entity->getProperties(); + motionState->pushToProperties(properties); + + /* + properties.setVelocity(newVelocity); + properties.setPosition(newPosition); + properties.setRotation(newRotation); + properties.setAngularVelocity(newAngularVelocity); + properties.setLastEdited(now); + */ + + EntityItemID id(entity->getID()); + _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, id, properties); + ++itemItr; + } else if (motionState->shouldRemoveFromOutgoingPhysics()) { + itemItr = _outgoingPhysics.erase(itemItr); + } else { + ++itemItr; + } } } + + item_itr = _movedEntities.begin(); + AACube domainBounds(glm::vec3(0.0f,0.0f,0.0f), 1.0f); + while (item_itr != _movedEntities.end()) { + EntityItem* entity = *item_itr; + void* physicsInfo = entity->getPhysicsInfo(); + if (physicsInfo) { + ObjectMotionState* motionState = static_cast(physicsInfo); + + AACube oldCube, newCube; + motionState->getBoundingCubes(oldCube, newCube); + + // check to see if this movement has sent the entity outside of the domain. + if (!domainBounds.touches(newCube)) { + //qDebug() << "Entity " << entity->getEntityItemID() << " moved out of domain bounds."; + entitiesToDelete << entity->getEntityItemID(); + clearEntityState(entity); + } else if (newCube != oldCube) { + moveOperator.addEntityToMoveList(entity, oldCube, newCube); + } + } + } + // TODO: check for entities that have exited the world boundaries } @@ -84,9 +129,6 @@ void PhysicsEngine::addEntity(EntityItem* entity) { if (!physicsInfo) { assert(!_entities.contains(entity)); _entities.insert(entity); - if (entity->isMortal()) { - _mortalEntities.insert(entity); - } EntityMotionState* motionState = new EntityMotionState(entity); if (addObject(motionState)) { entity->setPhysicsInfo(static_cast(motionState)); @@ -108,12 +150,11 @@ void PhysicsEngine::removeEntity(EntityItem* entity) { entity->setPhysicsInfo(NULL); } _entities.remove(entity); - _mortalEntities.remove(entity); } /// \param entity pointer to EntityItem to that may have changed in a way that would affect its simulation void PhysicsEngine::entityChanged(EntityItem* entity) { - _changedEntities.insert(entity); + _incomingPhysics.insert(entity); } void PhysicsEngine::clearEntities() { @@ -127,8 +168,7 @@ void PhysicsEngine::clearEntities() { } } _entities.clear(); - _changedEntities.clear(); - _mortalEntities.clear(); + _incomingPhysics.clear(); } // virtual @@ -165,15 +205,18 @@ void PhysicsEngine::init() { } } +const float FIXED_SUBSTEP = 1.0f / 60.0f; + void PhysicsEngine::stepSimulation() { - const float MAX_TIMESTEP = 1.0f / 30.0f; - const int MAX_NUM_SUBSTEPS = 2; - const float FIXED_SUBSTEP = 1.0f / 60.0f; + const int MAX_NUM_SUBSTEPS = 4; + const float MAX_TIMESTEP = (float)MAX_NUM_SUBSTEPS * FIXED_SUBSTEP; float dt = 1.0e-6f * (float)(_clock.getTimeMicroseconds()); _clock.reset(); float timeStep = btMin(dt, MAX_TIMESTEP); - _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, FIXED_SUBSTEP); + // TODO: Andrew to build a list of outgoingChanges when motionStates are synched, then send the updates out + int numSubSteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, FIXED_SUBSTEP); + _frameCount += (uint32_t)numSubSteps; } bool PhysicsEngine::addVoxel(const glm::vec3& position, float scale) { @@ -250,7 +293,7 @@ bool PhysicsEngine::addObject(ObjectMotionState* motionState) { btVector3 inertia(0.0f, 0.0f, 0.0f); float mass = 0.0f; btRigidBody* body = NULL; - switch(motionState->getMotionType()) { + switch(motionState->computeMotionType()) { case MOTION_TYPE_KINEMATIC: { body = new btRigidBody(mass, motionState, shape, inertia); body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); @@ -322,7 +365,7 @@ bool PhysicsEngine::updateObject(ObjectMotionState* motionState, uint32_t flags) // private void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags) { - MotionType newType = motionState->getMotionType(); + MotionType newType = motionState->computeMotionType(); // pull body out of physics engine _dynamicsWorld->removeRigidBody(body); diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 2656a3f8cf..20ca033e65 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -112,6 +112,18 @@ public: /// \return true if entity updated bool updateObject(ObjectMotionState* motionState, uint32_t flags); + /// \return duration of fixed simulation substep + float getFixedSubStep() const; + + /// \return number of simulation frames the physics engine has taken + uint32_t getFrameCount() const { return _frameCount; } + + /// \return substep remainder used for Bullet MotionState extrapolation + // Bullet will extrapolate the positions provided to MotionState::setWorldTransform() in an effort to provide + // smoother visible motion when the render frame rate does not match that of the simulation loop. We provide + // access to this fraction for improved filtering of update packets to interested parties. + float getSubStepRemainder() { _dynamicsWorld->getLocalTimeAccumulation(); } + protected: void updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags); void updateObjectEasy(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags); @@ -129,9 +141,11 @@ private: btHashMap _voxels; // EntitySimulation stuff - QSet _entities; - QSet _changedEntities; - QSet _mortalEntities; + QSet _entities; // all entities that we track + QSet _incomingPhysics; // entities with pending physics changes by script or packet + QSet _outgoingPhysics; // entites with pending transform changes by physics simulation + + uint32_t _frameCount; }; #else // USE_BULLET_PHYSICS diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.h b/libraries/physics/src/ThreadSafeDynamicsWorld.h index 980ba7ae88..96cff8bda1 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.h +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.h @@ -38,6 +38,11 @@ public: 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; } + private: EntityTree* _entities; };