work in progress room scale tracking work.

sensor to world matrix stored in MyAvatar
override setPosition and setOrientation to move sensor to world matrix.

interface/src/avatar/MyAvatar.cpp

@@ -104,8 +104,7 @@ MyAvatar::MyAvatar() :
                           DEFAULT_REAL_WORLD_FIELD_OF_VIEW_DEGREES),
     _firstPersonSkeletonModel(this),
     _prevShouldDrawHead(true),
-    _prevRoomBodyPos(0, 0, 0),
-    _currRoomBodyPos(0, 0, 0)
+    _sensorToWorldMat()
 {
     _firstPersonSkeletonModel.setIsFirstPerson(true);
 
@@ -169,9 +168,7 @@ void MyAvatar::update(float deltaTime) {
     Head* head = getHead();
     head->relaxLean(deltaTime);
     updateFromTrackers(deltaTime);
-    if (qApp->isHMDMode() && isRoomTracking) {
-        updateRoomTracking(deltaTime);
-    }
+
     //  Get audio loudness data from audio input device
     auto audio = DependencyManager::get<AudioClient>();
     head->setAudioLoudness(audio->getLastInputLoudness());
@@ -1505,12 +1502,24 @@ void MyAvatar::updatePosition(float deltaTime) {
     _moving = speed > MOVING_SPEED_THRESHOLD;
 
     if (qApp->isHMDMode() && isRoomTracking) {
-        glm::vec3 newPos = (_currRoomBodyPos - _prevRoomBodyPos) + getPosition();
-        /*
-        qCDebug(interfaceapp, "updatePosition");
-        qCDebug(interfaceapp, "\tnewPos = (%.5f, %.5f, %.5f)", newPos.x, newPos.y, newPos.z);
-        */
-        setPosition(newPos);
+        // hmd is in sensor space.
+        const glm::vec3 hmdPosition = qApp->getHeadPosition();
+        const glm::quat hmdOrientation = qApp->getHeadOrientation();
+        const glm::quat hmdOrientationYawOnly = cancelOutRollAndPitch(hmdOrientation);
+
+        // In sensor space, figure out where the avatar body should be,
+        // by applying offsets from the avatar's neck & head joints.
+        vec3 localEyes = _skeletonModel.getDefaultEyeModelPosition();
+        vec3 localNeck;
+        if (_skeletonModel.getLocalNeckPosition(localNeck)) {
+            glm::vec3 eyeToNeck = hmdOrientation * (localNeck - localEyes);
+            glm::vec3 neckToRoot = hmdOrientationYawOnly * -localNeck;
+            glm::vec3 roomBodyPos = hmdPosition + eyeToNeck + neckToRoot;
+
+            // now convert from sensor space into world coordinates
+            glm::vec3 worldBodyPos = _sensorToWorldMat * roomBodyPos;
+            setAvatarPosition(worldBodyPos);
+        }
     }
 }
 
@@ -1577,26 +1586,6 @@ void MyAvatar::maybeUpdateBillboard() {
     sendBillboardPacket();
 }
 
-void MyAvatar::updateRoomTracking(float deltaTime) {
-    vec3 localEyes = _skeletonModel.getDefaultEyeModelPosition();
-    vec3 localNeck;
-    if (_skeletonModel.getLocalNeckPosition(localNeck)) {
-        glm::vec3 eyeToNeck = qApp->getHeadOrientation() * (localNeck - localEyes);
-        glm::vec3 neckToRoot = qApp->getHeadOrientation() * localNeck;
-        glm::vec3 roomBodyPos = qApp->getHeadPosition() + eyeToNeck + neckToRoot;
-
-        _prevRoomBodyPos = _currRoomBodyPos;
-        _currRoomBodyPos = roomBodyPos;
-
-        //qCDebug(interfaceapp, "updateRoomTracking()");
-        //qCDebug(interfaceapp, "\troomBodyPos = (%.5f, %.5f, %.5f)", roomBodyPos.x, roomBodyPos.y, roomBodyPos.z);
-        glm::vec3 delta = _currRoomBodyPos - _prevRoomBodyPos;
-        //qCDebug(interfaceapp, "\tdelta = (%.5f, %.5f, %.5f)", delta.x, delta.y, delta.z);
-        glm::vec3 e = _skeletonModel.getDefaultEyeModelPosition();
-        //qCDebug(interfaceapp, "\teye pos = (%.5f, %.5f, %.5f)", e.x, e.y, e.z);
-    }
-}
-
 void MyAvatar::increaseSize() {
     if ((1.0f + SCALING_RATIO) * _targetScale < MAX_AVATAR_SCALE) {
         _targetScale *= (1.0f + SCALING_RATIO);
@@ -1623,6 +1612,20 @@ void MyAvatar::goToLocation(const glm::vec3& newPosition,
     qCDebug(interfaceapp).nospace() << "MyAvatar goToLocation - moving to " << newPosition.x << ", "
         << newPosition.y << ", " << newPosition.z;
 
+    glm::mat4 m;
+
+    // Set the orientation of the sensor room, not the avatar itself.
+    if (hasOrientation) {
+        qCDebug(interfaceapp).nospace() << "MyAvatar goToLocation - new orientation is "
+            << newOrientation.x << ", " << newOrientation.y << ", " << newOrientation.z << ", " << newOrientation.w;
+
+        // TODO: FIXME: add support for shouldFaceLocation
+        m = mat4_cast(newOrientation);
+    }
+    m[3] = glm::vec4(newPosition, 1);
+    _sensorToWorldMat = m;
+
+    /*
     glm::vec3 shiftedPosition = newPosition;
 
     if (hasOrientation) {
@@ -1645,6 +1648,7 @@ void MyAvatar::goToLocation(const glm::vec3& newPosition,
     }
 
     slamPosition(shiftedPosition);
+    */
     emit transformChanged();
 }
 
@@ -1715,3 +1719,25 @@ void MyAvatar::relayDriveKeysToCharacterController() {
         _characterController.jump();
     }
 }
+
+void MyAvatar::setAvatarPosition(glm::vec3 pos) {
+    AvatarData::setPosition(pos);
+}
+
+void MyAvatar::setAvatarOrientation(glm::quat quat) {
+    AvatarData::setOrientation(quat);
+}
+
+// these are overriden, because they must move the sensor mat, such that the avatar will be at the given location.
+void MyAvatar::setPosition(const glm::vec3 position, bool overideReferential) {
+    glm::vec3 sensorPos = qApp->getHeadPosition();
+    _sensorToWorldMat[3] = glm::vec3(position - sensorPos, 1);
+}
+
+void MyAvatar::setOrientation(const glm::quat& orientation, bool overideReferential) {
+    glm::mat4 sensorMat = cancelOutRollAndPitch(createMatFromQuatAndPos(qApp->getHeadOrientation(), qApp->getHeadPosition()));
+    gmm::mat4 worldMat = createMatFromQuatAndPos(_orientation, _position);
+    _sensorToWorldMat = worldMat * inverse(sensorMat);
+}
+
+

interface/src/avatar/MyAvatar.h

@@ -174,8 +174,6 @@ public:
     static const float ZOOM_MAX;
     static const float ZOOM_DEFAULT;
 
-    const glm::vec3& getRoomBodyPos() const { return _currRoomBodyPos; }
-
 public slots:
     void increaseSize();
     void decreaseSize();
@@ -207,13 +205,23 @@ public slots:
     void loadLastRecording();
 
     virtual void rebuildSkeletonBody();
-    
+
+    // these are overriden, because they must move the sensor mat, such that the avatar will be at the given location.
+    virtual void setPosition(const glm::vec3 position, bool overideReferential) override;
+    virtual void setOrientation(const glm::quat& orientation, bool overideReferential) overide;
+
+    glm::mat4 getSensorToWorldMat() const { return _sensorToWorldMat; }
+
 signals:
     void transformChanged();
     void newCollisionSoundURL(const QUrl& url);
 
 private:
 
+    // these set the avatars position in world space without effecting the sensor location.
+    void setAvatarPosition(glm::vec3 pos);
+    void setAvatarOrientation(glm::quat quat);
+
     bool cameraInsideHead() const;
 
     // These are made private for MyAvatar so that you will use the "use" methods instead
@@ -268,7 +276,6 @@ private:
     void updatePosition(float deltaTime);
     void updateCollisionSound(const glm::vec3& penetration, float deltaTime, float frequency);
     void maybeUpdateBillboard();
-    void updateRoomTracking(float deltaTime);
 
     // Avatar Preferences
     bool _useFullAvatar = false;
@@ -283,9 +290,8 @@ private:
     // used for rendering when in first person view or when in an HMD.
     SkeletonModel _firstPersonSkeletonModel;
     bool _prevShouldDrawHead;
-    glm::vec3 _prevRoomBodyPos;
-    glm::vec3 _currRoomBodyPos;
-    glm::mat4 _headWorldTransformMat;
+
+    glm::mat4 _sensorToWorldMat;
 };
 
 #endif // hifi_MyAvatar_h

libraries/avatars/src/AvatarData.h

@@ -192,7 +192,7 @@ public:
     void setBodyRoll(float bodyRoll) { _bodyRoll = bodyRoll; }
 
     glm::quat getOrientation() const;
-    void setOrientation(const glm::quat& orientation, bool overideReferential = false);
+    virtual void setOrientation(const glm::quat& orientation, bool overideReferential = false);
 
     glm::quat getHeadOrientation() const { return _headData->getOrientation(); }
     void setHeadOrientation(const glm::quat& orientation) { _headData->setOrientation(orientation); }

libraries/display-plugins/src/display-plugins/openvr/OpenVrDisplayPlugin.cpp

@@ -23,6 +23,7 @@
 #include <ViewFrustum.h>
 
 #include "OpenVrHelpers.h"
+#include "GLMHelpers.h"
 
 #include <QLoggingCategory>
 Q_DECLARE_LOGGING_CATEGORY(displayplugins)
@@ -175,24 +176,7 @@ glm::mat4 OpenVrDisplayPlugin::getModelview(Eye eye, const mat4& baseModelview)
 }
 
 void OpenVrDisplayPlugin::resetSensors() {
-    // _hmd->ResetSeatedZeroPose(); AJT: not working
-
-    mat4 pose = _trackedDevicePoseMat4[0];
-    vec3 xAxis = vec3(pose[0]);
-    vec3 yAxis = vec3(pose[1]);
-    vec3 zAxis = vec3(pose[2]);
-
-    // cancel out the roll and pitch
-    vec3 newZ = (zAxis.x == 0 && zAxis.z == 0) ? vec3(1, 0, 0) : glm::normalize(vec3(zAxis.x, 0, zAxis.z));
-    vec3 newX = glm::cross(vec3(0, 1, 0), newZ);
-    vec3 newY = glm::cross(newZ, newX);
-
-    mat4 m;
-    m[0] = vec4(newX, 0);
-    m[1] = vec4(newY, 0);
-    m[2] = vec4(newZ, 0);
-    m[3] = pose[3];
-    _sensorResetMat = glm::inverse(m);
+    _sensorResetMat = glm::inverse(cancelOutRollAndPitch(_trackedDevicePoseMat4[0]));
 }
 
 glm::mat4 OpenVrDisplayPlugin::getEyePose(Eye eye) const {

libraries/shared/src/GLMHelpers.h

@@ -150,4 +150,42 @@ T toNormalizedDeviceScale(const T& value, const T& size) {
 #define PITCH(euler) euler.x
 #define ROLL(euler) euler.z
 
+// create matrix from orientation and position
+glm::mat4 createMatFromQuatAndPos(glm::quat q, glm::vec3 p) {
+    glm::mat4 m = glm::mat4_cast(q);
+    m[3] = glm::vec4(p, 1);
+    return m;
+}
+
+// cancel out roll and pitch
+glm::quat cancelOutRollAndPitch(glm::quat q) {
+    glm::vec3 xAxis = q * glm::vec3(1, 0, 0);
+    glm::vec3 yAxis = q * glm::vec3(0, 1, 0);
+    glm::vec3 zAxis = q * glm::vec3(0, 0, 1);
+
+    // cancel out the roll and pitch
+    glm::vec3 newZ = (zAxis.x == 0 && zAxis.z == 0) ? vec3(1, 0, 0) : glm::normalize(vec3(zAxis.x, 0, zAxis.z));
+    glm::vec3 newX = glm::cross(vec3(0, 1, 0), newZ);
+    glm::vec3 newY = glm::cross(newZ, newX);
+
+    glm::mat4 temp(glm::vec4(newX, 0), glm::vec4(newY, 0), glm::vec4(newZ, 0), glm::vec4(0, 0, 0, 1));
+    return glm::quat_cast(temp);
+}
+
+// cancel out roll and pitch
+glm::mat4 cancelOutRollAndPitch(glm::mat4 m) {
+    glm::vec3 xAxis = glm::vec3(m[0]);
+    glm::vec3 yAxis = glm::vec3(m[1]);
+    glm::vec3 zAxis = glm::vec3(m[2]);
+
+    // cancel out the roll and pitch
+    glm::vec3 newZ = (zAxis.x == 0 && zAxis.z == 0) ? vec3(1, 0, 0) : glm::normalize(vec3(zAxis.x, 0, zAxis.z));
+    glm::vec3 newX = glm::cross(vec3(0, 1, 0), newZ);
+    glm::vec3 newY = glm::cross(newZ, newX);
+
+    glm::mat4 temp(glm::vec4(newX, 0), glm::vec4(newY, 0), glm::vec4(newZ, 0), m[3]);
+    return temp;
+}
+
+
 #endif // hifi_GLMHelpers_h