From 1b4b38c85bb3d6aaa410ad29e834a7b40ad50310 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Wed, 8 Jun 2016 11:28:49 -0700 Subject: [PATCH 01/12] when parent is an avatar, set velocity to zero (still incorrect, but avoids server-side problems) rather than the world-frame velocity --- libraries/shared/src/SpatiallyNestable.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 2a3cb4af47..ddc82e2169 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -422,7 +422,7 @@ void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) { // causes EntityItem::stepKinematicMotion to have an effect on the equipped entity, // which causes it to drift from the hand. if (hasAncestorOfType(NestableType::Avatar)) { - _velocity = velocity; + _velocity = glm::vec3(0.0f); } else { // TODO: take parent angularVelocity into account. _velocity = glm::inverse(parentTransform.getRotation()) * (velocity - parentVelocity); From 5cbaf05e45cb708402adc1bf1a7c44c0f5cadc7e Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Wed, 8 Jun 2016 11:55:17 -0700 Subject: [PATCH 02/12] also zero angular velocity for child of avatar --- libraries/shared/src/SpatiallyNestable.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index ddc82e2169..02a8d60fba 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -479,7 +479,12 @@ void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity, boo glm::vec3 parentAngularVelocity = getParentAngularVelocity(success); Transform parentTransform = getParentTransform(success); _angularVelocityLock.withWriteLock([&] { - _angularVelocity = glm::inverse(parentTransform.getRotation()) * (angularVelocity - parentAngularVelocity); + if (hasAncestorOfType(NestableType::Avatar)) { + // TODO: this is done to keep entity-server from doing extrapolation, and isn't right. + _angularVelocity = glm::vec3(0.0f); + } else { + _angularVelocity = glm::inverse(parentTransform.getRotation()) * (angularVelocity - parentAngularVelocity); + } }); } From 201d7a7d3b616d26e0f5b4a6f23c2a3ad5ef520d Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Wed, 8 Jun 2016 12:11:08 -0700 Subject: [PATCH 03/12] when parent is an avatar, set velocity to zero (still incorrect, but avoids server-side problems) rather than the world-frame velocity --- libraries/shared/src/SpatiallyNestable.cpp | 32 ++++++++++++---------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 02a8d60fba..6872b1d197 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -415,12 +415,8 @@ void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) { glm::vec3 parentVelocity = getParentVelocity(success); Transform parentTransform = getParentTransform(success); _velocityLock.withWriteLock([&] { - // HACK: until we are treating _velocity the same way we treat _position (meaning, - // _velocity is a vs parent value and any request for a world-frame velocity must - // be computed), do this to avoid equipped (parenting-grabbed) things from drifting. - // turning a zero velocity into a non-zero _velocity (because the avatar is moving) - // causes EntityItem::stepKinematicMotion to have an effect on the equipped entity, - // which causes it to drift from the hand. + // HACK: The entity-server doesn't know where avatars are. In order to avoid having the server + // try to do simple extrapolation, set relative velocities to zero if this is a child of an avatar. if (hasAncestorOfType(NestableType::Avatar)) { _velocity = glm::vec3(0.0f); } else { @@ -898,13 +894,21 @@ void SpatiallyNestable::setLocalTransformAndVelocities( _transformLock.withWriteLock([&] { _transform = localTransform; }); - // linear velocity - _velocityLock.withWriteLock([&] { - _velocity = localVelocity; - }); - // angular velocity - _angularVelocityLock.withWriteLock([&] { - _angularVelocity = localAngularVelocity; - }); + // linear and angular velocity + if (hasAncestorOfType(NestableType::Avatar)) { + _velocityLock.withWriteLock([&] { + _velocity = glm::vec3(0.0f); + }); + _angularVelocityLock.withWriteLock([&] { + _angularVelocity = glm::vec3(0.0f); + }); + } else { + _velocityLock.withWriteLock([&] { + _velocity = localVelocity; + }); + _angularVelocityLock.withWriteLock([&] { + _angularVelocity = localAngularVelocity; + }); + } locationChanged(false); } From a1766539f402a249758f87640163e6ea2edded26 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Wed, 8 Jun 2016 12:17:07 -0700 Subject: [PATCH 04/12] don't do simple simulation on children of avatars --- libraries/entities/src/EntitySimulation.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/entities/src/EntitySimulation.cpp b/libraries/entities/src/EntitySimulation.cpp index 893ae83cc5..b99ab76b7a 100644 --- a/libraries/entities/src/EntitySimulation.cpp +++ b/libraries/entities/src/EntitySimulation.cpp @@ -261,7 +261,11 @@ void EntitySimulation::moveSimpleKinematics(const quint64& now) { SetOfEntities::iterator itemItr = _simpleKinematicEntities.begin(); while (itemItr != _simpleKinematicEntities.end()) { EntityItemPointer entity = *itemItr; - if (entity->isMovingRelativeToParent() && !entity->getPhysicsInfo()) { + if (entity->isMovingRelativeToParent() && + !entity->getPhysicsInfo() && + // The entity-server doesn't know where avatars are, so don't attempt to do simple extrapolation for + // children of avatars. + !entity->hasAncestorOfType(NestableType::Avatar)) { entity->simulate(now); _entitiesToSort.insert(entity); ++itemItr; From d7fc789ea705df7de5c601e48a2c041e9705445d Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Wed, 8 Jun 2016 13:47:52 -0700 Subject: [PATCH 05/12] try harder to not do simple-simulation on things that have an avatar ancestor --- libraries/entities/src/EntitySimulation.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/entities/src/EntitySimulation.cpp b/libraries/entities/src/EntitySimulation.cpp index b99ab76b7a..8419e4ac6a 100644 --- a/libraries/entities/src/EntitySimulation.cpp +++ b/libraries/entities/src/EntitySimulation.cpp @@ -261,11 +261,14 @@ void EntitySimulation::moveSimpleKinematics(const quint64& now) { SetOfEntities::iterator itemItr = _simpleKinematicEntities.begin(); while (itemItr != _simpleKinematicEntities.end()) { EntityItemPointer entity = *itemItr; - if (entity->isMovingRelativeToParent() && - !entity->getPhysicsInfo() && - // The entity-server doesn't know where avatars are, so don't attempt to do simple extrapolation for - // children of avatars. - !entity->hasAncestorOfType(NestableType::Avatar)) { + + // The entity-server doesn't know where avatars are, so don't attempt to do simple extrapolation for + // children of avatars. + bool ancestryIsKnown; + entity->getMaximumAACube(ancestryIsKnown); + bool hasAvatarAncestor = entity->hasAncestorOfType(NestableType::Avatar); + + if (entity->isMovingRelativeToParent() && !entity->getPhysicsInfo() && ancestryIsKnown && !hasAvatarAncestor) { entity->simulate(now); _entitiesToSort.insert(entity); ++itemItr; From a312218722c6be133d303421c917f82dfa92a77d Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Wed, 8 Jun 2016 14:07:16 -0700 Subject: [PATCH 06/12] back out some changes --- libraries/shared/src/SpatiallyNestable.cpp | 41 +++++++++------------- 1 file changed, 16 insertions(+), 25 deletions(-) diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 6872b1d197..2a3cb4af47 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -415,10 +415,14 @@ void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) { glm::vec3 parentVelocity = getParentVelocity(success); Transform parentTransform = getParentTransform(success); _velocityLock.withWriteLock([&] { - // HACK: The entity-server doesn't know where avatars are. In order to avoid having the server - // try to do simple extrapolation, set relative velocities to zero if this is a child of an avatar. + // HACK: until we are treating _velocity the same way we treat _position (meaning, + // _velocity is a vs parent value and any request for a world-frame velocity must + // be computed), do this to avoid equipped (parenting-grabbed) things from drifting. + // turning a zero velocity into a non-zero _velocity (because the avatar is moving) + // causes EntityItem::stepKinematicMotion to have an effect on the equipped entity, + // which causes it to drift from the hand. if (hasAncestorOfType(NestableType::Avatar)) { - _velocity = glm::vec3(0.0f); + _velocity = velocity; } else { // TODO: take parent angularVelocity into account. _velocity = glm::inverse(parentTransform.getRotation()) * (velocity - parentVelocity); @@ -475,12 +479,7 @@ void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity, boo glm::vec3 parentAngularVelocity = getParentAngularVelocity(success); Transform parentTransform = getParentTransform(success); _angularVelocityLock.withWriteLock([&] { - if (hasAncestorOfType(NestableType::Avatar)) { - // TODO: this is done to keep entity-server from doing extrapolation, and isn't right. - _angularVelocity = glm::vec3(0.0f); - } else { - _angularVelocity = glm::inverse(parentTransform.getRotation()) * (angularVelocity - parentAngularVelocity); - } + _angularVelocity = glm::inverse(parentTransform.getRotation()) * (angularVelocity - parentAngularVelocity); }); } @@ -894,21 +893,13 @@ void SpatiallyNestable::setLocalTransformAndVelocities( _transformLock.withWriteLock([&] { _transform = localTransform; }); - // linear and angular velocity - if (hasAncestorOfType(NestableType::Avatar)) { - _velocityLock.withWriteLock([&] { - _velocity = glm::vec3(0.0f); - }); - _angularVelocityLock.withWriteLock([&] { - _angularVelocity = glm::vec3(0.0f); - }); - } else { - _velocityLock.withWriteLock([&] { - _velocity = localVelocity; - }); - _angularVelocityLock.withWriteLock([&] { - _angularVelocity = localAngularVelocity; - }); - } + // linear velocity + _velocityLock.withWriteLock([&] { + _velocity = localVelocity; + }); + // angular velocity + _angularVelocityLock.withWriteLock([&] { + _angularVelocity = localAngularVelocity; + }); locationChanged(false); } From 5650ef9d52b46d486639731747eb2acb154dcfba Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Thu, 9 Jun 2016 14:35:17 -0700 Subject: [PATCH 07/12] have code where physics guesses at server values also avoid doing simple simulation of children of avatars --- libraries/entities/src/EntitySimulation.cpp | 2 +- libraries/physics/src/EntityMotionState.cpp | 16 +++++++++++++--- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/libraries/entities/src/EntitySimulation.cpp b/libraries/entities/src/EntitySimulation.cpp index 8419e4ac6a..a29ea8e2c8 100644 --- a/libraries/entities/src/EntitySimulation.cpp +++ b/libraries/entities/src/EntitySimulation.cpp @@ -263,7 +263,7 @@ void EntitySimulation::moveSimpleKinematics(const quint64& now) { EntityItemPointer entity = *itemItr; // The entity-server doesn't know where avatars are, so don't attempt to do simple extrapolation for - // children of avatars. + // children of avatars. See related code in EntityMotionState::remoteSimulationOutOfSync. bool ancestryIsKnown; entity->getMaximumAACube(ancestryIsKnown); bool hasAvatarAncestor = entity->hasAncestorOfType(NestableType::Avatar); diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index 053bfcbd85..be7862ade3 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -317,9 +317,19 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { if (glm::length2(_serverVelocity) > 0.0f) { _serverVelocity += _serverAcceleration * dt; _serverVelocity *= powf(1.0f - _body->getLinearDamping(), dt); - // NOTE: we ignore the second-order acceleration term when integrating - // the position forward because Bullet also does this. - _serverPosition += dt * _serverVelocity; + + // the entity-server doesn't know where avatars are, so it doesn't do simple extrapolation for children of + // avatars. We are trying to guess what values the entity server has, so we don't do it here, either. See + // related code in EntitySimulation::moveSimpleKinematics. + bool ancestryIsKnown; + _entity->getMaximumAACube(ancestryIsKnown); + bool hasAvatarAncestor = _entity->hasAncestorOfType(NestableType::Avatar); + + if (ancestryIsKnown && !hasAvatarAncestor) { + // NOTE: we ignore the second-order acceleration term when integrating + // the position forward because Bullet also does this. + _serverPosition += dt * _serverVelocity; + } } if (_entity->actionDataNeedsTransmit()) { From e1ae2a193f0e3c510dcaa101ea5a21a984615437 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Fri, 10 Jun 2016 11:49:15 -0700 Subject: [PATCH 08/12] EntityMotionState now uses parent-relative position and rotation and velocity when deciding if it needs to send an edit packet to the entity-server --- libraries/physics/src/EntityMotionState.cpp | 41 ++++++------ libraries/physics/src/PhysicsEngine.cpp | 5 ++ libraries/shared/src/SpatiallyNestable.cpp | 69 +++++++++++++++++++++ libraries/shared/src/SpatiallyNestable.h | 22 ++++--- 4 files changed, 110 insertions(+), 27 deletions(-) diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index be7862ade3..28f7756d9a 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -85,10 +85,10 @@ void EntityMotionState::updateServerPhysicsVariables() { return; } - _serverPosition = _entity->getPosition(); - _serverRotation = _entity->getRotation(); - _serverVelocity = _entity->getVelocity(); - _serverAngularVelocity = _entity->getAngularVelocity(); + Transform localTransform; + _entity->getLocalTransformAndVelocities(localTransform, _serverVelocity, _serverAngularVelocity); + _serverPosition = localTransform.getTranslation(); + _serverRotation = localTransform.getRotation(); _serverAcceleration = _entity->getAcceleration(); _serverActionData = _entity->getActionData(); } @@ -274,11 +274,11 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { // 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 = bulletToGLM(xform.getOrigin()); - _serverRotation = bulletToGLM(xform.getRotation()); - _serverVelocity = getBodyLinearVelocityGTSigma(); + _serverPosition = _entity->worldPositionToParent(bulletToGLM(xform.getOrigin())); + _serverRotation = _entity->worldRotationToParent(bulletToGLM(xform.getRotation())); + _serverVelocity = _entity->worldVelocityToParent(getBodyLinearVelocityGTSigma()); _serverAcceleration = Vectors::ZERO; - _serverAngularVelocity = bulletToGLM(_body->getAngularVelocity()); + _serverAngularVelocity = _entity->worldVelocityToParent(bulletToGLM(_body->getAngularVelocity())); _lastStep = simulationStep; _serverActionData = _entity->getActionData(); _numInactiveUpdates = 1; @@ -315,9 +315,6 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { _lastStep = simulationStep; if (glm::length2(_serverVelocity) > 0.0f) { - _serverVelocity += _serverAcceleration * dt; - _serverVelocity *= powf(1.0f - _body->getLinearDamping(), dt); - // the entity-server doesn't know where avatars are, so it doesn't do simple extrapolation for children of // avatars. We are trying to guess what values the entity server has, so we don't do it here, either. See // related code in EntitySimulation::moveSimpleKinematics. @@ -326,6 +323,9 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { bool hasAvatarAncestor = _entity->hasAncestorOfType(NestableType::Avatar); if (ancestryIsKnown && !hasAvatarAncestor) { + _serverVelocity += _serverAcceleration * dt; + _serverVelocity *= powf(1.0f - _body->getLinearDamping(), dt); + // NOTE: we ignore the second-order acceleration term when integrating // the position forward because Bullet also does this. _serverPosition += dt * _serverVelocity; @@ -351,7 +351,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { // compute position error btTransform worldTrans = _body->getWorldTransform(); - glm::vec3 position = bulletToGLM(worldTrans.getOrigin()); + glm::vec3 position = _entity->worldPositionToParent(bulletToGLM(worldTrans.getOrigin())); float dx2 = glm::distance2(position, _serverPosition); const float MAX_POSITION_ERROR_SQUARED = 0.000004f; // corresponds to 2mm @@ -386,7 +386,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 = bulletToGLM(worldTrans.getRotation()); + glm::quat actualRotation = _entity->worldRotationToParent(bulletToGLM(worldTrans.getRotation())); #ifdef WANT_DEBUG if ((fabsf(glm::dot(actualRotation, _serverRotation)) < MIN_ROTATION_DOT)) { @@ -491,11 +491,11 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_ } // remember properties for local server prediction - _serverPosition = _entity->getPosition(); - _serverRotation = _entity->getRotation(); - _serverVelocity = _entity->getVelocity(); + Transform localTransform; + _entity->getLocalTransformAndVelocities(localTransform, _serverVelocity, _serverAngularVelocity); + _serverPosition = localTransform.getTranslation(); + _serverRotation = localTransform.getRotation(); _serverAcceleration = _entity->getAcceleration(); - _serverAngularVelocity = _entity->getAngularVelocity(); _serverActionData = _entity->getActionData(); EntityItemProperties properties; @@ -600,7 +600,7 @@ uint32_t EntityMotionState::getIncomingDirtyFlags() { if (_body && _entity) { dirtyFlags = _entity->getDirtyFlags(); - if (dirtyFlags | Simulation::DIRTY_SIMULATOR_ID) { + if (dirtyFlags & Simulation::DIRTY_SIMULATOR_ID) { // when SIMULATOR_ID changes we must check for reinterpretation of asymmetric collision mask // bits for the avatar groups (e.g. MY_AVATAR vs OTHER_AVATAR) uint8_t entityCollisionMask = _entity->getCollisionless() ? 0 : _entity->getCollisionMask(); @@ -613,8 +613,9 @@ uint32_t EntityMotionState::getIncomingDirtyFlags() { // we add DIRTY_MOTION_TYPE if the body's motion type disagrees with entity velocity settings int bodyFlags = _body->getCollisionFlags(); bool isMoving = _entity->isMovingRelativeToParent(); - if (((bodyFlags & btCollisionObject::CF_STATIC_OBJECT) && isMoving) || - (bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving)) { + if (((bodyFlags & btCollisionObject::CF_STATIC_OBJECT) && isMoving) // || + // (bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving) + ) { dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; } } diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index d3247ec62c..6f8acfa6a7 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -75,6 +75,7 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { motionState->setMotionType(motionType); switch(motionType) { case MOTION_TYPE_KINEMATIC: { + qDebug() << " --> KINEMATIC"; if (!body) { btCollisionShape* shape = motionState->getShape(); assert(shape); @@ -91,6 +92,8 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { break; } case MOTION_TYPE_DYNAMIC: { + qDebug() << " --> DYNAMIC"; + mass = motionState->getMass(); btCollisionShape* shape = motionState->getShape(); assert(shape); @@ -117,6 +120,8 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { } case MOTION_TYPE_STATIC: default: { + qDebug() << " --> STATIC"; + if (!body) { assert(motionState->getShape()); body = new btRigidBody(mass, motionState, motionState->getShape(), inertia); diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 2a3cb4af47..6edf80ab98 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -174,6 +174,66 @@ 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) { + Transform result; + QSharedPointer parentFinder = DependencyManager::get(); + if (!parentFinder) { + success = false; + return glm::vec3(); + } + + Transform parentTransform; + auto parentWP = parentFinder->find(parentID, success); + if (!success) { + return glm::vec3(); + } + + auto parent = parentWP.lock(); + if (!parentID.isNull() && !parent) { + success = false; + return glm::vec3(); + } + + if (parent) { + parentTransform = parent->getTransform(parentJointIndex, success); + if (!success) { + return glm::vec3(); + } + parentTransform.setScale(1.0f); // TODO: scale + } + success = true; + + parentTransform.setTranslation(glm::vec3(0.0f)); + + Transform velocityTransform; + velocityTransform.setTranslation(velocity); + Transform myWorldTransform; + Transform::mult(myWorldTransform, parentTransform, velocityTransform); + myWorldTransform.setTranslation(velocity); + Transform::inverseMult(result, parentTransform, myWorldTransform); + 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) { @@ -214,6 +274,15 @@ 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 04bb57a688..ffb00ac040 100644 --- a/libraries/shared/src/SpatiallyNestable.h +++ b/libraries/shared/src/SpatiallyNestable.h @@ -46,11 +46,17 @@ public: virtual void setParentJointIndex(quint16 parentJointIndex); static glm::vec3 worldToLocal(const glm::vec3& position, const QUuid& parentID, int parentJointIndex, bool& success); + static glm::vec3 worldVelocityToLocal(const glm::vec3& position, const QUuid& parentID, + int parentJointIndex, bool& success); static glm::quat worldToLocal(const glm::quat& orientation, const QUuid& parentID, int parentJointIndex, bool& success); 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); @@ -144,6 +150,15 @@ public: bool hasAncestorOfType(NestableType nestableType); + void getLocalTransformAndVelocities(Transform& localTransform, + glm::vec3& localVelocity, + glm::vec3& localAngularVelocity) const; + + void setLocalTransformAndVelocities( + const Transform& localTransform, + const glm::vec3& localVelocity, + const glm::vec3& localAngularVelocity); + protected: const NestableType _nestableType; // EntityItem or an AvatarData QUuid _id; @@ -151,13 +166,6 @@ protected: quint16 _parentJointIndex { 0 }; // which joint of the parent is this relative to? SpatiallyNestablePointer getParentPointer(bool& success) const; - void getLocalTransformAndVelocities(Transform& localTransform, glm::vec3& localVelocity, glm::vec3& localAngularVelocity) const; - - void setLocalTransformAndVelocities( - const Transform& localTransform, - const glm::vec3& localVelocity, - const glm::vec3& localAngularVelocity); - mutable SpatiallyNestableWeakPointer _parent; virtual void beParentOfChild(SpatiallyNestablePointer newChild) const; From 41c399897a2eec5d3ad3ed43d59aa12c1879f0ed Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Fri, 10 Jun 2016 12:08:13 -0700 Subject: [PATCH 09/12] remove some debug prints --- libraries/physics/src/EntityMotionState.cpp | 2 ++ libraries/physics/src/PhysicsEngine.cpp | 5 ----- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index 28f7756d9a..e5957ce7b2 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -613,6 +613,8 @@ uint32_t EntityMotionState::getIncomingDirtyFlags() { // we add DIRTY_MOTION_TYPE if the body's motion type disagrees with entity velocity settings int bodyFlags = _body->getCollisionFlags(); bool isMoving = _entity->isMovingRelativeToParent(); + + // XXX what's right, here? if (((bodyFlags & btCollisionObject::CF_STATIC_OBJECT) && isMoving) // || // (bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving) ) { diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 6f8acfa6a7..d3247ec62c 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -75,7 +75,6 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { motionState->setMotionType(motionType); switch(motionType) { case MOTION_TYPE_KINEMATIC: { - qDebug() << " --> KINEMATIC"; if (!body) { btCollisionShape* shape = motionState->getShape(); assert(shape); @@ -92,8 +91,6 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { break; } case MOTION_TYPE_DYNAMIC: { - qDebug() << " --> DYNAMIC"; - mass = motionState->getMass(); btCollisionShape* shape = motionState->getShape(); assert(shape); @@ -120,8 +117,6 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { } case MOTION_TYPE_STATIC: default: { - qDebug() << " --> STATIC"; - if (!body) { assert(motionState->getShape()); body = new btRigidBody(mass, motionState, motionState->getShape(), inertia); From 52245f25f28645b66caaeee686fe752cf6e07d23 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Mon, 13 Jun 2016 11:48:45 -0700 Subject: [PATCH 10/12] rework code that transforms global _server* values into parent-frame values --- libraries/physics/src/EntityMotionState.cpp | 39 ++++++++++++++++++--- 1 file changed, 35 insertions(+), 4 deletions(-) diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp index e5957ce7b2..5fffc9901b 100644 --- a/libraries/physics/src/EntityMotionState.cpp +++ b/libraries/physics/src/EntityMotionState.cpp @@ -274,9 +274,39 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) { // 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 = _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(); + } + + + _serverAcceleration = Vectors::ZERO; _serverAngularVelocity = _entity->worldVelocityToParent(bulletToGLM(_body->getAngularVelocity())); _lastStep = simulationStep; @@ -614,8 +644,9 @@ uint32_t EntityMotionState::getIncomingDirtyFlags() { int bodyFlags = _body->getCollisionFlags(); bool isMoving = _entity->isMovingRelativeToParent(); - // XXX what's right, here? if (((bodyFlags & btCollisionObject::CF_STATIC_OBJECT) && isMoving) // || + // TODO -- there is opportunity for an optimization here, but this currently causes + // excessive re-insertion of the rigid body. // (bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving) ) { dirtyFlags |= Simulation::DIRTY_MOTION_TYPE; From eba518cb65634edb8dd47824ecb01f25eb9b27ed Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Mon, 13 Jun 2016 14:26:41 -0700 Subject: [PATCH 11/12] try to make code that converts bullet-calculated values to parent-frame values more effecient --- libraries/physics/src/EntityMotionState.cpp | 53 +++++++-------------- libraries/shared/src/SpatiallyNestable.cpp | 27 ----------- libraries/shared/src/SpatiallyNestable.h | 4 -- 3 files changed, 17 insertions(+), 67 deletions(-) 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); From f8f62a4ff1120f4a83cea57b82b614ed80fa0d4f Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Mon, 13 Jun 2016 15:10:52 -0700 Subject: [PATCH 12/12] remove unused function --- libraries/shared/src/SpatiallyNestable.cpp | 42 ---------------------- libraries/shared/src/SpatiallyNestable.h | 2 -- 2 files changed, 44 deletions(-) diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp index 29a033f340..2a3cb4af47 100644 --- a/libraries/shared/src/SpatiallyNestable.cpp +++ b/libraries/shared/src/SpatiallyNestable.cpp @@ -174,48 +174,6 @@ glm::vec3 SpatiallyNestable::worldToLocal(const glm::vec3& position, return result.getTranslation(); } -glm::vec3 SpatiallyNestable::worldVelocityToLocal(const glm::vec3& velocity, // can be linear or angular - const QUuid& parentID, int parentJointIndex, - bool& success) { - Transform result; - QSharedPointer parentFinder = DependencyManager::get(); - if (!parentFinder) { - success = false; - return glm::vec3(); - } - - Transform parentTransform; - auto parentWP = parentFinder->find(parentID, success); - if (!success) { - return glm::vec3(); - } - - auto parent = parentWP.lock(); - if (!parentID.isNull() && !parent) { - success = false; - return glm::vec3(); - } - - if (parent) { - parentTransform = parent->getTransform(parentJointIndex, success); - if (!success) { - return glm::vec3(); - } - parentTransform.setScale(1.0f); // TODO: scale - } - success = true; - - parentTransform.setTranslation(glm::vec3(0.0f)); - - Transform velocityTransform; - velocityTransform.setTranslation(velocity); - Transform myWorldTransform; - Transform::mult(myWorldTransform, parentTransform, velocityTransform); - myWorldTransform.setTranslation(velocity); - Transform::inverseMult(result, parentTransform, myWorldTransform); - return result.getTranslation(); -} - glm::quat SpatiallyNestable::worldToLocal(const glm::quat& orientation, const QUuid& parentID, int parentJointIndex, bool& success) { diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h index 23beffda53..c2563a1188 100644 --- a/libraries/shared/src/SpatiallyNestable.h +++ b/libraries/shared/src/SpatiallyNestable.h @@ -46,8 +46,6 @@ public: virtual void setParentJointIndex(quint16 parentJointIndex); static glm::vec3 worldToLocal(const glm::vec3& position, const QUuid& parentID, int parentJointIndex, bool& success); - static glm::vec3 worldVelocityToLocal(const glm::vec3& position, const QUuid& parentID, - int parentJointIndex, bool& success); static glm::quat worldToLocal(const glm::quat& orientation, const QUuid& parentID, int parentJointIndex, bool& success); static glm::vec3 localToWorld(const glm::vec3& position, const QUuid& parentID, int parentJointIndex, bool& success);