From 872622c6f75dad8a5053e071ba74f6f950930fa9 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 20 Apr 2016 11:26:38 -0700 Subject: [PATCH] fix acceleration of for server-side kinematics --- libraries/entities/src/EntityItem.cpp | 5 +- libraries/physics/src/EntityMotionState.cpp | 91 ++++++++++++--------- libraries/physics/src/EntityMotionState.h | 6 +- 3 files changed, 54 insertions(+), 48 deletions(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index a012e2b514..a51e5164df 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -873,7 +873,6 @@ void EntityItem::simulate(const quint64& now) { #endif if (!hasActions()) { - setAcceleration(_gravity); if (!stepKinematicMotion(timeElapsed)) { // this entity is no longer moving // flag it to transition from KINEMATIC to STATIC @@ -937,7 +936,7 @@ bool EntityItem::stepKinematicMotion(float 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 gravity + // yes acceleration // acceleration is in world-frame but we need it in local-frame glm::vec3 linearAcceleration = _acceleration; bool success; @@ -959,7 +958,7 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) { linearVelocity += deltaVelocity; } } else { - // no gravity + // no acceleration if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) { linearVelocity = Vectors::ZERO; } else { diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index 079589f2ae..d5e86814f9 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), @@ -186,13 +186,18 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const { 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->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())); @@ -433,51 +438,42 @@ 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(); + glm::vec3 error = _measuredAcceleration - gravity; + float errorLength = glm::length(error); + int numSteps = (int)(step - _lastStep); // 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 (!_body->isStaticOrKinematicObject() && 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; } @@ -625,9 +621,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() { @@ -651,8 +647,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 }; };