From d7b13d226f5b469098c7556ebef75199165d0d7d Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 4 Apr 2016 12:36:18 -0700 Subject: [PATCH 1/2] Application: fix for jittery hands when moving. The issue was that the hands are sampled in sensor coordinates. But when the avatar is moved via physics, the sensor to world matrix must be updated to move the hands in the world by the correct amount. These new hand positions are then computed before IK is performed. --- interface/src/Application.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 0172b3ce3a..9e97fba466 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -1470,10 +1470,6 @@ void Application::paintGL() { // update the avatar with a fresh HMD pose getMyAvatar()->updateFromHMDSensorMatrix(getHMDSensorPose()); - // update sensorToWorldMatrix for camera and hand controllers - getMyAvatar()->updateSensorToWorldMatrix(); - - auto lodManager = DependencyManager::get(); @@ -3406,6 +3402,9 @@ void Application::update(float deltaTime) { avatarManager->updateOtherAvatars(deltaTime); } + // update sensorToWorldMatrix for camera and hand controllers + getMyAvatar()->updateSensorToWorldMatrix(); + qApp->updateMyAvatarLookAtPosition(); { From 69bb0ebd741f56f388e560728359d759b8d9d5ff Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Mon, 4 Apr 2016 16:11:54 -0700 Subject: [PATCH 2/2] 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: