add support for non-physical kinematic movement

libraries/entities/src/EntityItem.cpp

@@ -520,7 +520,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef
         bytesRead += readEntitySubclassDataFromBuffer(dataAt, (bytesLeftToRead - bytesRead), args, propertyFlags, overwriteLocalData);
 
         recalculateCollisionShape();
-        if (overwriteLocalData && (getDirtyFlags() & EntityItem::DIRTY_POSITION)) {
+        if (overwriteLocalData && (getDirtyFlags() & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY))) {
             _lastSimulated = now;
         }
     }
@@ -770,6 +770,9 @@ void EntityItem::simulateSimpleKinematicMotion(float timeElapsed) {
         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!!!
@@ -800,10 +803,18 @@ void EntityItem::simulateSimpleKinematicMotion(float timeElapsed) {
             velocity += getGravity() * timeElapsed;
         }
 
-        // NOTE: the simulation should NOT set any DirtyFlags on this entity
+        float speed = glm::length(velocity);
+        const float EPSILON_LINEAR_VELOCITY_LENGTH = 0.001f; // 1mm/sec
+        if (speed < EPSILON_LINEAR_VELOCITY_LENGTH) {
+            setVelocity(ENTITY_ITEM_ZERO_VEC3);
+            if (speed > 0.0f) {
+                _dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE;
+            }
+        } else {
             setPosition(position); // this will automatically recalculate our collision shape
             setVelocity(velocity);
         }
+    }
 }
 
 bool EntityItem::isMoving() const {
@@ -886,7 +897,7 @@ bool EntityItem::setProperties(const EntityItemProperties& properties) {
         if (_created != UNKNOWN_CREATED_TIME) {
             setLastEdited(now);
         }
-        if (getDirtyFlags() & EntityItem::DIRTY_POSITION) {
+        if (getDirtyFlags() & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY)) {
             _lastSimulated = now;
         }
     }

libraries/physics/src/EntityMotionState.cpp

@@ -14,7 +14,7 @@
 
 #include "BulletUtil.h"
 #include "EntityMotionState.h"
-#include "SimpleEntityKinematicController.h"
+#include "PhysicsEngine.h"
 
 
 QSet<EntityItem*>* _outgoingEntityList;
@@ -41,8 +41,6 @@ EntityMotionState::~EntityMotionState() {
     assert(_entity);
     _entity->setPhysicsInfo(NULL);
     _entity = NULL;
-    delete _kinematicController;
-    _kinematicController = NULL;
 }
 
 MotionType EntityMotionState::computeMotionType() const {
@@ -52,13 +50,15 @@ MotionType EntityMotionState::computeMotionType() const {
     return _entity->isMoving() ?  MOTION_TYPE_KINEMATIC : MOTION_TYPE_STATIC;
 }
 
-void EntityMotionState::addKinematicController() {
+void EntityMotionState::updateKinematicState(uint32_t substep) {
-    if (!_kinematicController) {
+    setKinematic(_entity->isMoving(), substep);
-        _kinematicController = new SimpleEntityKinematicController(_entity);
+}
-        _kinematicController->start();
+
-    } else {
+void EntityMotionState::stepKinematicSimulation(uint32_t substep) {
-        _kinematicController->start();
+    assert(_isKinematic);
-    }
+    float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
+    _entity->simulateSimpleKinematicMotion(dt);
+    _lastKinematicSubstep = substep;
 }
 
 // This callback is invoked by the physics simulation in two cases:
@@ -67,8 +67,11 @@ void EntityMotionState::addKinematicController() {
 // (2) at the beginning of each simulation frame 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 (_kinematicController && _kinematicController->isRunning()) {
+    if (_isKinematic) {
-        _kinematicController->stepForward();
+        uint32_t substep = PhysicsEngine::getNumSubsteps();
+        // remove const-ness so we can actually update this instance
+        EntityMotionState* thisMotion = const_cast<EntityMotionState*>(this);
+        thisMotion->stepKinematicSimulation(substep);
     }
     worldTrans.setOrigin(glmToBullet(_entity->getPositionInMeters() - ObjectMotionState::getWorldOffset()));
     worldTrans.setRotation(glmToBullet(_entity->getRotation()));
@@ -229,6 +232,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
 uint32_t EntityMotionState::getIncomingDirtyFlags() const { 
     uint32_t dirtyFlags = _entity->getDirtyFlags(); 
 
+    if (_body) {
         // we add DIRTY_MOTION_TYPE if the body's motion type disagrees with entity velocity settings
         int bodyFlags = _body->getCollisionFlags();
         bool isMoving = _entity->isMoving();
@@ -236,5 +240,6 @@ uint32_t EntityMotionState::getIncomingDirtyFlags() const {
                 (bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving)) {
             dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE; 
         }
+    }
     return dirtyFlags;
 }

libraries/physics/src/EntityMotionState.h

@@ -14,7 +14,6 @@
 
 #include <AACube.h>
 
-#include "KinematicController.h"
 #include "ObjectMotionState.h"
 
 class EntityItem;
@@ -39,8 +38,8 @@ public:
     /// \return MOTION_TYPE_DYNAMIC or MOTION_TYPE_STATIC based on params set in EntityItem
     MotionType computeMotionType() const;
 
-    // virtual override for ObjectMotionState
+    void updateKinematicState(uint32_t substep);
-    void addKinematicController();
+    void stepKinematicSimulation(uint32_t substep);
 
     // this relays incoming position/rotation to the RigidBody
     void getWorldTransform(btTransform& worldTrans) const;

libraries/physics/src/ObjectMotionState.cpp

@@ -12,7 +12,6 @@
 #include <math.h>
 
 #include "BulletUtil.h"
-#include "KinematicController.h"
 #include "ObjectMotionState.h"
 #include "PhysicsEngine.h"
 
@@ -56,10 +55,6 @@ ObjectMotionState::ObjectMotionState() :
 ObjectMotionState::~ObjectMotionState() {
     // NOTE: you MUST remove this MotionState from the world before you call the dtor.
     assert(_body == NULL);
-    if (_kinematicController) {
-        delete _kinematicController;
-        _kinematicController = NULL;
-    }
 }
 
 void ObjectMotionState::setFriction(float friction) {
@@ -108,6 +103,15 @@ bool ObjectMotionState::doesNotNeedToSendUpdate() const {
 
 bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
     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());
+        _sentVelocity = bulletToGLM(_body->getLinearVelocity());
+        _sentAngularVelocity = bulletToGLM(_body->getAngularVelocity());
+        _sentFrame = simulationFrame;
+        return false;
+    }
+
     float dt = (float)(simulationFrame - _sentFrame) * PHYSICS_ENGINE_FIXED_SUBSTEP;
     _sentFrame = simulationFrame;
     bool isActive = _body->isActive();
@@ -164,13 +168,6 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
     return (fabsf(glm::dot(actualRotation, _sentRotation)) < MIN_ROTATION_DOT);
 }
 
-void ObjectMotionState::removeKinematicController() {
-    if (_kinematicController) {
-        delete _kinematicController;
-        _kinematicController = NULL;
-    }
-}
-
 void ObjectMotionState::setRigidBody(btRigidBody* body) {
     // give the body a (void*) back-pointer to this ObjectMotionState
     if (_body != body) {
@@ -183,3 +180,8 @@ void ObjectMotionState::setRigidBody(btRigidBody* body) {
         }
     }
 }
+
+void ObjectMotionState::setKinematic(bool kinematic, uint32_t substep) {
+    _isKinematic = kinematic;
+    _lastKinematicSubstep = substep;
+}

libraries/physics/src/ObjectMotionState.h

@@ -46,7 +46,6 @@ const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = EntityItem::DIRTY_POSITION | Entit
 
 
 class OctreeEditPacketSender;
-class KinematicController;
 
 class ObjectMotionState : public btMotionState {
 public:
@@ -93,11 +92,15 @@ public:
 
     virtual MotionType computeMotionType() const = 0;
 
-    virtual void addKinematicController() = 0;
+    virtual void updateKinematicState(uint32_t substep) = 0;
-    virtual void removeKinematicController();
 
     btRigidBody* getRigidBody() const { return _body; }
 
+    bool isKinematic() const { return _isKinematic; }
+
+    void setKinematic(bool kinematic, uint32_t substep);
+    virtual void stepKinematicSimulation(uint32_t substep) = 0;
+
     friend class PhysicsEngine;
 protected:
     void setRigidBody(btRigidBody* body);
@@ -114,6 +117,9 @@ protected:
 
     btRigidBody* _body;
 
+    bool _isKinematic = false;
+    uint32_t _lastKinematicSubstep = 0;
+
     bool _sentMoving;   // true if last update was moving
     int _numNonMovingUpdates; // RELIABLE_SEND_HACK for "not so reliable" resends of packets for non-moving objects
 
@@ -124,8 +130,6 @@ protected:
     glm::vec3 _sentVelocity;
     glm::vec3 _sentAngularVelocity; // radians per second
     glm::vec3 _sentAcceleration;
-
-    KinematicController* _kinematicController = NULL;
 };
 
 #endif // hifi_ObjectMotionState_h

libraries/physics/src/PhysicsEngine.cpp

@@ -62,7 +62,13 @@ void PhysicsEngine::addEntityInternal(EntityItem* entity) {
             entity->setPhysicsInfo(static_cast<void*>(motionState));
             _entityMotionStates.insert(motionState);
             addObject(shapeInfo, shape, motionState);
-        } else {
+        } else if (entity->isMoving()) {
+            EntityMotionState* motionState = new EntityMotionState(entity);
+            entity->setPhysicsInfo(static_cast<void*>(motionState));
+            _entityMotionStates.insert(motionState);
+
+            motionState->setKinematic(true, _numSubsteps);
+            _nonPhysicalKinematicObjects.insert(motionState);
             // We failed to add the entity to the simulation.  Probably because we couldn't create a shape for it.
             //qDebug() << "failed to add entity " << entity->getEntityItemID() << " to physics engine";
         }
@@ -74,10 +80,16 @@ void PhysicsEngine::removeEntityInternal(EntityItem* entity) {
     void* physicsInfo = entity->getPhysicsInfo();
     if (physicsInfo) {
         EntityMotionState* motionState = static_cast<EntityMotionState*>(physicsInfo);
+        if (motionState->getRigidBody()) {
             removeObject(motionState);
+        } else {
+            // only need to hunt in this list when there is no RigidBody
+            _nonPhysicalKinematicObjects.remove(motionState);
+        }
         _entityMotionStates.remove(motionState);
         _incomingChanges.remove(motionState);
         _outgoingPackets.remove(motionState);
+        // NOTE: EntityMotionState dtor will remove its backpointer from EntityItem
         delete motionState;
     }
 }
@@ -117,6 +129,7 @@ void PhysicsEngine::clearEntitiesInternal() {
         delete (*stateItr);
     }
     _entityMotionStates.clear();
+    _nonPhysicalKinematicObjects.clear();
     _incomingChanges.clear();
     _outgoingPackets.clear();
 }
@@ -127,19 +140,75 @@ void PhysicsEngine::relayIncomingChangesToSimulation() {
     QSet<ObjectMotionState*>::iterator stateItr = _incomingChanges.begin();
     while (stateItr != _incomingChanges.end()) {
         ObjectMotionState* motionState = *stateItr;
+        ++stateItr;
         uint32_t flags = motionState->getIncomingDirtyFlags() & DIRTY_PHYSICS_FLAGS;
 
+        bool removeMotionState = false;
         btRigidBody* body = motionState->getRigidBody();
         if (body) {
             if (flags & HARD_DIRTY_PHYSICS_FLAGS) {
                 // a HARD update requires the body be pulled out of physics engine, changed, then reinserted
                 // but it also handles all EASY changes
-                updateObjectHard(body, motionState, flags);
+                bool success = updateObjectHard(body, motionState, flags);
+                if (!success) {
+                    // NOTE: since updateObjectHard() failed we know that motionState has been removed 
+                    // from simulation and body has been deleted.  Depending on what else has changed
+                    // we might need to remove motionState altogether...
+                    if (flags & EntityItem::DIRTY_VELOCITY) {
+                        motionState->updateKinematicState(_numSubsteps);
+                        if (motionState->isKinematic()) {
+                            // all is NOT lost, we still need to move this object around kinematically
+                            _nonPhysicalKinematicObjects.insert(motionState);
+                        } else {
+                            // no need to keep motionState around
+                            removeMotionState = true;
+                        }
+                    } else {
+                         // no need to keep motionState around
+                         removeMotionState = true;
+                    }
+                }
             } else if (flags) {
                 // an EASY update does NOT require that the body be pulled out of physics engine
                 // hence the MotionState has all the knowledge and authority to perform the update.
                 motionState->updateObjectEasy(flags, _numSubsteps);
             }
+        } else {
+            // the only way we should ever get here (motionState exists but no body) is when the object
+            // is undergoing non-physical kinematic motion.
+            assert(_nonPhysicalKinematicObjects.contains(motionState));
+
+            // it is possible that the changes are such that the object can now be added to the physical simulation
+            if (flags & EntityItem::DIRTY_SHAPE) {
+                ShapeInfo shapeInfo;
+                motionState->computeShapeInfo(shapeInfo);
+                btCollisionShape* shape = _shapeManager.getShape(shapeInfo);
+                if (shape) {
+                    addObject(shapeInfo, shape, motionState);
+                    _nonPhysicalKinematicObjects.remove(motionState);
+                } else if (flags & EntityItem::DIRTY_VELOCITY) {
+                    // although we couldn't add the object to the simulation, might need to update kinematic motion...
+                    motionState->updateKinematicState(_numSubsteps);
+                    if (!motionState->isKinematic()) {
+                        _nonPhysicalKinematicObjects.remove(motionState);
+                        removeMotionState = true;
+                    }
+                }
+            } else if (flags & EntityItem::DIRTY_VELOCITY) {
+                // although we still can't add to physics simulation, might need to update kinematic motion...
+                motionState->updateKinematicState(_numSubsteps);
+                if (!motionState->isKinematic()) {
+                    _nonPhysicalKinematicObjects.remove(motionState);
+                    removeMotionState = true;
+                }
+            }
+        }
+        if (removeMotionState) {
+            // if we get here then there is no need to keep this motionState around (no physics or kinematics)
+            _outgoingPackets.remove(motionState);
+            // NOTE: motionState will clean up its own backpointers in the Object
+            delete motionState;
+            continue;
         }
 
         // NOTE: the grand order of operations is:
@@ -152,7 +221,6 @@ void PhysicsEngine::relayIncomingChangesToSimulation() {
         // outgoing changes at this point.
         motionState->clearOutgoingPacketFlags(flags); // clear outgoing flags that were trumped
         motionState->clearIncomingDirtyFlags(flags);  // clear incoming flags that were processed
-        ++stateItr;
     }
     _incomingChanges.clear();
 }
@@ -229,6 +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();
     unlock();
 
     // This is step (3) which is done outside of stepSimulation() so we can lock _entityTree.
@@ -248,6 +317,18 @@ void PhysicsEngine::stepSimulation() {
     computeCollisionEvents();
 }
 
+// TODO: need to update non-physical kinematic objects
+void PhysicsEngine::stepNonPhysicalKinematics() {
+    QSet<ObjectMotionState*>::iterator stateItr = _nonPhysicalKinematicObjects.begin();
+    while (stateItr != _nonPhysicalKinematicObjects.end()) {
+        ObjectMotionState* motionState = *stateItr;
+        motionState->stepKinematicSimulation(_numSubsteps);
+        ++stateItr;
+    }
+}
+
+// TODO?: need to occasionally scan for stopped non-physical kinematics objects
+
 void PhysicsEngine::computeCollisionEvents() {
     // update all contacts
     int numManifolds = _collisionDispatcher->getNumManifolds();
@@ -322,7 +403,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap
             body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
             body->updateInertiaTensor();
             motionState->setRigidBody(body);
-            motionState->addKinematicController();
+            motionState->setKinematic(true, _numSubsteps);
             const float KINEMATIC_LINEAR_VELOCITY_THRESHOLD = 0.01f;  // 1 cm/sec
             const float KINEMATIC_ANGULAR_VELOCITY_THRESHOLD = 0.01f;  // ~1 deg/sec
             body->setSleepingThresholds(KINEMATIC_LINEAR_VELOCITY_THRESHOLD, KINEMATIC_ANGULAR_VELOCITY_THRESHOLD);
@@ -334,6 +415,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap
             body = new btRigidBody(mass, motionState, shape, inertia);
             body->updateInertiaTensor();
             motionState->setRigidBody(body);
+            motionState->setKinematic(false, _numSubsteps);
             motionState->updateObjectVelocities();
             // NOTE: Bullet will deactivate any object whose velocity is below these thresholds for longer than 2 seconds.
             // (the 2 seconds is determined by: static btRigidBody::gDeactivationTime
@@ -348,6 +430,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap
             body->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);
             body->updateInertiaTensor();
             motionState->setRigidBody(body);
+            motionState->setKinematic(false, _numSubsteps);
             break;
         }
     }
@@ -358,7 +441,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap
     _dynamicsWorld->addRigidBody(body);
 }
 
