From 3f52d237edbf2cc7d810782f8b4c623d6d5e200f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 31 Mar 2016 10:27:17 -0700 Subject: [PATCH 01/11] adding Bullet profiler instances for better stats --- libraries/physics/src/EntityMotionState.cpp | 2 ++ libraries/physics/src/PhysicsEngine.cpp | 1 + .../physics/src/ThreadSafeDynamicsWorld.cpp | 26 ++++++++++++++++++- .../physics/src/ThreadSafeDynamicsWorld.h | 1 + 4 files changed, 29 insertions(+), 1 deletion(-) diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index e3952ba1d6..7b83c2e2ce 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -175,11 +175,13 @@ 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(); diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 0040c19c3d..74f6f90900 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -240,6 +240,7 @@ 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 diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp index b6f3487f1a..5fe99f137c 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp @@ -67,7 +67,10 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep saveKinematicState(fixedTimeStep*clampedSimulationSteps); - applyGravity(); + { + BT_PROFILE("applyGravity"); + applyGravity(); + } for (int i=0;igetActivationState() != ISLAND_SLEEPING) + { + if (body->isKinematicObject()) + { + //to calculate velocities next frame + body->saveKinematicState(timeStep); + } + } + } +} + + diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.h b/libraries/physics/src/ThreadSafeDynamicsWorld.h index e9708149da..68062d8d29 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.h +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.h @@ -41,6 +41,7 @@ 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 From 442b52313fbee5398df9311b35c234f06ba5ec2b Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 1 Apr 2016 12:02:01 -0700 Subject: [PATCH 02/11] faster kinematic motion calculations --- libraries/entities/src/EntityItem.cpp | 139 +++++++-------------- libraries/shared/src/SpatiallyNestable.cpp | 50 ++++++-- libraries/shared/src/SpatiallyNestable.h | 7 ++ 3 files changed, 93 insertions(+), 103 deletions(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index 431d638063..69614d0d12 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -876,123 +876,76 @@ void EntityItem::simulate(const quint64& now) { } void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { -#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()) { + if (hasActions() || timeElapsed < 0.0f) { return; } - if (hasLocalAngularVelocity()) { - glm::vec3 localAngularVelocity = getLocalAngularVelocity(); + const float MAX_TIME_ELAPSED = 1.0f; // seconds + timeElapsed = glm::min(timeElapsed, MAX_TIME_ELAPSED); + Transform transform; + glm::vec3 linearVelocity; + glm::vec3 angularVelocity; + getLocalEverything(transform, linearVelocity, angularVelocity); + + bool isMoving = false; + if (glm::length2(angularVelocity) > 0.0f) { // angular damping if (_angularDamping > 0.0f) { - localAngularVelocity *= powf(1.0f - _angularDamping, timeElapsed); - #ifdef WANT_DEBUG - qCDebug(entities) << " angularDamping :" << _angularDamping; - qCDebug(entities) << " newAngularVelocity:" << localAngularVelocity; - #endif + angularVelocity *= powf(1.0f - _angularDamping, timeElapsed); } - float angularSpeed = glm::length(localAngularVelocity); - - 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; + const float EPSILON_ANGULAR_VELOCITY_LENGTH_SQUARED = 0.0017453f * 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec + if (glm::length2(angularVelocity) < EPSILON_ANGULAR_VELOCITY_LENGTH_SQUARED) { + angularVelocity = 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 = getRotation(); + glm::quat rotation = transform.getRotation(); float dt = timeElapsed; - while (dt > PHYSICS_ENGINE_FIXED_SUBSTEP) { - glm::quat dQ = computeBulletRotationStep(localAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP); + while (dt > 0.0f) { + glm::quat dQ = computeBulletRotationStep(angularVelocity, glm::min(dt, PHYSICS_ENGINE_FIXED_SUBSTEP)); rotation = glm::normalize(dQ * rotation); dt -= PHYSICS_ENGINE_FIXED_SUBSTEP; } - // NOTE: this final partial substep can drift away from a real Bullet simulation however - // 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); - rotation = glm::normalize(dQ * rotation); - - setRotation(rotation); + transform.setRotation(rotation); + isMoving = true; } - - setLocalAngularVelocity(localAngularVelocity); } - - if (hasLocalVelocity()) { - - // acceleration is in the global frame, so transform it into the local frame. - // TODO: Move this into SpatiallyNestable. - bool success; - Transform transform = getParentTransform(success); - glm::vec3 localAcceleration(glm::vec3::_null); - if (success) { - localAcceleration = glm::inverse(transform.getRotation()) * getAcceleration(); - } else { - localAcceleration = getAcceleration(); - } - + if (glm::length2(linearVelocity) > 0.0f) { // linear damping - 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 + linearVelocity *= powf(1.0f - _damping, timeElapsed); } - // integrate position forward - glm::vec3 localPosition = getLocalPosition(); - glm::vec3 newLocalPosition = localPosition + (localVelocity * timeElapsed) + 0.5f * localAcceleration * timeElapsed * timeElapsed; - - #ifdef WANT_DEBUG - qCDebug(entities) << " EntityItem::simulate()...."; - qCDebug(entities) << " timeElapsed:" << timeElapsed; - qCDebug(entities) << " old AACube:" << getMaximumAACube(); - qCDebug(entities) << " old position:" << localPosition; - qCDebug(entities) << " old velocity:" << localVelocity; - qCDebug(entities) << " old getAABox:" << getAABox(); - qCDebug(entities) << " newPosition:" << newPosition; - qCDebug(entities) << " glm::distance(newPosition, position):" << glm::distance(newLocalPosition, localPosition); - #endif - - localPosition = newLocalPosition; - - // apply effective acceleration, which will be the same as gravity if the Entity isn't at rest. - localVelocity += localAcceleration * timeElapsed; - - float speed = glm::length(localVelocity); - const float EPSILON_LINEAR_VELOCITY_LENGTH = 0.001f; // 1mm/sec - if (speed < EPSILON_LINEAR_VELOCITY_LENGTH) { - setVelocity(ENTITY_ITEM_ZERO_VEC3); - if (setFlags && speed > 0.0f) { - _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; + glm::vec3 linearAcceleration = _acceleration; + if (glm::length2(_acceleration) > 0.0f) { + // acceleration is in world-frame but we need it in local-frame + bool success; + Transform parentTransform = getParentTransform(success); + if (success) { + linearAcceleration = glm::inverse(parentTransform.getRotation()) * linearAcceleration; } - } else { - setLocalPosition(localPosition); - setLocalVelocity(localVelocity); } - #ifdef WANT_DEBUG - qCDebug(entities) << " new position:" << position; - qCDebug(entities) << " new velocity:" << velocity; - qCDebug(entities) << " new AACube:" << getMaximumAACube(); - qCDebug(entities) << " old getAABox:" << getAABox(); - #endif + // integrate linearVelocity + linearVelocity += linearAcceleration * timeElapsed; + + const float EPSILON_LINEAR_VELOCITY_LENGTH_SQUARED = 1.0e-6f; // 1mm/sec ^2 + if (glm::length2(linearVelocity) < EPSILON_LINEAR_VELOCITY_LENGTH_SQUARED) { + setVelocity(ENTITY_ITEM_ZERO_VEC3); + } else { + // integrate position forward + // NOTE: we're using the NEW linear velocity, which is why we negate the acceleration term + glm::vec3 position = transform.getTranslation() + (linearVelocity * timeElapsed) - 0.5f * linearAcceleration * timeElapsed * timeElapsed; + transform.setTranslation(position); + isMoving = true; + } + } + setLocalEverything(transform, linearVelocity, angularVelocity); + if (!isMoving) { + // flag this entity to be removed from kinematic motion + _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; } } diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 13bf5d9054..550864265f 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -90,11 +90,9 @@ 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(thisPointer); + parent->forgetChild(getThisPointer()); _parentKnowsMe = false; _parent.reset(); } @@ -112,16 +110,11 @@ SpatiallyNestablePointer SpatiallyNestable::getParentPointer(bool& success) cons parent = _parent.lock(); if (parent) { - parent->beParentOfChild(thisPointer); + parent->beParentOfChild(getThisPointer()); _parentKnowsMe = true; } - if (parent || parentID.isNull()) { - success = true; - } else { - success = false; - } - + success = (parent || parentID.isNull()); return parent; } @@ -849,3 +842,40 @@ AACube SpatiallyNestable::getQueryAACube() const { } return result; } + +void SpatiallyNestable::getLocalEverything( + 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::setLocalEverything( + 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(); +} diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h index 379f2facd7..6546d46e51 100644 --- a/libraries/shared/src/SpatiallyNestable.h +++ b/libraries/shared/src/SpatiallyNestable.h @@ -149,6 +149,13 @@ protected: quint16 _parentJointIndex { 0 }; // which joint of the parent is this relative to? SpatiallyNestablePointer getParentPointer(bool& success) const; + void getLocalEverything(Transform& localTransform, glm::vec3& localVelocity, glm::vec3& localAngularVelocity) const; + + void setLocalEverything( + const Transform& localTransform, + const glm::vec3& localVelocity, + const glm::vec3& localAngularVelocity); + mutable SpatiallyNestableWeakPointer _parent; virtual void beParentOfChild(SpatiallyNestablePointer newChild) const; From e1602b57faad3e92c2c3862bda45016587882719 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 1 Apr 2016 12:03:21 -0700 Subject: [PATCH 03/11] faster isNaN checks --- libraries/shared/src/GLMHelpers.cpp | 8 -------- libraries/shared/src/GLMHelpers.h | 4 ++-- libraries/shared/src/SharedUtil.cpp | 17 +---------------- libraries/shared/src/SharedUtil.h | 6 +++--- libraries/shared/src/Transform.cpp | 4 ---- libraries/shared/src/Transform.h | 2 +- 6 files changed, 7 insertions(+), 34 deletions(-) diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index d21d88d212..53abb3827d 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -463,14 +463,6 @@ 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); diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 469ca1fc81..8b1446d4e5 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/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); -bool isNaN(glm::vec3 value); -bool isNaN(glm::quat value); +inline bool isNaN(const glm::vec3& value) { return isNaN(value.x) || isNaN(value.y) || isNaN(value.z); } +inline bool isNaN(const glm::quat& value) { return isNaN(value.w) || isNaN(value.x) || isNaN(value.y) || isNaN(value.z); } glm::mat4 orthoInverse(const glm::mat4& m); diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp index 30d4726bcc..d48bddbd88 100644 --- a/libraries/shared/src/SharedUtil.cpp +++ b/libraries/shared/src/SharedUtil.cpp @@ -247,12 +247,6 @@ 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 @@ -260,12 +254,7 @@ void setSemiNibbleAt(unsigned char& byte, int bitIndex, int value) { bool isInEnvironment(const char* environment) { char* environmentString = getenv("HIFI_ENVIRONMENT"); - - if (environmentString && strcmp(environmentString, environment) == 0) { - return true; - } else { - return false; - } + return (environmentString && strcmp(environmentString, environment) == 0); } ////////////////////////////////////////////////////////////////////////////////////////// @@ -632,10 +621,6 @@ 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; diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h index 8fb65a5247..e9201b4a92 100644 --- a/libraries/shared/src/SharedUtil.h +++ b/libraries/shared/src/SharedUtil.h @@ -180,11 +180,11 @@ private: static int DEADBEEF_SIZE; }; -bool isBetween(int64_t value, int64_t max, int64_t min); - +/// \return true when value is between max and min +inline bool isBetween(int64_t value, int64_t max, int64_t min) { return ((value <= max) && (value >= min)); } /// \return bool is the float NaN -bool isNaN(float value); +inline bool isNaN(float value) { return value != value; } QString formatUsecTime(float usecs, int prec = 3); QString formatSecondsElapsed(float seconds); diff --git a/libraries/shared/src/Transform.cpp b/libraries/shared/src/Transform.cpp index a3a3c05731..c51b3dae4b 100644 --- a/libraries/shared/src/Transform.cpp +++ b/libraries/shared/src/Transform.cpp @@ -150,7 +150,3 @@ QJsonObject Transform::toJson(const Transform& transform) { } return result; } - -bool Transform::containsNaN() const { - return isNaN(_rotation) || isNaN(_scale) || isNaN(_translation); -} diff --git a/libraries/shared/src/Transform.h b/libraries/shared/src/Transform.h index 1024173cbd..1e1d10c54b 100644 --- a/libraries/shared/src/Transform.h +++ b/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; + bool containsNaN() const { return isNaN(_rotation) || isNaN(glm::dot(_scale, _translation)); } protected: From 25fbf926df007496267efcc3c1b97331a430834a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 1 Apr 2016 12:04:27 -0700 Subject: [PATCH 04/11] CollisionWorld only updates _active_ Aabbs we manually set/clear active state of static objects that need their Aabbs updated also fixing a bug when starting kinematic motion --- libraries/physics/src/EntityMotionState.cpp | 17 ++++++--------- libraries/physics/src/EntityMotionState.h | 6 +----- libraries/physics/src/ObjectMotionState.cpp | 18 ++++++++++++---- libraries/physics/src/ObjectMotionState.h | 10 +++++---- libraries/physics/src/PhysicsEngine.cpp | 24 ++++++++++++++++----- libraries/physics/src/PhysicsEngine.h | 6 +++--- 6 files changed, 50 insertions(+), 31 deletions(-) diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index 7b83c2e2ce..5f168d2e33 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -94,7 +94,7 @@ void EntityMotionState::updateServerPhysicsVariables() { } // virtual -bool EntityMotionState::handleEasyChanges(uint32_t& flags) { +void EntityMotionState::handleEasyChanges(uint32_t& flags) { assert(entityTreeIsLocked()); updateServerPhysicsVariables(); ObjectMotionState::handleEasyChanges(flags); @@ -137,8 +137,6 @@ bool EntityMotionState::handleEasyChanges(uint32_t& flags) { if ((flags & Simulation::DIRTY_PHYSICS_ACTIVATION) && !_body->isActive()) { _body->activate(); } - - return true; } @@ -422,19 +420,18 @@ 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 (getAccelerationNearlyGravityCount() < STEPS_TO_DECIDE_BALLISTIC) { - // only increment this if we haven't reached the threshold yet. this is to avoid - // overflowing the counter. - incrementAccelerationNearlyGravityCount(); + if (_accelerationNearlyGravityCount < STEPS_TO_DECIDE_BALLISTIC) { + // only increment this if we haven't reached the threshold yet, to avoid overflowing the counter + ++_accelerationNearlyGravityCount; } } else { - // acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter - resetAccelerationNearlyGravityCount(); + // acceleration wasn't similar to this entity's gravity, reset the counter + _accelerationNearlyGravityCount = 0; } // 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 (getAccelerationNearlyGravityCount() >= STEPS_TO_DECIDE_BALLISTIC) { + if (_accelerationNearlyGravityCount >= STEPS_TO_DECIDE_BALLISTIC) { _entity->setAcceleration(_entity->getGravity()); } else { _entity->setAcceleration(glm::vec3(0.0f)); diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index ac16ec6d5d..eb42d03b52 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/libraries/physics/src/EntityMotionState.h @@ -29,7 +29,7 @@ public: virtual ~EntityMotionState(); void updateServerPhysicsVariables(); - virtual bool handleEasyChanges(uint32_t& flags) override; + virtual void handleEasyChanges(uint32_t& flags) override; virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override; /// \return PhysicsMotionType based on params set in EntityItem @@ -51,10 +51,6 @@ 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(); } diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index 482c3146f8..6b2022e204 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -39,7 +39,7 @@ const glm::vec3& ObjectMotionState::getWorldOffset() { } // static -uint32_t worldSimulationStep = 0; +uint32_t worldSimulationStep = 1; void ObjectMotionState::setWorldSimulationStep(uint32_t step) { assert(step > worldSimulationStep); worldSimulationStep = step; @@ -164,7 +164,7 @@ void ObjectMotionState::setRigidBody(btRigidBody* body) { } } -bool ObjectMotionState::handleEasyChanges(uint32_t& flags) { +void ObjectMotionState::handleEasyChanges(uint32_t& flags) { if (flags & Simulation::DIRTY_POSITION) { btTransform worldTrans = _body->getWorldTransform(); btVector3 newPosition = glmToBullet(getObjectPosition()); @@ -183,6 +183,10 @@ bool 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()); @@ -192,6 +196,10 @@ bool 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) { @@ -232,8 +240,6 @@ bool ObjectMotionState::handleEasyChanges(uint32_t& flags) { if (flags & Simulation::DIRTY_MASS) { updateBodyMassProperties(); } - - return true; } bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) { @@ -292,6 +298,10 @@ 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); diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index bb78eb12d6..44cb88721c 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/libraries/physics/src/ObjectMotionState.h @@ -50,11 +50,12 @@ 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 | - Simulation::DIRTY_PHYSICS_ACTIVATION); +const uint32_t DIRTY_PHYSICS_FLAGS = (uint32_t)(HARD_DIRTY_PHYSICS_FLAGS | EASY_DIRTY_PHYSICS_FLAGS); // These are the outgoing flags that the PhysicsEngine can affect: const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES; @@ -80,11 +81,12 @@ public: ObjectMotionState(btCollisionShape* shape); ~ObjectMotionState(); - virtual bool handleEasyChanges(uint32_t& flags); + virtual void 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; } diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 74f6f90900..55861e9b3a 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -50,6 +50,7 @@ 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)); + _dynamicsWorld->setForceUpdateAllAabbs(false); } } @@ -80,6 +81,7 @@ 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); @@ -189,12 +191,18 @@ VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& ob stillNeedChange.push_back(object); } } else if (flags & EASY_DIRTY_PHYSICS_FLAGS) { - if (object->handleEasyChanges(flags)) { - object->clearIncomingDirtyFlags(); - } else { - stillNeedChange.push_back(object); - } + object->handleEasyChanges(flags); + object->clearIncomingDirtyFlags(); } + 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 outselves + for (int i = 0; i < _activeStaticBodies.size(); ++i) { + _dynamicsWorld->updateSingleAabb(_activeStaticBodies[i]); } return stillNeedChange; } @@ -389,6 +397,12 @@ 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 (int i = 0; i < _activeStaticBodies.size(); ++i) { + _activeStaticBodies[i]->forceActivationState(ISLAND_SLEEPING); + } + _activeStaticBodies.clear(); _dynamicsWorld->synchronizeMotionStates(); _hasOutgoingChanges = false; return _dynamicsWorld->getChangedMotionStates(); diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index f644d6f5b2..18e4016fec 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -13,9 +13,9 @@ #define hifi_PhysicsEngine_h #include +#include #include -#include #include #include @@ -41,7 +41,7 @@ public: }; typedef std::map ContactMap; -typedef QVector CollisionEvents; +typedef std::vector CollisionEvents; class PhysicsEngine { public: @@ -110,6 +110,7 @@ private: ContactMap _contactMap; CollisionEvents _collisionEvents; QHash _objectActions; + std::vector _activeStaticBodies; glm::vec3 _originOffset; QUuid _sessionID; @@ -121,7 +122,6 @@ private: bool _dumpNextStats = false; bool _hasOutgoingChanges = false; - }; typedef std::shared_ptr PhysicsEnginePointer; From de5fe705a33e171016b9855c9383c147620ba312 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 1 Apr 2016 12:06:47 -0700 Subject: [PATCH 05/11] optimize Bullet broadphase using collision groups --- libraries/shared/src/PhysicsCollisionGroups.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/shared/src/PhysicsCollisionGroups.h b/libraries/shared/src/PhysicsCollisionGroups.h index 6d320e69cb..794f338dc5 100644 --- a/libraries/shared/src/PhysicsCollisionGroups.h +++ b/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_STATIC); +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_DYNAMIC = BULLET_COLLISION_MASK_DEFAULT; -const int16_t BULLET_COLLISION_MASK_KINEMATIC = BULLET_COLLISION_MASK_DEFAULT; +const int16_t BULLET_COLLISION_MASK_KINEMATIC = BULLET_COLLISION_MASK_STATIC; // MY_AVATAR does not collide with itself const int16_t BULLET_COLLISION_MASK_MY_AVATAR = ~(BULLET_COLLISION_GROUP_COLLISIONLESS | BULLET_COLLISION_GROUP_MY_AVATAR); From 20914df3303e79cd61c79a1c91222171e4c15956 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 1 Apr 2016 13:31:14 -0700 Subject: [PATCH 06/11] fix warning about signed/unsigned comparison --- libraries/physics/src/PhysicsEngine.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 55861e9b3a..973be0b4e8 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -201,7 +201,7 @@ VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& ob // 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 outselves - for (int i = 0; i < _activeStaticBodies.size(); ++i) { + for (size_t i = 0; i < _activeStaticBodies.size(); ++i) { _dynamicsWorld->updateSingleAabb(_activeStaticBodies[i]); } return stillNeedChange; @@ -399,7 +399,7 @@ 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 (int i = 0; i < _activeStaticBodies.size(); ++i) { + for (size_t i = 0; i < _activeStaticBodies.size(); ++i) { _activeStaticBodies[i]->forceActivationState(ISLAND_SLEEPING); } _activeStaticBodies.clear(); From 0830c55bcfacef6c6f2fa49ad636acae83b4ed8d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 1 Apr 2016 17:48:56 -0700 Subject: [PATCH 07/11] getLocalEverything->getLocalTransformAndVelocities --- libraries/entities/src/EntityItem.cpp | 4 ++-- libraries/shared/src/SpatiallyNestable.cpp | 4 ++-- libraries/shared/src/SpatiallyNestable.h | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index 69614d0d12..5c825495b9 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -886,7 +886,7 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { Transform transform; glm::vec3 linearVelocity; glm::vec3 angularVelocity; - getLocalEverything(transform, linearVelocity, angularVelocity); + getLocalTransformAndVelocities(transform, linearVelocity, angularVelocity); bool isMoving = false; if (glm::length2(angularVelocity) > 0.0f) { @@ -942,7 +942,7 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { isMoving = true; } } - setLocalEverything(transform, linearVelocity, angularVelocity); + setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity); if (!isMoving) { // flag this entity to be removed from kinematic motion _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 550864265f..3cbd3c5ac9 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -843,7 +843,7 @@ AACube SpatiallyNestable::getQueryAACube() const { return result; } -void SpatiallyNestable::getLocalEverything( +void SpatiallyNestable::getLocalTransformAndVelocities( Transform& transform, glm::vec3& velocity, glm::vec3& angularVelocity) const { @@ -861,7 +861,7 @@ void SpatiallyNestable::getLocalEverything( }); } -void SpatiallyNestable::setLocalEverything( +void SpatiallyNestable::setLocalTransformAndVelocities( const Transform& localTransform, const glm::vec3& localVelocity, const glm::vec3& localAngularVelocity) { diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h index 6546d46e51..ef70d0231b 100644 --- a/libraries/shared/src/SpatiallyNestable.h +++ b/libraries/shared/src/SpatiallyNestable.h @@ -149,9 +149,9 @@ protected: quint16 _parentJointIndex { 0 }; // which joint of the parent is this relative to? SpatiallyNestablePointer getParentPointer(bool& success) const; - void getLocalEverything(Transform& localTransform, glm::vec3& localVelocity, glm::vec3& localAngularVelocity) const; + void getLocalTransformAndVelocities(Transform& localTransform, glm::vec3& localVelocity, glm::vec3& localAngularVelocity) const; - void setLocalEverything( + void setLocalTransformAndVelocities( const Transform& localTransform, const glm::vec3& localVelocity, const glm::vec3& localAngularVelocity); From 7ea81f39379105104e8e3505b31025599c42dbe6 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Sun, 3 Apr 2016 19:15:01 -0700 Subject: [PATCH 08/11] add and fix comments --- libraries/physics/src/ObjectMotionState.cpp | 5 ++++- libraries/physics/src/PhysicsEngine.cpp | 8 +++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index 6b2022e204..e902758461 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -38,8 +38,11 @@ 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 = 1; void ObjectMotionState::setWorldSimulationStep(uint32_t step) { assert(step > worldSimulationStep); worldSimulationStep = step; diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 973be0b4e8..be80149c96 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -50,6 +50,12 @@ 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); } } @@ -200,7 +206,7 @@ VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& ob } // 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 outselves + // so we must do it ourselves for (size_t i = 0; i < _activeStaticBodies.size(); ++i) { _dynamicsWorld->updateSingleAabb(_activeStaticBodies[i]); } From 74058ac049336dbdf7a332b6458be7ee68b7e8f5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Sun, 3 Apr 2016 19:15:16 -0700 Subject: [PATCH 09/11] more correct moving test for ballistic kinematics --- libraries/entities/src/EntityItem.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index 5c825495b9..5b9791ca7b 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -919,7 +919,8 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { } glm::vec3 linearAcceleration = _acceleration; - if (glm::length2(_acceleration) > 0.0f) { + bool nonZeroAcceleration = (glm::length2(_acceleration) > 0.0f); + if (nonZeroAcceleration) { // acceleration is in world-frame but we need it in local-frame bool success; Transform parentTransform = getParentTransform(success); @@ -928,17 +929,20 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { } } + // integrate position forward + glm::vec3 position = transform.getTranslation() + (linearVelocity * timeElapsed) + 0.5f * linearAcceleration * timeElapsed * timeElapsed; + transform.setTranslation(position); + // integrate linearVelocity linearVelocity += linearAcceleration * timeElapsed; const float EPSILON_LINEAR_VELOCITY_LENGTH_SQUARED = 1.0e-6f; // 1mm/sec ^2 if (glm::length2(linearVelocity) < EPSILON_LINEAR_VELOCITY_LENGTH_SQUARED) { setVelocity(ENTITY_ITEM_ZERO_VEC3); + if (nonZeroAcceleration) { + isMoving = true; + } } else { - // integrate position forward - // NOTE: we're using the NEW linear velocity, which is why we negate the acceleration term - glm::vec3 position = transform.getTranslation() + (linearVelocity * timeElapsed) - 0.5f * linearAcceleration * timeElapsed * timeElapsed; - transform.setTranslation(position); isMoving = true; } } From a53cb2e532e568caed6f6e1b40d8cccecf2a4fa1 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 4 Apr 2016 11:10:50 -0700 Subject: [PATCH 10/11] use _gravity rather than _acceleration also tweaks and comments about supporting low gravity --- libraries/entities/src/EntityItem.cpp | 74 +++++++++++++++++++-------- 1 file changed, 53 insertions(+), 21 deletions(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index 5b9791ca7b..578d57273f 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -895,9 +895,9 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { angularVelocity *= powf(1.0f - _angularDamping, timeElapsed); } - const float EPSILON_ANGULAR_VELOCITY_LENGTH_SQUARED = 0.0017453f * 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec - if (glm::length2(angularVelocity) < EPSILON_ANGULAR_VELOCITY_LENGTH_SQUARED) { - angularVelocity = ENTITY_ITEM_ZERO_VEC3; + const float MIN_KINEMATIC_ANGULAR_SPEED_SQUARED = 0.0017453f * 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec + if (glm::length2(angularVelocity) < MIN_KINEMATIC_ANGULAR_SPEED_SQUARED) { + angularVelocity = Vectors::ZERO; } else { // for improved agreement with the way Bullet integrates rotations we use an approximation // and break the integration into bullet-sized substeps @@ -912,43 +912,75 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { isMoving = true; } } - if (glm::length2(linearVelocity) > 0.0f) { + glm::vec3 position = transform.getTranslation(); + + const float MIN_KINEMATIC_LINEAR_SPEED_SQUARED = 1.0e-6f; // 1mm/sec ^2 + bool hasLinearVelocity = (glm::length2(linearVelocity) > MIN_KINEMATIC_LINEAR_SPEED_SQUARED ); + if (hasLinearVelocity) { // linear damping if (_damping > 0.0f) { linearVelocity *= powf(1.0f - _damping, timeElapsed); } - glm::vec3 linearAcceleration = _acceleration; - bool nonZeroAcceleration = (glm::length2(_acceleration) > 0.0f); - if (nonZeroAcceleration) { - // acceleration is in world-frame but we need it in local-frame - bool success; - Transform parentTransform = getParentTransform(success); - if (success) { - linearAcceleration = glm::inverse(parentTransform.getRotation()) * linearAcceleration; - } + // integrate position forward sans acceleration + position += linearVelocity * timeElapsed; + } + + const float MIN_KINEMATIC_GRAVITY_MOTION_SQUARED = 1.0e-6f; // 0.001 mm/sec^2 + bool hasGravity = (glm::length2(_gravity) > MIN_KINEMATIC_GRAVITY_MOTION_SQUARED); + if (hasGravity) { + // acceleration is in world-frame but we need it in local-frame + glm::vec3 linearAcceleration = _gravity; + bool success; + Transform parentTransform = getParentTransform(success); + if (success) { + linearAcceleration = glm::inverse(parentTransform.getRotation()) * linearAcceleration; } - // integrate position forward - glm::vec3 position = transform.getTranslation() + (linearVelocity * timeElapsed) + 0.5f * linearAcceleration * timeElapsed * timeElapsed; - transform.setTranslation(position); + // integrate position's acceleration term + position += 0.5f * linearAcceleration * timeElapsed * timeElapsed; // integrate linearVelocity linearVelocity += linearAcceleration * timeElapsed; + } - const float EPSILON_LINEAR_VELOCITY_LENGTH_SQUARED = 1.0e-6f; // 1mm/sec ^2 - if (glm::length2(linearVelocity) < EPSILON_LINEAR_VELOCITY_LENGTH_SQUARED) { - setVelocity(ENTITY_ITEM_ZERO_VEC3); - if (nonZeroAcceleration) { + if (hasLinearVelocity || hasGravity) { + // We MUST eventually stop kinematic motion for slow entities otherwise they will take + // a looooong time to settle down, so we remeasure linear speed and zero the velocity + // if it is too small. + if (glm::length2(linearVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) { + linearVelocity = Vectors::ZERO; + if (!hasLinearVelocity) { + // Despite some gravity the final linear velocity is still too small to defeat the + // "effective resistance of free-space" which means we must reset position back to + // where it started since otherwise the entity may creep vveerrryy sslloowwllyy at + // a constant speed. + position = transform.getTranslation(); + + // Ultimately what this means is that there is some minimum gravity we can + // 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, + // but the theoretical minimum gravity for zero damping at 90Hz is: + // + // minGravity = minSpeed / dt = 0.001 m/sec * 90 /sec = 0.09 m/sec^2 + // + // In practice the true minimum is half that value, since if the frame rate is ever + // less than the expected then sometimes dt will be twice as long. + // + // Since we don't set isMoving true here this entity is destined to transition to + // STATIC unless it has some angular motion keeping it alive. + } else { isMoving = true; } } else { isMoving = true; } } + transform.setTranslation(position); setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity); + if (!isMoving) { - // flag this entity to be removed from kinematic motion + // flag this entity to transition from KINEMATIC to STATIC _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; } } From a08ab8e127d066aacf73ca12cac9679f1a8b19f1 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Mon, 4 Apr 2016 11:48:45 -0700 Subject: [PATCH 11/11] restore setFlag in simulateKinematicMotion() --- libraries/entities/src/EntityItem.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index 578d57273f..9fa58c9187 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -979,7 +979,7 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) { transform.setTranslation(position); setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity); - if (!isMoving) { + if (setFlags && !isMoving) { // flag this entity to transition from KINEMATIC to STATIC _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; }