diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index 5fffc9901b..8f22c576f0 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -271,44 +271,25 @@ bool EntityMotionState::isCandidateForOwnership() const { bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { // NOTE: we only get here if we think we own the simulation assert(_body); + + bool parentTransformSuccess; + Transform localToWorld = _entity->getParentTransform(parentTransformSuccess); + Transform worldToLocal; + Transform worldVelocityToLocal; + if (parentTransformSuccess) { + localToWorld.evalInverse(worldToLocal); + worldVelocityToLocal = worldToLocal; + worldVelocityToLocal.setTranslation(glm::vec3(0.0f)); + } + // if we've never checked before, our _lastStep will be 0, and we need to initialize our state if (_lastStep == 0) { btTransform xform = _body->getWorldTransform(); - - // _serverPosition = _entity->worldPositionToParent(bulletToGLM(xform.getOrigin())); - // _serverRotation = _entity->worldRotationToParent(bulletToGLM(xform.getRotation())); - // _serverVelocity = _entity->worldVelocityToParent(getBodyLinearVelocityGTSigma()); - - - _serverPosition = bulletToGLM(xform.getOrigin()); - _serverRotation = bulletToGLM(xform.getRotation()); - _serverVelocity = getBodyLinearVelocityGTSigma(); - bool success; - Transform parentTransform = _entity->getParentTransform(success); - if (success) { - Transform bodyTransform; - bodyTransform.setTranslation(_serverPosition); - bodyTransform.setRotation(_serverRotation); - Transform result; - Transform::inverseMult(result, parentTransform, bodyTransform); - _serverPosition = result.getTranslation(); - _serverRotation = result.getRotation(); - - // transform velocity into parent-frame - parentTransform.setTranslation(glm::vec3(0.0f)); - Transform velocityTransform; - velocityTransform.setTranslation(_serverVelocity); - Transform myWorldTransform; - Transform::mult(myWorldTransform, parentTransform, velocityTransform); - myWorldTransform.setTranslation(_serverVelocity); - Transform::inverseMult(result, parentTransform, myWorldTransform); - _serverVelocity = result.getTranslation(); - } - - - + _serverPosition = worldToLocal.transform(bulletToGLM(xform.getOrigin())); + _serverRotation = worldToLocal.getRotation() * bulletToGLM(xform.getRotation()); + _serverVelocity = worldVelocityToLocal.transform(getBodyLinearVelocityGTSigma()); _serverAcceleration = Vectors::ZERO; - _serverAngularVelocity = _entity->worldVelocityToParent(bulletToGLM(_body->getAngularVelocity())); + _serverAngularVelocity = worldVelocityToLocal.transform(bulletToGLM(_body->getAngularVelocity())); _lastStep = simulationStep; _serverActionData = _entity->getActionData(); _numInactiveUpdates = 1; @@ -381,7 +362,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { // compute position error btTransform worldTrans = _body->getWorldTransform(); - glm::vec3 position = _entity->worldPositionToParent(bulletToGLM(worldTrans.getOrigin())); + glm::vec3 position = worldToLocal.transform(bulletToGLM(worldTrans.getOrigin())); float dx2 = glm::distance2(position, _serverPosition); const float MAX_POSITION_ERROR_SQUARED = 0.000004f; // corresponds to 2mm @@ -416,7 +397,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { } } const float MIN_ROTATION_DOT = 0.99999f; // This corresponds to about 0.5 degrees of rotation - glm::quat actualRotation = _entity->worldRotationToParent(bulletToGLM(worldTrans.getRotation())); + glm::quat actualRotation = worldToLocal.getRotation() * bulletToGLM(worldTrans.getRotation()); #ifdef WANT_DEBUG if ((fabsf(glm::dot(actualRotation, _serverRotation)) < MIN_ROTATION_DOT)) { diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 6edf80ab98..29a033f340 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -174,15 +174,6 @@ glm::vec3 SpatiallyNestable::worldToLocal(const glm::vec3& position, return result.getTranslation(); } -glm::vec3 SpatiallyNestable::worldPositionToParent(const glm::vec3& position) { - bool success; - glm::vec3 result = SpatiallyNestable::worldToLocal(position, getParentID(), getParentJointIndex(), success); - if (!success) { - qDebug() << "Warning -- worldToLocal failed" << getID(); - } - return result; -} - glm::vec3 SpatiallyNestable::worldVelocityToLocal(const glm::vec3& velocity, // can be linear or angular const QUuid& parentID, int parentJointIndex, bool& success) { @@ -225,15 +216,6 @@ glm::vec3 SpatiallyNestable::worldVelocityToLocal(const glm::vec3& velocity, // return result.getTranslation(); } -glm::vec3 SpatiallyNestable::worldVelocityToParent(const glm::vec3& velocity) { - bool success; - glm::vec3 result = SpatiallyNestable::worldVelocityToLocal(velocity, getParentID(), getParentJointIndex(), success); - if (!success) { - qDebug() << "Warning -- worldVelocityToLocal failed" << getID(); - } - return result; -} - glm::quat SpatiallyNestable::worldToLocal(const glm::quat& orientation, const QUuid& parentID, int parentJointIndex, bool& success) { @@ -274,15 +256,6 @@ glm::quat SpatiallyNestable::worldToLocal(const glm::quat& orientation, return result.getRotation(); } -glm::quat SpatiallyNestable::worldRotationToParent(const glm::quat& orientation) { - bool success; - glm::quat result = SpatiallyNestable::worldToLocal(orientation, getParentID(), getParentJointIndex(), success); - if (!success) { - qDebug() << "Warning -- worldToLocal failed" << getID(); - } - return result; -} - glm::vec3 SpatiallyNestable::localToWorld(const glm::vec3& position, const QUuid& parentID, int parentJointIndex, bool& success) { diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h index ffb00ac040..23beffda53 100644 --- a/libraries/shared/src/SpatiallyNestable.h +++ b/libraries/shared/src/SpatiallyNestable.h @@ -53,10 +53,6 @@ public: static glm::vec3 localToWorld(const glm::vec3& position, const QUuid& parentID, int parentJointIndex, bool& success); static glm::quat localToWorld(const glm::quat& orientation, const QUuid& parentID, int parentJointIndex, bool& success); - glm::vec3 worldPositionToParent(const glm::vec3& position); - glm::vec3 worldVelocityToParent(const glm::vec3& velocity); - glm::quat worldRotationToParent(const glm::quat& orientation); - // world frame virtual const Transform getTransform(bool& success, int depth = 0) const; virtual void setTransform(const Transform& transform, bool& success);