Merge pull request #10319 from hyperlogic/feature/head-standard-action

Head, Hips and Chest IK can now be controlled from Controller inputs

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" }
     ]
 }

interface/src/Application.cpp

@@ -4355,7 +4355,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;
@@ -4403,6 +4409,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
 

interface/src/avatar/MyAvatar.cpp

@@ -82,6 +82,18 @@ const float MyAvatar::ZOOM_MIN = 0.5f;
 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::vec3 DEFAULT_AVATAR_MIDDLE_EYE_POS { 0.0f, 0.6f, 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::vec3 DEFAULT_AVATAR_SPINE2_POS { 0.0f, 0.32f, 0.02f };
+static const glm::vec3 DEFAULT_AVATAR_HIPS_POS { 0.0f, 0.0f, 0.0f };
+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(QThread* thread, RigPointer rig) :
     Avatar(thread, rig),
     _wasPushing(false),
@@ -655,13 +667,8 @@ void MyAvatar::updateFromTrackers(float deltaTime) {
     if (inHmd) {
         estimatedPosition = extractTranslation(getHMDSensorMatrix());
         estimatedPosition.x *= -1.0f;
-        _trackedHeadPosition = estimatedPosition;
-
-        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()));
     }
 
@@ -1378,6 +1385,65 @@ 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& head) {
+    if (controller::InputDevice::getLowVelocityFilter()) {
+        auto oldHeadPose = getHeadControllerPoseInSensorFrame();
+        _headControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldHeadPose, head));
+    } else {
+        _headControllerPoseInSensorFrameCache.set(head);
+    }
+}
+
+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();
@@ -2220,22 +2286,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 rigNeckPos = neckIndex != -1 ? _rig->getAbsoluteDefaultPose(neckIndex).trans() : DEFAULT_AVATAR_NECK_POS;
-    glm::vec3 rigHipsPos = hipsIndex != -1 ? _rig->getAbsoluteDefaultPose(hipsIndex).trans() : DEFAULT_RIG_HIPS_POS;
+    glm::vec3 rigHipsPos = hipsIndex != -1 ? _rig->getAbsoluteDefaultPose(hipsIndex).trans() : DEFAULT_AVATAR_HIPS_POS;
 
     glm::vec3 localEyes = (rigMiddleEyePos - rigHipsPos);
     glm::vec3 localNeck = (rigNeckPos - rigHipsPos);
@@ -2599,6 +2660,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) {

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(); }
@@ -453,6 +452,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& head);
+    controller::Pose getHeadControllerPoseInSensorFrame() const;
+    controller::Pose getHeadControllerPoseInWorldFrame() const;
+    controller::Pose getHeadControllerPoseInAvatarFrame() const;
+
     bool hasDriveInput() const;
 
     Q_INVOKABLE void setCharacterControllerEnabled(bool enabled);
@@ -461,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);
@@ -693,9 +713,11 @@ private:
     // These are stored in SENSOR frame
     ThreadSafeValueCache<controller::Pose> _leftHandControllerPoseInSensorFrameCache { controller::Pose() };
     ThreadSafeValueCache<controller::Pose> _rightHandControllerPoseInSensorFrameCache { controller::Pose() };
-
     ThreadSafeValueCache<controller::Pose> _leftFootControllerPoseInSensorFrameCache{ controller::Pose() };
     ThreadSafeValueCache<controller::Pose> _rightFootControllerPoseInSensorFrameCache{ controller::Pose() };
+    ThreadSafeValueCache<controller::Pose> _hipsControllerPoseInSensorFrameCache{ controller::Pose() };
+    ThreadSafeValueCache<controller::Pose> _spine2ControllerPoseInSensorFrameCache{ controller::Pose() };
+    ThreadSafeValueCache<controller::Pose> _headControllerPoseInSensorFrameCache{ controller::Pose() };
 
     bool _hmdLeanRecenterEnabled = true;
 

interface/src/avatar/SkeletonModel.cpp

@@ -107,33 +107,49 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
         Rig::HeadParameters headParams;
 
+        // 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 {
             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());
                 glm::mat4 worldToRig = glm::inverse(rigToWorld);
                 glm::mat4 rigHMDMat = worldToRig * worldHMDMat;