-bool PhysicsEngine::removeObject(ObjectMotionState* motionState) {
+void PhysicsEngine::removeObject(ObjectMotionState* motionState) {
     assert(motionState);
     btRigidBody* body = motionState->getRigidBody();
     if (body) {
@@ -369,16 +452,14 @@ bool PhysicsEngine::removeObject(ObjectMotionState* motionState) {
         _shapeManager.releaseShape(shapeInfo);
         delete body;
         motionState->setRigidBody(NULL);
-        motionState->removeKinematicController();
+        motionState->setKinematic(false, _numSubsteps);
 
         removeContacts(motionState);
-        return true;
     }
-    return false;
 }
 
 // private
-void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags) {
+bool PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags) {
     MotionType newType = motionState->computeMotionType();
 
     // pull body out of physics engine
@@ -393,7 +474,16 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
         ShapeInfo shapeInfo;
         motionState->computeShapeInfo(shapeInfo);
         btCollisionShape* newShape = _shapeManager.getShape(shapeInfo);
-        if (newShape != oldShape) {
+        if (!newShape) {
+            // FAIL! we are unable to support these changes!
+            _shapeManager.releaseShape(oldShape);
+
+            delete body;
+            motionState->setRigidBody(NULL);
+            motionState->setKinematic(false, _numSubsteps);
+            removeContacts(motionState);
+            return false;
+        } else if (newShape != oldShape) {
             // BUG: if shape doesn't change but density does then we won't compute new mass properties
             // TODO: fix this BUG by replacing DIRTY_MASS with DIRTY_DENSITY and then fix logic accordingly.
             body->setCollisionShape(newShape);
@@ -426,7 +516,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
 
             body->setMassProps(0.0f, btVector3(0.0f, 0.0f, 0.0f));
             body->updateInertiaTensor();
-            motionState->addKinematicController();
+            motionState->setKinematic(true, _numSubsteps);
             break;
         }
         case MOTION_TYPE_DYNAMIC: {
@@ -443,7 +533,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
                 body->updateInertiaTensor();
             }
             body->forceActivationState(ACTIVE_TAG);
-            motionState->removeKinematicController();
+            motionState->setKinematic(false, _numSubsteps);
             break;
         }
         default: {
@@ -458,7 +548,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
 
             body->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f));
             body->setAngularVelocity(btVector3(0.0f, 0.0f, 0.0f));
-            motionState->removeKinematicController();
+            motionState->setKinematic(false, _numSubsteps);
             break;
         }
     }
