Merge pull request #4124 from AndrewMeadows/inertia

support for simple kinematic motion

interface/src/Application.h

@@ -33,6 +33,7 @@
 #include <NodeList.h>
 #include <OctreeQuery.h>
 #include <PacketHeaders.h>
+#include <PhysicsEngine.h>
 #include <ScriptEngine.h>
 #include <TextureCache.h>
 #include <ViewFrustum.h>

libraries/entities/src/EntityItem.cpp

@@ -758,6 +758,54 @@ void EntityItem::simulate(const quint64& now) {
     _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) {
+            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
+            // "ground" plane of the domain, but for now it's what we've got
+            velocity += getGravity() * timeElapsed;
+        }
+        
+        // NOTE: the simulation should NOT set any DirtyFlags on this entity
+        setPosition(position); // this will automatically recalculate our collision shape
+        setVelocity(velocity);
+    }
+}
+
 bool EntityItem::isMoving() const {
     return hasVelocity() || hasAngularVelocity();
 }

libraries/entities/src/EntityItem.h

@@ -128,6 +128,8 @@ public:
     
     // perform linear extrapolation for SimpleEntitySimulation
     void simulate(const quint64& now);
+
+    void simulateSimpleKinematicMotion(float timeElapsed);
     
     virtual bool needsToCallUpdate() const { return false; }
 

libraries/physics/src/EntityMotionState.cpp

@@ -16,6 +16,8 @@
 #include "BulletUtil.h"
 #endif // USE_BULLET_PHYSICS
 #include "EntityMotionState.h"
+#include "SimpleEntityKinematicController.h"
+
 
 QSet<EntityItem*>* _outgoingEntityList;
 
@@ -40,13 +42,24 @@ EntityMotionState::~EntityMotionState() {
     assert(_entity);
     _entity->setPhysicsInfo(NULL);
     _entity = NULL;
+    delete _kinematicController;
+    _kinematicController = NULL;
 }
 
 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.
-    return _entity->getCollisionsWillMove() ? MOTION_TYPE_DYNAMIC : MOTION_TYPE_STATIC;
+    if (_entity->getCollisionsWillMove()) {
+        return MOTION_TYPE_DYNAMIC;
+    }
+    return _entity->isMoving() ?  MOTION_TYPE_KINEMATIC : MOTION_TYPE_STATIC;
+}
+
+void EntityMotionState::addKinematicController() {
+    if (!_kinematicController) {
+        _kinematicController = new SimpleEntityKinematicController(_entity);
+        _kinematicController->start();
+    } else {
+        _kinematicController->start();
+    }
 }
 
 #ifdef USE_BULLET_PHYSICS
@@ -56,6 +69,9 @@ MotionType EntityMotionState::computeMotionType() const {
 // (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()) {
+        _kinematicController->stepForward();
+    }
     worldTrans.setOrigin(glmToBullet(_entity->getPositionInMeters() - ObjectMotionState::getWorldOffset()));
     worldTrans.setRotation(glmToBullet(_entity->getRotation()));
 }
@@ -215,3 +231,17 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
     }
 #endif // USE_BULLET_PHYSICS
 }
+
+uint32_t EntityMotionState::getIncomingDirtyFlags() const { 
+    uint32_t dirtyFlags = _entity->getDirtyFlags(); 
+#ifdef USE_BULLET_PHYSICS 
+    // we add DIRTY_MOTION_TYPE if the body's motion type disagrees with entity velocity settings
+    int bodyFlags = _body->getCollisionFlags();
+    bool isMoving = _entity->isMoving();
+    if (((bodyFlags & btCollisionObject::CF_STATIC_OBJECT) && isMoving) ||
+            (bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving)) {
+        dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE; 
+    }
+#endif // USE_BULLET_PHYSICS
+    return dirtyFlags;
+}

libraries/physics/src/EntityMotionState.h

@@ -15,11 +15,14 @@
 #include <AACube.h>
 
 #include "ObjectMotionState.h"
+
 #ifndef USE_BULLET_PHYSICS
 // ObjectMotionState stubbery
+#include "KinematicController.h"
 class ObjectMotionState {
 public:
     // so that this stub implementation is not completely empty we give the class a data member
+    KinematicController* _kinematicController;
     bool _stubData;
 };
 #endif // USE_BULLET_PHYSICS
@@ -39,12 +42,16 @@ public:
     static void setOutgoingEntityList(QSet<EntityItem*>* list);
     static void enqueueOutgoingEntity(EntityItem* entity);
 
+    EntityMotionState() = delete; // prevent compiler from making default ctor
     EntityMotionState(EntityItem* item);
     virtual ~EntityMotionState();
 
     /// \return MOTION_TYPE_DYNAMIC or MOTION_TYPE_STATIC based on params set in EntityItem
     MotionType computeMotionType() const;
 
+    // virtual override for ObjectMotionState
+    void addKinematicController();
+
 #ifdef USE_BULLET_PHYSICS
     // this relays incoming position/rotation to the RigidBody
     void getWorldTransform(btTransform& worldTrans) const;
@@ -62,7 +69,7 @@ public:
 
     void sendUpdate(OctreeEditPacketSender* packetSender, uint32_t frame);
 
-    uint32_t getIncomingDirtyFlags() const { return _entity->getDirtyFlags(); }
+    uint32_t getIncomingDirtyFlags() const;
     void clearIncomingDirtyFlags(uint32_t flags) { _entity->clearDirtyFlags(flags); }
 
 protected:

libraries/physics/src/KinematicController.cpp

@@ -0,0 +1,22 @@
+//
+//  KinematicController.cpp
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2015.01.13
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "KinematicController.h"
+#include "PhysicsEngine.h"
+
+KinematicController::KinematicController() { 
+    _lastFrame = PhysicsEngine::getFrameCount(); 
+}
+
+void KinematicController::start() { 
+    _enabled = true; 
+    _lastFrame = PhysicsEngine::getFrameCount(); 
+}

libraries/physics/src/KinematicController.h

@@ -0,0 +1,36 @@
+//
+//  KinematicController.h
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2015.01.13
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_KinematicController_h
+#define hifi_KinematicController_h
+
+#include <stdint.h>
+
+/// KinematicController defines an API for derived classes.
+
+class KinematicController {
+public:
+    KinematicController();
+
+    virtual ~KinematicController() {}
+
+    virtual void stepForward() = 0;
+
+    void start();
+    void stop() { _enabled = false; }
+    bool isRunning() const { return _enabled; }
+
+protected:
+    bool _enabled = false;
+    uint32_t _lastFrame;
+};
+
+#endif // hifi_KinematicController_h

libraries/physics/src/ObjectMotionState.cpp

@@ -14,7 +14,9 @@
 #include <math.h>
 
 #include "BulletUtil.h"
+#include "KinematicController.h"
 #include "ObjectMotionState.h"
+#include "PhysicsEngine.h"
 
 const float MIN_DENSITY = 200.0f;
 const float DEFAULT_DENSITY = 1000.0f;
