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

@@ -85,10 +85,10 @@ void EntityMotionState::updateServerPhysicsVariables() {
         return;
     }
 
-    _serverPosition = _entity->getPosition();
+    Transform localTransform;
-    _serverRotation = _entity->getRotation();
+    _entity->getLocalTransformAndVelocities(localTransform, _serverVelocity, _serverAngularVelocity);
-    _serverVelocity = _entity->getVelocity();
+    _serverPosition = localTransform.getTranslation();
-    _serverAngularVelocity = _entity->getAngularVelocity();
+    _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());
+        _serverPosition = _entity->worldPositionToParent(bulletToGLM(xform.getOrigin()));
-        _serverRotation = bulletToGLM(xform.getRotation());
+        _serverRotation = _entity->worldRotationToParent(bulletToGLM(xform.getRotation()));
-        _serverVelocity = getBodyLinearVelocityGTSigma();
+        _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();
+    Transform localTransform;
-    _serverRotation = _entity->getRotation();
+    _entity->getLocalTransformAndVelocities(localTransform, _serverVelocity, _serverAngularVelocity);
-    _serverVelocity = _entity->getVelocity();
+    _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) ||
+        if (((bodyFlags & btCollisionObject::CF_STATIC_OBJECT) && isMoving) // ||
-                (bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving)) {
+            // (bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving)
+            ) {
             dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
         }
     }

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

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<SpatialParentFinder> parentFinder = DependencyManager::get<SpatialParentFinder>();
+    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) {

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;