From bdbc6ddcc13eff1c8f28ee298d0f2afc807f9298 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Thu, 25 May 2017 10:50:41 -0700 Subject: [PATCH] change some code to look at head-controller position rather than hmd position --- interface/src/Application.cpp | 4 +- interface/src/avatar/MyAvatar.cpp | 55 ++++++++----------- interface/src/avatar/MyAvatar.h | 11 ++-- interface/src/avatar/MySkeletonModel.cpp | 2 +- .../src/scripting/HMDScriptingInterface.cpp | 2 +- libraries/controllers/src/controllers/Pose.h | 4 ++ 6 files changed, 39 insertions(+), 39 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index ae490c05e7..7926905273 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3953,6 +3953,7 @@ void Application::updateMyAvatarLookAtPosition() { lookAtPosition.x = -lookAtPosition.x; } if (isHMD) { + // TODO -- this code is probably wrong, getHeadPose() returns something in sensor frame, not avatar glm::mat4 headPose = getActiveDisplayPlugin()->getHeadPose(); glm::quat hmdRotation = glm::quat_cast(headPose); lookAtSpot = _myCamera.getPosition() + myAvatar->getOrientation() * (hmdRotation * lookAtPosition); @@ -3995,7 +3996,8 @@ void Application::updateMyAvatarLookAtPosition() { } else { // I am not looking at anyone else, so just look forward if (isHMD) { - glm::mat4 worldHMDMat = myAvatar->getSensorToWorldMatrix() * myAvatar->getHMDSensorMatrix(); + glm::mat4 worldHMDMat = myAvatar->getSensorToWorldMatrix() * + (glm::mat4)myAvatar->getHeadControllerPoseInSensorFrame(); lookAtSpot = transformPoint(worldHMDMat, glm::vec3(0.0f, 0.0f, -TREE_SCALE)); } else { lookAtSpot = myAvatar->getHead()->getEyePosition() + diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index b4e418b6a2..4bb9a2ef5f 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -401,7 +401,7 @@ void MyAvatar::update(float deltaTime) { // update moving average of HMD facing in xz plane. const float HMD_FACING_TIMESCALE = 4.0f; // very slow average float tau = deltaTime / HMD_FACING_TIMESCALE; - _hmdSensorFacingMovingAverage = lerp(_hmdSensorFacingMovingAverage, _hmdSensorFacing, tau); + _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, _headControllerFacing, tau); if (_smoothOrientationTimer < SMOOTH_TIME_ORIENTATION) { _rotationChanged = usecTimestampNow(); @@ -409,16 +409,18 @@ void MyAvatar::update(float deltaTime) { } #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE - glm::vec3 p = transformPoint(getSensorToWorldMatrix(), _hmdSensorPosition + glm::vec3(_hmdSensorFacingMovingAverage.x, 0.0f, _hmdSensorFacingMovingAverage.y)); + glm::vec3 p = transformPoint(getSensorToWorldMatrix(), getHeadControllerPoseInAvatarFrame() + + glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y)); DebugDraw::getInstance().addMarker("facing-avg", getOrientation(), p, glm::vec4(1.0f)); - p = transformPoint(getSensorToWorldMatrix(), _hmdSensorPosition + glm::vec3(_hmdSensorFacing.x, 0.0f, _hmdSensorFacing.y)); + p = transformPoint(getSensorToWorldMatrix(), getHMDSensorPosition() + + glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y)); DebugDraw::getInstance().addMarker("facing", getOrientation(), p, glm::vec4(1.0f)); #endif if (_goToPending) { setPosition(_goToPosition); setOrientation(_goToOrientation); - _hmdSensorFacingMovingAverage = _hmdSensorFacing; // reset moving average + _headControllerFacingMovingAverage = _headControllerFacing; // reset moving average _goToPending = false; // updateFromHMDSensorMatrix (called from paintGL) expects that the sensorToWorldMatrix is updated for any position changes // that happen between render and Application::update (which calls updateSensorToWorldMatrix to do so). @@ -625,7 +627,7 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { _hmdSensorMatrix = hmdSensorMatrix; auto newHmdSensorPosition = extractTranslation(hmdSensorMatrix); - if (newHmdSensorPosition != _hmdSensorPosition && + if (newHmdSensorPosition != getHMDSensorPosition() && glm::length(newHmdSensorPosition) > MAX_HMD_ORIGIN_DISTANCE) { qWarning() << "Invalid HMD sensor position " << newHmdSensorPosition; // Ignore unreasonable HMD sensor data @@ -633,7 +635,7 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { } _hmdSensorPosition = newHmdSensorPosition; _hmdSensorOrientation = glm::quat_cast(hmdSensorMatrix); - _hmdSensorFacing = getFacingDir2D(_hmdSensorOrientation); + _headControllerFacing = getFacingDir2D(_headControllerPoseInSensorFrameCache.get().rotation); } void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeValueCache& matrixCache) { @@ -671,7 +673,7 @@ void MyAvatar::updateSensorToWorldMatrix() { // Update avatar head rotation with sensor data void MyAvatar::updateFromTrackers(float deltaTime) { - glm::vec3 estimatedPosition, estimatedRotation; + glm::vec3 estimatedRotation; bool inHmd = qApp->isHMDMode(); bool playing = DependencyManager::get()->isPlaying(); @@ -682,11 +684,7 @@ void MyAvatar::updateFromTrackers(float deltaTime) { FaceTracker* tracker = qApp->getActiveFaceTracker(); bool inFacetracker = tracker && !FaceTracker::isMuted(); - if (inHmd) { - estimatedPosition = extractTranslation(getHMDSensorMatrix()); - estimatedPosition.x *= -1.0f; - } else if (inFacetracker) { - estimatedPosition = tracker->getHeadTranslation(); + if (inFacetracker) { estimatedRotation = glm::degrees(safeEulerAngles(tracker->getHeadRotation())); } @@ -1884,20 +1882,14 @@ void MyAvatar::updateOrientation(float deltaTime) { getHead()->setBasePitch(getHead()->getBasePitch() + getDriveKey(PITCH) * _pitchSpeed * deltaTime); - if (qApp->isHMDMode()) { - glm::quat orientation = glm::quat_cast(getSensorToWorldMatrix()) * getHMDSensorOrientation(); - glm::quat bodyOrientation = getWorldBodyOrientation(); - glm::quat localOrientation = glm::inverse(bodyOrientation) * orientation; - - // these angles will be in radians - // ... so they need to be converted to degrees before we do math... - glm::vec3 euler = glm::eulerAngles(localOrientation) * DEGREES_PER_RADIAN; - - Head* head = getHead(); - head->setBaseYaw(YAW(euler)); - head->setBasePitch(PITCH(euler)); - head->setBaseRoll(ROLL(euler)); - } + glm::quat localOrientation = getHeadControllerPoseInAvatarFrame(); + // these angles will be in radians + // ... so they need to be converted to degrees before we do math... + glm::vec3 euler = glm::eulerAngles(localOrientation) * DEGREES_PER_RADIAN; + Head* head = getHead(); + head->setBaseYaw(YAW(euler)); + head->setBasePitch(PITCH(euler)); + head->setBaseRoll(ROLL(euler)); } void MyAvatar::updateActionMotor(float deltaTime) { @@ -2332,8 +2324,8 @@ glm::quat MyAvatar::getWorldBodyOrientation() const { glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { // HMD is in sensor space. - const glm::vec3 hmdPosition = getHMDSensorPosition(); - const glm::quat hmdOrientation = getHMDSensorOrientation(); + const glm::vec3 hmdPosition = getHeadControllerPoseInSensorFrame(); + const glm::quat hmdOrientation = getHeadControllerPoseInSensorFrame(); const glm::quat hmdOrientationYawOnly = cancelOutRollAndPitch(hmdOrientation); const Rig& rig = _skeletonModel->getRig(); @@ -2478,7 +2470,7 @@ bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, co } else { const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); // 30 degrees glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix); - return glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD; + return glm::dot(myAvatar.getHeadControllerFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD; } } @@ -2625,9 +2617,10 @@ glm::mat4 MyAvatar::computeCameraRelativeHandControllerMatrix(const glm::mat4& c cameraWorldMatrix *= createMatFromScaleQuatAndPos(vec3(-1.0f, 1.0f, 1.0f), glm::quat(), glm::vec3()); } - // compute a NEW sensorToWorldMatrix for the camera. The equation is cameraWorldMatrix = cameraSensorToWorldMatrix * _hmdSensorMatrix. + // compute a NEW sensorToWorldMatrix for the camera. + // The equation is cameraWorldMatrix = cameraSensorToWorldMatrix * _hmdSensorMatrix. // here we solve for the unknown cameraSensorToWorldMatrix. - glm::mat4 cameraSensorToWorldMatrix = cameraWorldMatrix * glm::inverse(_hmdSensorMatrix); + glm::mat4 cameraSensorToWorldMatrix = cameraWorldMatrix * glm::inverse(getHMDSensorMatrix()); // Using the new cameraSensorToWorldMatrix, compute where the controller is in world space. glm::mat4 controllerWorldMatrix = cameraSensorToWorldMatrix * controllerSensorMatrix; diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index fde350a43e..05615a600d 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -185,7 +185,6 @@ public: const glm::mat4& getHMDSensorMatrix() const { return _hmdSensorMatrix; } const glm::vec3& getHMDSensorPosition() const { return _hmdSensorPosition; } const glm::quat& getHMDSensorOrientation() const { return _hmdSensorOrientation; } - const glm::vec2& getHMDSensorFacingMovingAverage() const { return _hmdSensorFacingMovingAverage; } Q_INVOKABLE void setOrientationVar(const QVariant& newOrientationVar); Q_INVOKABLE QVariant getOrientationVar() const; @@ -470,6 +469,8 @@ public: controller::Pose getHeadControllerPoseInSensorFrame() const; controller::Pose getHeadControllerPoseInWorldFrame() const; controller::Pose getHeadControllerPoseInAvatarFrame() const; + const glm::vec2& getHeadControllerFacingMovingAverage() const { return _headControllerFacingMovingAverage; } + bool hasDriveInput() const; @@ -664,13 +665,13 @@ private: // working copies -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access glm::mat4 _sensorToWorldMatrix { glm::mat4() }; - // cache of the current HMD sensor position and orientation - // in sensor space. + // cache of the current HMD sensor position and orientation in sensor space. glm::mat4 _hmdSensorMatrix; glm::quat _hmdSensorOrientation; glm::vec3 _hmdSensorPosition; - glm::vec2 _hmdSensorFacing; // facing vector in xz plane - glm::vec2 _hmdSensorFacingMovingAverage { 0, 0 }; // facing vector in xz plane + // cache head controller pose in sensor space + glm::vec2 _headControllerFacing; // facing vector in xz plane + glm::vec2 _headControllerFacingMovingAverage { 0, 0 }; // facing vector in xz plane // cache of the current body position and orientation of the avatar's body, // in sensor space. diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index 828a5f8a01..a98055ad35 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -59,7 +59,7 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { } else { if (qApp->isHMDMode()) { // get HMD position from sensor space into world space, and back into rig space - glm::mat4 worldHMDMat = myAvatar->getSensorToWorldMatrix() * myAvatar->getHMDSensorMatrix(); + glm::mat4 worldHMDMat = myAvatar->getHeadControllerPoseInWorldFrame(); glm::mat4 rigToWorld = createMatFromQuatAndPos(getRotation(), getTranslation()); glm::mat4 worldToRig = glm::inverse(rigToWorld); glm::mat4 rigHMDMat = worldToRig * worldHMDMat; diff --git a/interface/src/scripting/HMDScriptingInterface.cpp b/interface/src/scripting/HMDScriptingInterface.cpp index 95bf5eb028..d0e8e74d85 100644 --- a/interface/src/scripting/HMDScriptingInterface.cpp +++ b/interface/src/scripting/HMDScriptingInterface.cpp @@ -118,7 +118,7 @@ bool HMDScriptingInterface::getHUDLookAtPosition3D(glm::vec3& result) const { glm::mat4 HMDScriptingInterface::getWorldHMDMatrix() const { auto myAvatar = DependencyManager::get()->getMyAvatar(); - return myAvatar->getSensorToWorldMatrix() * myAvatar->getHMDSensorMatrix(); + return myAvatar->getSensorToWorldMatrix() * (glm::mat4)myAvatar->getHeadControllerPoseInSensorFrame(); } glm::vec3 HMDScriptingInterface::getPosition() const { diff --git a/libraries/controllers/src/controllers/Pose.h b/libraries/controllers/src/controllers/Pose.h index a6d1360f9f..10cb65a421 100644 --- a/libraries/controllers/src/controllers/Pose.h +++ b/libraries/controllers/src/controllers/Pose.h @@ -43,6 +43,10 @@ namespace controller { Pose transform(const glm::mat4& mat) const; Pose postTransform(const glm::mat4& mat) const; + operator glm::mat4() const { return createMatFromQuatAndPos(rotation, translation); } + operator glm::quat() const { return rotation; } + operator glm::vec3() const { return translation; } + static QScriptValue toScriptValue(QScriptEngine* engine, const Pose& event); static void fromScriptValue(const QScriptValue& object, Pose& event); };