diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h index ecfb7b5dcd..f9559a375b 100644 --- a/libraries/entities/src/EntityItem.h +++ b/libraries/entities/src/EntityItem.h @@ -368,6 +368,7 @@ public: void* getPhysicsInfo() const { return _physicsInfo; } void setPhysicsInfo(void* data) { _physicsInfo = data; } + 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..420da5a1e0 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -256,25 +256,32 @@ 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(); + + // If transform or velocities are flagged as dirty it means a network or scripted change + // occured between the beginning and end of the stepSimulation() and we DON'T want to apply + // these physics simulation results. + uint32_t flags = _entity->getDirtyFlags() & (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES); + if (!flags) { + // flags are clear + _entity->setWorldTransform(bulletToGLM(worldTrans.getOrigin()), bulletToGLM(worldTrans.getRotation())); + _entity->setWorldVelocity(getBodyLinearVelocity()); + _entity->setWorldAngularVelocity(getBodyAngularVelocity()); + _entity->setLastSimulated(usecTimestampNow()); + } else { + // only set properties NOT flagged + if (!(flags & Simulation::DIRTY_TRANSFORM)) { + _entity->setWorldTransform(bulletToGLM(worldTrans.getOrigin()), bulletToGLM(worldTrans.getRotation())); + } + if (!(flags & Simulation::DIRTY_LINEAR_VELOCITY)) { + _entity->setWorldVelocity(getBodyLinearVelocity()); + } + if (!(flags & Simulation::DIRTY_ANGULAR_VELOCITY)) { + _entity->setWorldAngularVelocity(getBodyAngularVelocity()); + } + if (flags != (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES)) { + _entity->setLastSimulated(usecTimestampNow()); + } } - 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()); if (_entity->getSimulatorID().isNull()) { _loopsWithoutOwner++; @@ -530,9 +537,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); + zeroCleanObjectVelocities(); _numInactiveUpdates++; } else { glm::vec3 gravity = _entity->getGravity(); @@ -559,9 +564,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); + zeroCleanObjectVelocities(); } } _numInactiveUpdates = 0; @@ -818,3 +821,22 @@ bool EntityMotionState::shouldBeLocallyOwned() const { void EntityMotionState::upgradeOutgoingPriority(uint8_t priority) { _outgoingPriority = glm::max(_outgoingPriority, priority); } + +void EntityMotionState::zeroCleanObjectVelocities() const { + // If transform or velocities are flagged as dirty it means a network or scripted change + // occured between the beginning and end of the stepSimulation() and we DON'T want to apply + // these physics simulation results. + uint32_t flags = _entity->getDirtyFlags() & (Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES); + if (!flags) { + _entity->setWorldVelocity(glm::vec3(0.0f)); + _entity->setWorldAngularVelocity(glm::vec3(0.0f)); + } else { + if (!(flags & Simulation::DIRTY_LINEAR_VELOCITY)) { + _entity->setWorldVelocity(glm::vec3(0.0f)); + } + if (!(flags & Simulation::DIRTY_ANGULAR_VELOCITY)) { + _entity->setWorldAngularVelocity(glm::vec3(0.0f)); + } + } + _entity->setAcceleration(glm::vec3(0.0f)); +} diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h index ddfd7e1e4c..784273d600 100644 --- a/libraries/physics/src/EntityMotionState.h +++ b/libraries/physics/src/EntityMotionState.h @@ -87,6 +87,7 @@ public: protected: // changes _outgoingPriority only if priority is larger void upgradeOutgoingPriority(uint8_t priority); + void zeroCleanObjectVelocities() const; #ifdef WANT_DEBUG_ENTITY_TREE_LOCKS bool entityTreeIsLocked() const; 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);