-
+                _rig->computeHeadFromHMD(AnimPose(rigHMDMat), headParams.rigHeadPosition, headParams.rigHeadOrientation);
-            headParams.rigHeadPosition = extractTranslation(rigHMDMat);
+                headParams.headEnabled = true;
-            headParams.rigHeadOrientation = extractRotation(rigHMDMat);
-            headParams.worldHeadOrientation = extractRotation(worldHMDMat);
-
-            // TODO: if hips target sensor is valid.
-            // Copy it into headParams.hipsMatrix, and set headParams.hipsEnabled to true.
-
-            headParams.hipsEnabled = false;
             } else {
-            headParams.hipsEnabled = false;
+                // 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.isInHMD = false;
+                // preMult 180 is necessary to convert from avatar to rig coordinates.
-
+                // postMult 180 is necessary to convert head from -z forward to z forward.
-            // We don't have a valid localHeadPosition.
+                headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180;
-            headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame();
+                headParams.headEnabled = false;
-            headParams.worldHeadOrientation = head->getFinalOrientationInWorldFrame();
+            }
+        }
+
+        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;
+        }
+
+        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;
         headParams.isTalking = head->getTimeWithoutTalking() <= 1.5f;
 
         _rig->updateFromHeadParameters(headParams, deltaTime);
@@ -193,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();
@@ -225,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();

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);
@@ -1028,101 +1027,73 @@ 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.
+    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) {
-    updateEyeJoint(params.leftEyeJointIndex, params.modelTranslation, params.modelRotation,
+    updateEyeJoint(params.leftEyeJointIndex, params.modelTranslation, params.modelRotation, params.eyeLookAt, params.eyeSaccade);
-                   params.worldHeadOrientation, params.eyeLookAt, params.eyeSaccade);
+    updateEyeJoint(params.rightEyeJointIndex, params.modelTranslation, params.modelRotation, params.eyeLookAt, params.eyeSaccade);
-    updateEyeJoint(params.rightEyeJointIndex, params.modelTranslation, params.modelRotation,
-                   params.worldHeadOrientation, params.eyeLookAt, params.eyeSaccade);
 }
 
