From e1ae2a193f0e3c510dcaa101ea5a21a984615437 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Fri, 10 Jun 2016 11:49:15 -0700 Subject: [PATCH] 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;