From 0cd2863df452a39b309a298b14c68322b7b73ad7 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Tue, 18 Apr 2017 14:42:32 -0700 Subject: [PATCH 01/11] start on routing HMD position and rotation through controller interface (cherry picked from commit bca5241bae7674ff7e41c037d84bb78e5152046a) --- interface/resources/controllers/standard.json | 7 +- interface/src/Application.cpp | 7 ++ interface/src/avatar/MyAvatar.cpp | 68 ++++++++++++++++++- interface/src/avatar/MyAvatar.h | 17 ++++- .../controllers/src/controllers/Actions.cpp | 3 + .../controllers/src/controllers/Actions.h | 3 + 6 files changed, 101 insertions(+), 4 deletions(-) diff --git a/interface/resources/controllers/standard.json b/interface/resources/controllers/standard.json index 53285ea974..62eec9bc3c 100644 --- a/interface/resources/controllers/standard.json +++ b/interface/resources/controllers/standard.json @@ -61,6 +61,11 @@ { "from": "Standard.RightHand", "to": "Actions.RightHand" }, { "from": "Standard.LeftFoot", "to": "Actions.LeftFoot" }, - { "from": "Standard.RightFoot", "to": "Actions.RightFoot" } + { "from": "Standard.RightFoot", "to": "Actions.RightFoot" }, + + { "from": "Standard.Hips", "to": "Actions.Hips" }, + { "from": "Standard.Spine2", "to": "Actions.Spine2" }, + + { "from": "Standard.Head", "to": "Actions.Head" } ] } diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index c6cd185034..5b014f7009 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4395,6 +4395,13 @@ void Application::update(float deltaTime) { controller::Pose rightFootPose = userInputMapper->getPoseState(controller::Action::RIGHT_FOOT); myAvatar->setFootControllerPosesInSensorFrame(leftFootPose.transform(avatarToSensorMatrix), rightFootPose.transform(avatarToSensorMatrix)); + controller::Pose hipsPose = userInputMapper->getPoseState(controller::Action::HIPS); + controller::Pose spine2Pose = userInputMapper->getPoseState(controller::Action::SPINE2); + myAvatar->setSpineControllerPosesInSensorFrame(hipsPose.transform(avatarToSensorMatrix), spine2Pose.transform(avatarToSensorMatrix)); + + controller::Pose headPose = userInputMapper->getPoseState(controller::Action::HEAD); + myAvatar->setHeadControllerPoseInSensorFrame(headPose.transform(avatarToSensorMatrix)); + updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process... updateDialogs(deltaTime); // update various stats dialogs if present diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index da09bfba7e..0906dc7f7f 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -642,6 +642,7 @@ void MyAvatar::updateSensorToWorldMatrix() { updateJointFromController(controller::Action::RIGHT_HAND, _controllerRightHandMatrixCache); } + // Update avatar head rotation with sensor data void MyAvatar::updateFromTrackers(float deltaTime) { glm::vec3 estimatedPosition, estimatedRotation; @@ -660,8 +661,9 @@ void MyAvatar::updateFromTrackers(float deltaTime) { estimatedPosition.x *= -1.0f; _trackedHeadPosition = estimatedPosition; - const float OCULUS_LEAN_SCALE = 0.05f; - estimatedPosition /= OCULUS_LEAN_SCALE; + // wut + // const float OCULUS_LEAN_SCALE = 0.05f; + // estimatedPosition /= OCULUS_LEAN_SCALE; } else if (inFacetracker) { estimatedPosition = tracker->getHeadTranslation(); _trackedHeadPosition = estimatedPosition; @@ -1384,6 +1386,68 @@ controller::Pose MyAvatar::getRightFootControllerPoseInAvatarFrame() const { return getRightFootControllerPoseInWorldFrame().transform(invAvatarMatrix); } +void MyAvatar::setSpineControllerPosesInSensorFrame(const controller::Pose& hips, const controller::Pose& spine2) { + if (controller::InputDevice::getLowVelocityFilter()) { + auto oldHipsPose = getHipsControllerPoseInSensorFrame(); + auto oldSpine2Pose = getSpine2ControllerPoseInSensorFrame(); + _hipsControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldHipsPose, hips)); + _spine2ControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldSpine2Pose, spine2)); + } else { + _hipsControllerPoseInSensorFrameCache.set(hips); + _spine2ControllerPoseInSensorFrameCache.set(spine2); + } +} + +controller::Pose MyAvatar::getHipsControllerPoseInSensorFrame() const { + return _hipsControllerPoseInSensorFrameCache.get(); +} + +controller::Pose MyAvatar::getSpine2ControllerPoseInSensorFrame() const { + return _spine2ControllerPoseInSensorFrameCache.get(); +} + +controller::Pose MyAvatar::getHipsControllerPoseInWorldFrame() const { + return _hipsControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); +} + +controller::Pose MyAvatar::getSpine2ControllerPoseInWorldFrame() const { + return _spine2ControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); +} + +controller::Pose MyAvatar::getHipsControllerPoseInAvatarFrame() const { + glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getHipsControllerPoseInWorldFrame().transform(invAvatarMatrix); +} + +controller::Pose MyAvatar::getSpine2ControllerPoseInAvatarFrame() const { + glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getSpine2ControllerPoseInWorldFrame().transform(invAvatarMatrix); +} + +void MyAvatar::setHeadControllerPoseInSensorFrame(const controller::Pose& headPose) { + bool inHmd = qApp->isHMDMode(); + Head* head = getHead(); + if (inHmd) { + _headControllerPoseInSensorFrameCache.set(headPose); + _trackedHeadPosition = headPose.translation; + head->setDeltaPitch(headPose.rotation.x); + head->setDeltaYaw(headPose.rotation.y); + head->setDeltaRoll(headPose.rotation.z); + } +} + +controller::Pose MyAvatar::getHeadControllerPoseInSensorFrame() const { + return _headControllerPoseInSensorFrameCache.get(); +} + +controller::Pose MyAvatar::getHeadControllerPoseInWorldFrame() const { + return _headControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); +} + +controller::Pose MyAvatar::getHeadControllerPoseInAvatarFrame() const { + glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getHeadControllerPoseInWorldFrame().transform(invAvatarMatrix); +} void MyAvatar::updateMotors() { _characterController.clearMotors(); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 0146bd11a4..01b4dc824b 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -453,6 +453,19 @@ public: controller::Pose getLeftFootControllerPoseInAvatarFrame() const; controller::Pose getRightFootControllerPoseInAvatarFrame() const; + void setSpineControllerPosesInSensorFrame(const controller::Pose& hips, const controller::Pose& spine2); + controller::Pose getHipsControllerPoseInSensorFrame() const; + controller::Pose getSpine2ControllerPoseInSensorFrame() const; + controller::Pose getHipsControllerPoseInWorldFrame() const; + controller::Pose getSpine2ControllerPoseInWorldFrame() const; + controller::Pose getHipsControllerPoseInAvatarFrame() const; + controller::Pose getSpine2ControllerPoseInAvatarFrame() const; + + void setHeadControllerPoseInSensorFrame(const controller::Pose& headPose); + controller::Pose getHeadControllerPoseInSensorFrame() const; + controller::Pose getHeadControllerPoseInWorldFrame() const; + controller::Pose getHeadControllerPoseInAvatarFrame() const; + bool hasDriveInput() const; Q_INVOKABLE void setCharacterControllerEnabled(bool enabled); @@ -693,9 +706,11 @@ private: // These are stored in SENSOR frame ThreadSafeValueCache _leftHandControllerPoseInSensorFrameCache { controller::Pose() }; ThreadSafeValueCache _rightHandControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _leftFootControllerPoseInSensorFrameCache{ controller::Pose() }; ThreadSafeValueCache _rightFootControllerPoseInSensorFrameCache{ controller::Pose() }; + ThreadSafeValueCache _hipsControllerPoseInSensorFrameCache{ controller::Pose() }; + ThreadSafeValueCache _spine2ControllerPoseInSensorFrameCache{ controller::Pose() }; + ThreadSafeValueCache _headControllerPoseInSensorFrameCache{ controller::Pose() }; bool _hmdLeanRecenterEnabled = true; diff --git a/libraries/controllers/src/controllers/Actions.cpp b/libraries/controllers/src/controllers/Actions.cpp index 300fa684d9..62a10c851f 100644 --- a/libraries/controllers/src/controllers/Actions.cpp +++ b/libraries/controllers/src/controllers/Actions.cpp @@ -53,6 +53,9 @@ namespace controller { makePosePair(Action::RIGHT_HAND, "RightHand"), makePosePair(Action::LEFT_FOOT, "LeftFoot"), makePosePair(Action::RIGHT_FOOT, "RightFoot"), + makePosePair(Action::HIPS, "Hips"), + makePosePair(Action::SPINE2, "Spine2"), + makePosePair(Action::HEAD, "Head"), makeButtonPair(Action::LEFT_HAND_CLICK, "LeftHandClick"), makeButtonPair(Action::RIGHT_HAND_CLICK, "RightHandClick"), diff --git a/libraries/controllers/src/controllers/Actions.h b/libraries/controllers/src/controllers/Actions.h index edf3dee07a..534f01d865 100644 --- a/libraries/controllers/src/controllers/Actions.h +++ b/libraries/controllers/src/controllers/Actions.h @@ -44,6 +44,9 @@ enum class Action { RIGHT_HAND, LEFT_FOOT, RIGHT_FOOT, + HIPS, + SPINE2, + HEAD, LEFT_HAND_CLICK, RIGHT_HAND_CLICK, From 23592f4a53294ff2c24b44a97f87abc053a5beb3 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Wed, 19 Apr 2017 07:21:50 -0700 Subject: [PATCH 02/11] get rid of _trackedHeadPosition (cherry picked from commit e909938b232e11400832ae7dd29de16968967668) --- interface/src/avatar/MyAvatar.cpp | 8 -------- interface/src/avatar/MyAvatar.h | 1 - 2 files changed, 9 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 0906dc7f7f..659077188f 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -642,7 +642,6 @@ void MyAvatar::updateSensorToWorldMatrix() { updateJointFromController(controller::Action::RIGHT_HAND, _controllerRightHandMatrixCache); } - // Update avatar head rotation with sensor data void MyAvatar::updateFromTrackers(float deltaTime) { glm::vec3 estimatedPosition, estimatedRotation; @@ -659,14 +658,8 @@ void MyAvatar::updateFromTrackers(float deltaTime) { if (inHmd) { estimatedPosition = extractTranslation(getHMDSensorMatrix()); estimatedPosition.x *= -1.0f; - _trackedHeadPosition = estimatedPosition; - - // wut - // const float OCULUS_LEAN_SCALE = 0.05f; - // estimatedPosition /= OCULUS_LEAN_SCALE; } else if (inFacetracker) { estimatedPosition = tracker->getHeadTranslation(); - _trackedHeadPosition = estimatedPosition; estimatedRotation = glm::degrees(safeEulerAngles(tracker->getHeadRotation())); } @@ -1429,7 +1422,6 @@ void MyAvatar::setHeadControllerPoseInSensorFrame(const controller::Pose& headPo Head* head = getHead(); if (inHmd) { _headControllerPoseInSensorFrameCache.set(headPose); - _trackedHeadPosition = headPose.translation; head->setDeltaPitch(headPose.rotation.x); head->setDeltaYaw(headPose.rotation.y); head->setDeltaRoll(headPose.rotation.z); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 01b4dc824b..cea5962638 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -353,7 +353,6 @@ public: eyeContactTarget getEyeContactTarget(); - Q_INVOKABLE glm::vec3 getTrackedHeadPosition() const { return _trackedHeadPosition; } Q_INVOKABLE glm::vec3 getHeadPosition() const { return getHead()->getPosition(); } Q_INVOKABLE float getHeadFinalYaw() const { return getHead()->getFinalYaw(); } Q_INVOKABLE float getHeadFinalRoll() const { return getHead()->getFinalRoll(); } From dc19f3772643eff3516e19435136c043bf9fb27c Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 25 Apr 2017 15:04:47 -0700 Subject: [PATCH 03/11] input/controller routing thru skeleton model and rig. --- interface/src/avatar/SkeletonModel.cpp | 39 +++++++++++++++++++------- libraries/animation/src/Rig.cpp | 2 +- libraries/animation/src/Rig.h | 12 ++++---- libraries/shared/src/GLMHelpers.cpp | 5 ++++ libraries/shared/src/GLMHelpers.h | 7 +++++ 5 files changed, 49 insertions(+), 16 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 3cf866fb6b..ccee65d3e6 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -108,8 +108,6 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::HeadParameters headParams; if (qApp->isHMDMode()) { - headParams.isInHMD = true; - // get HMD position from sensor space into world space, and back into rig space glm::mat4 worldHMDMat = myAvatar->getSensorToWorldMatrix() * myAvatar->getHMDSensorMatrix(); glm::mat4 rigToWorld = createMatFromQuatAndPos(getRotation(), getTranslation()); @@ -119,18 +117,39 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { headParams.rigHeadPosition = extractTranslation(rigHMDMat); headParams.rigHeadOrientation = extractRotation(rigHMDMat); headParams.worldHeadOrientation = extractRotation(worldHMDMat); + headParams.headEnabled = true; + } else { + auto avatarHeadPose = myAvatar->getHeadControllerPoseInAvatarFrame(); + if (avatarHeadPose.isValid()) { + glm::mat4 rigHeadMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation()); + headParams.rigHeadPosition = extractTranslation(rigHeadMat); + headParams.rigHeadOrientation = glmExtractRotation(rigHeadMat); + headParams.worldHeadOrientation = myAvatar->getHeadControllerPoseInWorldFrame().getTranslation(); + headParams.headEnabled = true; + } else { + // even though full head IK is disabled, the rig still needs the head orientation to rotate the head up and down in desktop mode. + headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame(); + headParams.worldHeadOrientation = head->getFinalOrientationInWorldFrame(); + headParams.headEnabled = false; + } + } - // TODO: if hips target sensor is valid. - // Copy it into headParams.hipsMatrix, and set headParams.hipsEnabled to true. - - headParams.hipsEnabled = false; + auto avatarHipsPose = myAvatar->getHipsControllerPoseInAvatarFrame(); + if (avatarHipsPose.isValid()) { + glm::mat4 rigHipsMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation()); + headParams.hipsMatrix = rigHipsMat; + headParams.hipsEnabled = true; } else { headParams.hipsEnabled = false; - headParams.isInHMD = false; + } - // We don't have a valid localHeadPosition. - headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame(); - headParams.worldHeadOrientation = head->getFinalOrientationInWorldFrame(); + auto avatarSpine2Pose = myAvatar->getSpine2ControllerPoseInAvatarFrame(); + if (avatarSpine2Pose.isValid()) { + glm::mat4 rigSpine2Mat = Matrices::Y_180 * createMatFromQuatAndPos(avatarSpine2Pose.getRotation(), avatarSpine2Pose.getTranslation()); + headParams.spine2Matrix = rigSpine2Mat; + headParams.spine2Enabled = true; + } else { + headParams.spine2Enabled = false; } headParams.neckJointIndex = geometry.neckJointIndex; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 116758b1ba..9ef2a554c6 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1082,7 +1082,7 @@ void Rig::computeHeadNeckAnimVars(const AnimPose& hmdPose, glm::vec3& headPositi void Rig::updateNeckJoint(int index, const HeadParameters& params) { if (_animSkeleton && index >= 0 && index < _animSkeleton->getNumJoints()) { glm::quat yFlip180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)); - if (params.isInHMD) { + if (params.headEnabled) { glm::vec3 headPos, neckPos; glm::quat headRot, neckRot; diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 89f0d624f9..b66ca95042 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -42,12 +42,14 @@ public: }; struct HeadParameters { - glm::quat worldHeadOrientation = glm::quat(); // world space (-z forward) - glm::quat rigHeadOrientation = glm::quat(); // rig space (-z forward) - glm::vec3 rigHeadPosition = glm::vec3(); // rig space - glm::mat4 hipsMatrix = glm::mat4(); // rig space + glm::mat4 hipsMatrix = glm::mat4(); // rig space + glm::mat4 spine2Matrix = glm::mat4(); // rig space + glm::quat worldHeadOrientation = glm::quat(); // world space (-z forward) + glm::quat rigHeadOrientation = glm::quat(); // rig space (-z forward) + glm::vec3 rigHeadPosition = glm::vec3(); // rig space bool hipsEnabled = false; - bool isInHMD = false; + bool headEnabled = false; + bool spine2Enabled = false; int neckJointIndex = -1; bool isTalking = false; }; diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp index ec244553f8..db42fef8bc 100644 --- a/libraries/shared/src/GLMHelpers.cpp +++ b/libraries/shared/src/GLMHelpers.cpp @@ -38,6 +38,11 @@ const quat Quaternions::X_180{ 0.0f, 1.0f, 0.0f, 0.0f }; const quat Quaternions::Y_180{ 0.0f, 0.0f, 1.0f, 0.0f }; const quat Quaternions::Z_180{ 0.0f, 0.0f, 0.0f, 1.0f }; +const mat4 Matrices::IDENTITY { glm::mat4() }; +const mat4 Matrices::X_180 { createMatFromQuatAndPos(Quaternions::X_180, Vectors::ZERO) }; +const mat4 Matrices::Y_180 { createMatFromQuatAndPos(Quaternions::Y_180, Vectors::ZERO) }; +const mat4 Matrices::Z_180 { createMatFromQuatAndPos(Quaternions::Z_180, Vectors::ZERO) }; + // Safe version of glm::mix; based on the code in Nick Bobick's article, // http://www.gamasutra.com/features/19980703/quaternions_01.htm (via Clyde, // https://github.com/threerings/clyde/blob/master/src/main/java/com/threerings/math/Quaternion.java) diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h index deb87930fc..fd75fa416c 100644 --- a/libraries/shared/src/GLMHelpers.h +++ b/libraries/shared/src/GLMHelpers.h @@ -54,6 +54,13 @@ const glm::vec3 IDENTITY_FORWARD = glm::vec3( 0.0f, 0.0f,-1.0f); glm::quat safeMix(const glm::quat& q1, const glm::quat& q2, float alpha); +class Matrices { +public: + static const mat4 IDENTITY; + static const mat4 X_180; + static const mat4 Y_180; + static const mat4 Z_180; +}; class Quaternions { public: From 1931a8d1f6982b9264b650e7166905249799f709 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 25 Apr 2017 15:05:52 -0700 Subject: [PATCH 04/11] viveMotionCapture.js: can now disable puck control, with a second controller squeeze --- scripts/developer/tests/viveMotionCapture.js | 130 +++++++++---------- 1 file changed, 65 insertions(+), 65 deletions(-) diff --git a/scripts/developer/tests/viveMotionCapture.js b/scripts/developer/tests/viveMotionCapture.js index 6cb0f92b9b..5496b475be 100644 --- a/scripts/developer/tests/viveMotionCapture.js +++ b/scripts/developer/tests/viveMotionCapture.js @@ -8,7 +8,7 @@ var TRACKED_OBJECT_POSES = [ "TrackedObject12", "TrackedObject13", "TrackedObject14", "TrackedObject15" ]; -var calibrated = false; +var triggerPressHandled = false; var rightTriggerPressed = false; var leftTriggerPressed = false; @@ -43,21 +43,6 @@ var SENSOR_CONFIG_NAMES = [ "Auto" ]; -var ANIM_VARS = [ - "leftFootType", - "leftFootPosition", - "leftFootRotation", - "rightFootType", - "rightFootPosition", - "rightFootRotation", - "hipsType", - "hipsPosition", - "hipsRotation", - "spine2Type", - "spine2Position", - "spine2Rotation" -]; - var sensorConfig = AUTO; var Y_180 = {x: 0, y: 1, z: 0, w: 0}; @@ -86,7 +71,7 @@ function computeDefaultToReferenceXform() { return defaultToReferenceXform; } else { - return new Xform.ident(); + return Xform.ident(); } } @@ -200,71 +185,86 @@ function computeIKTargetXform(jointInfo) { function update(dt) { if (rightTriggerPressed && leftTriggerPressed) { - if (!calibrated) { - calibrate(); - calibrated = true; - + if (!triggerPressHandled) { + triggerPressHandled = true; if (handlerId) { - MyAvatar.removeAnimationStateHandler(handlerId); - } + print("AJT: UN-CALIBRATE!"); - handlerId = MyAvatar.addAnimationStateHandler(function (props) { - - var result = {}, xform; - if (rightFoot) { - xform = computeIKTargetXform(rightFoot); - result.rightFootType = ikTypes.RotationAndPosition; - result.rightFootPosition = xform.pos; - result.rightFootRotation = xform.rot; - } else { - result.rightFootType = props.rightFootType; - result.rightFootPosition = props.rightFootPosition; - result.rightFootRotation = props.rightFootRotation; + // go back to normal, vive pucks will be ignored. + leftFoot = undefined; + rightFoot = undefined; + hips = undefined; + spine2 = undefined; + if (handlerId) { + print("AJT: un-hooking animation state handler"); + MyAvatar.removeAnimationStateHandler(handlerId); + handlerId = undefined; } + } else { + print("AJT: CALIBRATE!"); + calibrate(); + + var animVars = []; if (leftFoot) { - xform = computeIKTargetXform(leftFoot); - result.leftFootType = ikTypes.RotationAndPosition; - result.leftFootPosition = xform.pos; - result.leftFootRotation = xform.rot; - } else { - result.leftFootType = props.leftFootType; - result.leftFootPosition = props.leftFootPosition; - result.leftFootRotation = props.leftFootRotation; + animVars.push("leftFootType"); + animVars.push("leftFootPosition"); + animVars.push("leftFootRotation"); + } + if (rightFoot) { + animVars.push("rightFootType"); + animVars.push("rightFootPosition"); + animVars.push("rightFootRotation"); } - if (hips) { - xform = computeIKTargetXform(hips); - result.hipsType = ikTypes.RotationAndPosition; - result.hipsPosition = xform.pos; - result.hipsRotation = xform.rot; - } else { - result.hipsType = props.hipsType; - result.hipsPosition = props.hipsPosition; - result.hipsRotation = props.hipsRotation; + animVars.push("hipsType"); + animVars.push("hipsPosition"); + animVars.push("hipsRotation"); } - if (spine2) { - xform = computeIKTargetXform(spine2); - result.spine2Type = ikTypes.RotationAndPosition; - result.spine2Position = xform.pos; - result.spine2Rotation = xform.rot; - } else { - result.spine2Type = ikTypes.Off; + animVars.push("spine2Type"); + animVars.push("spine2Position"); + animVars.push("spine2Rotation"); } - return result; - }, ANIM_VARS); - + // hook up new anim state handler that maps vive pucks to ik system. + handlerId = MyAvatar.addAnimationStateHandler(function (props) { + var result = {}, xform; + if (rightFoot) { + xform = computeIKTargetXform(rightFoot); + result.rightFootType = ikTypes.RotationAndPosition; + result.rightFootPosition = xform.pos; + result.rightFootRotation = xform.rot; + } + if (leftFoot) { + xform = computeIKTargetXform(leftFoot); + result.leftFootType = ikTypes.RotationAndPosition; + result.leftFootPosition = xform.pos; + result.leftFootRotation = xform.rot; + } + if (hips) { + xform = computeIKTargetXform(hips); + result.hipsType = ikTypes.RotationAndPosition; + result.hipsPosition = xform.pos; + result.hipsRotation = xform.rot; + } + if (spine2) { + xform = computeIKTargetXform(spine2); + result.spine2Type = ikTypes.RotationAndPosition; + result.spine2Position = xform.pos; + result.spine2Rotation = xform.rot; + } + return result; + }, animVars); + } } } else { - calibrated = false; + triggerPressHandled = false; } var drawMarkers = false; if (drawMarkers) { var RED = {x: 1, y: 0, z: 0, w: 1}; - var GREEN = {x: 0, y: 1, z: 0, w: 1}; var BLUE = {x: 0, y: 0, z: 1, w: 1}; if (leftFoot) { @@ -304,4 +304,4 @@ Script.scriptEnding.connect(function () { Controller.disableMapping(MAPPING_NAME); Script.update.disconnect(update); }); -var TRIGGER_OFF_VALUE = 0.1; + From 1fc9f4c93dc7b8654653667128ea8ac383875793 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 25 Apr 2017 17:22:47 -0700 Subject: [PATCH 05/11] Hips and Spine are routed thru input system properly --- libraries/animation/src/Rig.cpp | 22 +++++++++++++++---- .../src/controllers/StandardController.cpp | 3 +++ 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 9ef2a554c6..53f76d82ff 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1028,13 +1028,18 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { if (params.hipsEnabled) { _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition); _animVars.set("hipsPosition", extractTranslation(params.hipsMatrix)); - _animVars.set("hipsRotation", glmExtractRotation(params.hipsMatrix) * Quaternions::Y_180); + _animVars.set("hipsRotation", glmExtractRotation(params.hipsMatrix)); } else { _animVars.set("hipsType", (int)IKTarget::Type::Unknown); } - // by default this IK target is disabled. - _animVars.set("spine2Type", (int)IKTarget::Type::Unknown); + if (params.spine2Enabled) { + _animVars.set("spine2Type", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("spine2Position", extractTranslation(params.spine2Matrix)); + _animVars.set("spine2Rotation", glmExtractRotation(params.spine2Matrix)); + } else { + _animVars.set("spine2Type", (int)IKTarget::Type::Unknown); + } } void Rig::updateFromEyeParameters(const EyeParameters& params) { @@ -1105,7 +1110,16 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { _animVars.set("headPosition", headPos); _animVars.set("headRotation", headRot); - _animVars.set("headType", (int)IKTarget::Type::HmdHead); + + if (params.hipsEnabled) { + // Since there is an explicit hips ik target, switch the head to use the more generic RotationAndPosition IK chain type. + // this will allow the spine to bend more, ensuring that it can reach the head target position. + _animVars.set("headType", (int)IKTarget::Type::RotationAndPosition); + } else { + // When there is no hips IK target, use the HmdHead IK chain type. This will make the spine very stiff, + // but because the IK _hipsOffset is enabled, the hips will naturally follow underneath the head. + _animVars.set("headType", (int)IKTarget::Type::HmdHead); + } _animVars.set("neckPosition", neckPos); _animVars.set("neckRotation", neckRot); _animVars.set("neckType", (int)IKTarget::Type::Unknown); // 'Unknown' disables the target diff --git a/libraries/controllers/src/controllers/StandardController.cpp b/libraries/controllers/src/controllers/StandardController.cpp index cc90ee7b49..d8c98eb63b 100644 --- a/libraries/controllers/src/controllers/StandardController.cpp +++ b/libraries/controllers/src/controllers/StandardController.cpp @@ -104,6 +104,9 @@ Input::NamedVector StandardController::getAvailableInputs() const { makePair(RIGHT_HAND, "RightHand"), makePair(LEFT_FOOT, "LeftFoot"), makePair(RIGHT_FOOT, "RightFoot"), + makePair(HIPS, "Hips"), + makePair(SPINE2, "Spine2"), + makePair(HEAD, "Head"), // Aliases, PlayStation style names makePair(LB, "L1"), From 1b9e60944762b74981a621b5abb8573771a21aaa Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 25 Apr 2017 17:23:15 -0700 Subject: [PATCH 06/11] viveMotionCapture.js: updated to route pucks through input system Removed use of animationStateHandler() to set IK targets. --- scripts/developer/tests/viveMotionCapture.js | 119 +++++++++---------- 1 file changed, 58 insertions(+), 61 deletions(-) diff --git a/scripts/developer/tests/viveMotionCapture.js b/scripts/developer/tests/viveMotionCapture.js index 5496b475be..4aa5cbf86f 100644 --- a/scripts/developer/tests/viveMotionCapture.js +++ b/scripts/developer/tests/viveMotionCapture.js @@ -11,18 +11,20 @@ var TRACKED_OBJECT_POSES = [ var triggerPressHandled = false; var rightTriggerPressed = false; var leftTriggerPressed = false; +var calibrationCount = 0; -var MAPPING_NAME = "com.highfidelity.viveMotionCapture"; - -var mapping = Controller.newMapping(MAPPING_NAME); -mapping.from([Controller.Standard.RTClick]).peek().to(function (value) { +var TRIGGER_MAPPING_NAME = "com.highfidelity.viveMotionCapture.triggers"; +var triggerMapping = Controller.newMapping(TRIGGER_MAPPING_NAME); +triggerMapping.from([Controller.Standard.RTClick]).peek().to(function (value) { rightTriggerPressed = (value !== 0) ? true : false; }); -mapping.from([Controller.Standard.LTClick]).peek().to(function (value) { +triggerMapping.from([Controller.Standard.LTClick]).peek().to(function (value) { leftTriggerPressed = (value !== 0) ? true : false; }); +Controller.enableMapping(TRIGGER_MAPPING_NAME); -Controller.enableMapping(MAPPING_NAME); +var CONTROLLER_MAPPING_NAME = "com.highfidelity.viveMotionCapture.controller"; +var controllerMapping; var leftFoot; var rightFoot; @@ -92,7 +94,8 @@ function calibrate() { if (pose.valid) { poses.push({ channel: channel, - pose: pose + pose: pose, + lastestPose: pose }); } }); @@ -177,85 +180,76 @@ var ikTypes = { var handlerId; -function computeIKTargetXform(jointInfo) { - var pose = Controller.getPoseValue(jointInfo.channel); +function convertJointInfoToPose(jointInfo) { + var latestPose = jointInfo.latestPose; var offsetXform = jointInfo.offsetXform; - return Xform.mul(Y_180_XFORM, Xform.mul(new Xform(pose.rotation, pose.translation), offsetXform)); + var xform = Xform.mul(new Xform(latestPose.rotation, latestPose.translation), offsetXform); + return { + valid: true, + translation: xform.pos, + rotation: xform.rot, + velocity: Vec3.sum(latestPose.velocity, Vec3.cross(latestPose.angularVelocity, Vec3.subtract(xform.pos, latestPose.translation))), + angularVelocity: latestPose.angularVelocity + }; } function update(dt) { if (rightTriggerPressed && leftTriggerPressed) { if (!triggerPressHandled) { triggerPressHandled = true; - if (handlerId) { - print("AJT: UN-CALIBRATE!"); + if (controllerMapping) { // go back to normal, vive pucks will be ignored. + print("AJT: UN-CALIBRATE!"); + leftFoot = undefined; rightFoot = undefined; hips = undefined; spine2 = undefined; - if (handlerId) { - print("AJT: un-hooking animation state handler"); - MyAvatar.removeAnimationStateHandler(handlerId); - handlerId = undefined; - } + + Controller.disableMapping(CONTROLLER_MAPPING_NAME + calibrationCount); + controllerMapping = undefined; + } else { print("AJT: CALIBRATE!"); calibrate(); + calibrationCount++; - var animVars = []; + controllerMapping = Controller.newMapping(CONTROLLER_MAPPING_NAME + calibrationCount); if (leftFoot) { - animVars.push("leftFootType"); - animVars.push("leftFootPosition"); - animVars.push("leftFootRotation"); + controllerMapping.from(leftFoot.channel).to(function (pose) { + leftFoot.latestPose = pose; + }); + controllerMapping.from(function () { + return convertJointInfoToPose(leftFoot); + }).to(Controller.Standard.LeftFoot); } if (rightFoot) { - animVars.push("rightFootType"); - animVars.push("rightFootPosition"); - animVars.push("rightFootRotation"); + controllerMapping.from(rightFoot.channel).to(function (pose) { + rightFoot.latestPose = pose; + }); + controllerMapping.from(function () { + return convertJointInfoToPose(rightFoot); + }).to(Controller.Standard.RightFoot); } if (hips) { - animVars.push("hipsType"); - animVars.push("hipsPosition"); - animVars.push("hipsRotation"); + controllerMapping.from(hips.channel).to(function (pose) { + hips.latestPose = pose; + }); + controllerMapping.from(function () { + return convertJointInfoToPose(hips); + }).to(Controller.Standard.Hips); } if (spine2) { - animVars.push("spine2Type"); - animVars.push("spine2Position"); - animVars.push("spine2Rotation"); + controllerMapping.from(spine2.channel).to(function (pose) { + spine2.latestPose = pose; + }); + controllerMapping.from(function () { + return convertJointInfoToPose(spine2); + }).to(Controller.Standard.Spine2); } - - // hook up new anim state handler that maps vive pucks to ik system. - handlerId = MyAvatar.addAnimationStateHandler(function (props) { - var result = {}, xform; - if (rightFoot) { - xform = computeIKTargetXform(rightFoot); - result.rightFootType = ikTypes.RotationAndPosition; - result.rightFootPosition = xform.pos; - result.rightFootRotation = xform.rot; - } - if (leftFoot) { - xform = computeIKTargetXform(leftFoot); - result.leftFootType = ikTypes.RotationAndPosition; - result.leftFootPosition = xform.pos; - result.leftFootRotation = xform.rot; - } - if (hips) { - xform = computeIKTargetXform(hips); - result.hipsType = ikTypes.RotationAndPosition; - result.hipsPosition = xform.pos; - result.hipsRotation = xform.rot; - } - if (spine2) { - xform = computeIKTargetXform(spine2); - result.spine2Type = ikTypes.RotationAndPosition; - result.spine2Position = xform.pos; - result.spine2Rotation = xform.rot; - } - return result; - }, animVars); + Controller.enableMapping(CONTROLLER_MAPPING_NAME + calibrationCount); } } } else { @@ -301,7 +295,10 @@ function update(dt) { Script.update.connect(update); Script.scriptEnding.connect(function () { - Controller.disableMapping(MAPPING_NAME); + Controller.disableMapping(TRIGGER_MAPPING_NAME); + if (controllerMapping) { + Controller.disableMapping(CONTROLLER_MAPPING_NAME + calibrationCount); + } Script.update.disconnect(update); }); From ef43ff4b99565a9fb4d485b5ece1d47536a3331f Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 26 Apr 2017 11:55:24 -0700 Subject: [PATCH 07/11] Add default pose matrices to InputCalibrationData structure --- interface/src/Application.cpp | 8 +- interface/src/avatar/MyAvatar.cpp | 100 ++++++++++++++++-- interface/src/avatar/MyAvatar.h | 8 ++ libraries/controllers/src/controllers/Input.h | 12 ++- 4 files changed, 116 insertions(+), 12 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 5b014f7009..a9f882f73f 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4347,7 +4347,13 @@ void Application::update(float deltaTime) { controller::InputCalibrationData calibrationData = { myAvatar->getSensorToWorldMatrix(), createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()), - myAvatar->getHMDSensorMatrix() + myAvatar->getHMDSensorMatrix(), + myAvatar->getCenterEyeCalibrationMat(), + myAvatar->getHeadCalibrationMat(), + myAvatar->getSpine2CalibrationMat(), + myAvatar->getHipsCalibrationMat(), + myAvatar->getLeftFootCalibrationMat(), + myAvatar->getRightFootCalibrationMat() }; InputPluginPointer keyboardMousePlugin; diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 659077188f..9acbf14242 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -83,6 +83,22 @@ const float MyAvatar::ZOOM_MIN = 0.5f; const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; +// 2 meter tall dude (in avatar coordinates) +static const glm::vec3 DEFAULT_AVATAR_MIDDLE_EYE_POS { 0.0f, 0.9f, 0.0f }; +static const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 }; +static const glm::vec3 DEFAULT_AVATAR_HEAD_POS { 0.0f, 0.8f, 0.0f }; +static const glm::quat DEFAULT_AVATAR_HEAD_ROT { Quaternions::Y_180 }; +static const glm::vec3 DEFAULT_AVATAR_NECK_POS { 0.0f, 0.7f, 0.0f }; +static const glm::quat DEFAULT_AVATAR_NECK_ROT { Quaternions::Y_180 }; +static const glm::vec3 DEFAULT_AVATAR_SPINE2_POS { 0.0f, 0.5f, 0.0f }; +static const glm::quat DEFAULT_AVATAR_SPINE2_ROT { Quaternions::Y_180}; +static const glm::vec3 DEFAULT_AVATAR_HIPS_POS { 0.0f, 0.05f, 0.0f }; +static const glm::quat DEFAULT_AVATAR_HIPS_ROT { Quaternions::Y_180 }; +static const glm::vec3 DEFAULT_AVATAR_LEFTFOOT_POS { -0.1f, -0.9f, 0.0f }; // AJT: TODO: WRONG FIX ME +static const glm::quat DEFAULT_AVATAR_LEFTFOOT_ROT { Quaternions::IDENTITY }; // AJT: TODO: WRONG FIX ME +static const glm::vec3 DEFAULT_AVATAR_RIGHTFOOT_POS { 0.1f, -0.9f, 0.0f }; // AJT: TODO: WRONG FIX ME +static const glm::quat DEFAULT_AVATAR_RIGHTFOOT_ROT { Quaternions::IDENTITY }; // AJT: TODO: WRONG FIX ME + MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), _wasPushing(false), @@ -2283,22 +2299,17 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const { const glm::quat hmdOrientation = getHMDSensorOrientation(); const glm::quat hmdOrientationYawOnly = cancelOutRollAndPitch(hmdOrientation); - // 2 meter tall dude (in rig coordinates) - const glm::vec3 DEFAULT_RIG_MIDDLE_EYE_POS(0.0f, 0.9f, 0.0f); - const glm::vec3 DEFAULT_RIG_NECK_POS(0.0f, 0.70f, 0.0f); - const glm::vec3 DEFAULT_RIG_HIPS_POS(0.0f, 0.05f, 0.0f); - int rightEyeIndex = _rig->indexOfJoint("RightEye"); int leftEyeIndex = _rig->indexOfJoint("LeftEye"); int neckIndex = _rig->indexOfJoint("Neck"); int hipsIndex = _rig->indexOfJoint("Hips"); - glm::vec3 rigMiddleEyePos = DEFAULT_RIG_MIDDLE_EYE_POS; + glm::vec3 rigMiddleEyePos = DEFAULT_AVATAR_MIDDLE_EYE_POS; if (leftEyeIndex >= 0 && rightEyeIndex >= 0) { rigMiddleEyePos = (_rig->getAbsoluteDefaultPose(leftEyeIndex).trans() + _rig->getAbsoluteDefaultPose(rightEyeIndex).trans()) / 2.0f; } - glm::vec3 rigNeckPos = neckIndex != -1 ? _rig->getAbsoluteDefaultPose(neckIndex).trans() : DEFAULT_RIG_NECK_POS; - glm::vec3 rigHipsPos = hipsIndex != -1 ? _rig->getAbsoluteDefaultPose(hipsIndex).trans() : DEFAULT_RIG_HIPS_POS; + glm::vec3 rigNeckPos = neckIndex != -1 ? _rig->getAbsoluteDefaultPose(neckIndex).trans() : DEFAULT_AVATAR_NECK_POS; + glm::vec3 rigHipsPos = hipsIndex != -1 ? _rig->getAbsoluteDefaultPose(hipsIndex).trans() : DEFAULT_AVATAR_HIPS_POS; glm::vec3 localEyes = (rigMiddleEyePos - rigHipsPos); glm::vec3 localNeck = (rigNeckPos - rigHipsPos); @@ -2662,6 +2673,79 @@ glm::vec3 MyAvatar::getAbsoluteJointTranslationInObjectFrame(int index) const { } } +glm::mat4 MyAvatar::getCenterEyeCalibrationMat() const { + // TODO: as an optimization cache this computation, then invalidate the cache when the avatar model is changed. + int rightEyeIndex = _rig->indexOfJoint("RightEye"); + int leftEyeIndex = _rig->indexOfJoint("LeftEye"); + if (rightEyeIndex >= 0 && leftEyeIndex >= 0) { + auto centerEyePos = (getAbsoluteDefaultJointTranslationInObjectFrame(rightEyeIndex) + getAbsoluteDefaultJointTranslationInObjectFrame(leftEyeIndex)) * 0.5f; + auto centerEyeRot = Quaternions::Y_180; + return createMatFromQuatAndPos(centerEyeRot, centerEyePos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_MIDDLE_EYE_POS, DEFAULT_AVATAR_MIDDLE_EYE_POS); + } +} + +glm::mat4 MyAvatar::getHeadCalibrationMat() const { + // TODO: as an optimization cache this computation, then invalidate the cache when the avatar model is changed. + int headIndex = _rig->indexOfJoint("Head"); + if (headIndex >= 0) { + auto headPos = getAbsoluteDefaultJointTranslationInObjectFrame(headIndex); + auto headRot = getAbsoluteDefaultJointRotationInObjectFrame(headIndex); + return createMatFromQuatAndPos(headRot, headPos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_HEAD_POS, DEFAULT_AVATAR_HEAD_POS); + } +} + +glm::mat4 MyAvatar::getSpine2CalibrationMat() const { + // TODO: as an optimization cache this computation, then invalidate the cache when the avatar model is changed. + int spine2Index = _rig->indexOfJoint("Spine2"); + if (spine2Index >= 0) { + auto spine2Pos = getAbsoluteDefaultJointTranslationInObjectFrame(spine2Index); + auto spine2Rot = getAbsoluteDefaultJointRotationInObjectFrame(spine2Index); + return createMatFromQuatAndPos(spine2Rot, spine2Pos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_SPINE2_POS, DEFAULT_AVATAR_SPINE2_POS); + } +} + +glm::mat4 MyAvatar::getHipsCalibrationMat() const { + // TODO: as an optimization cache this computation, then invalidate the cache when the avatar model is changed. + int hipsIndex = _rig->indexOfJoint("Hips"); + if (hipsIndex >= 0) { + auto hipsPos = getAbsoluteDefaultJointTranslationInObjectFrame(hipsIndex); + auto hipsRot = getAbsoluteDefaultJointRotationInObjectFrame(hipsIndex); + return createMatFromQuatAndPos(hipsRot, hipsPos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_HIPS_POS, DEFAULT_AVATAR_HIPS_POS); + } +} + +glm::mat4 MyAvatar::getLeftFootCalibrationMat() const { + // TODO: as an optimization cache this computation, then invalidate the cache when the avatar model is changed. + int leftFootIndex = _rig->indexOfJoint("LeftFoot"); + if (leftFootIndex >= 0) { + auto leftFootPos = getAbsoluteDefaultJointTranslationInObjectFrame(leftFootIndex); + auto leftFootRot = getAbsoluteDefaultJointRotationInObjectFrame(leftFootIndex); + return createMatFromQuatAndPos(leftFootRot, leftFootPos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_LEFTFOOT_POS, DEFAULT_AVATAR_LEFTFOOT_POS); + } +} + +glm::mat4 MyAvatar::getRightFootCalibrationMat() const { + // TODO: as an optimization cache this computation, then invalidate the cache when the avatar model is changed. + int rightFootIndex = _rig->indexOfJoint("RightFoot"); + if (rightFootIndex >= 0) { + auto rightFootPos = getAbsoluteDefaultJointTranslationInObjectFrame(rightFootIndex); + auto rightFootRot = getAbsoluteDefaultJointRotationInObjectFrame(rightFootIndex); + return createMatFromQuatAndPos(rightFootRot, rightFootPos); + } else { + return createMatFromQuatAndPos(DEFAULT_AVATAR_RIGHTFOOT_POS, DEFAULT_AVATAR_RIGHTFOOT_POS); + } +} + bool MyAvatar::pinJoint(int index, const glm::vec3& position, const glm::quat& orientation) { auto hipsIndex = getJointIndex("Hips"); if (index != hipsIndex) { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index cea5962638..01b6496e12 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -473,6 +473,14 @@ public: virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override; virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override; + // all calibration matrices are in absolute avatar space. + glm::mat4 getCenterEyeCalibrationMat() const; + glm::mat4 getHeadCalibrationMat() const; + glm::mat4 getSpine2CalibrationMat() const; + glm::mat4 getHipsCalibrationMat() const; + glm::mat4 getLeftFootCalibrationMat() const; + glm::mat4 getRightFootCalibrationMat() const; + void addHoldAction(AvatarActionHold* holdAction); // thread-safe void removeHoldAction(AvatarActionHold* holdAction); // thread-safe void updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose& postUpdatePose); diff --git a/libraries/controllers/src/controllers/Input.h b/libraries/controllers/src/controllers/Input.h index 9c7f09d526..65c78cd6ea 100644 --- a/libraries/controllers/src/controllers/Input.h +++ b/libraries/controllers/src/controllers/Input.h @@ -16,9 +16,15 @@ namespace controller { struct InputCalibrationData { - glm::mat4 sensorToWorldMat; - glm::mat4 avatarMat; - glm::mat4 hmdSensorMat; + glm::mat4 sensorToWorldMat; // sensor to world + glm::mat4 avatarMat; // avatar to world + glm::mat4 hmdSensorMat; // hmd pos and orientation in sensor space + glm::mat4 defaultCenterEyeMat; // default pose for the center of the eyes in avatar space. + glm::mat4 defaultHeadMat; // default pose for head joint in avatar space + glm::mat4 defaultSpine2; // default pose for spine2 joint in avatar space + glm::mat4 defaultHips; // default pose for hips joint in avatar space + glm::mat4 defaultLeftFoot; // default pose for leftFoot joint in avatar space + glm::mat4 defaultRightFoot; // default pose for leftFoot joint in avatar space }; enum class ChannelType { From 764f6c69eaa4dbed7a06f0104b93864bb28c9367 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 27 Apr 2017 17:39:25 -0700 Subject: [PATCH 08/11] Head input action will override the HMD for IK. --- interface/src/avatar/MyAvatar.cpp | 34 +++++----- interface/src/avatar/MyAvatar.h | 2 +- interface/src/avatar/SkeletonModel.cpp | 37 +++++----- libraries/animation/src/Rig.cpp | 71 ++++---------------- libraries/animation/src/Rig.h | 13 ++-- scripts/developer/tests/viveMotionCapture.js | 44 ++++++++++++ 6 files changed, 97 insertions(+), 104 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 9acbf14242..70d650e7a5 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -83,21 +83,21 @@ const float MyAvatar::ZOOM_MIN = 0.5f; const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; -// 2 meter tall dude (in avatar coordinates) -static const glm::vec3 DEFAULT_AVATAR_MIDDLE_EYE_POS { 0.0f, 0.9f, 0.0f }; +// default values, used when avatar is missing joints... (avatar space) static const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 }; -static const glm::vec3 DEFAULT_AVATAR_HEAD_POS { 0.0f, 0.8f, 0.0f }; +static const glm::vec3 DEFAULT_AVATAR_MIDDLE_EYE_POS { 0.0f, 0.6f, 0.0f }; static const glm::quat DEFAULT_AVATAR_HEAD_ROT { Quaternions::Y_180 }; -static const glm::vec3 DEFAULT_AVATAR_NECK_POS { 0.0f, 0.7f, 0.0f }; +static const glm::vec3 DEFAULT_AVATAR_HEAD_POS { 0.0f, 0.53f, 0.0f }; +static const glm::vec3 DEFAULT_AVATAR_NECK_POS { 0.0f, 0.445f, 0.025f }; static const glm::quat DEFAULT_AVATAR_NECK_ROT { Quaternions::Y_180 }; -static const glm::vec3 DEFAULT_AVATAR_SPINE2_POS { 0.0f, 0.5f, 0.0f }; +static const glm::vec3 DEFAULT_AVATAR_SPINE2_POS { 0.0f, 0.32f, 0.02f }; static const glm::quat DEFAULT_AVATAR_SPINE2_ROT { Quaternions::Y_180}; -static const glm::vec3 DEFAULT_AVATAR_HIPS_POS { 0.0f, 0.05f, 0.0f }; +static const glm::vec3 DEFAULT_AVATAR_HIPS_POS { 0.0f, 0.0f, 0.0f }; static const glm::quat DEFAULT_AVATAR_HIPS_ROT { Quaternions::Y_180 }; -static const glm::vec3 DEFAULT_AVATAR_LEFTFOOT_POS { -0.1f, -0.9f, 0.0f }; // AJT: TODO: WRONG FIX ME -static const glm::quat DEFAULT_AVATAR_LEFTFOOT_ROT { Quaternions::IDENTITY }; // AJT: TODO: WRONG FIX ME -static const glm::vec3 DEFAULT_AVATAR_RIGHTFOOT_POS { 0.1f, -0.9f, 0.0f }; // AJT: TODO: WRONG FIX ME -static const glm::quat DEFAULT_AVATAR_RIGHTFOOT_ROT { Quaternions::IDENTITY }; // AJT: TODO: WRONG FIX ME +static const glm::vec3 DEFAULT_AVATAR_LEFTFOOT_POS { -0.08f, -0.96f, 0.029f}; +static const glm::quat DEFAULT_AVATAR_LEFTFOOT_ROT { -0.40167322754859924f, 0.9154590368270874f, -0.005437685176730156f, -0.023744143545627594f }; +static const glm::vec3 DEFAULT_AVATAR_RIGHTFOOT_POS { 0.08f, -0.96f, 0.029f }; +static const glm::quat DEFAULT_AVATAR_RIGHTFOOT_ROT { -0.4016716778278351f, 0.9154615998268127f, 0.0053307069465518f, 0.023696165531873703f }; MyAvatar::MyAvatar(RigPointer rig) : Avatar(rig), @@ -1433,14 +1433,12 @@ controller::Pose MyAvatar::getSpine2ControllerPoseInAvatarFrame() const { return getSpine2ControllerPoseInWorldFrame().transform(invAvatarMatrix); } -void MyAvatar::setHeadControllerPoseInSensorFrame(const controller::Pose& headPose) { - bool inHmd = qApp->isHMDMode(); - Head* head = getHead(); - if (inHmd) { - _headControllerPoseInSensorFrameCache.set(headPose); - head->setDeltaPitch(headPose.rotation.x); - head->setDeltaYaw(headPose.rotation.y); - head->setDeltaRoll(headPose.rotation.z); +void MyAvatar::setHeadControllerPoseInSensorFrame(const controller::Pose& head) { + if (controller::InputDevice::getLowVelocityFilter()) { + auto oldHeadPose = getHeadControllerPoseInSensorFrame(); + _headControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldHeadPose, head)); + } else { + _headControllerPoseInSensorFrameCache.set(head); } } diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 01b6496e12..dc249e0411 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -460,7 +460,7 @@ public: controller::Pose getHipsControllerPoseInAvatarFrame() const; controller::Pose getSpine2ControllerPoseInAvatarFrame() const; - void setHeadControllerPoseInSensorFrame(const controller::Pose& headPose); + void setHeadControllerPoseInSensorFrame(const controller::Pose& head); controller::Pose getHeadControllerPoseInSensorFrame() const; controller::Pose getHeadControllerPoseInWorldFrame() const; controller::Pose getHeadControllerPoseInAvatarFrame() const; diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index ccee65d3e6..9fad9e09fb 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -107,29 +107,27 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Rig::HeadParameters headParams; - 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 rigToWorld = createMatFromQuatAndPos(getRotation(), getTranslation()); - glm::mat4 worldToRig = glm::inverse(rigToWorld); - glm::mat4 rigHMDMat = worldToRig * worldHMDMat; - - headParams.rigHeadPosition = extractTranslation(rigHMDMat); - headParams.rigHeadOrientation = extractRotation(rigHMDMat); - headParams.worldHeadOrientation = extractRotation(worldHMDMat); + // input action is the highest priority source for head orientation. + auto avatarHeadPose = myAvatar->getHeadControllerPoseInAvatarFrame(); + if (avatarHeadPose.isValid()) { + glm::mat4 rigHeadMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation()); + headParams.rigHeadPosition = extractTranslation(rigHeadMat); + headParams.rigHeadOrientation = glmExtractRotation(rigHeadMat); headParams.headEnabled = true; } else { - auto avatarHeadPose = myAvatar->getHeadControllerPoseInAvatarFrame(); - if (avatarHeadPose.isValid()) { - glm::mat4 rigHeadMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation()); - headParams.rigHeadPosition = extractTranslation(rigHeadMat); - headParams.rigHeadOrientation = glmExtractRotation(rigHeadMat); - headParams.worldHeadOrientation = myAvatar->getHeadControllerPoseInWorldFrame().getTranslation(); + 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 rigToWorld = createMatFromQuatAndPos(getRotation(), getTranslation()); + glm::mat4 worldToRig = glm::inverse(rigToWorld); + glm::mat4 rigHMDMat = worldToRig * worldHMDMat; + _rig->computeHeadFromHMD(AnimPose(rigHMDMat), headParams.rigHeadPosition, headParams.rigHeadOrientation); headParams.headEnabled = true; } else { // even though full head IK is disabled, the rig still needs the head orientation to rotate the head up and down in desktop mode. - headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame(); - headParams.worldHeadOrientation = head->getFinalOrientationInWorldFrame(); + // preMult 180 is necessary to convert from avatar to rig coordinates. + // postMult 180 is necessary to convert head from -z forward to z forward. + headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180; headParams.headEnabled = false; } } @@ -152,7 +150,6 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { headParams.spine2Enabled = false; } - headParams.neckJointIndex = geometry.neckJointIndex; headParams.isTalking = head->getTimeWithoutTalking() <= 1.5f; _rig->updateFromHeadParameters(headParams, deltaTime); @@ -212,7 +209,6 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { Model::updateRig(deltaTime, parentTransform); Rig::EyeParameters eyeParams; - eyeParams.worldHeadOrientation = headParams.worldHeadOrientation; eyeParams.eyeLookAt = lookAt; eyeParams.eyeSaccade = head->getSaccade(); eyeParams.modelRotation = getRotation(); @@ -244,7 +240,6 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { head->setBaseRoll(glm::degrees(-eulers.z)); Rig::EyeParameters eyeParams; - eyeParams.worldHeadOrientation = head->getFinalOrientationInWorldFrame(); eyeParams.eyeLookAt = lookAt; eyeParams.eyeSaccade = glm::vec3(0.0f); eyeParams.modelRotation = getRotation(); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 53f76d82ff..700761b248 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -46,7 +46,6 @@ static bool isEqual(const glm::quat& p, const glm::quat& q) { const glm::vec3 DEFAULT_RIGHT_EYE_POS(-0.3f, 0.9f, 0.0f); const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 0.9f, 0.0f); const glm::vec3 DEFAULT_HEAD_POS(0.0f, 0.75f, 0.0f); -const glm::vec3 DEFAULT_NECK_POS(0.0f, 0.70f, 0.0f); void Rig::overrideAnimation(const QString& url, float fps, bool loop, float firstFrame, float lastFrame) { @@ -1020,7 +1019,7 @@ glm::quat Rig::getJointDefaultRotationInParentFrame(int jointIndex) { } void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { - updateNeckJoint(params.neckJointIndex, params); + updateHeadAnimVars(params); _animVars.set("isTalking", params.isTalking); _animVars.set("notIsTalking", !params.isTalking); @@ -1043,74 +1042,40 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { } void Rig::updateFromEyeParameters(const EyeParameters& params) { - updateEyeJoint(params.leftEyeJointIndex, params.modelTranslation, params.modelRotation, - params.worldHeadOrientation, params.eyeLookAt, params.eyeSaccade); - updateEyeJoint(params.rightEyeJointIndex, params.modelTranslation, params.modelRotation, - params.worldHeadOrientation, params.eyeLookAt, params.eyeSaccade); + updateEyeJoint(params.leftEyeJointIndex, params.modelTranslation, params.modelRotation, params.eyeLookAt, params.eyeSaccade); + updateEyeJoint(params.rightEyeJointIndex, params.modelTranslation, params.modelRotation, params.eyeLookAt, params.eyeSaccade); } -void Rig::computeHeadNeckAnimVars(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut, - glm::vec3& neckPositionOut, glm::quat& neckOrientationOut) const { +void Rig::computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut) const { // the input hmd values are in avatar/rig space const glm::vec3& hmdPosition = hmdPose.trans(); - const glm::quat& hmdOrientation = hmdPose.rot(); + + // the HMD looks down the negative z axis, but the head bone looks down the z axis, so apply a 180 degree rotation. + const glm::quat& hmdOrientation = hmdPose.rot() * Quaternions::Y_180; // TODO: cache jointIndices int rightEyeIndex = indexOfJoint("RightEye"); int leftEyeIndex = indexOfJoint("LeftEye"); int headIndex = indexOfJoint("Head"); - int neckIndex = indexOfJoint("Neck"); glm::vec3 absRightEyePos = rightEyeIndex != -1 ? getAbsoluteDefaultPose(rightEyeIndex).trans() : DEFAULT_RIGHT_EYE_POS; glm::vec3 absLeftEyePos = leftEyeIndex != -1 ? getAbsoluteDefaultPose(leftEyeIndex).trans() : DEFAULT_LEFT_EYE_POS; glm::vec3 absHeadPos = headIndex != -1 ? getAbsoluteDefaultPose(headIndex).trans() : DEFAULT_HEAD_POS; - glm::vec3 absNeckPos = neckIndex != -1 ? getAbsoluteDefaultPose(neckIndex).trans() : DEFAULT_NECK_POS; glm::vec3 absCenterEyePos = (absRightEyePos + absLeftEyePos) / 2.0f; glm::vec3 eyeOffset = absCenterEyePos - absHeadPos; - glm::vec3 headOffset = absHeadPos - absNeckPos; - // apply simplistic head/neck model - - // head headPositionOut = hmdPosition - hmdOrientation * eyeOffset; + headOrientationOut = hmdOrientation; - - // neck - neckPositionOut = hmdPosition - hmdOrientation * (headOffset + eyeOffset); - - // slerp between default orientation and hmdOrientation - neckOrientationOut = safeMix(hmdOrientation, _animSkeleton->getRelativeDefaultPose(neckIndex).rot(), 0.5f); } -void Rig::updateNeckJoint(int index, const HeadParameters& params) { - if (_animSkeleton && index >= 0 && index < _animSkeleton->getNumJoints()) { - glm::quat yFlip180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)); +void Rig::updateHeadAnimVars(const HeadParameters& params) { + if (_animSkeleton) { if (params.headEnabled) { - glm::vec3 headPos, neckPos; - glm::quat headRot, neckRot; - - AnimPose hmdPose(glm::vec3(1.0f), params.rigHeadOrientation * yFlip180, params.rigHeadPosition); - computeHeadNeckAnimVars(hmdPose, headPos, headRot, neckPos, neckRot); - - // debug rendering -#ifdef DEBUG_RENDERING - const glm::vec4 red(1.0f, 0.0f, 0.0f, 1.0f); - const glm::vec4 green(0.0f, 1.0f, 0.0f, 1.0f); - - // transform from bone into avatar space - AnimPose headPose(glm::vec3(1), headRot, headPos); - DebugDraw::getInstance().addMyAvatarMarker("headTarget", headPose.rot, headPose.trans, red); - - // transform from bone into avatar space - AnimPose neckPose(glm::vec3(1), neckRot, neckPos); - DebugDraw::getInstance().addMyAvatarMarker("neckTarget", neckPose.rot, neckPose.trans, green); -#endif - - _animVars.set("headPosition", headPos); - _animVars.set("headRotation", headRot); - + _animVars.set("headPosition", params.rigHeadPosition); + _animVars.set("headRotation", params.rigHeadOrientation); if (params.hipsEnabled) { // Since there is an explicit hips ik target, switch the head to use the more generic RotationAndPosition IK chain type. // this will allow the spine to bend more, ensuring that it can reach the head target position. @@ -1120,23 +1085,15 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { // but because the IK _hipsOffset is enabled, the hips will naturally follow underneath the head. _animVars.set("headType", (int)IKTarget::Type::HmdHead); } - _animVars.set("neckPosition", neckPos); - _animVars.set("neckRotation", neckRot); - _animVars.set("neckType", (int)IKTarget::Type::Unknown); // 'Unknown' disables the target - } else { _animVars.unset("headPosition"); - _animVars.set("headRotation", params.rigHeadOrientation * yFlip180); - _animVars.set("headAndNeckType", (int)IKTarget::Type::RotationOnly); + _animVars.set("headRotation", params.rigHeadOrientation); _animVars.set("headType", (int)IKTarget::Type::RotationOnly); - _animVars.unset("neckPosition"); - _animVars.unset("neckRotation"); - _animVars.set("neckType", (int)IKTarget::Type::RotationOnly); } } } -void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::quat& worldHeadOrientation, const glm::vec3& lookAtSpot, const glm::vec3& saccade) { +void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAtSpot, const glm::vec3& saccade) { // TODO: does not properly handle avatar scale. diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index b66ca95042..2d024628f5 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -44,18 +44,15 @@ public: struct HeadParameters { glm::mat4 hipsMatrix = glm::mat4(); // rig space glm::mat4 spine2Matrix = glm::mat4(); // rig space - glm::quat worldHeadOrientation = glm::quat(); // world space (-z forward) glm::quat rigHeadOrientation = glm::quat(); // rig space (-z forward) glm::vec3 rigHeadPosition = glm::vec3(); // rig space bool hipsEnabled = false; bool headEnabled = false; bool spine2Enabled = false; - int neckJointIndex = -1; bool isTalking = false; }; struct EyeParameters { - glm::quat worldHeadOrientation = glm::quat(); glm::vec3 eyeLookAt = glm::vec3(); // world space glm::vec3 eyeSaccade = glm::vec3(); // world space glm::vec3 modelTranslation = glm::vec3(); @@ -232,6 +229,9 @@ public: void setEnableDebugDrawIKTargets(bool enableDebugDrawIKTargets) { _enableDebugDrawIKTargets = enableDebugDrawIKTargets; } + // input assumed to be in rig space + void computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut) const; + signals: void onLoadComplete(); @@ -241,10 +241,9 @@ protected: void applyOverridePoses(); void buildAbsoluteRigPoses(const AnimPoseVec& relativePoses, AnimPoseVec& absolutePosesOut); - void updateNeckJoint(int index, const HeadParameters& params); - void computeHeadNeckAnimVars(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut, - glm::vec3& neckPositionOut, glm::quat& neckOrientationOut) const; - void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::quat& worldHeadOrientation, const glm::vec3& lookAt, const glm::vec3& saccade); + void updateHeadAnimVars(const HeadParameters& params); + + void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; AnimPose _modelOffset; // model to rig space diff --git a/scripts/developer/tests/viveMotionCapture.js b/scripts/developer/tests/viveMotionCapture.js index 4aa5cbf86f..e7fb8566dc 100644 --- a/scripts/developer/tests/viveMotionCapture.js +++ b/scripts/developer/tests/viveMotionCapture.js @@ -26,6 +26,7 @@ Controller.enableMapping(TRIGGER_MAPPING_NAME); var CONTROLLER_MAPPING_NAME = "com.highfidelity.viveMotionCapture.controller"; var controllerMapping; +var head; var leftFoot; var rightFoot; var hips; @@ -77,8 +78,29 @@ function computeDefaultToReferenceXform() { } } +function computeHeadOffsetXform() { + var leftEyeIndex = MyAvatar.getJointIndex("LeftEye"); + var rightEyeIndex = MyAvatar.getJointIndex("RightEye"); + var headIndex = MyAvatar.getJointIndex("Head"); + if (leftEyeIndex > 0 && rightEyeIndex > 0 && headIndex > 0) { + var defaultHeadXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(headIndex), + MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(headIndex)); + var defaultLeftEyeXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(leftEyeIndex), + MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(leftEyeIndex)); + var defaultRightEyeXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(rightEyeIndex), + MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(rightEyeIndex)); + var defaultCenterEyePos = Vec3.multiply(0.5, Vec3.sum(defaultLeftEyeXform.pos, defaultRightEyeXform.pos)); + var defaultCenterEyeXform = new Xform(defaultLeftEyeXform.rot, defaultCenterEyePos); + + return Xform.mul(defaultCenterEyeXform.inv(), defaultHeadXform); + } else { + return undefined; + } +} + function calibrate() { + head = undefined; leftFoot = undefined; rightFoot = undefined; hips = undefined; @@ -86,6 +108,13 @@ function calibrate() { var defaultToReferenceXform = computeDefaultToReferenceXform(); + var headOffsetXform = computeHeadOffsetXform(); + print("AJT: computed headOffsetXform " + (headOffsetXform ? JSON.stringify(headOffsetXform) : "undefined")); + + if (headOffsetXform) { + head = { offsetXform: headOffsetXform }; + } + var poses = []; if (Controller.Hardware.Vive) { TRACKED_OBJECT_POSES.forEach(function (key) { @@ -202,6 +231,7 @@ function update(dt) { // go back to normal, vive pucks will be ignored. print("AJT: UN-CALIBRATE!"); + head = undefined; leftFoot = undefined; rightFoot = undefined; hips = undefined; @@ -217,6 +247,20 @@ function update(dt) { controllerMapping = Controller.newMapping(CONTROLLER_MAPPING_NAME + calibrationCount); + if (head) { + controllerMapping.from(function () { + var worldToAvatarXform = (new Xform(MyAvatar.orientation, MyAvatar.position)).inv(); + head.latestPose = { + valid: true, + translation: worldToAvatarXform.xformPoint(HMD.position), + rotation: Quat.multiply(worldToAvatarXform.rot, Quat.multiply(HMD.orientation, Y_180)), // postMult 180 rot flips head direction + velocity: {x: 0, y: 0, z: 0}, // TODO: currently this is unused anyway... + angularVelocity: {x: 0, y: 0, z: 0} + }; + return convertJointInfoToPose(head); + }).to(Controller.Standard.Head); + } + if (leftFoot) { controllerMapping.from(leftFoot.channel).to(function (pose) { leftFoot.latestPose = pose; From f3cc1c2d86d7bcb5e568ac3dca7bedd6544ab906 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 28 Apr 2017 11:30:06 -0700 Subject: [PATCH 09/11] Added hipsContorllerTest.js for QA Testing --- scripts/developer/tests/hipsControllerTest.js | 105 ++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 scripts/developer/tests/hipsControllerTest.js diff --git a/scripts/developer/tests/hipsControllerTest.js b/scripts/developer/tests/hipsControllerTest.js new file mode 100644 index 0000000000..5c6a4811e5 --- /dev/null +++ b/scripts/developer/tests/hipsControllerTest.js @@ -0,0 +1,105 @@ +// +// hipsControllerTest.js +// +// Created by Anthony Thibault on 4/24/17 +// Copyright 2017 High Fidelity, Inc. +// +// Test procedural manipulation of the Avatar hips via the controller system. +// Pull the left and right triggers on your hand controllers, you hips should begin to gyrate in an amusing mannor. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html + +/* global Xform */ +Script.include("/~/system/libraries/Xform.js"); + +var triggerPressHandled = false; +var rightTriggerPressed = false; +var leftTriggerPressed = false; + +var MAPPING_NAME = "com.highfidelity.hipsIkTest"; + +var mapping = Controller.newMapping(MAPPING_NAME); +mapping.from([Controller.Standard.RTClick]).peek().to(function (value) { + rightTriggerPressed = (value !== 0) ? true : false; +}); +mapping.from([Controller.Standard.LTClick]).peek().to(function (value) { + leftTriggerPressed = (value !== 0) ? true : false; +}); + +Controller.enableMapping(MAPPING_NAME); + +var CONTROLLER_MAPPING_NAME = "com.highfidelity.hipsIkTest.controller"; +var controllerMapping; + +var ZERO = {x: 0, y: 0, z: 0}; +var X_AXIS = {x: 1, y: 0, z: 0}; +var Y_AXIS = {x: 0, y: 1, z: 0}; +var Y_180 = {x: 0, y: 1, z: 0, w: 0}; +var Y_180_XFORM = new Xform(Y_180, {x: 0, y: 0, z: 0}); + +var hips = undefined; + +function computeCurrentXform(jointIndex) { + var currentXform = new Xform(MyAvatar.getAbsoluteJointRotationInObjectFrame(jointIndex), + MyAvatar.getAbsoluteJointTranslationInObjectFrame(jointIndex)); + return currentXform; +} + +function calibrate() { + hips = computeCurrentXform(MyAvatar.getJointIndex("Hips")); +} + +function circleOffset(radius, theta, normal) { + var pos = {x: radius * Math.cos(theta), y: radius * Math.sin(theta), z: 0}; + var lookAtRot = Quat.lookAt(normal, ZERO, X_AXIS); + return Vec3.multiplyQbyV(lookAtRot, pos); +} + +var calibrationCount = 0; + +function update(dt) { + if (rightTriggerPressed && leftTriggerPressed) { + if (!triggerPressHandled) { + triggerPressHandled = true; + if (controllerMapping) { + hips = undefined; + Controller.disableMapping(CONTROLLER_MAPPING_NAME + calibrationCount); + controllerMapping = undefined; + } else { + calibrate(); + calibrationCount++; + controllerMapping = Controller.newMapping(CONTROLLER_MAPPING_NAME + calibrationCount); + + var n = Y_AXIS; + var t = 0; + if (hips) { + controllerMapping.from(function () { + t += (1 / 60) * 4; + return { + valid: true, + translation: Vec3.sum(hips.pos, circleOffset(0.1, t, n)), + rotation: hips.rot, + velocity: ZERO, + angularVelocity: ZERO + }; + }).to(Controller.Standard.Hips); + } + Controller.enableMapping(CONTROLLER_MAPPING_NAME + calibrationCount); + } + } + } else { + triggerPressHandled = false; + } +} + +Script.update.connect(update); + +Script.scriptEnding.connect(function () { + Controller.disableMapping(MAPPING_NAME); + if (controllerMapping) { + Controller.disableMapping(CONTROLLER_MAPPING_NAME + calibrationCount); + } + Script.update.disconnect(update); +}); + From 58de67ba119268b45280b4a0fad83b91183e2d9b Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 28 Apr 2017 14:12:00 -0700 Subject: [PATCH 10/11] Fix tests/controllers/src/main.cpp clang/gcc warnings --- tests/controllers/src/main.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/tests/controllers/src/main.cpp b/tests/controllers/src/main.cpp index 2c8f361fac..e697bd501f 100644 --- a/tests/controllers/src/main.cpp +++ b/tests/controllers/src/main.cpp @@ -114,6 +114,12 @@ int main(int argc, char** argv) { last = now; InputCalibrationData calibrationData = { + glm::mat4(), + glm::mat4(), + glm::mat4(), + glm::mat4(), + glm::mat4(), + glm::mat4(), glm::mat4(), glm::mat4(), glm::mat4() @@ -130,6 +136,12 @@ int main(int argc, char** argv) { { InputCalibrationData calibrationData = { + glm::mat4(), + glm::mat4(), + glm::mat4(), + glm::mat4(), + glm::mat4(), + glm::mat4(), glm::mat4(), glm::mat4(), glm::mat4() From 0c09823f6731e15e50c5bf361144ea120fb94d20 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 1 May 2017 09:10:31 -0700 Subject: [PATCH 11/11] clang warning fixes, unused variables... --- interface/src/avatar/MyAvatar.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 05e5277746..2966f8a95d 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -83,16 +83,12 @@ const float MyAvatar::ZOOM_MAX = 25.0f; const float MyAvatar::ZOOM_DEFAULT = 1.5f; // default values, used when avatar is missing joints... (avatar space) -static const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 }; +// static const glm::quat DEFAULT_AVATAR_MIDDLE_EYE_ROT { Quaternions::Y_180 }; static const glm::vec3 DEFAULT_AVATAR_MIDDLE_EYE_POS { 0.0f, 0.6f, 0.0f }; -static const glm::quat DEFAULT_AVATAR_HEAD_ROT { Quaternions::Y_180 }; static const glm::vec3 DEFAULT_AVATAR_HEAD_POS { 0.0f, 0.53f, 0.0f }; static const glm::vec3 DEFAULT_AVATAR_NECK_POS { 0.0f, 0.445f, 0.025f }; -static const glm::quat DEFAULT_AVATAR_NECK_ROT { Quaternions::Y_180 }; static const glm::vec3 DEFAULT_AVATAR_SPINE2_POS { 0.0f, 0.32f, 0.02f }; -static const glm::quat DEFAULT_AVATAR_SPINE2_ROT { Quaternions::Y_180}; static const glm::vec3 DEFAULT_AVATAR_HIPS_POS { 0.0f, 0.0f, 0.0f }; -static const glm::quat DEFAULT_AVATAR_HIPS_ROT { Quaternions::Y_180 }; static const glm::vec3 DEFAULT_AVATAR_LEFTFOOT_POS { -0.08f, -0.96f, 0.029f}; static const glm::quat DEFAULT_AVATAR_LEFTFOOT_ROT { -0.40167322754859924f, 0.9154590368270874f, -0.005437685176730156f, -0.023744143545627594f }; static const glm::vec3 DEFAULT_AVATAR_RIGHTFOOT_POS { 0.08f, -0.96f, 0.029f };