diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index e0e53c6fe5..cad583bd21 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -2670,6 +2670,10 @@ void Application::update(float deltaTime) { _physicsEngine.stepSimulation(); _entities.getTree()->unlock(); + // AJT: delay the head pose until after the physics step. + _headPosition = glm::vec3(getActiveDisplayPlugin()->getHeadPose()[3]); + _headOrientation = glm::quat_cast(getActiveDisplayPlugin()->getHeadPose()); + if (_physicsEngine.hasOutgoingChanges()) { _entities.getTree()->lockForWrite(); _entitySimulation.lock(); @@ -4865,11 +4869,11 @@ void Application::shutdownPlugins() { } glm::vec3 Application::getHeadPosition() const { - return glm::vec3(getActiveDisplayPlugin()->getHeadPose()[3]); + return _headPosition; } glm::quat Application::getHeadOrientation() const { - return glm::quat_cast(getActiveDisplayPlugin()->getHeadPose()); + return _headOrientation; } glm::uvec2 Application::getCanvasSize() const { diff --git a/interface/src/Application.h b/interface/src/Application.h index 9699ae5933..5aa6cd7eff 100644 --- a/interface/src/Application.h +++ b/interface/src/Application.h @@ -668,6 +668,9 @@ private: Overlays _overlays; ApplicationOverlay _applicationOverlay; ApplicationCompositor _compositor; + + glm::vec3 _headPosition; + glm::quat _headOrientation; }; #endif // hifi_Application_h diff --git a/interface/src/avatar/Head.cpp b/interface/src/avatar/Head.cpp index ed7d84dd24..8c0861d8bc 100644 --- a/interface/src/avatar/Head.cpp +++ b/interface/src/avatar/Head.cpp @@ -27,6 +27,8 @@ using namespace std; +static bool isRoomTracking = true; + Head::Head(Avatar* owningAvatar) : HeadData((AvatarData*)owningAvatar), _returnHeadToCenter(false), @@ -116,12 +118,15 @@ void Head::simulate(float deltaTime, bool isMine, bool billboard) { } } } - // Twist the upper body to follow the rotation of the head, but only do this with my avatar, - // since everyone else will see the full joint rotations for other people. - const float BODY_FOLLOW_HEAD_YAW_RATE = 0.1f; - const float BODY_FOLLOW_HEAD_FACTOR = 0.66f; - float currentTwist = getTorsoTwist(); - setTorsoTwist(currentTwist + (getFinalYaw() * BODY_FOLLOW_HEAD_FACTOR - currentTwist) * BODY_FOLLOW_HEAD_YAW_RATE); + + if (!isRoomTracking) { + // Twist the upper body to follow the rotation of the head, but only do this with my avatar, + // since everyone else will see the full joint rotations for other people. + const float BODY_FOLLOW_HEAD_YAW_RATE = 0.1f; + const float BODY_FOLLOW_HEAD_FACTOR = 0.66f; + float currentTwist = getTorsoTwist(); + setTorsoTwist(currentTwist + (getFinalYaw() * BODY_FOLLOW_HEAD_FACTOR - currentTwist) * BODY_FOLLOW_HEAD_YAW_RATE); + } } if (!(_isFaceTrackerConnected || billboard)) { diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index eba2159e30..ddee3881a9 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -155,11 +155,12 @@ void MyAvatar::reset() { void MyAvatar::update(float deltaTime) { - /* - qCDebug(interfaceapp, "update()"); + qCDebug(interfaceapp, "update() *************"); glm::vec3 pos = getPosition(); qCDebug(interfaceapp, "\tpos = (%.5f, %.5f, %.5f)", pos.x, pos.y, pos.z); - */ + glm::vec3 axis = glm::axis(getOrientation()); + float angle = glm::angle(getOrientation()); + qCDebug(interfaceapp, "\trot = axis = (%.5f, %.5f, %.5f), angle = %.5f", axis.x, axis.y, axis.z, angle); if (_referential) { _referential->update(); @@ -1299,9 +1300,12 @@ void MyAvatar::updateOrientation(float deltaTime) { _bodyYawDelta += _driveKeys[ROT_LEFT] * YAW_SPEED * deltaTime; getHead()->setBasePitch(getHead()->getBasePitch() + (_driveKeys[ROT_UP] - _driveKeys[ROT_DOWN]) * PITCH_SPEED * deltaTime); + // AJT: disable arrow key movement. +/* // update body orientation by movement inputs setOrientation(getOrientation() * glm::quat(glm::radians(glm::vec3(0.0f, _bodyYawDelta, 0.0f) * deltaTime))); +*/ // decay body rotation momentum const float BODY_SPIN_FRICTION = 7.5f; @@ -1329,22 +1333,8 @@ void MyAvatar::updateOrientation(float deltaTime) { head->setBasePitch(PITCH(euler)); head->setBaseRoll(ROLL(euler)); - mat4 pose = glm::mat4_cast(qApp->getHeadOrientation()); - 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]; - - setOrientation(glm::quat(m)); - + glm::quat q = calcBodyOrientationFromSensors(); + setAvatarOrientation(q); } } @@ -1502,24 +1492,7 @@ void MyAvatar::updatePosition(float deltaTime) { _moving = speed > MOVING_SPEED_THRESHOLD; if (qApp->isHMDMode() && isRoomTracking) { - // 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); - } + setAvatarPosition(calcBodyPositionFromSensors()); } } @@ -1614,17 +1587,29 @@ void MyAvatar::goToLocation(const glm::vec3& newPosition, glm::mat4 m; + // AJT: FIXME, goToLocation doens't work with orientation. + /* // 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; + glm::vec3 axis = glm::axis(newOrientation); + qCDebug(interfaceapp).nospace() << "MyAvatar goToLocation - new orientation is axis = (" + << axis.x << ", " << axis.y << ", " << axis.z << "), theta = " << glm::angle(newOrientation); + // TODO: FIXME: add support for shouldFaceLocation - m = mat4_cast(newOrientation); + m = glm::mat4_cast(newOrientation); } + */ m[3] = glm::vec4(newPosition, 1); _sensorToWorldMat = m; + qCDebug(interfaceapp, "\tsensorMat = | %10.4f %10.4f %10.4f %10.4f |", m[0][0], m[1][0], m[2][0], m[3][0]); + qCDebug(interfaceapp, "\t | %10.4f %10.4f %10.4f %10.4f |", m[0][1], m[1][1], m[2][1], m[3][1]); + qCDebug(interfaceapp, "\t | %10.4f %10.4f %10.4f %10.4f |", m[0][2], m[1][2], m[2][2], m[3][2]); + qCDebug(interfaceapp, "\t | %10.4f %10.4f %10.4f %10.4f |", m[0][3], m[1][3], m[2][3], m[3][3]); + /* glm::vec3 shiftedPosition = newPosition; @@ -1721,23 +1706,64 @@ void MyAvatar::relayDriveKeysToCharacterController() { } void MyAvatar::setAvatarPosition(glm::vec3 pos) { - AvatarData::setPosition(pos); + qCDebug(interfaceapp, "setAvatarPosition = (%.5f, %.5f, %.5f)", pos.x, pos.y, pos.z); + Avatar::setPosition(pos); } void MyAvatar::setAvatarOrientation(glm::quat quat) { - AvatarData::setOrientation(quat); + glm::vec3 axis = glm::axis(quat); + float angle = glm::angle(quat); + qCDebug(interfaceapp, "setAvatarOrientation axis = (%.5f, %.5f, %.5f), theta = %.5f", axis.x, axis.y, axis.z, angle); + Avatar::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); + qCDebug(interfaceapp, "setPosition = (%.5f, %.5f, %.5f)", position.x, position.y, position.z); + glm::vec3 bodyPos = calcBodyPositionFromSensors(); + glm::vec3 desiredPos = position; + glm::vec3 sensorPos(_sensorToWorldMat[3]); + _sensorToWorldMat[3] = glm::vec4(desiredPos - bodyPos + sensorPos, 1); + setAvatarPosition(position); } 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); + glm::vec3 axis = glm::axis(orientation); + float angle = glm::angle(orientation); + qCDebug(interfaceapp, "setOrientation axis = (%.5f, %.5f, %.5f), theta = %.5f", axis.x, axis.y, axis.z, angle); + + glm::vec3 bodyPos = calcBodyPositionFromSensors(); + glm::quat bodyOrientation = calcBodyOrientationFromSensors(); + glm::mat4 bodyMat = createMatFromQuatAndPos(bodyOrientation, bodyPos); + glm::mat4 desiredMat = createMatFromQuatAndPos(orientation, bodyPos); + _sensorToWorldMat = desiredMat * glm::inverse(bodyMat) * _sensorToWorldMat; + setAvatarOrientation(orientation); } +glm::vec3 MyAvatar::calcBodyPositionFromSensors() const { + // 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(0.0f, 0.6f, 0.0f); // start with some kind of guess if the skeletonModel is not loaded yet. + _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 + return transformPoint(_sensorToWorldMat, roomBodyPos); +} + +glm::quat MyAvatar::calcBodyOrientationFromSensors() const { + const glm::quat hmdOrientation = qApp->getHeadOrientation(); + const glm::quat hmdOrientationYawOnly = cancelOutRollAndPitch(hmdOrientation); + + // TODO: do a beter calculation of bodyOrientation for now just use hmd facing. + return glm::quat_cast(_sensorToWorldMat) * hmdOrientationYawOnly; +} diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index e651f9c383..cbbf731a0b 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -207,8 +207,8 @@ public slots: 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; + virtual void setPosition(const glm::vec3 position, bool overideReferential = false) override; + virtual void setOrientation(const glm::quat& orientation, bool overideReferential = false) override; glm::mat4 getSensorToWorldMat() const { return _sensorToWorldMat; } @@ -222,6 +222,9 @@ private: void setAvatarPosition(glm::vec3 pos); void setAvatarOrientation(glm::quat quat); + glm::vec3 calcBodyPositionFromSensors() const; + glm::quat calcBodyOrientationFromSensors() const; + bool cameraInsideHead() const; // These are made private for MyAvatar so that you will use the "use" methods instead diff --git a/libraries/display-plugins/src/display-plugins/openvr/OpenVrDisplayPlugin.cpp b/libraries/display-plugins/src/display-plugins/openvr/OpenVrDisplayPlugin.cpp index 158b3e51ed..cf96fa70b0 100644 --- a/libraries/display-plugins/src/display-plugins/openvr/OpenVrDisplayPlugin.cpp +++ b/libraries/display-plugins/src/display-plugins/openvr/OpenVrDisplayPlugin.cpp @@ -27,7 +27,7 @@ #include Q_DECLARE_LOGGING_CATEGORY(displayplugins) -Q_LOGGING_CATEGORY(displayplugins, "hifi.physics") +Q_LOGGING_CATEGORY(displayplugins, "hifi.displayplugins") const QString OpenVrDisplayPlugin::NAME("OpenVR (Vive)"); @@ -210,6 +210,9 @@ void OpenVrDisplayPlugin::finishFrame() { openvr_for_each_eye([&](vr::Hmd_Eye eye) { _eyesData[eye]._pose = _trackedDevicePoseMat4[0]; }); + + glm::vec3 p(_trackedDevicePoseMat4[0][3]); + qCDebug(displayplugins, "trackPos = (%.5f, %.5f, %.5f)", p.x, p.y, p.z); }; #endif diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index 53b3b90560..a9b7f98507 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -347,4 +347,44 @@ QRectF glmToRect(const glm::vec2 & pos, const glm::vec2 & size) { return result; } +// 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; +} + +glm::vec3 transformPoint(const glm::mat4& m, glm::vec3 p) { + glm::vec4 temp = m * glm::vec4(p, 1); + return glm::vec3(temp.x / temp.w, temp.y / temp.w, temp.z / temp.w); +} diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index 788aa50d5c..b2090af3e8 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/libraries/shared/src/GLMHelpers.h @@ -150,42 +150,9 @@ 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; -} - +glm::mat4 createMatFromQuatAndPos(glm::quat q, glm::vec3 p); +glm::quat cancelOutRollAndPitch(glm::quat q); +glm::mat4 cancelOutRollAndPitch(glm::mat4 m); +glm::vec3 transformPoint(const glm::mat4& m, glm::vec3 p); #endif // hifi_GLMHelpers_h