diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index 65811c5c57..7260e5e782 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -731,7 +731,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef // we want to extrapolate the motion forward to compensate for packet travel time, but // we don't want the side effect of flag setting. - simulateKinematicMotion(skipTimeForward, false); + stepKinematicMotion(skipTimeForward); } if (overwriteLocalData) { @@ -872,130 +872,113 @@ void EntityItem::simulate(const quint64& now) { qCDebug(entities) << " ********** EntityItem::simulate() .... SETTING _lastSimulated=" << _lastSimulated; #endif - simulateKinematicMotion(timeElapsed); + if (!hasActions()) { + if (!stepKinematicMotion(timeElapsed)) { + // this entity is no longer moving + // flag it to transition from KINEMATIC to STATIC + _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; + setAcceleration(Vectors::ZERO); + } + } _lastSimulated = 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()) { - return; +bool EntityItem::stepKinematicMotion(float timeElapsed) { + if (timeElapsed <= 0.0f) { + return true; } - if (hasLocalAngularVelocity()) { - glm::vec3 localAngularVelocity = getLocalAngularVelocity(); + const float MAX_TIME_ELAPSED = 1.0f; // seconds + if (timeElapsed > MAX_TIME_ELAPSED) { + qCWarning(entities) << "kinematic timestep = " << timeElapsed << " truncated to " << MAX_TIME_ELAPSED; + } + timeElapsed = glm::min(timeElapsed, MAX_TIME_ELAPSED); + Transform transform; + glm::vec3 linearVelocity; + glm::vec3 angularVelocity; + getLocalTransformAndVelocities(transform, linearVelocity, angularVelocity); + + bool moving = 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 MIN_KINEMATIC_ANGULAR_SPEED_SQUARED = + KINEMATIC_ANGULAR_SPEED_THRESHOLD * KINEMATIC_ANGULAR_SPEED_THRESHOLD; + 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 - 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); - - bool success; - setOrientation(rotation, success, false); + transform.setRotation(rotation); } - - setLocalAngularVelocity(localAngularVelocity); + moving = true; } - 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(); - } + glm::vec3 position = transform.getTranslation(); + float linearSpeedSquared = glm::length2(linearVelocity); + const float MIN_KINEMATIC_LINEAR_SPEED_SQUARED = + KINEMATIC_LINEAR_SPEED_THRESHOLD * KINEMATIC_LINEAR_SPEED_THRESHOLD; + if (linearSpeedSquared > 0.0f) { + glm::vec3 deltaVelocity = Vectors::ZERO; // 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 + deltaVelocity = (powf(1.0f - _damping, timeElapsed) - 1.0f) * linearVelocity; } - // integrate position forward - glm::vec3 localPosition = getLocalPosition(); - glm::vec3 newLocalPosition = localPosition + (localVelocity * timeElapsed) + 0.5f * localAcceleration * timeElapsed * timeElapsed; + const float MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED = 1.0e-4f; // 0.01 m/sec^2 + if (glm::length2(_acceleration) > MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED) { + // yes acceleration + // acceleration is in world-frame but we need it in local-frame + glm::vec3 linearAcceleration = _acceleration; + bool success; + Transform parentTransform = getParentTransform(success); + if (success) { + linearAcceleration = glm::inverse(parentTransform.getRotation()) * linearAcceleration; + } + deltaVelocity += linearAcceleration * 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; + if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED + && glm::length2(deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED + && glm::length2(linearVelocity + deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) { + linearVelocity = Vectors::ZERO; + } else { + // NOTE: we do NOT include the second-order acceleration term (0.5 * a * dt^2) + // when computing the displacement because Bullet also ignores that term. Yes, + // this is an approximation and it works best when dt is small. + position += timeElapsed * linearVelocity; + linearVelocity += deltaVelocity; } } else { - setLocalPosition(localPosition); - setLocalVelocity(localVelocity); + // no acceleration + if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) { + linearVelocity = Vectors::ZERO; + } else { + // NOTE: we don't use second-order acceleration term for linear displacement + // because Bullet doesn't use it. + position += timeElapsed * linearVelocity; + linearVelocity += deltaVelocity; + } } - - #ifdef WANT_DEBUG - qCDebug(entities) << " new position:" << position; - qCDebug(entities) << " new velocity:" << velocity; - qCDebug(entities) << " new AACube:" << getMaximumAACube(); - qCDebug(entities) << " old getAABox:" << getAABox(); - #endif + moving = true; } + + if (moving) { + transform.setTranslation(position); + setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity); + } + return moving; } bool EntityItem::isMoving() const { diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index 08550c9ce5..c145334f80 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -152,7 +152,7 @@ public: // perform linear extrapolation for SimpleEntitySimulation void simulate(const quint64& now); - void simulateKinematicMotion(float timeElapsed, bool setFlags=true); + bool stepKinematicMotion(float timeElapsed); // return 'true' if moving virtual bool needsToCallUpdate() const { return false; } diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index cd6c54675d..fbec7cd3e0 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -56,8 +56,8 @@ EntityMotionState::EntityMotionState(btCollisionShape* shape, EntityItemPointer _serverGravity(0.0f), _serverAcceleration(0.0f), _serverActionData(QByteArray()), - _lastVelocity(glm::vec3(0.0f)), - _measuredAcceleration(glm::vec3(0.0f)), + _lastVelocity(0.0f), + _measuredAcceleration(0.0f), _nextOwnershipBid(0), _measuredDeltaTime(0.0f), _lastMeasureStep(0), @@ -178,19 +178,26 @@ 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. + // of the physics simulation and uses full gravity for acceleration. + _entity->setAcceleration(_entity->getGravity()); uint32_t thisStep = ObjectMotionState::getWorldSimulationStep(); float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP; - _entity->simulateKinematicMotion(dt); + _entity->stepKinematicMotion(dt); // bypass const-ness so we can remember the step const_cast(this)->_lastKinematicStep = thisStep; + + // and set the acceleration-matches-gravity count high so that if we send an update + // it will use the correct acceleration for remote simulations + _accelerationNearlyGravityCount = (uint8_t)(-1); } worldTrans.setOrigin(glmToBullet(getObjectPosition())); worldTrans.setRotation(glmToBullet(_entity->getRotation())); @@ -270,6 +277,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { _serverPosition = bulletToGLM(xform.getOrigin()); _serverRotation = bulletToGLM(xform.getRotation()); _serverVelocity = getBodyLinearVelocityGTSigma(); + _serverAcceleration = Vectors::ZERO; _serverAngularVelocity = bulletToGLM(_body->getAngularVelocity()); _lastStep = simulationStep; _serverActionData = _entity->getActionData(); @@ -309,6 +317,8 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { if (glm::length2(_serverVelocity) > 0.0f) { _serverVelocity += _serverAcceleration * dt; _serverVelocity *= powf(1.0f - _body->getLinearDamping(), dt); + // NOTE: we ignore the second-order acceleration term when integrating + // the position forward because Bullet also does this. _serverPosition += dt * _serverVelocity; } @@ -334,19 +344,23 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { glm::vec3 position = bulletToGLM(worldTrans.getOrigin()); float dx2 = glm::distance2(position, _serverPosition); - - const float MAX_POSITION_ERROR_SQUARED = 0.000004f; // Sqrt() - corresponds to 2 millimeters + const float MAX_POSITION_ERROR_SQUARED = 0.000004f; // corresponds to 2mm if (dx2 > MAX_POSITION_ERROR_SQUARED) { - - #ifdef WANT_DEBUG - qCDebug(physics) << ".... (dx2 > MAX_POSITION_ERROR_SQUARED) ...."; - qCDebug(physics) << "wasPosition:" << wasPosition; - qCDebug(physics) << "bullet position:" << position; - qCDebug(physics) << "_serverPosition:" << _serverPosition; - qCDebug(physics) << "dx2:" << dx2; - #endif - - return true; + // we don't mind larger position error when the object has high speed + // so we divide by speed and check again + float speed2 = glm::length2(_serverVelocity); + const float MIN_ERROR_RATIO_SQUARED = 0.0025f; // corresponds to 5% error in 1 second + const float MIN_SPEED_SQUARED = 1.0e-6f; // corresponds to 1mm/sec + if (speed2 < MIN_SPEED_SQUARED || dx2 / speed2 > MIN_ERROR_RATIO_SQUARED) { + #ifdef WANT_DEBUG + qCDebug(physics) << ".... (dx2 > MAX_POSITION_ERROR_SQUARED) ...."; + qCDebug(physics) << "wasPosition:" << wasPosition; + qCDebug(physics) << "bullet position:" << position; + qCDebug(physics) << "_serverPosition:" << _serverPosition; + qCDebug(physics) << "dx2:" << dx2; + #endif + return true; + } } if (glm::length2(_serverAngularVelocity) > 0.0f) { @@ -400,7 +414,11 @@ bool EntityMotionState::shouldSendUpdate(uint32_t simulationStep) { if (_entity->getSimulatorID() != Physics::getSessionUUID()) { // we don't own the simulation - bool shouldBid = _outgoingPriority > 0 && // but we would like to own it and + + // NOTE: we do not volunteer to own kinematic or static objects + uint8_t insufficientPriority = _body->isStaticOrKinematicObject() ? VOLUNTEER_SIMULATION_PRIORITY : 0; + + bool shouldBid = _outgoingPriority > insufficientPriority && // but we would like to own it AND usecTimestampNow() > _nextOwnershipBid; // it is time to bid again if (shouldBid && _outgoingPriority < _entity->getSimulationPriority()) { // we are insufficiently interested so clear our interest @@ -420,51 +438,39 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ if (!_body->isActive()) { // make sure all derivatives are zero - glm::vec3 zero(0.0f); - _entity->setVelocity(zero); - _entity->setAngularVelocity(zero); - _entity->setAcceleration(zero); + _entity->setVelocity(Vectors::ZERO); + _entity->setAngularVelocity(Vectors::ZERO); + _entity->setAcceleration(Vectors::ZERO); _numInactiveUpdates++; } else { - const uint8_t STEPS_TO_DECIDE_BALLISTIC = 4; - float gravityLength = glm::length(_entity->getGravity()); - float accVsGravity = glm::abs(glm::length(_measuredAcceleration) - gravityLength); - 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(); - } - } else { - // acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter - resetAccelerationNearlyGravityCount(); - } + glm::vec3 gravity = _entity->getGravity(); // 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) { - _entity->setAcceleration(_entity->getGravity()); + const uint8_t STEPS_TO_DECIDE_BALLISTIC = 4; + if (_accelerationNearlyGravityCount >= STEPS_TO_DECIDE_BALLISTIC) { + _entity->setAcceleration(gravity); } else { - _entity->setAcceleration(glm::vec3(0.0f)); + _entity->setAcceleration(Vectors::ZERO); } - const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f; // 5 cm/sec - const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f; // ~5 deg/sec + if (!_body->isStaticOrKinematicObject()) { + const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f; // 5 cm/sec + const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f; // ~5 deg/sec - bool movingSlowlyLinear = - glm::length2(_entity->getVelocity()) < (DYNAMIC_LINEAR_VELOCITY_THRESHOLD * DYNAMIC_LINEAR_VELOCITY_THRESHOLD); - bool movingSlowlyAngular = glm::length2(_entity->getAngularVelocity()) < - (DYNAMIC_ANGULAR_VELOCITY_THRESHOLD * DYNAMIC_ANGULAR_VELOCITY_THRESHOLD); - bool movingSlowly = movingSlowlyLinear && movingSlowlyAngular && _entity->getAcceleration() == glm::vec3(0.0f); + bool movingSlowlyLinear = + glm::length2(_entity->getVelocity()) < (DYNAMIC_LINEAR_VELOCITY_THRESHOLD * DYNAMIC_LINEAR_VELOCITY_THRESHOLD); + bool movingSlowlyAngular = glm::length2(_entity->getAngularVelocity()) < + (DYNAMIC_ANGULAR_VELOCITY_THRESHOLD * DYNAMIC_ANGULAR_VELOCITY_THRESHOLD); + bool movingSlowly = movingSlowlyLinear && movingSlowlyAngular && _entity->getAcceleration() == Vectors::ZERO; - if (movingSlowly) { - // velocities might not be zero, but we'll fake them as such, which will hopefully help convince - // other simulating observers to deactivate their own copies - glm::vec3 zero(0.0f); - _entity->setVelocity(zero); - _entity->setAngularVelocity(zero); + if (movingSlowly) { + // velocities might not be zero, but we'll fake them as such, which will hopefully help convince + // other simulating observers to deactivate their own copies + glm::vec3 zero(0.0f); + _entity->setVelocity(zero); + _entity->setAngularVelocity(zero); + } } _numInactiveUpdates = 0; } @@ -612,9 +618,9 @@ void EntityMotionState::resetMeasuredBodyAcceleration() { if (_body) { _lastVelocity = getBodyLinearVelocityGTSigma(); } else { - _lastVelocity = glm::vec3(0.0f); + _lastVelocity = Vectors::ZERO; } - _measuredAcceleration = glm::vec3(0.0f); + _measuredAcceleration = Vectors::ZERO; } void EntityMotionState::measureBodyAcceleration() { @@ -638,8 +644,23 @@ void EntityMotionState::measureBodyAcceleration() { _lastStep = ObjectMotionState::getWorldSimulationStep(); _numInactiveUpdates = 0; } + + glm::vec3 gravity = _entity->getGravity(); + float gravityLength = glm::length(gravity); + float accVsGravity = glm::abs(glm::length(_measuredAcceleration) - gravityLength); + 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 < (uint8_t)(-2)) { + ++_accelerationNearlyGravityCount; + } + } else { + // acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter + _accelerationNearlyGravityCount = 0; + } } } + glm::vec3 EntityMotionState::getObjectLinearVelocityChange() const { // This is the dampened change in linear velocity, as calculated in measureBodyAcceleration: dv = a * dt // It is generally only meaningful during the lifespan of collision. In particular, it is not meaningful diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index 938f3e84c1..8f1532bf8f 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/libraries/physics/src/EntityMotionState.h @@ -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(); } @@ -123,7 +119,7 @@ protected: uint32_t _lastStep; // last step of server extrapolation uint8_t _loopsWithoutOwner; - uint8_t _accelerationNearlyGravityCount; + mutable uint8_t _accelerationNearlyGravityCount; uint8_t _numInactiveUpdates { 1 }; uint8_t _outgoingPriority { 0 }; }; diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index 0468e02cb8..de435e80da 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -45,6 +45,7 @@ void ObjectMotionState::setWorldSimulationStep(uint32_t step) { worldSimulationStep = step; } +// static uint32_t ObjectMotionState::getWorldSimulationStep() { return worldSimulationStep; } @@ -298,6 +299,12 @@ void ObjectMotionState::updateBodyVelocities() { _body->setActivationState(ACTIVE_TAG); } +void ObjectMotionState::updateLastKinematicStep() { + // NOTE: we init to worldSimulationStep - 1 so that: when any object transitions to kinematic + // it will compute a non-zero dt on its first step. + _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 4293df6b3f..c0c10c6f71 100644 --- a/libraries/physics/src/ObjectMotionState.h +++ b/libraries/physics/src/ObjectMotionState.h @@ -86,6 +86,8 @@ public: 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 54419a9c1a..5523abf4e2 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -86,9 +86,8 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); body->updateInertiaTensor(); motionState->updateBodyVelocities(); - 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); + motionState->updateLastKinematicStep(); + body->setSleepingThresholds(KINEMATIC_LINEAR_SPEED_THRESHOLD, KINEMATIC_ANGULAR_SPEED_THRESHOLD); break; } case MOTION_TYPE_DYNAMIC: { @@ -109,9 +108,7 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { // NOTE: Bullet will deactivate any object whose velocity is below these thresholds for longer than 2 seconds. // (the 2 seconds is determined by: static btRigidBody::gDeactivationTime - const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f; // 5 cm/sec - const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f; // ~5 deg/sec - body->setSleepingThresholds(DYNAMIC_LINEAR_VELOCITY_THRESHOLD, DYNAMIC_ANGULAR_VELOCITY_THRESHOLD); + body->setSleepingThresholds(DYNAMIC_LINEAR_SPEED_THRESHOLD, DYNAMIC_ANGULAR_SPEED_THRESHOLD); if (!motionState->isMoving()) { // try to initialize this object as inactive body->forceActivationState(ISLAND_SLEEPING); @@ -252,6 +249,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 diff --git a/libraries/shared/src/PhysicsHelpers.h b/libraries/shared/src/PhysicsHelpers.h index 28ded661f0..67442187a3 100644 --- a/libraries/shared/src/PhysicsHelpers.h +++ b/libraries/shared/src/PhysicsHelpers.h @@ -22,6 +22,11 @@ const int PHYSICS_ENGINE_MAX_NUM_SUBSTEPS = 6; // Bullet will start to "lose time" at 10 FPS. const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 90.0f; +const float DYNAMIC_LINEAR_SPEED_THRESHOLD = 0.05f; // 5 cm/sec +const float DYNAMIC_ANGULAR_SPEED_THRESHOLD = 0.087266f; // ~5 deg/sec +const float KINEMATIC_LINEAR_SPEED_THRESHOLD = 0.001f; // 1 mm/sec +const float KINEMATIC_ANGULAR_SPEED_THRESHOLD = 0.008f; // ~0.5 deg/sec + // return incremental rotation (Bullet-style) caused by angularVelocity over timeStep glm::quat computeBulletRotationStep(const glm::vec3& angularVelocity, float timeStep); diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index c4cb4f94ba..2a3cb4af47 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; } @@ -426,7 +419,7 @@ void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) { // _velocity is a vs parent value and any request for a world-frame velocity must // be computed), do this to avoid equipped (parenting-grabbed) things from drifting. // turning a zero velocity into a non-zero _velocity (because the avatar is moving) - // causes EntityItem::simulateKinematicMotion to have an effect on the equipped entity, + // causes EntityItem::stepKinematicMotion to have an effect on the equipped entity, // which causes it to drift from the hand. if (hasAncestorOfType(NestableType::Avatar)) { _velocity = velocity; @@ -873,3 +866,40 @@ bool SpatiallyNestable::hasAncestorOfType(NestableType nestableType) { return parent->hasAncestorOfType(nestableType); } + +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(false); +} diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h index 6485c23b87..04bb57a688 100644 --- a/libraries/shared/src/SpatiallyNestable.h +++ b/libraries/shared/src/SpatiallyNestable.h @@ -151,6 +151,13 @@ 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;