@@ -64,6 +66,10 @@ 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) {
@@ -110,30 +116,23 @@ bool ObjectMotionState::doesNotNeedToSendUpdate() const {
     return !_body->isActive() && _numNonMovingUpdates > MAX_NUM_NON_MOVING_UPDATES;
 }
 
-const float FIXED_SUBSTEP = 1.0f / 60.0f;
-
 bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
     assert(_body);
-    float dt = (float)(simulationFrame - _sentFrame) * FIXED_SUBSTEP;
+    float dt = (float)(simulationFrame - _sentFrame) * PHYSICS_ENGINE_FIXED_SUBSTEP;
     _sentFrame = simulationFrame;
     bool isActive = _body->isActive();
 
-    if (isActive) {
-        const float MAX_UPDATE_PERIOD_FOR_ACTIVE_THINGS = 10.0f;
-        if (dt > MAX_UPDATE_PERIOD_FOR_ACTIVE_THINGS) {
-            return true;
-        }
-    } else if (_sentMoving) { 
-        if (!isActive) {
+    if (!isActive) {
+        if (_sentMoving) { 
             // this object just went inactive so send an update immediately
             return true;
-        }
-    } else {
-        const float NON_MOVING_UPDATE_PERIOD = 1.0f;
-        if (dt > NON_MOVING_UPDATE_PERIOD && _numNonMovingUpdates < MAX_NUM_NON_MOVING_UPDATES) {
-            // RELIABLE_SEND_HACK: since we're not yet using a reliable method for non-moving update packets we repeat these
-            // at a faster rate than the MAX period above, and only send a limited number of them.
-            return true;
+        } else {
+            const float NON_MOVING_UPDATE_PERIOD = 1.0f;
+            if (dt > NON_MOVING_UPDATE_PERIOD && _numNonMovingUpdates < MAX_NUM_NON_MOVING_UPDATES) {
+                // RELIABLE_SEND_HACK: since we're not yet using a reliable method for non-moving update packets we repeat these
+                // at a faster rate than the MAX period above, and only send a limited number of them.
+                return true;
+            }
         }
     }
 
@@ -175,4 +174,11 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
     return (fabsf(glm::dot(actualRotation, _sentRotation)) < MIN_ROTATION_DOT);
 }
 
+void ObjectMotionState::removeKinematicController() {
+    if (_kinematicController) {
+        delete _kinematicController;
+        _kinematicController = NULL;
+    }
+}
+
 #endif // USE_BULLET_PHYSICS

libraries/physics/src/ObjectMotionState.h

@@ -22,7 +22,6 @@ enum MotionType {
 
 // The update flags trigger two varieties of updates: "hard" which require the body to be pulled 
 // and re-added to the physics engine and "easy" which just updates the body properties.
-typedef unsigned int uint32_t;
 const uint32_t HARD_DIRTY_PHYSICS_FLAGS = (uint32_t)(EntityItem::DIRTY_MOTION_TYPE | EntityItem::DIRTY_SHAPE);
 const uint32_t EASY_DIRTY_PHYSICS_FLAGS = (uint32_t)(EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY |
                 EntityItem::DIRTY_MASS | EntityItem::DIRTY_COLLISION_GROUP);
@@ -42,6 +41,7 @@ const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = EntityItem::DIRTY_POSITION | Entit
 #include "ShapeInfo.h"
 
 class OctreeEditPacketSender;
+class KinematicController;
 
 class ObjectMotionState : public btMotionState {
 public:
@@ -87,6 +87,9 @@ public:
 
     virtual MotionType computeMotionType() const = 0;
 
+    virtual void addKinematicController() = 0;
+    virtual void removeKinematicController();
+
     friend class PhysicsEngine;
 protected:
     // TODO: move these materials properties to EntityItem
@@ -110,6 +113,8 @@ protected:
     glm::vec3 _sentVelocity;
     glm::vec3 _sentAngularVelocity; // radians per second
     glm::vec3 _sentAcceleration;
+
+    KinematicController* _kinematicController = NULL;
 };
 
 #endif // USE_BULLET_PHYSICS

libraries/physics/src/PhysicsEngine.cpp

@@ -10,13 +10,20 @@
 //
 
 #include "PhysicsEngine.h"
+
+
+static uint32_t _frameCount;
+
+// static
+uint32_t PhysicsEngine::getFrameCount() {
+    return _frameCount;
+}
+
 #ifdef USE_BULLET_PHYSICS
 
 #include "ShapeInfoUtil.h"
 #include "ThreadSafeDynamicsWorld.h"
 
-class EntityTree;
-
 PhysicsEngine::PhysicsEngine(const glm::vec3& offset)
     :   _collisionConfig(NULL), 
         _collisionDispatcher(NULL), 
@@ -24,8 +31,7 @@ PhysicsEngine::PhysicsEngine(const glm::vec3& offset)
         _constraintSolver(NULL), 
         _dynamicsWorld(NULL),
         _originOffset(offset),
-        _entityPacketSender(NULL),
-        _frameCount(0) {
+        _entityPacketSender(NULL) {
 }
 
 PhysicsEngine::~PhysicsEngine() {
@@ -196,8 +202,6 @@ void PhysicsEngine::init(EntityEditPacketSender* packetSender) {
     EntityMotionState::setOutgoingEntityList(&_entitiesToBeSorted);
 }
 
-const float FIXED_SUBSTEP = 1.0f / 60.0f;
-
 void PhysicsEngine::stepSimulation() {
     lock();
     // NOTE: the grand order of operations is:
@@ -210,13 +214,13 @@ void PhysicsEngine::stepSimulation() {
     relayIncomingChangesToSimulation();
 
     const int MAX_NUM_SUBSTEPS = 4;
-    const float MAX_TIMESTEP = (float)MAX_NUM_SUBSTEPS * FIXED_SUBSTEP;
+    const float MAX_TIMESTEP = (float)MAX_NUM_SUBSTEPS * PHYSICS_ENGINE_FIXED_SUBSTEP;
     float dt = 1.0e-6f * (float)(_clock.getTimeMicroseconds());
     _clock.reset();
     float timeStep = btMin(dt, MAX_TIMESTEP);
 
     // This is step (2).
-    int numSubSteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, FIXED_SUBSTEP);
+    int numSubSteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, PHYSICS_ENGINE_FIXED_SUBSTEP);
     _frameCount += (uint32_t)numSubSteps;
     unlock();
 
@@ -257,9 +261,12 @@ bool PhysicsEngine::addObject(ObjectMotionState* motionState) {
             case MOTION_TYPE_KINEMATIC: {
                 body = new btRigidBody(mass, motionState, shape, inertia);
                 body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
-                body->setActivationState(DISABLE_DEACTIVATION);
                 body->updateInertiaTensor();
                 motionState->_body = body;
+                motionState->addKinematicController();
+                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);
                 break;
             }
             case MOTION_TYPE_DYNAMIC: {
@@ -271,9 +278,9 @@ bool PhysicsEngine::addObject(ObjectMotionState* motionState) {
                 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
-                const float LINEAR_VELOCITY_THRESHOLD = 0.05f;  // 5 cm/sec
-                const float ANGULAR_VELOCITY_THRESHOLD = 0.087266f;  // ~5 deg/sec
-                body->setSleepingThresholds(LINEAR_VELOCITY_THRESHOLD, ANGULAR_VELOCITY_THRESHOLD);
+                const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f;  // 5 cm/sec
+                const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f;  // ~5 deg/sec
+                body->setSleepingThresholds(DYNAMIC_LINEAR_VELOCITY_THRESHOLD, DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
                 break;
             }
             case MOTION_TYPE_STATIC:
@@ -307,6 +314,7 @@ bool PhysicsEngine::removeObject(ObjectMotionState* motionState) {
         _shapeManager.releaseShape(shapeInfo);
         delete body;
         motionState->_body = NULL;
+        motionState->removeKinematicController();
         return true;
     }
     return false;
@@ -361,6 +369,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
 
             body->setMassProps(0.0f, btVector3(0.0f, 0.0f, 0.0f));
             body->updateInertiaTensor();
+            motionState->addKinematicController();
             break;
         }
         case MOTION_TYPE_DYNAMIC: {
@@ -377,6 +386,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
                 body->updateInertiaTensor();
             }
             body->forceActivationState(ACTIVE_TAG);
+            motionState->removeKinematicController();
             break;
         }
         default: {
@@ -391,6 +401,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();
             break;
         }
     }

libraries/physics/src/PhysicsEngine.h

@@ -12,7 +12,9 @@
 #ifndef hifi_PhysicsEngine_h
 #define hifi_PhysicsEngine_h
 
-typedef unsigned int uint32_t;
+#include <stdint.h>
+
+const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 60.0f;
 
 #ifdef USE_BULLET_PHYSICS
 
@@ -29,8 +31,11 @@ typedef unsigned int uint32_t;
 
 const float HALF_SIMULATION_EXTENT = 512.0f; // meters
 
+class ObjectMotionState;
+
 class PhysicsEngine : public EntitySimulation {
 public:
+    static uint32_t getFrameCount();
 
     PhysicsEngine(const glm::vec3& offset);
 
@@ -68,9 +73,6 @@ public:
     /// \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; }
-
 protected:
     void updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
     void updateObjectEasy(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
@@ -92,13 +94,13 @@ private:
     QSet<ObjectMotionState*> _outgoingPackets; // MotionStates with pending changes that need to be sent over wire
 
     EntityEditPacketSender* _entityPacketSender;
-
-    uint32_t _frameCount;
 };
 
 #else // USE_BULLET_PHYSICS
 // PhysicsEngine stubbery until Bullet is required
 class PhysicsEngine {
+public:
+    static uint32_t getFrameCount();
 };
 #endif // USE_BULLET_PHYSICS
 #endif // hifi_PhysicsEngine_h

libraries/physics/src/PhysicsEntity.h

@@ -21,10 +21,6 @@
 #include <CollisionInfo.h>
 #include <RayIntersectionInfo.h>
 
-#ifdef USE_BULLET_PHYSICS
-#include "PhysicsEngine.h"
-#endif // USE_BULLET_PHYSICS
-
 class Shape;
 class PhysicsSimulation;
 

libraries/physics/src/SimpleEntityKinematicController.cpp

@@ -0,0 +1,21 @@
+//
+//  SimpleEntityKinematicController.cpp
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2015.01.13
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "PhysicsEngine.h"
+#include "SimpleEntityKinematicController.h"
+
+void SimpleEntityKinematicController:: stepForward() {
+    uint32_t frame = PhysicsEngine::getFrameCount();
+    float dt = (frame - _lastFrame) * PHYSICS_ENGINE_FIXED_SUBSTEP;
+    _entity->simulateSimpleKinematicMotion(dt);
+    _lastFrame = frame;
+}
+

libraries/physics/src/SimpleEntityKinematicController.h

@@ -0,0 +1,38 @@
+//
+//  SimpleEntityKinematicController.h
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2015.01.13
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_SimpleEntityKinematicController_h
+#define hifi_SimpleEntityKinematicController_h
+
+/// SimpleKinematicConstroller performs simple exrapolation of velocities.
+
+#include <assert.h>
+#include <glm/glm.hpp>
+
+#include <EntityItem.h>
+
+#include "KinematicController.h"
+
+class SimpleEntityKinematicController : public KinematicController {
+public:
+    SimpleEntityKinematicController() = delete; // prevent compiler from making a default ctor
+
+    SimpleEntityKinematicController(EntityItem* entity) : KinematicController(), _entity(entity) { assert(entity); }
+
+    ~SimpleEntityKinematicController() { _entity = NULL; }
+
+    void stepForward();
+
+private:
+    EntityItem* _entity;
+};
+
+#endif // hifi_SimpleEntityKinematicController_h