From 3d3bfcf7a315a682972d741125007260c461af60 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 16 Jan 2018 12:35:03 -0800 Subject: [PATCH] check dirty flags when harvesting physics results --- libraries/entities/src/EntityItem.cpp | 47 +++++++++++++++++++++ libraries/entities/src/EntityItem.h | 8 ++++ libraries/physics/src/EntityMotionState.cpp | 33 ++++----------- libraries/shared/src/GLMHelpers.h | 1 + libraries/shared/src/SpatiallyNestable.cpp | 30 +++++++++++++ libraries/shared/src/SpatiallyNestable.h | 1 + 6 files changed, 95 insertions(+), 25 deletions(-) diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp index fe5213baa8..8ea1de7666 100644 --- a/libraries/entities/src/EntityItem.cpp +++ b/libraries/entities/src/EntityItem.cpp @@ -1610,6 +1610,37 @@ void EntityItem::setPosition(const glm::vec3& value) { } } +void EntityItem::setWorldTransformAndVelocitiesUnlessDirtyFlags( + const glm::vec3& position, + const glm::quat& orientation, + const glm::vec3& linearVelocity, + const glm::vec3& angularVelocity) { + // only ever call this for harvesting results of physics simulation + // if a dirty bit is set then an update arrived (via script or network) overriding the physics simulation + uint32_t flags = _dirtyFlags & (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES); + if (!flags) { + // flags are clear + setWorldTransform(position, orientation); + setWorldVelocity(linearVelocity); + setWorldAngularVelocity(angularVelocity); + setLastSimulated(usecTimestampNow()); + } else { + // only set properties NOT flagged + if (!(flags & Simulation::DIRTY_TRANSFORM)) { + setWorldTransform(position, orientation); + } + if (!(flags & Simulation::DIRTY_LINEAR_VELOCITY)) { + setWorldVelocity(linearVelocity); + } + if (!(flags & Simulation::DIRTY_ANGULAR_VELOCITY)) { + setWorldAngularVelocity(angularVelocity); + } + if (flags != (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES)) { + setLastSimulated(usecTimestampNow()); + } + } +} + void EntityItem::setParentID(const QUuid& value) { QUuid oldParentID = getParentID(); if (oldParentID != value) { @@ -1739,6 +1770,22 @@ void EntityItem::setVelocity(const glm::vec3& value) { } } +void EntityItem::zeroAllVelocitiesUnlessDirtyFlags() { + uint32_t flags = _dirtyFlags & (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES); + if (!flags) { + setWorldVelocity(glm::vec3(0.0f)); + setWorldAngularVelocity(glm::vec3(0.0f)); + } else { + if (!(flags & Simulation::DIRTY_LINEAR_VELOCITY)) { + setWorldVelocity(glm::vec3(0.0f)); + } + if (!(flags & Simulation::DIRTY_ANGULAR_VELOCITY)) { + setWorldAngularVelocity(glm::vec3(0.0f)); + } + } + _acceleration = glm::vec3(0.0f); +} + void EntityItem::setDamping(float value) { auto clampedDamping = glm::clamp(value, 0.0f, 1.0f); withWriteLock([&] { diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index ecfb7b5dcd..db2e7f7641 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -355,6 +355,7 @@ public: void setRotation(glm::quat orientation); void setVelocity(const glm::vec3& velocity); + void zeroAllVelocitiesUnlessDirtyFlags(); uint32_t getDirtyFlags() const; void markDirtyFlags(uint32_t mask); @@ -368,6 +369,13 @@ public: void* getPhysicsInfo() const { return _physicsInfo; } void setPhysicsInfo(void* data) { _physicsInfo = data; } + + void setWorldTransformAndVelocitiesUnlessDirtyFlags( + const glm::vec3& position, + const glm::quat& orientation, + const glm::vec3& linearVelocity, + const glm::vec3& angularVelocity); + EntityTreeElementPointer getElement() const { return _element; } EntityTreePointer getTree() const; virtual SpatialParentTree* getParentTree() const override; diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index 33ac887f4f..fe3e242a70 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -256,25 +256,12 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { assert(_entity); assert(entityTreeIsLocked()); measureBodyAcceleration(); - bool positionSuccess; - _entity->setWorldPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset(), positionSuccess, false); - if (!positionSuccess) { - static QString repeatedMessage = - LogHandler::getInstance().addRepeatedMessageRegex("EntityMotionState::setWorldTransform " - "setPosition failed.*"); - qCDebug(physics) << "EntityMotionState::setWorldTransform setPosition failed" << _entity->getID(); - } - bool orientationSuccess; - _entity->setWorldOrientation(bulletToGLM(worldTrans.getRotation()), orientationSuccess, false); - if (!orientationSuccess) { - static QString repeatedMessage = - LogHandler::getInstance().addRepeatedMessageRegex("EntityMotionState::setWorldTransform " - "setOrientation failed.*"); - qCDebug(physics) << "EntityMotionState::setWorldTransform setOrientation failed" << _entity->getID(); - } - _entity->setVelocity(getBodyLinearVelocity()); - _entity->setAngularVelocity(getBodyAngularVelocity()); - _entity->setLastSimulated(usecTimestampNow()); + + _entity->setWorldTransformAndVelocitiesUnlessDirtyFlags( + bulletToGLM(worldTrans.getOrigin()), + bulletToGLM(worldTrans.getRotation()), + getBodyLinearVelocity(), + getBodyAngularVelocity()); if (_entity->getSimulatorID().isNull()) { _loopsWithoutOwner++; @@ -530,9 +517,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ if (!_body->isActive()) { // make sure all derivatives are zero - _entity->setVelocity(Vectors::ZERO); - _entity->setAngularVelocity(Vectors::ZERO); - _entity->setAcceleration(Vectors::ZERO); + _entity->zeroAllVelocitiesUnlessDirtyFlags(); _numInactiveUpdates++; } else { glm::vec3 gravity = _entity->getGravity(); @@ -559,9 +544,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ 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); + _entity->zeroAllVelocitiesUnlessDirtyFlags(); } } _numInactiveUpdates = 0; diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 973998b927..4f761a4aac 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/libraries/shared/src/GLMHelpers.h @@ -253,6 +253,7 @@ 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); } +inline bool isNaN(const glm::mat3& value) { return isNaN(value * glm::vec3(1.0f)); } glm::mat4 orthoInverse(const glm::mat4& m); diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 20a5a76b73..324cee3417 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -464,6 +464,36 @@ glm::vec3 SpatiallyNestable::localToWorldDimensions(const glm::vec3& dimensions, return dimensions; } +void SpatiallyNestable::setWorldTransform(const glm::vec3& position, const glm::quat& orientation) { + // guard against introducing NaN into the transform + if (isNaN(orientation) || isNaN(position)) { + return; + } + + bool changed = false; + bool success = true; + Transform parentTransform = getParentTransform(success); + _transformLock.withWriteLock([&] { + Transform myWorldTransform; + Transform::mult(myWorldTransform, parentTransform, _transform); + if (myWorldTransform.getRotation() != orientation) { + changed = true; + myWorldTransform.setRotation(orientation); + } + if (myWorldTransform.getTranslation() != position) { + changed = true; + myWorldTransform.setTranslation(position); + } + if (changed) { + Transform::inverseMult(_transform, parentTransform, myWorldTransform); + _translationChanged = usecTimestampNow(); + } + }); + if (success && changed) { + locationChanged(false); + } +} + glm::vec3 SpatiallyNestable::getWorldPosition(bool& success) const { return getTransform(success).getTranslation(); } diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h index 2a315e9230..090ca4c266 100644 --- a/libraries/shared/src/SpatiallyNestable.h +++ b/libraries/shared/src/SpatiallyNestable.h @@ -87,6 +87,7 @@ public: virtual Transform getParentTransform(bool& success, int depth = 0) const; + void setWorldTransform(const glm::vec3& position, const glm::quat& orientation); virtual glm::vec3 getWorldPosition(bool& success) const; virtual glm::vec3 getWorldPosition() const; virtual void setWorldPosition(const glm::vec3& position, bool& success, bool tellPhysics = true);