-void Rig::computeHeadNeckAnimVars(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut,
+void Rig::computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut) const {
-                                  glm::vec3& neckPositionOut, glm::quat& neckOrientationOut) 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) {
+void Rig::updateHeadAnimVars(const HeadParameters& params) {
-    if (_animSkeleton && index >= 0 && index < _animSkeleton->getNumJoints()) {
+    if (_animSkeleton) {
-        glm::quat yFlip180 = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f));
+        if (params.headEnabled) {
-        if (params.isInHMD) {
+            _animVars.set("headPosition", params.rigHeadPosition);
-            glm::vec3 headPos, neckPos;
+            _animVars.set("headRotation", params.rigHeadOrientation);
-            glm::quat headRot, neckRot;
+            if (params.hipsEnabled) {
-
+                // Since there is an explicit hips ik target, switch the head to use the more generic RotationAndPosition IK chain type.
-            AnimPose hmdPose(glm::vec3(1.0f), params.rigHeadOrientation * yFlip180, params.rigHeadPosition);
+                // this will allow the spine to bend more, ensuring that it can reach the head target position.
-            computeHeadNeckAnimVars(hmdPose, headPos, headRot, neckPos, neckRot);
+                _animVars.set("headType", (int)IKTarget::Type::RotationAndPosition);
-
+            } else {
-            // debug rendering
+                // When there is no hips IK target, use the HmdHead IK chain type.  This will make the spine very stiff,
-#ifdef DEBUG_RENDERING
+                // but because the IK _hipsOffset is enabled, the hips will naturally follow underneath the head.
-            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("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("headRotation", params.rigHeadOrientation);
-            _animVars.set("headAndNeckType", (int)IKTarget::Type::RotationOnly);
             _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.
 

libraries/animation/src/Rig.h

@@ -42,18 +42,17 @@ public:
     };
 
     struct HeadParameters {
-        glm::quat worldHeadOrientation = glm::quat();  // world space (-z forward)
+        glm::mat4 hipsMatrix = glm::mat4();           // rig space
+        glm::mat4 spine2Matrix = glm::mat4();         // rig space
         glm::quat rigHeadOrientation = glm::quat();   // rig space (-z forward)
         glm::vec3 rigHeadPosition = glm::vec3();      // rig space
-        glm::mat4 hipsMatrix = glm::mat4();          // rig space
         bool hipsEnabled = false;
-        bool isInHMD = false;
+        bool headEnabled = false;
-        int neckJointIndex = -1;
+        bool spine2Enabled = false;
         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();
@@ -230,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();
 
@@ -239,10 +241,9 @@ protected:
     void applyOverridePoses();
     void buildAbsoluteRigPoses(const AnimPoseVec& relativePoses, AnimPoseVec& absolutePosesOut);
 
-    void updateNeckJoint(int index, const HeadParameters& params);
+    void updateHeadAnimVars(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::vec3& lookAt, const glm::vec3& saccade);
-    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 calcAnimAlpha(float speed, const std::vector<float>& referenceSpeeds, float* alphaOut) const;
 
     AnimPose _modelOffset;  // model to rig space

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"),

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,

libraries/controllers/src/controllers/Input.h

@@ -16,9 +16,15 @@
 namespace controller {
 
 struct InputCalibrationData {
-    glm::mat4 sensorToWorldMat;
+    glm::mat4 sensorToWorldMat;    // sensor to world
-    glm::mat4 avatarMat;
+    glm::mat4 avatarMat;           // avatar to world
-    glm::mat4 hmdSensorMat;
+    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 {

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"),

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)

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:

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);
+});
+

scripts/developer/tests/viveMotionCapture.js

@@ -11,19 +11,22 @@ var TRACKED_OBJECT_POSES = [
 var triggerPressHandled = false;
 var rightTriggerPressed = false;
 var leftTriggerPressed = false;
+var calibrationCount = 0;
 
-var MAPPING_NAME = "com.highfidelity.viveMotionCapture";
+var TRIGGER_MAPPING_NAME = "com.highfidelity.viveMotionCapture.triggers";
-
+var triggerMapping = Controller.newMapping(TRIGGER_MAPPING_NAME);
-var mapping = Controller.newMapping(MAPPING_NAME);
+triggerMapping.from([Controller.Standard.RTClick]).peek().to(function (value) {
-mapping.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 head;
 var leftFoot;
 var rightFoot;
 var hips;
@@ -75,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;
@@ -84,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) {
@@ -92,7 +123,8 @@ function calibrate() {
             if (pose.valid) {
                 poses.push({
                     channel: channel,
-                    pose: pose
+                    pose: pose,
+                    lastestPose: pose
                 });
             }
         });
@@ -177,85 +209,91 @@ var ikTypes = {
 
 var handlerId;
 
-function computeIKTargetXform(jointInfo) {
+function convertJointInfoToPose(jointInfo) {
-    var pose = Controller.getPoseValue(jointInfo.channel);
+    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) {
+            if (controllerMapping) {
-                print("AJT: UN-CALIBRATE!");
 
                 // go back to normal, vive pucks will be ignored.
+                print("AJT: UN-CALIBRATE!");
+
+                head = undefined;
                 leftFoot = undefined;
                 rightFoot = undefined;
                 hips = undefined;
                 spine2 = undefined;
-                if (handlerId) {
+
-                    print("AJT: un-hooking animation state handler");
+                Controller.disableMapping(CONTROLLER_MAPPING_NAME + calibrationCount);
-                    MyAvatar.removeAnimationStateHandler(handlerId);
+                controllerMapping = undefined;
-                    handlerId = undefined;
+
-                }
             } else {
                 print("AJT: CALIBRATE!");
                 calibrate();
+                calibrationCount++;
 
-                var animVars = [];
+                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) {
-                    animVars.push("leftFootType");
+                    controllerMapping.from(leftFoot.channel).to(function (pose) {
-                    animVars.push("leftFootPosition");
+                        leftFoot.latestPose = pose;
-                    animVars.push("leftFootRotation");
+                    });
+                    controllerMapping.from(function () {
+                        return convertJointInfoToPose(leftFoot);
+                    }).to(Controller.Standard.LeftFoot);
                 }
                 if (rightFoot) {
-                    animVars.push("rightFootType");
+                    controllerMapping.from(rightFoot.channel).to(function (pose) {
-                    animVars.push("rightFootPosition");
+                        rightFoot.latestPose = pose;
-                    animVars.push("rightFootRotation");
+                    });
+                    controllerMapping.from(function () {
+                        return convertJointInfoToPose(rightFoot);
+                    }).to(Controller.Standard.RightFoot);
                 }
                 if (hips) {
-                    animVars.push("hipsType");
+                    controllerMapping.from(hips.channel).to(function (pose) {
-                    animVars.push("hipsPosition");
+                        hips.latestPose = pose;
-                    animVars.push("hipsRotation");
+                    });
+                    controllerMapping.from(function () {
+                        return convertJointInfoToPose(hips);
+                    }).to(Controller.Standard.Hips);
                 }
                 if (spine2) {
-                    animVars.push("spine2Type");
+                    controllerMapping.from(spine2.channel).to(function (pose) {
-                    animVars.push("spine2Position");
+                        spine2.latestPose = pose;
-                    animVars.push("spine2Rotation");
+                    });
+                    controllerMapping.from(function () {
+                        return convertJointInfoToPose(spine2);
+                    }).to(Controller.Standard.Spine2);
                 }
-
+                Controller.enableMapping(CONTROLLER_MAPPING_NAME + calibrationCount);
-                // 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 {
@@ -301,7 +339,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);
 });
 

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()