From 69bb0ebd741f56f388e560728359d759b8d9d5ff Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Mon, 4 Apr 2016 16:11:54 -0700 Subject: [PATCH] Revert "faster kinematic motion for entities" --- libraries/entities/src/EntityItem.cpp | 161 ++++++++++-------- libraries/physics/src/EntityMotionState.cpp | 19 ++- libraries/physics/src/EntityMotionState.h | 6 +- libraries/physics/src/ObjectMotionState.cpp | 21 +-- libraries/physics/src/ObjectMotionState.h | 10 +- libraries/physics/src/PhysicsEngine.cpp | 31 +--- libraries/physics/src/PhysicsEngine.h | 6 +- .../physics/src/ThreadSafeDynamicsWorld.cpp | 26 +-- .../physics/src/ThreadSafeDynamicsWorld.h | 1 - libraries/shared/src/GLMHelpers.cpp | 8 + libraries/shared/src/GLMHelpers.h | 4 +- libraries/shared/src/PhysicsCollisionGroups.h | 4 +- libraries/shared/src/SharedUtil.cpp | 17 +- libraries/shared/src/SharedUtil.h | 6 +- libraries/shared/src/SpatiallyNestable.cpp | 50 ++---- libraries/shared/src/SpatiallyNestable.h | 7 - libraries/shared/src/Transform.cpp | 4 + libraries/shared/src/Transform.h | 2 +- 18 files changed, 164 insertions(+), 219 deletions(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index 9fa58c9187..431d638063 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/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 - timeElapsed = glm::min(timeElapsed, MAX_TIME_ELAPSED); + if (hasLocalAngularVelocity()) { + 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 - if (glm::length2(angularVelocity) < MIN_KINEMATIC_ANGULAR_SPEED_SQUARED) { - angularVelocity = Vectors::ZERO; + 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; } 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) { - glm::quat dQ = computeBulletRotationStep(angularVelocity, glm::min(dt, PHYSICS_ENGINE_FIXED_SUBSTEP)); + while (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); - isMoving = true; - } - } - glm::vec3 position = transform.getTranslation(); + // 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); - 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); + setRotation(rotation); } - // integrate position forward sans acceleration - position += linearVelocity * timeElapsed; + setLocalAngularVelocity(localAngularVelocity); } - 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; + if (hasLocalVelocity()) { + + // acceleration is in the global frame, so transform it into the local frame. + // TODO: Move this into SpatiallyNestable. 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 - position += 0.5f * linearAcceleration * timeElapsed * timeElapsed; + // 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 + } - // integrate linearVelocity - linearVelocity += linearAcceleration * timeElapsed; - } + // integrate position forward + glm::vec3 localPosition = getLocalPosition(); + glm::vec3 newLocalPosition = localPosition + (localVelocity * timeElapsed) + 0.5f * localAcceleration * timeElapsed * timeElapsed; - 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(); + #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 - // 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; + 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; } } else { - isMoving = true; + setLocalPosition(localPosition); + setLocalVelocity(localVelocity); } - } - transform.setTranslation(position); - setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity); - if (setFlags && !isMoving) { - // flag this entity to transition from KINEMATIC to STATIC - _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; + #ifdef WANT_DEBUG + qCDebug(entities) << " new position:" << position; + qCDebug(entities) << " new velocity:" << velocity; + qCDebug(entities) << " new AACube:" << getMaximumAACube(); + qCDebug(entities) << " old getAABox:" << getAABox(); + #endif } } diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index 5f168d2e33..e3952ba1d6 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/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) { - // only increment this if we haven't reached the threshold yet, to avoid overflowing the counter - ++_accelerationNearlyGravityCount; + 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(); } } else { - // acceleration wasn't similar to this entity's gravity, reset the counter - _accelerationNearlyGravityCount = 0; + // acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter + 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)); diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index eb42d03b52..ac16ec6d5d 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/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(); } diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index e902758461..482c3146f8 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/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); diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h index 44cb88721c..bb78eb12d6 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/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_PHYSICS_ACTIVATION); - + Simulation::DIRTY_SIMULATOR_ID | Simulation::DIRTY_SIMULATION_OWNERSHIP_PRIORITY); // 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; } diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index be80149c96..0040c19c3d 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/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); - object->clearIncomingDirtyFlags(); + if (object->handleEasyChanges(flags)) { + 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(); diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index 18e4016fec..f644d6f5b2 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 std::vector CollisionEvents; +typedef QVector CollisionEvents; class PhysicsEngine { public: @@ -110,7 +110,6 @@ private: ContactMap _contactMap; CollisionEvents _collisionEvents; QHash _objectActions; - std::vector _activeStaticBodies; glm::vec3 _originOffset; QUuid _sessionID; @@ -122,6 +121,7 @@ private: bool _dumpNextStats = false; bool _hasOutgoingChanges = false; + }; typedef std::shared_ptr PhysicsEnginePointer; diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp index 5fe99f137c..b6f3487f1a 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp +++ b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp @@ -67,10 +67,7 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep saveKinematicState(fixedTimeStep*clampedSimulationSteps); - { - BT_PROFILE("applyGravity"); - 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 68062d8d29..e9708149da 100644 --- a/libraries/physics/src/ThreadSafeDynamicsWorld.h +++ b/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 diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index 53abb3827d..d21d88d212 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/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); diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 8b1446d4e5..469ca1fc81 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); -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); } +bool isNaN(glm::vec3 value); +bool isNaN(glm::quat value); glm::mat4 orthoInverse(const glm::mat4& m); diff --git a/libraries/shared/src/PhysicsCollisionGroups.h b/libraries/shared/src/PhysicsCollisionGroups.h index 794f338dc5..6d320e69cb 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_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); diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp index d48bddbd88..30d4726bcc 100644 --- a/libraries/shared/src/SharedUtil.cpp +++ b/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; diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h index e9201b4a92..8fb65a5247 100644 --- a/libraries/shared/src/SharedUtil.h +++ b/libraries/shared/src/SharedUtil.h @@ -180,11 +180,11 @@ private: static int DEADBEEF_SIZE; }; -/// \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)); } +bool isBetween(int64_t value, int64_t max, int64_t 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); diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 3cbd3c5ac9..13bf5d9054 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/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(); -} diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h index ef70d0231b..379f2facd7 100644 --- a/libraries/shared/src/SpatiallyNestable.h +++ b/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; diff --git a/libraries/shared/src/Transform.cpp b/libraries/shared/src/Transform.cpp index c51b3dae4b..a3a3c05731 100644 --- a/libraries/shared/src/Transform.cpp +++ b/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); +} diff --git a/libraries/shared/src/Transform.h b/libraries/shared/src/Transform.h index 1e1d10c54b..1024173cbd 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 { return isNaN(_rotation) || isNaN(glm::dot(_scale, _translation)); } + bool containsNaN() const; protected: