From 834b75f4c467ebad0b6eb5b7f49ffa09a3851f14 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 6 Jul 2015 18:19:33 -0700 Subject: [PATCH] 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 | 88 ++++++++++++------- interface/src/avatar/MyAvatar.h | 20 +++-- libraries/avatars/src/AvatarData.h | 2 +- .../openvr/OpenVrDisplayPlugin.cpp | 20 +---- libraries/shared/src/GLMHelpers.h | 38 ++++++++ 5 files changed, 111 insertions(+), 57 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index b2a0c57f15..eba2159e30 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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(); 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); +} + + diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 038411c58c..e651f9c383 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/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 diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index c482f52bf1..f01b2be432 100644 --- a/libraries/avatars/src/AvatarData.h +++ b/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); } diff --git a/libraries/display-plugins/src/display-plugins/openvr/OpenVrDisplayPlugin.cpp b/libraries/display-plugins/src/display-plugins/openvr/OpenVrDisplayPlugin.cpp index 47e66935f9..158b3e51ed 100644 --- a/libraries/display-plugins/src/display-plugins/openvr/OpenVrDisplayPlugin.cpp +++ b/libraries/display-plugins/src/display-plugins/openvr/OpenVrDisplayPlugin.cpp @@ -23,6 +23,7 @@ #include #include "OpenVrHelpers.h" +#include "GLMHelpers.h" #include 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 { diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 6874f3b391..788aa50d5c 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/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