measure linear velocity of moving objects

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;
     }
 }
 

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); }

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 we've never checked before, our _sentStep will be 0, and we need to initialize our state
-    if (_sentFrame == 0) {
+    if (_sentStep == 0) {
-        _sentPosition = bulletToGLM(_body->getWorldTransform().getOrigin());
+        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;
+    int numSteps = simulationStep - _sentStep;
-    float dt = (float)(numFrames) * PHYSICS_ENGINE_FIXED_SUBSTEP;
+    float dt = (float)(numSteps) * PHYSICS_ENGINE_FIXED_SUBSTEP;
-    _sentFrame = simulationFrame;
+    _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);
         }
     }

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

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) {