diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index d84daa6b42..6838d977eb 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -62,6 +62,9 @@ void EntityMotionState::stepKinematicSimulation(quint64 now) { // which is different from physical kinematic motion (inside getWorldTransform()) // which steps in physics simulation time. _entity->simulate(now); + // TODO: we can't use ObjectMotionState::measureVelocityAndAcceleration() 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 { @@ -71,7 +74,7 @@ bool EntityMotionState::isMoving() const { // This callback is invoked by the physics simulation in two cases: // (1) when the RigidBody is first added to the world // (irregardless of MotionType: STATIC, DYNAMIC, or KINEMATIC) -// (2) at the beginning of each simulation frame for KINEMATIC RigidBody's -- +// (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) { @@ -89,9 +92,10 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const { worldTrans.setRotation(glmToBullet(_entity->getRotation())); } -// This callback is invoked by the physics simulation at the end of each simulation frame... +// This callback is invoked by the physics simulation at the end of each simulation step... // iff the corresponding RigidBody is DYNAMIC and has moved. void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { + measureVelocityAndAcceleration(); _entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset()); _entity->setRotation(bulletToGLM(worldTrans.getRotation())); @@ -116,7 +120,7 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { #endif } -void EntityMotionState::updateObjectEasy(uint32_t flags, uint32_t frame) { +void EntityMotionState::updateObjectEasy(uint32_t flags, uint32_t step) { if (flags & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY)) { if (flags & EntityItem::DIRTY_POSITION) { _sentPosition = _entity->getPosition() - ObjectMotionState::getWorldOffset(); @@ -131,7 +135,7 @@ void EntityMotionState::updateObjectEasy(uint32_t flags, uint32_t frame) { if (flags & EntityItem::DIRTY_VELOCITY) { updateObjectVelocities(); } - _sentFrame = frame; + _sentStep = step; } // TODO: entity support for friction and restitution @@ -179,7 +183,7 @@ float EntityMotionState::computeMass(const ShapeInfo& shapeInfo) const { return _entity->computeMass(); } -void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_t frame) { +void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_t step) { if (!_entity->isKnownID()) { return; // never update entities that are unknown } @@ -231,7 +235,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ } if (_numNonMovingUpdates <= 1) { // we only update lastEdited when we're sending new physics data - // (i.e. NOT when we just simulate the positions forward, nore when we resend non-moving data) + // (i.e. NOT when we just simulate the positions forward, nor when we resend non-moving data) // NOTE: Andrew & Brad to discuss. Let's make sure we're using lastEdited, lastSimulated, and lastUpdated correctly quint64 lastSimulated = _entity->getLastSimulated(); _entity->setLastEdited(lastSimulated); @@ -264,7 +268,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ // The outgoing flags only itemized WHAT to send, not WHETHER to send, hence we always set them // to the full set. These flags may be momentarily cleared by incoming external changes. _outgoingPacketFlags = DIRTY_PHYSICS_FLAGS; - _sentFrame = frame; + _sentStep = step; } } diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index 7214626fc4..710362e056 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/libraries/physics/src/EntityMotionState.h @@ -50,13 +50,13 @@ public: virtual void setWorldTransform(const btTransform& worldTrans); // these relay incoming values to the RigidBody - virtual void updateObjectEasy(uint32_t flags, uint32_t frame); + virtual void updateObjectEasy(uint32_t flags, uint32_t step); virtual void updateObjectVelocities(); virtual void computeShapeInfo(ShapeInfo& shapeInfo); virtual float computeMass(const ShapeInfo& shapeInfo) const; - virtual void sendUpdate(OctreeEditPacketSender* packetSender, uint32_t frame); + virtual void sendUpdate(OctreeEditPacketSender* packetSender, uint32_t step); virtual uint32_t getIncomingDirtyFlags() const; virtual void clearIncomingDirtyFlags(uint32_t flags) { _entity->clearDirtyFlags(flags); } diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index c9f416cc37..a66e60e93c 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -22,7 +22,7 @@ const float MAX_FRICTION = 10.0f; const float DEFAULT_RESTITUTION = 0.5f; -// origin of physics simulation in world frame +// origin of physics simulation in world-frame glm::vec3 _worldOffset(0.0f); // static @@ -35,6 +35,12 @@ const glm::vec3& ObjectMotionState::getWorldOffset() { return _worldOffset; } +// static +uint32_t _simulationStep = 0; +void ObjectMotionState::setSimulationStep(uint32_t step) { + assert(step > _simulationStep); + _simulationStep = step; +} ObjectMotionState::ObjectMotionState() : _friction(DEFAULT_FRICTION), @@ -46,12 +52,15 @@ ObjectMotionState::ObjectMotionState() : _sentMoving(false), _numNonMovingUpdates(0), _outgoingPacketFlags(DIRTY_PHYSICS_FLAGS), - _sentFrame(0), + _sentStep(0), _sentPosition(0.0f), _sentRotation(), _sentVelocity(0.0f), _sentAngularVelocity(0.0f), - _sentAcceleration(0.0f) { + _sentAcceleration(0.0f), + _lastSimulationStep(0), + _lastVelocity(0.0f), + _measuredAcceleration(0.0f) } ObjectMotionState::~ObjectMotionState() { @@ -59,6 +68,27 @@ ObjectMotionState::~ObjectMotionState() { assert(_body == NULL); } +void ObjectMotionState::measureVelocityAndAcceleration() { + // try to manually measure the true acceleration of the object + uint32_t numSubsteps = _simulationStep - _lastSimulationStep; + if (numSubsteps > 0) { + float dt = ((float)numSubsteps * PHYSICS_ENGINE_FIXED_SUBSTEP); + float invDt = 1.0f / dt; + _lastSimulationStep = _simulationStep; + + // 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 - _linearDamping, dt) - _lastVelocity) * invDt; + _lastVelocity = velocity; + } +} + +void ObjectMotionState::resetMeasuredVelocityAndAcceleration() { + _lastSimulationStep = _simulationStep; + _lastVelocity = bulletToGLM(_body->getLinearVelocity()); +} + void ObjectMotionState::setFriction(float friction) { _friction = btMax(btMin(fabsf(friction), MAX_FRICTION), 0.0f); } @@ -103,15 +133,16 @@ bool ObjectMotionState::doesNotNeedToSendUpdate() const { return !_body->isActive() && _numNonMovingUpdates > MAX_NUM_NON_MOVING_UPDATES; } -bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { +bool ObjectMotionState::shouldSendUpdate(uint32_t simulationStep) { assert(_body); - // if we've never checked before, our _sentFrame will be 0, and we need to initialize our state - if (_sentFrame == 0) { - _sentPosition = bulletToGLM(_body->getWorldTransform().getOrigin()); + // if we've never checked before, our _sentStep will be 0, and we need to initialize our state + if (_sentStep == 0) { + btTransform xform = _body->getWorldTransform(); + _sentPosition = bulletToGLM(xform.getOrigin()); + _sentRotation = bulletToGLM(xform.getRotation()); _sentVelocity = bulletToGLM(_body->getLinearVelocity()); - _sentRotation = bulletToGLM(_body->getWorldTransform().getRotation()); _sentAngularVelocity = bulletToGLM(_body->getAngularVelocity()); - _sentFrame = simulationFrame; + _sentStep = simulationStep; return false; } @@ -121,9 +152,9 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { glm::vec3 wasAngularVelocity = _sentAngularVelocity; #endif - int numFrames = simulationFrame - _sentFrame; - float dt = (float)(numFrames) * PHYSICS_ENGINE_FIXED_SUBSTEP; - _sentFrame = simulationFrame; + int numSteps = simulationStep - _sentStep; + float dt = (float)(numSteps) * PHYSICS_ENGINE_FIXED_SUBSTEP; + _sentStep = simulationStep; bool isActive = _body->isActive(); if (!isActive) { @@ -179,7 +210,7 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) { // Bullet caps the effective rotation velocity inside its rotation integration step, therefore // we must integrate with the same algorithm and timestep in order achieve similar results. - for (int i = 0; i < numFrames; ++i) { + for (int i = 0; i < numSteps; ++i) { _sentRotation = glm::normalize(computeBulletRotationStep(_sentAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP) * _sentRotation); } } diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index fb402a178d..2c77ca6b5e 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/libraries/physics/src/ObjectMotionState.h @@ -57,9 +57,14 @@ public: static void setWorldOffset(const glm::vec3& offset); static const glm::vec3& getWorldOffset(); + static void setSimulationStep(uint32_t step); + ObjectMotionState(); ~ObjectMotionState(); + void measureVelocityAndAcceleration(); + void resetMeasuredVelocityAndAcceleration(); + // An EASY update does not require the object to be removed and then reinserted into the PhysicsEngine virtual void updateObjectEasy(uint32_t flags, uint32_t frame) = 0; virtual void updateObjectVelocities() = 0; @@ -87,7 +92,7 @@ public: void clearOutgoingPacketFlags(uint32_t flags) { _outgoingPacketFlags &= ~flags; } bool doesNotNeedToSendUpdate() const; - virtual bool shouldSendUpdate(uint32_t simulationFrame); + virtual bool shouldSendUpdate(uint32_t simulationStep); virtual void sendUpdate(OctreeEditPacketSender* packetSender, uint32_t frame) = 0; virtual MotionType computeMotionType() const = 0; @@ -126,12 +131,16 @@ protected: int _numNonMovingUpdates; // RELIABLE_SEND_HACK for "not so reliable" resends of packets for non-moving objects uint32_t _outgoingPacketFlags; - uint32_t _sentFrame; + uint32_t _sentStep; glm::vec3 _sentPosition; // in simulation-frame (not world-frame) glm::quat _sentRotation;; glm::vec3 _sentVelocity; glm::vec3 _sentAngularVelocity; // radians per second glm::vec3 _sentAcceleration; + + uint32_t _lastSimulationStep; + glm::vec3 _lastVelocity; + glm::vec3 _measuredAcceleration; }; #endif // hifi_ObjectMotionState_h diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 209e23e006..34ac6732a7 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -193,6 +193,9 @@ void PhysicsEngine::relayIncomingChangesToSimulation() { // hence the MotionState has all the knowledge and authority to perform the update. motionState->updateObjectEasy(flags, _numSubsteps); } + if (flags & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY)) { + motionState->resetMeasuredVelocityAndAcceleration(); + } } else { // the only way we should ever get here (motionState exists but no body) is when the object // is undergoing non-physical kinematic motion. @@ -508,6 +511,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap body->setFriction(motionState->_friction); body->setDamping(motionState->_linearDamping, motionState->_angularDamping); _dynamicsWorld->addRigidBody(body); + motionState->resetMeasuredVelocityAndAcceleration(); } void PhysicsEngine::removeObjectFromBullet(ObjectMotionState* motionState) {