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);