experimental HMD hips tracking

interface/resources/meshes/defaultAvatar_full/avatar-animation.json

@@ -16,12 +16,14 @@
                         {
                             "jointName": "RightHand",
                             "positionVar": "rightHandPosition",
-                            "rotationVar": "rightHandRotation"
+                            "rotationVar": "rightHandRotation",
+                            "typeVar": "rightHandType"
                         },
                         {
                             "jointName": "LeftHand",
                             "positionVar": "leftHandPosition",
-                            "rotationVar": "leftHandRotation"
+                            "rotationVar": "leftHandRotation",
+                            "typeVar": "leftHandType"
                         },
                         {
                             "jointName": "RightFoot",
@@ -39,13 +41,13 @@
                             "jointName": "Neck",
                             "positionVar": "neckPosition",
                             "rotationVar": "neckRotation",
-                            "typeVar": "headAndNeckType"
+                            "typeVar": "neckType"
                         },
                         {
                             "jointName": "Head",
                             "positionVar": "headPosition",
                             "rotationVar": "headRotation",
-                            "typeVar": "headAndNeckType"
+                            "typeVar": "headType"
                         }
                     ]
                 },
@@ -63,7 +65,7 @@
                         "id": "spineLean",
                         "type": "manipulator",
                         "data": {
-                            "alpha": 1.0,
+                            "alpha": 0.0,
                             "joints": [
                                 { "var": "lean", "jointName": "Spine" }
                             ]

interface/src/avatar/MyAvatar.cpp

@@ -337,16 +337,18 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     const float STRAIGHTENING_LEAN_DURATION = 0.5f;  // seconds
 
     // define a vertical capsule
+    static float adebug = 1.0f; // adebug
     const float STRAIGHTENING_LEAN_CAPSULE_RADIUS = 0.2f;  // meters
     const float STRAIGHTENING_LEAN_CAPSULE_LENGTH = 0.05f;  // length of the cylinder part of the capsule in meters.
 
     auto newBodySensorMatrix = deriveBodyFromHMDSensor();
     glm::vec3 diff = extractTranslation(newBodySensorMatrix) - extractTranslation(_bodySensorMatrix);
-    if (!_straighteningLean && (capsuleCheck(diff, STRAIGHTENING_LEAN_CAPSULE_LENGTH, STRAIGHTENING_LEAN_CAPSULE_RADIUS) || hmdIsAtRest)) {
+    if (!_straighteningLean && (capsuleCheck(diff, adebug * STRAIGHTENING_LEAN_CAPSULE_LENGTH, adebug * STRAIGHTENING_LEAN_CAPSULE_RADIUS) || hmdIsAtRest)) {
 
         // begin homing toward derived body position.
         _straighteningLean = true;
         _straighteningLeanAlpha = 0.0f;
+        adebug = 1000.0f; // adebug
 
     } else if (_straighteningLean) {
 

libraries/animation/src/AnimInverseKinematics.cpp

@@ -98,8 +98,14 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
             target.setType(animVars.lookup(targetVar.typeVar, (int)IKTarget::Type::RotationAndPosition));
             if (target.getType() != IKTarget::Type::Unknown) {
                 AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses);
-                target.setPose(animVars.lookup(targetVar.rotationVar, defaultPose.rot), 
+                glm::quat rotation = animVars.lookup(targetVar.rotationVar, defaultPose.rot); 
-                            animVars.lookup(targetVar.positionVar, defaultPose.trans));
+                glm::vec3 translation = animVars.lookup(targetVar.positionVar, defaultPose.trans);
+                if (target.getType() == IKTarget::Type::HipsRelativeRotationAndPosition) {
+                    translation += _hipsOffset;
+                    // HACK to test whether lifting idle hands will release the shoulders
+                    translation.y += 0.10f;
+                }
+                target.setPose(rotation, translation);
                 target.setIndex(targetVar.jointIndex);
                 targets.push_back(target);
                 if (targetVar.jointIndex > _maxTargetIndex) {
@@ -125,6 +131,10 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
             }
         }
     }
+    static int adebug = 0; ++adebug; bool verbose = (0 == (adebug % 100));
+    if (verbose) {
+        std::cout << "adebug num targets = " << targets.size() << std::endl;  // adebug
+    }
 }
 
 void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets) {
@@ -175,7 +185,8 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
                 glm::vec3 leverArm = tipPosition - jointPosition;
 
                 glm::quat deltaRotation;
-                if (targetType == IKTarget::Type::RotationAndPosition) {
+                if (targetType == IKTarget::Type::RotationAndPosition || 
+                        targetType == IKTarget::Type::HipsRelativeRotationAndPosition) {
                     // compute the swing that would get get tip closer
                     glm::vec3 targetLine = target.getTranslation() - jointPosition;
                     glm::vec3 axis = glm::cross(leverArm, targetLine);
@@ -365,23 +376,28 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
 
             solveWithCyclicCoordinateDescent(targets);
 
+
             // compute the new target hips offset (for next frame)
             // by looking for discrepancies between where a targeted endEffector is
             // and where it wants to be (after IK solutions are done)
+            glm::vec3 hmdHipsOffset(100.0f);
             glm::vec3 newHipsOffset = Vectors::ZERO;
             for (auto& target: targets) {
                 int targetIndex = target.getIndex();
                 if (targetIndex == _headIndex && _headIndex != -1) {
                     // special handling for headTarget
-                    glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans;
-                    glm::vec3 over = under;
                     if (target.getType() == IKTarget::Type::RotationOnly) {
-                        over = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
+                        // we want to shift the hips to bring the underpose closer 
-                    } else if (target.getType() == IKTarget::Type::RotationAndPosition) {
+                        // to where the head happens to be (overpose)
-                        over = target.getTranslation();
+                        glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans;
+                        glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
+                        const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
+                        newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under);
+                    } else if (target.getType() == IKTarget::Type::HmdHead) {
+                        // we want to shift the hips to bring the head to its designated position
+                        glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
+                        hmdHipsOffset = _hipsOffset + target.getTranslation() - actual;
                     }
-                    const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
-                    newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (over - under);
                 } else if (target.getType() == IKTarget::Type::RotationAndPosition) {
                     glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans;
                     glm::vec3 targetPosition = target.getTranslation();
@@ -392,6 +408,10 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
             // smooth transitions by relaxing _hipsOffset toward the new value
             const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f;
             _hipsOffset += (newHipsOffset - _hipsOffset) * (dt / HIPS_OFFSET_SLAVE_TIMESCALE);
+            //_hipsOffset *= 0.0f; // adebug
+            if (glm::length2(hmdHipsOffset) < 100.0f) {
+                _hipsOffset = hmdHipsOffset;
+            }
         }
     }
     return _relativePoses;

libraries/animation/src/IKTarget.cpp

@@ -25,6 +25,9 @@ void IKTarget::setType(int type) {
         case (int)Type::HmdHead:
             _type = Type::HmdHead;
             break;
+        case (int)Type::HipsRelativeRotationAndPosition:
+            _type = Type::HipsRelativeRotationAndPosition;
+            break;
         default:
             _type = Type::Unknown;
     }

libraries/animation/src/IKTarget.h

@@ -18,6 +18,7 @@ public:
         RotationAndPosition,
         RotationOnly,
         HmdHead,
+        HipsRelativeRotationAndPosition,
         Unknown,
     };
 

libraries/animation/src/Rig.cpp

@@ -1058,9 +1058,11 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) {
 
                 _animVars.set("headPosition", headPos);
                 _animVars.set("headRotation", headRot);
-                _animVars.set("headAndNeckType", (int)IKTarget::Type::RotationAndPosition);
+                _animVars.set("headType", (int)IKTarget::Type::HmdHead);
                 _animVars.set("neckPosition", neckPos);
                 _animVars.set("neckRotation", neckRot);
+                //_animVars.set("neckType", (int)IKTarget::Type::RotationOnly);
+                _animVars.set("neckType", (int)IKTarget::Type::Unknown); // 'Unknown' disables the target
 
             } else {
 
@@ -1072,8 +1074,10 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) {
                 _animVars.unset("headPosition");
                 _animVars.set("headRotation", realLocalHeadOrientation);
                 _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);
             }
         } else if (!_enableAnimGraph) {
 
@@ -1131,16 +1135,20 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
         if (params.isLeftEnabled) {
             _animVars.set("leftHandPosition", rootBindPose.trans + rootBindPose.rot * yFlipHACK * params.leftPosition);
             _animVars.set("leftHandRotation", rootBindPose.rot * yFlipHACK * params.leftOrientation);
+            _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
         } else {
             _animVars.unset("leftHandPosition");
             _animVars.unset("leftHandRotation");
+            _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
         }
         if (params.isRightEnabled) {
             _animVars.set("rightHandPosition", rootBindPose.trans + rootBindPose.rot * yFlipHACK * params.rightPosition);
             _animVars.set("rightHandRotation", rootBindPose.rot * yFlipHACK * params.rightOrientation);
+            _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
         } else {
             _animVars.unset("rightHandPosition");
             _animVars.unset("rightHandRotation");
+            _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
         }
 
         // set leftHand grab vars