From 46bca30698138ff45b865199defbe01d161f2b1a Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 9 Jul 2015 11:29:53 -0700 Subject: [PATCH 1/4] MotionState reports zero velocity for slow objects This reduces the number of updates for an Entity that is effectively at rest due to a balance between some spring action and gravity, but nevertheless has a slight non-zero velocity at the end of each simulation step. --- libraries/physics/src/EntityMotionState.cpp | 6 +++--- libraries/physics/src/ObjectMotionState.cpp | 12 +++++++++++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index a975d21c4d..8d77f815b7 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -248,7 +248,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { btTransform xform = _body->getWorldTransform(); _serverPosition = bulletToGLM(xform.getOrigin()); _serverRotation = bulletToGLM(xform.getRotation()); - _serverVelocity = bulletToGLM(_body->getLinearVelocity()); + _serverVelocity = getBodyLinearVelocity(); _serverAngularVelocity = bulletToGLM(_body->getAngularVelocity()); _lastStep = simulationStep; _serverActionData = _entity->getActionData(); @@ -536,7 +536,7 @@ void EntityMotionState::bump(quint8 priority) { void EntityMotionState::resetMeasuredBodyAcceleration() { _lastMeasureStep = ObjectMotionState::getWorldSimulationStep(); if (_body) { - _lastVelocity = bulletToGLM(_body->getLinearVelocity()); + _lastVelocity = getBodyLinearVelocity(); } else { _lastVelocity = glm::vec3(0.0f); } @@ -555,7 +555,7 @@ void EntityMotionState::measureBodyAcceleration() { // Note: the integration equation for velocity uses damping: v1 = (v0 + a * dt) * (1 - D)^dt // hence the equation for acceleration is: a = (v1 / (1 - D)^dt - v0) / dt - glm::vec3 velocity = bulletToGLM(_body->getLinearVelocity()); + glm::vec3 velocity = getBodyLinearVelocity(); _measuredAcceleration = (velocity / powf(1.0f - _body->getLinearDamping(), dt) - _lastVelocity) * invDt; _lastVelocity = velocity; if (numSubsteps > PHYSICS_ENGINE_MAX_NUM_SUBSTEPS) { diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index 7ff119fb79..8e4bbdae1c 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -80,7 +80,17 @@ void ObjectMotionState::setBodyGravity(const glm::vec3& gravity) const { } glm::vec3 ObjectMotionState::getBodyLinearVelocity() const { - return bulletToGLM(_body->getLinearVelocity()); + // returns the body's velocity unless it is moving too slow in which case returns zero + btVector3 velocity = _body->getLinearVelocity(); + + // NOTE: the threshold to use here relates to the linear displacement threshold (dX) for sending updates + // to objects that are tracked server-side (e.g. entities which use dX = 2mm). Hence an object moving + // just under this velocity threshold would trigger an update about V/dX times per second. + const float MIN_LINEAR_SPEED_SQUARED = 0.0036f; // 6 mm/sec + if (velocity.length2() < MIN_LINEAR_SPEED_SQUARED) { + velocity *= 0.0f; + } + return bulletToGLM(velocity); } glm::vec3 ObjectMotionState::getObjectLinearVelocityChange() const { return glm::vec3(0.0f); // Subclasses override where meaningful. From 612e9649d8ab85c64cbab526f6f0d5849b323df5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 9 Jul 2015 11:32:30 -0700 Subject: [PATCH 2/4] fix update logic of ObjectActionOffset --- libraries/physics/src/ObjectActionOffset.cpp | 104 +++++++++++-------- libraries/physics/src/ObjectActionOffset.h | 1 + 2 files changed, 61 insertions(+), 44 deletions(-) diff --git a/libraries/physics/src/ObjectActionOffset.cpp b/libraries/physics/src/ObjectActionOffset.cpp index 7cf35b27d7..ff789dfefe 100644 --- a/libraries/physics/src/ObjectActionOffset.cpp +++ b/libraries/physics/src/ObjectActionOffset.cpp @@ -16,7 +16,11 @@ const uint16_t ObjectActionOffset::offsetVersion = 1; ObjectActionOffset::ObjectActionOffset(const QUuid& id, EntityItemPointer ownerEntity) : - ObjectAction(ACTION_TYPE_OFFSET, id, ownerEntity) { + ObjectAction(ACTION_TYPE_OFFSET, id, ownerEntity), + _pointToOffsetFrom(0.f), + _linearDistance(0.0f), + _linearTimeScale(FLT_MAX), + _positionalTargetSet(false) { #if WANT_DEBUG qDebug() << "ObjectActionOffset::ObjectActionOffset"; #endif @@ -44,6 +48,7 @@ void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) { unlock(); return; } + ObjectMotionState* motionState = static_cast(physicsInfo); btRigidBody* rigidBody = motionState->getRigidBody(); if (!rigidBody) { @@ -52,21 +57,32 @@ void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) { return; } - if (_positionalTargetSet) { - glm::vec3 offset = _pointToOffsetFrom - bulletToGLM(rigidBody->getCenterOfMassPosition()); - float offsetLength = glm::length(offset); - float offsetError = _linearDistance - offsetLength; - - // if (glm::abs(offsetError) > IGNORE_POSITION_DELTA) { - if (glm::abs(offsetError) > 0.0f) { - float offsetErrorAbs = glm::abs(offsetError); - float offsetErrorDirection = - offsetError / offsetErrorAbs; - glm::vec3 previousVelocity = bulletToGLM(rigidBody->getLinearVelocity()); - - glm::vec3 velocityAdjustment = glm::normalize(offset) * offsetErrorDirection * offsetErrorAbs / _linearTimeScale; - rigidBody->setLinearVelocity(glmToBullet(previousVelocity + velocityAdjustment)); - // rigidBody->setLinearVelocity(glmToBullet(velocityAdjustment)); + const float MAX_LINEAR_TIMESCALE = 600.0f; // 10 minutes is a long time + if (_positionalTargetSet && _linearTimeScale < MAX_LINEAR_TIMESCALE) { + if (_needsActivation) { rigidBody->activate(); + _needsActivation = false; + } + glm::vec3 objectPosition = bulletToGLM(rigidBody->getCenterOfMassPosition()); + glm::vec3 springAxis = objectPosition - _pointToOffsetFrom; // from anchor to object + float distance = glm::length(springAxis); + if (distance > FLT_EPSILON) { + springAxis /= distance; // normalize springAxis + + // compute (critically damped) target velocity of spring relaxation + glm::vec3 offset = (distance - _linearDistance) * springAxis; + glm::vec3 targetVelocity = (-1.0f / _linearTimeScale) * offset; + + // compute current velocity and its parallel component + glm::vec3 currentVelocity = bulletToGLM(rigidBody->getLinearVelocity()); + glm::vec3 parallelVelocity = glm::dot(currentVelocity, springAxis) * springAxis; + + // we blend the parallel component with the spring's target velocity to get the new velocity + float blend = deltaTimeStep / _linearTimeScale; + if (blend > 1.0f) { + blend = 1.0f; + } + rigidBody->setLinearVelocity(glmToBullet(currentVelocity + blend * (targetVelocity - parallelVelocity)); } } @@ -75,45 +91,45 @@ void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) { bool ObjectActionOffset::updateArguments(QVariantMap arguments) { - bool pOk0 = true; + bool ok = true; glm::vec3 pointToOffsetFrom = - EntityActionInterface::extractVec3Argument("offset action", arguments, "pointToOffsetFrom", pOk0, true); + EntityActionInterface::extractVec3Argument("offset action", arguments, "pointToOffsetFrom", ok, true); + if (!ok) { + return false; + } - bool pOk1 = true; + ok = true; float linearTimeScale = - EntityActionInterface::extractFloatArgument("offset action", arguments, "linearTimeScale", pOk1, false); + EntityActionInterface::extractFloatArgument("offset action", arguments, "linearTimeScale", ok, false); + if (ok) { + if (linearTimeScale <= 0.0f) { + qDebug() << "offset action -- linearTimeScale must be greater than zero."; + return false; + } + } else { + linearTimeScale = 0.1f; + } - bool pOk2 = true; + ok = true; float linearDistance = - EntityActionInterface::extractFloatArgument("offset action", arguments, "linearDistance", pOk2, false); - - if (!pOk0) { - return false; - } - if (pOk1 && linearTimeScale <= 0.0f) { - qDebug() << "offset action -- linearTimeScale must be greater than zero."; - return false; + EntityActionInterface::extractFloatArgument("offset action", arguments, "linearDistance", ok, false); + if (!ok) { + linearDistance = 0.0f; } - lockForWrite(); - - _pointToOffsetFrom = pointToOffsetFrom; - _positionalTargetSet = true; - - if (pOk1) { + // only change stuff if something actually changed + if (_pointToOffsetFrom != pointToOffsetFrom + || _linearTimeScale != linearTimeScale + || _linearDistance != linearDistance) { + lockForWrite(); + _pointToOffsetFrom = pointToOffsetFrom; _linearTimeScale = linearTimeScale; - } else { - _linearTimeScale = 0.1f; - } - - if (pOk2) { _linearDistance = linearDistance; - } else { - _linearDistance = 1.0f; + _positionalTargetSet = true; + _active = true; + _needsActivation = true; + unlock(); } - - _active = true; - unlock(); return true; } diff --git a/libraries/physics/src/ObjectActionOffset.h b/libraries/physics/src/ObjectActionOffset.h index 1918da6996..a0190f3832 100644 --- a/libraries/physics/src/ObjectActionOffset.h +++ b/libraries/physics/src/ObjectActionOffset.h @@ -36,6 +36,7 @@ public: float _linearDistance; float _linearTimeScale; bool _positionalTargetSet; + bool _needsActivation = true; }; #endif // hifi_ObjectActionOffset_h From ccf14b2e50123907743fce7487233e477d95276d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 9 Jul 2015 11:43:57 -0700 Subject: [PATCH 3/4] fix syntax error typo --- libraries/physics/src/ObjectActionOffset.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/physics/src/ObjectActionOffset.cpp b/libraries/physics/src/ObjectActionOffset.cpp index ff789dfefe..6189128997 100644 --- a/libraries/physics/src/ObjectActionOffset.cpp +++ b/libraries/physics/src/ObjectActionOffset.cpp @@ -82,7 +82,7 @@ void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) { if (blend > 1.0f) { blend = 1.0f; } - rigidBody->setLinearVelocity(glmToBullet(currentVelocity + blend * (targetVelocity - parallelVelocity)); + rigidBody->setLinearVelocity(glmToBullet(currentVelocity + blend * (targetVelocity - parallelVelocity))); } } From 55207a8fb1ce7da208cf0bd1d22b578dc1729661 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 9 Jul 2015 11:59:28 -0700 Subject: [PATCH 4/4] enforce coding standards --- libraries/physics/src/ObjectActionOffset.cpp | 2 +- libraries/physics/src/ObjectMotionState.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/physics/src/ObjectActionOffset.cpp b/libraries/physics/src/ObjectActionOffset.cpp index 6189128997..2391ded13d 100644 --- a/libraries/physics/src/ObjectActionOffset.cpp +++ b/libraries/physics/src/ObjectActionOffset.cpp @@ -17,7 +17,7 @@ const uint16_t ObjectActionOffset::offsetVersion = 1; ObjectActionOffset::ObjectActionOffset(const QUuid& id, EntityItemPointer ownerEntity) : ObjectAction(ACTION_TYPE_OFFSET, id, ownerEntity), - _pointToOffsetFrom(0.f), + _pointToOffsetFrom(0.0f), _linearDistance(0.0f), _linearTimeScale(FLT_MAX), _positionalTargetSet(false) { diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp index 8e4bbdae1c..aeec85f7ea 100644 --- a/libraries/physics/src/ObjectMotionState.cpp +++ b/libraries/physics/src/ObjectMotionState.cpp @@ -92,6 +92,7 @@ glm::vec3 ObjectMotionState::getBodyLinearVelocity() const { } return bulletToGLM(velocity); } + glm::vec3 ObjectMotionState::getObjectLinearVelocityChange() const { return glm::vec3(0.0f); // Subclasses override where meaningful. }