Bug fixes for avatars with no eyes

Changed default eye position to 1.9 meters because
the hifi_team avatars are 2.0 meters tall.

Also, prevent array access with negative indices when eye bones are missing.

ಠ_ಠ

interface/src/avatar/MyAvatar.cpp

@@ -379,6 +379,13 @@ void MyAvatar::updateHMDFollowVelocity() {
 // update sensor to world matrix from current body position and hmd sensor.
 // This is so the correct camera can be used for rendering.
 void MyAvatar::updateSensorToWorldMatrix() {
+
+#ifdef DEBUG_RENDERING
+    // draw marker about avatar's position
+    const glm::vec4 red(1.0f, 0.0f, 0.0f, 1.0f);
+    DebugDraw::getInstance().addMyAvatarMarker("pos", glm::quat(), glm::vec3(), red);
+#endif
+
     // update the sensor mat so that the body position will end up in the desired
     // position when driven from the head.
     glm::mat4 desiredMat = createMatFromQuatAndPos(getOrientation(), getPosition());
@@ -1859,6 +1866,7 @@ glm::quat MyAvatar::getWorldBodyOrientation() const {
     return glm::quat_cast(_sensorToWorldMatrix * _bodySensorMatrix);
 }
 
+#if 0
 // derive avatar body position and orientation from the current HMD Sensor location.
 // results are in sensor space
 glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
@@ -1876,6 +1884,66 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
     }
     return glm::mat4();
 }
+#else
+// old school meat hook style
+glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
+
+    // HMD is in sensor space.
+    const glm::vec3 hmdPosition = getHMDSensorPosition();
+    const glm::quat hmdOrientation = getHMDSensorOrientation();
+    const glm::quat hmdOrientationYawOnly = cancelOutRollAndPitch(hmdOrientation);
+
+    /*
+    const glm::vec3 DEFAULT_RIGHT_EYE_POS(-0.3f, 1.6f, 0.0f);
+    const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 1.6f, 0.0f);
+    const glm::vec3 DEFAULT_NECK_POS(0.0f, 1.5f, 0.0f);
+    const glm::vec3 DEFAULT_HIPS_POS(0.0f, 1.0f, 0.0f);
+    */
+
+    // 2 meter tall dude
+    const glm::vec3 DEFAULT_RIGHT_EYE_POS(-0.3f, 1.9f, 0.0f);
+    const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 1.9f, 0.0f);
+    const glm::vec3 DEFAULT_NECK_POS(0.0f, 1.70f, 0.0f);
+    const glm::vec3 DEFAULT_HIPS_POS(0.0f, 1.05f, 0.0f);
+
+    vec3 localEyes, localNeck;
+    if (!_debugDrawSkeleton) {
+        const glm::quat rotY180 = glm::angleAxis((float)PI, glm::vec3(0.0f, 1.0f, 0.0f));
+        localEyes = rotY180 * (((DEFAULT_RIGHT_EYE_POS + DEFAULT_LEFT_EYE_POS) / 2.0f) - DEFAULT_HIPS_POS);
+        localNeck = rotY180 * (DEFAULT_NECK_POS - DEFAULT_HIPS_POS);
+    } else {
+        // TODO: At the moment MyAvatar does not have access to the rig, which has the skeleton, which has the bind poses.
+        // for now use the _debugDrawSkeleton, which is initialized with the same FBX model as the rig.
+
+        // TODO: cache these indices.
+        int rightEyeIndex = _debugDrawSkeleton->nameToJointIndex("RightEye");
+        int leftEyeIndex = _debugDrawSkeleton->nameToJointIndex("LeftEye");
+        int neckIndex = _debugDrawSkeleton->nameToJointIndex("Neck");
+        int hipsIndex = _debugDrawSkeleton->nameToJointIndex("Hips");
+
+        glm::vec3 absRightEyePos = rightEyeIndex != -1 ? _debugDrawSkeleton->getAbsoluteBindPose(rightEyeIndex).trans : DEFAULT_RIGHT_EYE_POS;
+        glm::vec3 absLeftEyePos = leftEyeIndex != -1 ? _debugDrawSkeleton->getAbsoluteBindPose(leftEyeIndex).trans : DEFAULT_LEFT_EYE_POS;
+        glm::vec3 absNeckPos = neckIndex != -1 ? _debugDrawSkeleton->getAbsoluteBindPose(neckIndex).trans : DEFAULT_NECK_POS;
+        glm::vec3 absHipsPos = neckIndex != -1 ? _debugDrawSkeleton->getAbsoluteBindPose(hipsIndex).trans : DEFAULT_HIPS_POS;
+
+        const glm::quat rotY180 = glm::angleAxis((float)PI, glm::vec3(0.0f, 1.0f, 0.0f));
+        localEyes = rotY180 * (((absRightEyePos + absLeftEyePos) / 2.0f) - absHipsPos);
+        localNeck = rotY180 * (absNeckPos - absHipsPos);
+    }
+
+    // apply simplistic head/neck model
+    // figure out where the avatar body should be by applying offsets from the avatar's neck & head joints.
+
+    // eyeToNeck offset is relative full HMD orientation.
+    // while neckToRoot offset is only relative to HMDs yaw.
+    glm::vec3 eyeToNeck = hmdOrientation * (localNeck - localEyes);
+    glm::vec3 neckToRoot = hmdOrientationYawOnly * -localNeck;
+    glm::vec3 bodyPos = hmdPosition + eyeToNeck + neckToRoot;
+
+    // avatar facing is determined solely by hmd orientation.
+    return createMatFromQuatAndPos(hmdOrientationYawOnly, bodyPos);
+}
+#endif
 
 glm::vec3 MyAvatar::getPositionForAudio() {
     switch (_audioListenerMode) {

libraries/animation/src/AnimSkeleton.cpp

@@ -79,7 +79,7 @@ const QString& AnimSkeleton::getJointName(int jointIndex) const {
 }
 
 AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& poses) const {
-    if (jointIndex < 0) {
+    if (jointIndex < 0 || jointIndex >= (int)poses.size() || jointIndex >= (int)_joints.size()) {
         return AnimPose::identity;
     } else {
         return getAbsolutePose(_joints[jointIndex].parentIndex, poses) * poses[jointIndex];

libraries/animation/src/Rig.cpp

@@ -23,6 +23,19 @@
 #include "AnimSkeleton.h"
 #include "IKTarget.h"
 
+/*
+const glm::vec3 DEFAULT_RIGHT_EYE_POS(-0.3f, 1.6f, 0.0f);
+const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 1.6f, 0.0f);
+const glm::vec3 DEFAULT_HEAD_POS(0.0f, 1.55f, 0.0f);
+const glm::vec3 DEFAULT_NECK_POS(0.0f, 1.5f, 0.0f);
+*/
+
+// 2 meter tall dude
+const glm::vec3 DEFAULT_RIGHT_EYE_POS(-0.3f, 1.9f, 0.0f);
+const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 1.9f, 0.0f);
+const glm::vec3 DEFAULT_HEAD_POS(0.0f, 1.75f, 0.0f);
+const glm::vec3 DEFAULT_NECK_POS(0.0f, 1.70f, 0.0f);
+
 void insertSorted(QList<AnimationHandlePointer>& handles, const AnimationHandlePointer& handle) {
     for (QList<AnimationHandlePointer>::iterator it = handles.begin(); it != handles.end(); it++) {
         if (handle->getPriority() > (*it)->getPriority()) {
@@ -410,17 +423,19 @@ void Rig::calcAnimAlpha(float speed, const std::vector<float>& referenceSpeeds,
 void Rig::computeEyesInRootFrame(const AnimPoseVec& poses) {
     // TODO: use cached eye/hips indices for these calculations
     int numPoses = poses.size();
-    int rightEyeIndex = _animSkeleton->nameToJointIndex(QString("RightEye"));
+    int hipsIndex = _animSkeleton->nameToJointIndex(QString("Hips"));
-    int leftEyeIndex = _animSkeleton->nameToJointIndex(QString("LeftEye"));
+    int headIndex = _animSkeleton->nameToJointIndex(QString("Head"));
-    if (numPoses > rightEyeIndex && numPoses > leftEyeIndex
+    if (hipsIndex > 0 && headIndex > 0) {
-            && rightEyeIndex > 0 && leftEyeIndex > 0) {
+        int rightEyeIndex = _animSkeleton->nameToJointIndex(QString("RightEye"));
-        int hipsIndex = _animSkeleton->nameToJointIndex(QString("Hips"));
+        int leftEyeIndex = _animSkeleton->nameToJointIndex(QString("LeftEye"));
-        int headIndex = _animSkeleton->nameToJointIndex(QString("Head"));
+        if (numPoses > rightEyeIndex && numPoses > leftEyeIndex && rightEyeIndex > 0 && leftEyeIndex > 0) {
-        if (hipsIndex >= 0 && headIndex > 0) {
             glm::vec3 rightEye = _animSkeleton->getAbsolutePose(rightEyeIndex, poses).trans;
             glm::vec3 leftEye = _animSkeleton->getAbsolutePose(leftEyeIndex, poses).trans;
             glm::vec3 hips = _animSkeleton->getAbsolutePose(hipsIndex, poses).trans;
             _eyesInRootFrame = 0.5f * (rightEye + leftEye) - hips;
+        } else {
+            glm::vec3 hips = _animSkeleton->getAbsolutePose(hipsIndex, poses).trans;
+            _eyesInRootFrame = 0.5f * (DEFAULT_RIGHT_EYE_POS + DEFAULT_LEFT_EYE_POS) - hips;
         }
     }
 }
@@ -1172,11 +1187,6 @@ static void computeHeadNeckAnimVars(AnimSkeleton::ConstPointer skeleton, const A
     int headIndex = skeleton->nameToJointIndex("Head");
     int neckIndex = skeleton->nameToJointIndex("Neck");
 
-    const glm::vec3 DEFAULT_RIGHT_EYE_POS(-0.3f, 1.6f, 0.0f);
-    const glm::vec3 DEFAULT_LEFT_EYE_POS(0.3f, 1.6f, 0.0f);
-    const glm::vec3 DEFAULT_HEAD_POS(0.0f, 1.55f, 0.0f);
-    const glm::vec3 DEFAULT_NECK_POS(0.0f, 1.5f, 0.0f);
-
     // Use absolute bindPose positions just in case the relBindPose have rotations we don't expect.
     glm::vec3 absRightEyePos = rightEyeIndex != -1 ? skeleton->getAbsoluteBindPose(rightEyeIndex).trans : DEFAULT_RIGHT_EYE_POS;
     glm::vec3 absLeftEyePos = leftEyeIndex != -1 ? skeleton->getAbsoluteBindPose(leftEyeIndex).trans : DEFAULT_LEFT_EYE_POS;