@@ -467,4 +557,5 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
     _dynamicsWorld->addRigidBody(body);
 
     body->activate();
+    return true;
 }

libraries/physics/src/PhysicsEngine.h

@@ -67,6 +67,7 @@ public:
     virtual void init(EntityEditPacketSender* packetSender);
 
     void stepSimulation();
+    void stepNonPhysicalKinematics();
 
     void computeCollisionEvents();
 
@@ -81,15 +82,16 @@ public:
     void addObject(const ShapeInfo& shapeInfo, btCollisionShape* shape, ObjectMotionState* motionState);
 
     /// \param motionState pointer to Object's MotionState
-    /// \return true if Object removed
+    void removeObject(ObjectMotionState* motionState);
-    bool removeObject(ObjectMotionState* motionState);
 
     /// process queue of changed from external sources
     void relayIncomingChangesToSimulation();
 
 private:
     void removeContacts(ObjectMotionState* motionState);
-    void updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
+
+    // return 'true' of update was successful
+    bool updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
     void updateObjectEasy(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
 
     btClock _clock;
@@ -104,6 +106,7 @@ private:
 
     // EntitySimulation stuff
     QSet<EntityMotionState*> _entityMotionStates; // all entities that we track
+    QSet<ObjectMotionState*> _nonPhysicalKinematicObjects; // not in physics simulation, but still need kinematic simulation
     QSet<ObjectMotionState*> _incomingChanges; // entities with pending physics changes by script or packet
     QSet<ObjectMotionState*> _outgoingPackets; // MotionStates with pending changes that need to be sent over wire