update _lastSimulated for kinematic motion

libraries/entities/src/EntityItem.cpp

@@ -607,11 +607,6 @@ bool EntityItem::isRestingOnSurface() const {
 }
 
 void EntityItem::simulate(const quint64& now) {
-    if (_physicsInfo) {
-        // we rely on bullet for simulation, so bail
-        return;
-    }
-
     bool wantDebug = false;
     
     if (_lastSimulated == 0) {
@@ -661,9 +656,13 @@ void EntityItem::simulate(const quint64& now) {
         qDebug() << "     ********** EntityItem::simulate() .... SETTING _lastSimulated=" << _lastSimulated;
     }
 
-    if (hasAngularVelocity()) {
+    simulateKinematicMotion(timeElapsed);
-        glm::quat rotation = getRotation();
+    _lastSimulated = now;
+}
 
+void EntityItem::simulateKinematicMotion(float timeElapsed) {
+    bool wantDebug = false;
+    if (hasAngularVelocity()) {
         // angular damping
         glm::vec3 angularVelocity = getAngularVelocity();
         if (_angularDamping > 0.0f) {
@@ -679,6 +678,9 @@ void EntityItem::simulate(const quint64& now) {
         
         const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.1f; // 
         if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) {
+            if (angularSpeed > 0.0f) {
+                _dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE;
+            }
             setAngularVelocity(ENTITY_ITEM_ZERO_VEC3);
         } else {
             // NOTE: angularSpeed is currently in degrees/sec!!!
@@ -686,7 +688,7 @@ void EntityItem::simulate(const quint64& now) {
             float angle = timeElapsed * glm::radians(angularSpeed);
             glm::vec3 axis = _angularVelocity / angularSpeed;
             glm::quat  dQ = glm::angleAxis(angle, axis);
-            rotation = glm::normalize(dQ * rotation);
+            glm::quat rotation = glm::normalize(dQ * getRotation());
             setRotation(rotation);
         }
     }
@@ -722,80 +724,6 @@ void EntityItem::simulate(const quint64& now) {
         
         position = newPosition;
 
-        // handle bounces off the ground... We bounce at the distance to the bottom of our entity
-        if (position.y <= getDistanceToBottomOfEntity()) {
-            velocity = velocity * glm::vec3(1,-1,1);
-            position.y = getDistanceToBottomOfEntity();
-        }
-
-        // apply gravity
-        if (hasGravity()) { 
-            // handle resting on surface case, this is definitely a bit of a hack, and it only works on the
-            // "ground" plane of the domain, but for now it's what we've got
-            if (isRestingOnSurface()) {
-                velocity.y = 0.0f;
-                position.y = getDistanceToBottomOfEntity();
-            } else {
-                velocity += getGravity() * timeElapsed;
-            }
-        }
-        
-        // NOTE: we don't zero out very small velocities --> we rely on a remote Bullet simulation 
-        // to tell us when the entity has stopped.
-
-        // NOTE: the simulation should NOT set any DirtyFlags on this entity
-        setPosition(position); // this will automatically recalculate our collision shape
-        setVelocity(velocity);
-        
-        if (wantDebug) {        
-            qDebug() << "    new position:" << position;
-            qDebug() << "    new velocity:" << velocity;
-            qDebug() << "    new AACube:" << getMaximumAACube();
-            qDebug() << "    old getAABox:" << getAABox();
-        }
-    }
-
-    _lastSimulated = now;
-}
-
-void EntityItem::simulateSimpleKinematicMotion(float timeElapsed) {
-    if (hasAngularVelocity()) {
-        // angular damping
-        glm::vec3 angularVelocity = getAngularVelocity();
-        if (_angularDamping > 0.0f) {
-            angularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
-            setAngularVelocity(angularVelocity);
-        }
-
-        float angularSpeed = glm::length(_angularVelocity);
-        const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.1f; // 
-        if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) {
-            if (angularSpeed > 0.0f) {
-                _dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE;
-            }
-            setAngularVelocity(ENTITY_ITEM_ZERO_VEC3);
-        } else {
-            // NOTE: angularSpeed is currently in degrees/sec!!!
-            // TODO: Andrew to convert to radians/sec
-            float angle = timeElapsed * glm::radians(angularSpeed);
-            glm::vec3 axis = _angularVelocity / angularSpeed;
-            glm::quat  dQ = glm::angleAxis(angle, axis);
-            glm::quat rotation = getRotation();
-            rotation = glm::normalize(dQ * rotation);
-            setRotation(rotation);
-        }
-    }
-
-    if (hasVelocity()) {
-        // linear damping
-        glm::vec3 velocity = getVelocity();
-        if (_damping > 0.0f) {
-            velocity *= powf(1.0f - _damping, timeElapsed);
-        }
-
-        // integrate position forward
-        glm::vec3 position = getPosition() + (velocity * timeElapsed);
-
         // apply gravity
         if (hasGravity()) { 
             // handle resting on surface case, this is definitely a bit of a hack, and it only works on the
@@ -811,9 +739,16 @@ void EntityItem::simulateSimpleKinematicMotion(float timeElapsed) {
                 _dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE;
             }
         } else {
-            setPosition(position); // this will automatically recalculate our collision shape
+            setPosition(position);
             setVelocity(velocity);
         }
+
+        if (wantDebug) {        
+            qDebug() << "    new position:" << position;
+            qDebug() << "    new velocity:" << velocity;
+            qDebug() << "    new AACube:" << getMaximumAACube();
+            qDebug() << "    old getAABox:" << getAABox();
+        }
     }
 }
 

libraries/entities/src/EntityItem.h

@@ -82,6 +82,7 @@ public:
 
     void recordCreationTime();    // set _created to 'now'
     quint64 getLastSimulated() const { return _lastSimulated; } /// Last simulated time of this entity universal usecs
+    void setLastSimulated(quint64 now) { _lastSimulated = now; }
 
      /// Last edited time of this entity universal usecs
     quint64 getLastEdited() const { return _lastEdited; }
@@ -128,9 +129,8 @@ public:
     
     // perform linear extrapolation for SimpleEntitySimulation
     void simulate(const quint64& now);
+    void simulateKinematicMotion(float timeElapsed);
 
-    void simulateSimpleKinematicMotion(float timeElapsed);
-    
     virtual bool needsToCallUpdate() const { return false; }
 
     virtual void debugDump() const;

libraries/physics/src/EntityMotionState.cpp

@@ -54,11 +54,12 @@ void EntityMotionState::updateKinematicState(uint32_t substep) {
     setKinematic(_entity->isMoving(), substep);
 }
 
-void EntityMotionState::stepKinematicSimulation(uint32_t substep) {
+void EntityMotionState::stepKinematicSimulation(quint64 now) {
     assert(_isKinematic);
-    float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
+    // NOTE: this is non-physical kinematic motion which steps to real run-time (now)
-    _entity->simulateSimpleKinematicMotion(dt);
+    // which is different from physical kinematic motion (inside getWorldTransform())
-    _lastKinematicSubstep = substep;
+    // which steps in physics simulation time.
+    _entity->simulate(now);
 }
 
 // This callback is invoked by the physics simulation in two cases:
@@ -68,10 +69,15 @@ void EntityMotionState::stepKinematicSimulation(uint32_t substep) {
 //     it is an opportunity for outside code to update the object's simulation position
 void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
     if (_isKinematic) {
+        // This is physical kinematic motion which steps strictly by the subframe count
+        // of the physics simulation.
         uint32_t substep = PhysicsEngine::getNumSubsteps();
-        // remove const-ness so we can actually update this instance
+        float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
-        EntityMotionState* thisMotion = const_cast<EntityMotionState*>(this);
+        _entity->simulateKinematicMotion(dt);
-        thisMotion->stepKinematicSimulation(substep);
+        _entity->setLastSimulated(usecTimestampNow());
+
+        // bypass const-ness so we can remember the substep
+        const_cast<EntityMotionState*>(this)->_lastKinematicSubstep = substep;
     }
     worldTrans.setOrigin(glmToBullet(_entity->getPositionInMeters() - ObjectMotionState::getWorldOffset()));
     worldTrans.setRotation(glmToBullet(_entity->getRotation()));

libraries/physics/src/EntityMotionState.h

@@ -39,7 +39,7 @@ public:
     MotionType computeMotionType() const;
 
     void updateKinematicState(uint32_t substep);
-    void stepKinematicSimulation(uint32_t substep);
+    void stepKinematicSimulation(quint64 now);
 
     // this relays incoming position/rotation to the RigidBody
     void getWorldTransform(btTransform& worldTrans) const;

libraries/physics/src/ObjectMotionState.h

@@ -99,7 +99,7 @@ public:
     bool isKinematic() const { return _isKinematic; }
 
     void setKinematic(bool kinematic, uint32_t substep);
-    virtual void stepKinematicSimulation(uint32_t substep) = 0;
+    virtual void stepKinematicSimulation(quint64 now) = 0;
 
     friend class PhysicsEngine;
 protected:

libraries/physics/src/PhysicsEngine.cpp

@@ -297,7 +297,7 @@ void PhysicsEngine::stepSimulation() {
     // This is step (2).
     int numSubsteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP);
     _numSubsteps += (uint32_t)numSubsteps;
-    stepNonPhysicalKinematics();
+    stepNonPhysicalKinematics(usecTimestampNow());
     unlock();
 
     // This is step (3) which is done outside of stepSimulation() so we can lock _entityTree.
@@ -317,12 +317,11 @@ void PhysicsEngine::stepSimulation() {
     computeCollisionEvents();
 }
 
-// TODO: need to update non-physical kinematic objects
+void PhysicsEngine::stepNonPhysicalKinematics(const quint64& now) {
-void PhysicsEngine::stepNonPhysicalKinematics() {
     QSet<ObjectMotionState*>::iterator stateItr = _nonPhysicalKinematicObjects.begin();
     while (stateItr != _nonPhysicalKinematicObjects.end()) {
         ObjectMotionState* motionState = *stateItr;
-        motionState->stepKinematicSimulation(_numSubsteps);
+        motionState->stepKinematicSimulation(now);
         ++stateItr;
     }
 }

libraries/physics/src/PhysicsEngine.h

@@ -67,7 +67,7 @@ public:
     virtual void init(EntityEditPacketSender* packetSender);
 
     void stepSimulation();
-    void stepNonPhysicalKinematics();
+    void stepNonPhysicalKinematics(const quint64& now);
 
     void computeCollisionEvents();