Merge pull request #7569 from highfidelity/revert-7544-faster-kinematics

Revert "faster kinematic motion for entities"

libraries/entities/src/EntityItem.cpp

@@ -876,112 +876,123 @@ void EntityItem::simulate(const quint64& now) {
 }
 
 void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) {
-    if (hasActions() || timeElapsed < 0.0f) {
+#ifdef WANT_DEBUG
+    qCDebug(entities) << "EntityItem::simulateKinematicMotion timeElapsed" << timeElapsed;
+#endif
+
+    const float MIN_TIME_SKIP = 0.0f;
+    const float MAX_TIME_SKIP = 1.0f; // in seconds
+
+    timeElapsed = glm::clamp(timeElapsed, MIN_TIME_SKIP, MAX_TIME_SKIP);
+
+    if (hasActions()) {
         return;
     }
 
-    const float MAX_TIME_ELAPSED = 1.0f; // seconds
+    if (hasLocalAngularVelocity()) {
-    timeElapsed = glm::min(timeElapsed, MAX_TIME_ELAPSED);
+        glm::vec3 localAngularVelocity = getLocalAngularVelocity();
 
-    Transform transform;
-    glm::vec3 linearVelocity;
-    glm::vec3 angularVelocity;
-    getLocalTransformAndVelocities(transform, linearVelocity, angularVelocity);
-
-    bool isMoving = false;
-    if (glm::length2(angularVelocity) > 0.0f) {
         // angular damping
         if (_angularDamping > 0.0f) {
-            angularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
+            localAngularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
+            #ifdef WANT_DEBUG
+                qCDebug(entities) << "    angularDamping :" << _angularDamping;
+                qCDebug(entities) << "    newAngularVelocity:" << localAngularVelocity;
+            #endif
         }
 
-        const float MIN_KINEMATIC_ANGULAR_SPEED_SQUARED = 0.0017453f * 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec
+        float angularSpeed = glm::length(localAngularVelocity);
-        if (glm::length2(angularVelocity) < MIN_KINEMATIC_ANGULAR_SPEED_SQUARED) {
+
-            angularVelocity = Vectors::ZERO;
+        const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec
+        if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) {
+            if (setFlags && angularSpeed > 0.0f) {
+                _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
+            }
+            localAngularVelocity = ENTITY_ITEM_ZERO_VEC3;
         } else {
             // for improved agreement with the way Bullet integrates rotations we use an approximation
             // and break the integration into bullet-sized substeps
-            glm::quat rotation = transform.getRotation();
+            glm::quat rotation = getRotation();
             float dt = timeElapsed;
-            while (dt > 0.0f) {
+            while (dt > PHYSICS_ENGINE_FIXED_SUBSTEP) {
-                glm::quat  dQ = computeBulletRotationStep(angularVelocity, glm::min(dt, PHYSICS_ENGINE_FIXED_SUBSTEP));
+                glm::quat  dQ = computeBulletRotationStep(localAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP);
                 rotation = glm::normalize(dQ * rotation);
                 dt -= PHYSICS_ENGINE_FIXED_SUBSTEP;
             }
-            transform.setRotation(rotation);
+            // NOTE: this final partial substep can drift away from a real Bullet simulation however
-            isMoving = true;
+            // it only becomes significant for rapidly rotating objects
-        }
+            // (e.g. around PI/4 radians per substep, or 7.5 rotations/sec at 60 substeps/sec).
-    }
+            glm::quat  dQ = computeBulletRotationStep(localAngularVelocity, dt);
-    glm::vec3 position = transform.getTranslation();
+            rotation = glm::normalize(dQ * rotation);
 
-    const float MIN_KINEMATIC_LINEAR_SPEED_SQUARED = 1.0e-6f; // 1mm/sec ^2
+            setRotation(rotation);
-    bool hasLinearVelocity = (glm::length2(linearVelocity) > MIN_KINEMATIC_LINEAR_SPEED_SQUARED );
-    if (hasLinearVelocity) {
-        // linear damping
-        if (_damping > 0.0f) {
-            linearVelocity *= powf(1.0f - _damping, timeElapsed);
         }
 
-        // integrate position forward sans acceleration
+        setLocalAngularVelocity(localAngularVelocity);
-        position += linearVelocity * timeElapsed;
     }
 
-    const float MIN_KINEMATIC_GRAVITY_MOTION_SQUARED = 1.0e-6f; // 0.001 mm/sec^2
+    if (hasLocalVelocity()) {
-    bool hasGravity = (glm::length2(_gravity) > MIN_KINEMATIC_GRAVITY_MOTION_SQUARED);
+
-    if (hasGravity) {
+        // acceleration is in the global frame, so transform it into the local frame.
-        // acceleration is in world-frame but we need it in local-frame
+        // TODO: Move this into SpatiallyNestable.
-        glm::vec3 linearAcceleration = _gravity;
         bool success;
-        Transform parentTransform = getParentTransform(success);
+        Transform transform = getParentTransform(success);
+        glm::vec3 localAcceleration(glm::vec3::_null);
         if (success) {
-            linearAcceleration = glm::inverse(parentTransform.getRotation()) * linearAcceleration;
+            localAcceleration = glm::inverse(transform.getRotation()) * getAcceleration();
+        } else {
+            localAcceleration = getAcceleration();
         }
 
-        // integrate position's acceleration term
+        // linear damping
-        position += 0.5f * linearAcceleration * timeElapsed * timeElapsed;
+        glm::vec3 localVelocity = getLocalVelocity();
+        if (_damping > 0.0f) {
+            localVelocity *= powf(1.0f - _damping, timeElapsed);
+            #ifdef WANT_DEBUG
+                qCDebug(entities) << "    damping:" << _damping;
+                qCDebug(entities) << "    velocity AFTER dampingResistance:" << localVelocity;
+                qCDebug(entities) << "    glm::length(velocity):" << glm::length(localVelocity);
+            #endif
+        }
 
-        // integrate linearVelocity
+        // integrate position forward
-        linearVelocity += linearAcceleration * timeElapsed;
+        glm::vec3 localPosition = getLocalPosition();
-    }
+        glm::vec3 newLocalPosition = localPosition + (localVelocity * timeElapsed) + 0.5f * localAcceleration * timeElapsed * timeElapsed;
 
-    if (hasLinearVelocity || hasGravity) {
+        #ifdef WANT_DEBUG
-        // We MUST eventually stop kinematic motion for slow entities otherwise they will take
+            qCDebug(entities) << "  EntityItem::simulate()....";
-        // a looooong time to settle down, so we remeasure linear speed and zero the velocity
+            qCDebug(entities) << "    timeElapsed:" << timeElapsed;
-        // if it is too small.
+            qCDebug(entities) << "    old AACube:" << getMaximumAACube();
-        if (glm::length2(linearVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
+            qCDebug(entities) << "    old position:" << localPosition;
-            linearVelocity = Vectors::ZERO;
+            qCDebug(entities) << "    old velocity:" << localVelocity;
-            if (!hasLinearVelocity) {
+            qCDebug(entities) << "    old getAABox:" << getAABox();
-                // Despite some gravity the final linear velocity is still too small to defeat the
+            qCDebug(entities) << "    newPosition:" << newPosition;
-                // "effective resistance of free-space" which means we must reset position back to
+            qCDebug(entities) << "    glm::distance(newPosition, position):" << glm::distance(newLocalPosition, localPosition);
-                // where it started since otherwise the entity may creep vveerrryy sslloowwllyy at
+        #endif
-                // a constant speed.
-                position = transform.getTranslation();
 
-                // Ultimately what this means is that there is some minimum gravity we can
+        localPosition = newLocalPosition;
-                // fully support for kinematic motion.  It's exact value is a function of this
+
-                // entity's linearDamping and the hardcoded MIN_KINEMATIC_FOO parameters above,
+        // apply effective acceleration, which will be the same as gravity if the Entity isn't at rest.
-                // but the theoretical minimum gravity for zero damping at 90Hz is:
+        localVelocity += localAcceleration * timeElapsed;
-                //
+
-                // minGravity = minSpeed / dt = 0.001 m/sec * 90 /sec = 0.09 m/sec^2
+        float speed = glm::length(localVelocity);
-                //
+        const float EPSILON_LINEAR_VELOCITY_LENGTH = 0.001f; // 1mm/sec
-                // In practice the true minimum is half that value, since if the frame rate is ever
+        if (speed < EPSILON_LINEAR_VELOCITY_LENGTH) {
-                // less than the expected then sometimes dt will be twice as long.
+            setVelocity(ENTITY_ITEM_ZERO_VEC3);
-                //
+            if (setFlags && speed > 0.0f) {
-                // Since we don't set isMoving true here this entity is destined to transition to
+                _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
-                // STATIC unless it has some angular motion keeping it alive.
-            } else {
-                isMoving = true;
             }
         } else {
-            isMoving = true;
+            setLocalPosition(localPosition);
+            setLocalVelocity(localVelocity);
         }
-    }
-    transform.setTranslation(position);
-    setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity);
 
-    if (setFlags && !isMoving) {
+        #ifdef WANT_DEBUG
-        // flag this entity to transition from KINEMATIC to STATIC
+            qCDebug(entities) << "    new position:" << position;
-        _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
+            qCDebug(entities) << "    new velocity:" << velocity;
+            qCDebug(entities) << "    new AACube:" << getMaximumAACube();
+            qCDebug(entities) << "    old getAABox:" << getAABox();
+        #endif
     }
 }
 

libraries/physics/src/EntityMotionState.cpp

@@ -94,7 +94,7 @@ void EntityMotionState::updateServerPhysicsVariables() {
 }
 
 // virtual
-void EntityMotionState::handleEasyChanges(uint32_t& flags) {
+bool EntityMotionState::handleEasyChanges(uint32_t& flags) {
     assert(entityTreeIsLocked());
     updateServerPhysicsVariables();
     ObjectMotionState::handleEasyChanges(flags);
@@ -137,6 +137,8 @@ void EntityMotionState::handleEasyChanges(uint32_t& flags) {
     if ((flags & Simulation::DIRTY_PHYSICS_ACTIVATION) && !_body->isActive()) {
         _body->activate();
     }
+
+    return true;
 }
 
 
@@ -173,13 +175,11 @@ bool EntityMotionState::isMoving() const {
 // (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 {
-    BT_PROFILE("getWorldTransform");
     if (!_entity) {
         return;
     }
     assert(entityTreeIsLocked());
     if (_motionType == MOTION_TYPE_KINEMATIC) {
-        BT_PROFILE("kinematicIntegration");
         // This is physical kinematic motion which steps strictly by the subframe count
         // of the physics simulation.
         uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
@@ -420,18 +420,19 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, const Q
         const float ACCELERATION_EQUIVALENT_EPSILON_RATIO = 0.1f;
         if (accVsGravity < ACCELERATION_EQUIVALENT_EPSILON_RATIO * gravityLength) {
             // acceleration measured during the most recent simulation step was close to gravity.
-            if (_accelerationNearlyGravityCount < STEPS_TO_DECIDE_BALLISTIC) {
+            if (getAccelerationNearlyGravityCount() < STEPS_TO_DECIDE_BALLISTIC) {
-                // only increment this if we haven't reached the threshold yet, to avoid overflowing the counter
+                // only increment this if we haven't reached the threshold yet.  this is to avoid
-                ++_accelerationNearlyGravityCount;
+                // overflowing the counter.
+                incrementAccelerationNearlyGravityCount();
             }
         } else {
-            // acceleration wasn't similar to this entity's gravity, reset the counter
+            // acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter
-            _accelerationNearlyGravityCount = 0;
+            resetAccelerationNearlyGravityCount();
         }
 
         // if this entity has been accelerated at close to gravity for a certain number of simulation-steps, let
         // the entity server's estimates include gravity.
-        if (_accelerationNearlyGravityCount >= STEPS_TO_DECIDE_BALLISTIC) {
+        if (getAccelerationNearlyGravityCount() >= STEPS_TO_DECIDE_BALLISTIC) {
             _entity->setAcceleration(_entity->getGravity());
         } else {
             _entity->setAcceleration(glm::vec3(0.0f));

libraries/physics/src/EntityMotionState.h

@@ -29,7 +29,7 @@ public:
     virtual ~EntityMotionState();
 
     void updateServerPhysicsVariables();
-    virtual void handleEasyChanges(uint32_t& flags) override;
+    virtual bool handleEasyChanges(uint32_t& flags) override;
     virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
 
     /// \return PhysicsMotionType based on params set in EntityItem
@@ -51,6 +51,10 @@ public:
     virtual uint32_t getIncomingDirtyFlags() override;
     virtual void clearIncomingDirtyFlags() override;
 
+    void incrementAccelerationNearlyGravityCount() { _accelerationNearlyGravityCount++; }
+    void resetAccelerationNearlyGravityCount() { _accelerationNearlyGravityCount = 0; }
+    uint8_t getAccelerationNearlyGravityCount() { return _accelerationNearlyGravityCount; }
+
     virtual float getObjectRestitution() const override { return _entity->getRestitution(); }
     virtual float getObjectFriction() const override { return _entity->getFriction(); }
     virtual float getObjectLinearDamping() const override { return _entity->getDamping(); }

libraries/physics/src/ObjectMotionState.cpp

@@ -38,11 +38,8 @@ const glm::vec3& ObjectMotionState::getWorldOffset() {
     return _worldOffset;
 }
 
-// We init worldSimulationStep to 1 instead of 0 because we initialize _lastKineticStep to (worldSimulationStep - 1)
-// so that the object starts moving on the first frame that it was set kinematic.
-static uint32_t worldSimulationStep { 1 };
-
 // static
+uint32_t worldSimulationStep = 0;
 void ObjectMotionState::setWorldSimulationStep(uint32_t step) {
     assert(step > worldSimulationStep);
     worldSimulationStep = step;
@@ -167,7 +164,7 @@ void ObjectMotionState::setRigidBody(btRigidBody* body) {
     }
 }
 
-void ObjectMotionState::handleEasyChanges(uint32_t& flags) {
+bool ObjectMotionState::handleEasyChanges(uint32_t& flags) {
     if (flags & Simulation::DIRTY_POSITION) {
         btTransform worldTrans = _body->getWorldTransform();
         btVector3 newPosition = glmToBullet(getObjectPosition());
@@ -186,10 +183,6 @@ void ObjectMotionState::handleEasyChanges(uint32_t& flags) {
             worldTrans.setRotation(newRotation);
         }
         _body->setWorldTransform(worldTrans);
-        if (!(flags & HARD_DIRTY_PHYSICS_FLAGS) && _body->isStaticObject()) {
-            // force activate static body so its Aabb is updated later
-            _body->activate(true);
-        }
     } else if (flags & Simulation::DIRTY_ROTATION) {
         btTransform worldTrans = _body->getWorldTransform();
         btQuaternion newRotation = glmToBullet(getObjectRotation());
@@ -199,10 +192,6 @@ void ObjectMotionState::handleEasyChanges(uint32_t& flags) {
         }
         worldTrans.setRotation(newRotation);
         _body->setWorldTransform(worldTrans);
-        if (!(flags & HARD_DIRTY_PHYSICS_FLAGS) && _body->isStaticObject()) {
-            // force activate static body so its Aabb is updated later
-            _body->activate(true);
-        }
     }
 
     if (flags & Simulation::DIRTY_LINEAR_VELOCITY) {
@@ -243,6 +232,8 @@ void ObjectMotionState::handleEasyChanges(uint32_t& flags) {
     if (flags & Simulation::DIRTY_MASS) {
         updateBodyMassProperties();
     }
+
+    return true;
 }
 
 bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) {
@@ -301,10 +292,6 @@ void ObjectMotionState::updateBodyVelocities() {
     _body->setActivationState(ACTIVE_TAG);
 }
 
-void ObjectMotionState::updateLastKinematicStep() {
-    _lastKinematicStep = ObjectMotionState::getWorldSimulationStep() - 1;
-}
-
 void ObjectMotionState::updateBodyMassProperties() {
     float mass = getMass();
     btVector3 inertia(0.0f, 0.0f, 0.0f);

libraries/physics/src/ObjectMotionState.h

@@ -50,12 +50,11 @@ const uint32_t HARD_DIRTY_PHYSICS_FLAGS = (uint32_t)(Simulation::DIRTY_MOTION_TY
                                                      Simulation::DIRTY_COLLISION_GROUP);
 const uint32_t EASY_DIRTY_PHYSICS_FLAGS = (uint32_t)(Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES |
                                                      Simulation::DIRTY_MASS | Simulation::DIRTY_MATERIAL |
-                                                     Simulation::DIRTY_SIMULATOR_ID | Simulation::DIRTY_SIMULATION_OWNERSHIP_PRIORITY |
+                                                     Simulation::DIRTY_SIMULATOR_ID | Simulation::DIRTY_SIMULATION_OWNERSHIP_PRIORITY);
-                                                     Simulation::DIRTY_PHYSICS_ACTIVATION);
-
 
 // These are the set of incoming flags that the PhysicsEngine needs to hear about:
-const uint32_t DIRTY_PHYSICS_FLAGS = (uint32_t)(HARD_DIRTY_PHYSICS_FLAGS | EASY_DIRTY_PHYSICS_FLAGS);
+const uint32_t DIRTY_PHYSICS_FLAGS = (uint32_t)(HARD_DIRTY_PHYSICS_FLAGS | EASY_DIRTY_PHYSICS_FLAGS |
+                                                Simulation::DIRTY_PHYSICS_ACTIVATION);
 
 // These are the outgoing flags that the PhysicsEngine can affect:
 const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES;
@@ -81,12 +80,11 @@ public:
     ObjectMotionState(btCollisionShape* shape);
     ~ObjectMotionState();
 
-    virtual void handleEasyChanges(uint32_t& flags);
+    virtual bool handleEasyChanges(uint32_t& flags);
     virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine);
 
     void updateBodyMaterialProperties();
     void updateBodyVelocities();
-    void updateLastKinematicStep();
     virtual void updateBodyMassProperties();
 
     MotionStateType getType() const { return _type; }

libraries/physics/src/PhysicsEngine.cpp

@@ -50,13 +50,6 @@ void PhysicsEngine::init() {
         // default gravity of the world is zero, so each object must specify its own gravity
         // TODO: set up gravity zones
         _dynamicsWorld->setGravity(btVector3(0.0f, 0.0f, 0.0f));
-
-        // By default Bullet will update the Aabb's of all objects every frame, even statics.
-        // This can waste CPU cycles so we configure Bullet to only update ACTIVE objects here.
-        // However, this means when a static object is moved we must manually update its Aabb
-        // in order for its broadphase collision queries to work correctly. Look at how we use
-        // _activeStaticBodies to track and update the Aabb's of moved static objects.
-        _dynamicsWorld->setForceUpdateAllAabbs(false);
     }
 }
 
@@ -87,7 +80,6 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
             body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
             body->updateInertiaTensor();
             motionState->updateBodyVelocities();
-            motionState->updateLastKinematicStep();
             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);
@@ -197,18 +189,12 @@ VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& ob
                 stillNeedChange.push_back(object);
             }
         } else if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
-            object->handleEasyChanges(flags);
+            if (object->handleEasyChanges(flags)) {
-            object->clearIncomingDirtyFlags();
+                object->clearIncomingDirtyFlags();
+            } else {
+                stillNeedChange.push_back(object);
+            }
         }
-        if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) {
-            _activeStaticBodies.push_back(object->getRigidBody());
-        }
-    }
-    // active static bodies have changed (in an Easy way) and need their Aabbs updated
-    // but we've configured Bullet to NOT update them automatically (for improved performance)
-    // so we must do it ourselves
-    for (size_t i = 0; i < _activeStaticBodies.size(); ++i) {
-        _dynamicsWorld->updateSingleAabb(_activeStaticBodies[i]);
     }
     return stillNeedChange;
 }
@@ -254,7 +240,6 @@ void PhysicsEngine::stepSimulation() {
     float timeStep = btMin(dt, MAX_TIMESTEP);
 
     if (_myAvatarController) {
-        BT_PROFILE("avatarController");
         // TODO: move this stuff outside and in front of stepSimulation, because
         // the updateShapeIfNecessary() call needs info from MyAvatar and should
         // be done on the main thread during the pre-simulation stuff
@@ -403,12 +388,6 @@ const CollisionEvents& PhysicsEngine::getCollisionEvents() {
 
 const VectorOfMotionStates& PhysicsEngine::getOutgoingChanges() {
     BT_PROFILE("copyOutgoingChanges");
-    // Bullet will not deactivate static objects (it doesn't expect them to be active)
-    // so we must deactivate them ourselves
-    for (size_t i = 0; i < _activeStaticBodies.size(); ++i) {
-        _activeStaticBodies[i]->forceActivationState(ISLAND_SLEEPING);
-    }
-    _activeStaticBodies.clear();
     _dynamicsWorld->synchronizeMotionStates();
     _hasOutgoingChanges = false;
     return _dynamicsWorld->getChangedMotionStates();

libraries/physics/src/PhysicsEngine.h

@@ -13,9 +13,9 @@
 #define hifi_PhysicsEngine_h
 
 #include <stdint.h>
-#include <vector>
 
 #include <QUuid>
+#include <QVector>
 #include <btBulletDynamicsCommon.h>
 #include <BulletCollision/CollisionDispatch/btGhostObject.h>
 
@@ -41,7 +41,7 @@ public:
 };
 
 typedef std::map<ContactKey, ContactInfo> ContactMap;
-typedef std::vector<Collision> CollisionEvents;
+typedef QVector<Collision> CollisionEvents;
 
 class PhysicsEngine {
 public:
@@ -110,7 +110,6 @@ private:
     ContactMap _contactMap;
     CollisionEvents _collisionEvents;
     QHash<QUuid, EntityActionPointer> _objectActions;
-    std::vector<btRigidBody*> _activeStaticBodies;
 
     glm::vec3 _originOffset;
     QUuid _sessionID;
@@ -122,6 +121,7 @@ private:
 
     bool _dumpNextStats = false;
     bool _hasOutgoingChanges = false;
+
 };
 
 typedef std::shared_ptr<PhysicsEngine> PhysicsEnginePointer;

libraries/physics/src/ThreadSafeDynamicsWorld.cpp

@@ -67,10 +67,7 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep
 
         saveKinematicState(fixedTimeStep*clampedSimulationSteps);
 
-        {
+        applyGravity();
-            BT_PROFILE("applyGravity");
-            applyGravity();
-        }
 
         for (int i=0;i<clampedSimulationSteps;i++) {
             internalSingleStepSimulation(fixedTimeStep);
@@ -146,24 +143,3 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
     }
 }
 
-void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) {
-///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
-///to switch status _after_ adding kinematic objects to the world
-///fix it for Bullet 3.x release
-    BT_PROFILE("saveKinematicState");
-    for (int i=0;i<m_collisionObjects.size();i++)
-    {
-        btCollisionObject* colObj = m_collisionObjects[i];
-        btRigidBody* body = btRigidBody::upcast(colObj);
-        if (body && body->getActivationState() != ISLAND_SLEEPING)
-        {
-            if (body->isKinematicObject())
-            {
-                //to calculate velocities next frame
-                body->saveKinematicState(timeStep);
-            }
-        }
-    }
-}
-
-

libraries/physics/src/ThreadSafeDynamicsWorld.h

@@ -41,7 +41,6 @@ public:
                                           btScalar fixedTimeStep = btScalar(1.)/btScalar(60.),
                                           SubStepCallback onSubStep = []() { });
     virtual void synchronizeMotionStates() override;
-    virtual void saveKinematicState(btScalar timeStep) override;
 
     // btDiscreteDynamicsWorld::m_localTime is the portion of real-time that has not yet been simulated
     // but is used for MotionState::setWorldTransform() extrapolation (a feature that Bullet uses to provide

libraries/shared/src/GLMHelpers.cpp

@@ -463,6 +463,14 @@ glm::vec2 getFacingDir2D(const glm::mat4& m) {
     }
 }
 
+bool isNaN(glm::vec3 value) {
+    return isNaN(value.x) || isNaN(value.y) || isNaN(value.z);
+}
+
+bool isNaN(glm::quat value) {
+    return isNaN(value.w) || isNaN(value.x) || isNaN(value.y) || isNaN(value.z);
+}
+
 glm::mat4 orthoInverse(const glm::mat4& m) {
     glm::mat4 r = m;
     r[3] = glm::vec4(0.0f, 0.0f, 0.0f, 1.0f);

libraries/shared/src/GLMHelpers.h

@@ -229,8 +229,8 @@ void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& seconda
 glm::vec2 getFacingDir2D(const glm::quat& rot);
 glm::vec2 getFacingDir2D(const glm::mat4& m);
 
-inline bool isNaN(const glm::vec3& value) { return isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }
+bool isNaN(glm::vec3 value);
-inline bool isNaN(const glm::quat& value) { return isNaN(value.w) || isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }
+bool isNaN(glm::quat value);
 
 glm::mat4 orthoInverse(const glm::mat4& m);
 

libraries/shared/src/PhysicsCollisionGroups.h

@@ -51,10 +51,10 @@ const int16_t BULLET_COLLISION_GROUP_COLLISIONLESS = 1 << 14;
 const int16_t BULLET_COLLISION_MASK_DEFAULT = ~ BULLET_COLLISION_GROUP_COLLISIONLESS;
 
 // STATIC does not collide with itself (as optimization of physics simulation)
-const int16_t BULLET_COLLISION_MASK_STATIC = ~ (BULLET_COLLISION_GROUP_COLLISIONLESS | BULLET_COLLISION_GROUP_KINEMATIC | BULLET_COLLISION_GROUP_STATIC);
+const int16_t BULLET_COLLISION_MASK_STATIC = ~ (BULLET_COLLISION_GROUP_COLLISIONLESS | BULLET_COLLISION_GROUP_STATIC);
 
 const int16_t BULLET_COLLISION_MASK_DYNAMIC = BULLET_COLLISION_MASK_DEFAULT;
-const int16_t BULLET_COLLISION_MASK_KINEMATIC = BULLET_COLLISION_MASK_STATIC;
+const int16_t BULLET_COLLISION_MASK_KINEMATIC = BULLET_COLLISION_MASK_DEFAULT;
 
 // MY_AVATAR does not collide with itself
 const int16_t BULLET_COLLISION_MASK_MY_AVATAR = ~(BULLET_COLLISION_GROUP_COLLISIONLESS | BULLET_COLLISION_GROUP_MY_AVATAR);

libraries/shared/src/SharedUtil.cpp

@@ -247,6 +247,12 @@ int getNthBit(unsigned char byte, int ordinal) {
     return ERROR_RESULT;
 }
 
+bool isBetween(int64_t value, int64_t max, int64_t min) {
+    return ((value <= max) && (value >= min));
+}
+
+
+
 void setSemiNibbleAt(unsigned char& byte, int bitIndex, int value) {
     //assert(value <= 3 && value >= 0);
     byte |= ((value & 3) << (6 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11
@@ -254,7 +260,12 @@ void setSemiNibbleAt(unsigned char& byte, int bitIndex, int value) {
 
 bool isInEnvironment(const char* environment) {
     char* environmentString = getenv("HIFI_ENVIRONMENT");
-    return (environmentString && strcmp(environmentString, environment) == 0);
+
+    if (environmentString && strcmp(environmentString, environment) == 0) {
+        return true;
+    } else {
+        return false;
+    }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////
@@ -621,6 +632,10 @@ void debug::checkDeadBeef(void* memoryVoid, int size) {
     assert(memcmp((unsigned char*)memoryVoid, DEADBEEF, std::min(size, DEADBEEF_SIZE)) != 0);
 }
 
+bool isNaN(float value) {
+    return value != value;
+}
+
 QString formatUsecTime(float usecs, int prec) {
     static const quint64 SECONDS_PER_MINUTE = 60;
     static const quint64 USECS_PER_MINUTE = USECS_PER_SECOND * SECONDS_PER_MINUTE;

libraries/shared/src/SharedUtil.h

@@ -180,11 +180,11 @@ private:
     static int DEADBEEF_SIZE;
 };
 
-/// \return true when value is between max and min
+bool isBetween(int64_t value, int64_t max, int64_t min);
-inline bool isBetween(int64_t value, int64_t max, int64_t min) { return ((value <= max) && (value >= min)); }
+
 
 /// \return bool is the float NaN
-inline bool isNaN(float value) { return value != value; }
+bool isNaN(float value);
 
 QString formatUsecTime(float usecs, int prec = 3);
 QString formatSecondsElapsed(float seconds);

libraries/shared/src/SpatiallyNestable.cpp

@@ -90,9 +90,11 @@ SpatiallyNestablePointer SpatiallyNestable::getParentPointer(bool& success) cons
         return parent;
     }
 
+    SpatiallyNestablePointer thisPointer = getThisPointer();
+
     if (parent) {
         // we have a parent pointer but our _parentID doesn't indicate this parent.
-        parent->forgetChild(getThisPointer());
+        parent->forgetChild(thisPointer);
         _parentKnowsMe = false;
         _parent.reset();
     }
@@ -110,11 +112,16 @@ SpatiallyNestablePointer SpatiallyNestable::getParentPointer(bool& success) cons
 
     parent = _parent.lock();
     if (parent) {
-        parent->beParentOfChild(getThisPointer());
+        parent->beParentOfChild(thisPointer);
         _parentKnowsMe = true;
     }
 
-    success = (parent || parentID.isNull());
+    if (parent || parentID.isNull()) {
+        success = true;
+    } else {
+        success = false;
+    }
+
     return parent;
 }
 
@@ -842,40 +849,3 @@ AACube SpatiallyNestable::getQueryAACube() const {
     }
     return result;
 }
-
-void SpatiallyNestable::getLocalTransformAndVelocities(
-        Transform& transform,
-        glm::vec3& velocity,
-        glm::vec3& angularVelocity) const {
-    // transform
-    _transformLock.withReadLock([&] {
-        transform = _transform;
-    });
-    // linear velocity
-    _velocityLock.withReadLock([&] {
-        velocity = _velocity;
-    });
-    // angular velocity
-    _angularVelocityLock.withReadLock([&] {
-        angularVelocity = _angularVelocity;
-    });
-}
-
-void SpatiallyNestable::setLocalTransformAndVelocities(
-        const Transform& localTransform,
-        const glm::vec3& localVelocity,
-        const glm::vec3& localAngularVelocity) {
-    // transform
-    _transformLock.withWriteLock([&] {
-        _transform = localTransform;
-    });
-    // linear velocity
-    _velocityLock.withWriteLock([&] {
-        _velocity = localVelocity;
-    });
-    // angular velocity
-    _angularVelocityLock.withWriteLock([&] {
-        _angularVelocity = localAngularVelocity;
-    });
-    locationChanged();
-}

libraries/shared/src/SpatiallyNestable.h

@@ -149,13 +149,6 @@ protected:
     quint16 _parentJointIndex { 0 }; // which joint of the parent is this relative to?
     SpatiallyNestablePointer getParentPointer(bool& success) const;
 
-    void getLocalTransformAndVelocities(Transform& localTransform, glm::vec3& localVelocity, glm::vec3& localAngularVelocity) const;
-
-    void setLocalTransformAndVelocities(
-            const Transform& localTransform,
-            const glm::vec3& localVelocity,
-            const glm::vec3& localAngularVelocity);
-
     mutable SpatiallyNestableWeakPointer _parent;
 
     virtual void beParentOfChild(SpatiallyNestablePointer newChild) const;

libraries/shared/src/Transform.cpp

@@ -150,3 +150,7 @@ QJsonObject Transform::toJson(const Transform& transform) {
     }
     return result;
 }
+
+bool Transform::containsNaN() const {
+    return isNaN(_rotation) || isNaN(_scale) || isNaN(_translation);
+}

libraries/shared/src/Transform.h

@@ -145,7 +145,7 @@ public:
     Vec4 transform(const Vec4& pos) const;
     Vec3 transform(const Vec3& pos) const;
 
-    bool containsNaN() const { return isNaN(_rotation) || isNaN(glm::dot(_scale, _translation)); }
+    bool containsNaN() const;
 
 protected: