Avatar can now look straight down without spinning 180.

Also, come code clean up and removal of unnecessary Y_180 flips.

interface/src/avatar/MyAvatar.cpp

@@ -423,12 +423,12 @@ void MyAvatar::update(float deltaTime) {
     }
 
 #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE
-    glm::vec3 p = transformPoint(getSensorToWorldMatrix(), getControllerPoseInAvatarFrame(controller::Pose::HEAD) *
-                                 glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
-    DebugDraw::getInstance().addMarker("facing-avg", getOrientation(), p, glm::vec4(1.0f));
-    p = transformPoint(getSensorToWorldMatrix(), getHMDSensorPosition() +
-                       glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y));
-    DebugDraw::getInstance().addMarker("facing", getOrientation(), p, glm::vec4(1.0f));
+    auto sensorHeadPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
+    glm::vec3 worldHeadPos = transformPoint(getSensorToWorldMatrix(), sensorHeadPose.getTranslation());
+    glm::vec3 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacingMovingAverage.x, 0.0f, _headControllerFacingMovingAverage.y));
+    glm::vec3 worldFacing = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_headControllerFacing.x, 0.0f, _headControllerFacing.y));
+    DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacing, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f));
+    DebugDraw::getInstance().drawRay(worldHeadPos, worldHeadPos + worldFacingAverage, glm::vec4(0.0f, 0.0f, 1.0f, 1.0f));
 #endif
 
     if (_goToPending) {
@@ -702,7 +702,8 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     _hmdSensorOrientation = glmExtractRotation(hmdSensorMatrix);
     auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
     if (headPose.isValid()) {
-        _headControllerFacing = getFacingDir2D(headPose.rotation);
+        glm::quat bodyOrientation = computeBodyFacingFromHead(headPose.rotation, Vectors::UNIT_Y);
+        _headControllerFacing = getFacingDir2D(bodyOrientation);
     } else {
         _headControllerFacing = glm::vec2(1.0f, 0.0f);
     }
@@ -2817,6 +2818,7 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
     auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
     if (headPose.isValid()) {
         headPosition = headPose.translation;
+        // AJT: TODO: can remove this Y_180
         headOrientation = headPose.rotation * Quaternions::Y_180;
     }
     const glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation);
@@ -2839,6 +2841,8 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
     // eyeToNeck offset is relative full HMD orientation.
     // while neckToRoot offset is only relative to HMDs yaw.
     // Y_180 is necessary because rig is z forward and hmdOrientation is -z forward
+
+    // AJT: TODO: can remove this Y_180, if we remove the higher level one.
     glm::vec3 headToNeck = headOrientation * Quaternions::Y_180 * (localNeck - localHead);
     glm::vec3 neckToRoot = headOrientationYawOnly  * Quaternions::Y_180 * -localNeck;
 
@@ -2975,9 +2979,11 @@ glm::vec3 MyAvatar::computeCounterBalance() const {
 // headOrientation, headPosition and hipsPosition are in avatar space
 // returns the matrix of the hips in Avatar space
 static glm::mat4 computeNewHipsMatrix(glm::quat headOrientation, glm::vec3 headPosition, glm::vec3 hipsPosition) {
-    glm::quat headOrientationYawOnly = cancelOutRollAndPitch(headOrientation);
-    const float MIX_RATIO = 0.5f;
-    glm::quat hipsRot = safeLerp(Quaternions::IDENTITY, headOrientationYawOnly, MIX_RATIO);
+
+    glm::quat bodyOrientation = computeBodyFacingFromHead(headOrientation, Vectors::UNIT_Y);
+
+    const float MIX_RATIO = 0.3f;
+    glm::quat hipsRot = safeLerp(Quaternions::IDENTITY, bodyOrientation, MIX_RATIO);
     glm::vec3 hipsFacing = hipsRot * Vectors::UNIT_Z;
 
     glm::vec3 spineVec = headPosition - hipsPosition;
@@ -3013,14 +3019,14 @@ static void drawBaseOfSupport(float baseOfSupportScale, float footLocal, glm::ma
 
 // this function finds the hips position using a center of gravity model that
 // balances the head and hands with the hips over the base of support
-// returns the rotation and position of the Avatar in Sensor space
+// returns the rotation (-z forward) and position of the Avatar in Sensor space
 glm::mat4 MyAvatar::deriveBodyUsingCgModel() const {
     glm::mat4 sensorToWorldMat = getSensorToWorldMatrix();
     glm::mat4 worldToSensorMat = glm::inverse(sensorToWorldMat);
     auto headPose = getControllerPoseInSensorFrame(controller::Action::HEAD);
 
-    // the Y_180 is to flip the controller pose from -z forward to the head joint which is +z forward.
     glm::mat4 sensorHeadMat = createMatFromQuatAndPos(headPose.rotation * Quaternions::Y_180, headPose.translation);
+
     // convert into avatar space
     glm::mat4 avatarToWorldMat = getTransform().getMatrix();
     glm::mat4 avatarHeadMat = glm::inverse(avatarToWorldMat) * sensorToWorldMat * sensorHeadMat;
@@ -3038,6 +3044,7 @@ glm::mat4 MyAvatar::deriveBodyUsingCgModel() const {
     glm::mat4 avatarHipsMat = computeNewHipsMatrix(glmExtractRotation(avatarHeadMat), extractTranslation(avatarHeadMat), cgHipsPosition);
 
     // convert hips from avatar to sensor space
+    // The Y_180 is to convert from z forward to -z forward.
     return worldToSensorMat * avatarToWorldMat * avatarHipsMat;
 }
 
@@ -3205,9 +3212,7 @@ void MyAvatar::FollowHelper::decrementTimeRemaining(float dt) {
 bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
     const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); // 30 degrees
     glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix);
-    
     return glm::dot(-myAvatar.getHeadControllerFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD;
-    
 }
 
 bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {

interface/src/avatar/MyAvatar.h

@@ -978,14 +978,14 @@ public:
 
 
     // derive avatar body position and orientation from the current HMD Sensor location.
-    // results are in HMD frame
+    // results are in sensor frame (-z forward)
     glm::mat4 deriveBodyFromHMDSensor() const;
 
     glm::vec3 computeCounterBalance() const;
 
     // derive avatar body position and orientation from using the current HMD Sensor location in relation to the previous
     // location of the base of support of the avatar.
-    // results are in HMD frame
+    // results are in sensor frame (-z foward)
     glm::mat4 deriveBodyUsingCgModel() const;
 
     /**jsdoc
@@ -1495,8 +1495,8 @@ private:
     glm::quat _hmdSensorOrientation;
     glm::vec3 _hmdSensorPosition;
     // cache head controller pose in sensor space
-    glm::vec2 _headControllerFacing;  // facing vector in xz plane
-    glm::vec2 _headControllerFacingMovingAverage { 0, 0 };   // facing vector in xz plane
+    glm::vec2 _headControllerFacing;  // facing vector in xz plane (sensor space)
+    glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f };   // facing vector in xz plane (sensor space)
 
     // cache of the current body position and orientation of the avatar's body,
     // in sensor space.

interface/src/avatar/MySkeletonModel.cpp

@@ -83,6 +83,7 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) {
         hipsPos = headPos + tiltRot * (hipsPos - headPos);
     }
 
+    // AJT: TODO can we remove this?
     return AnimPose(hipsRot * Quaternions::Y_180, hipsPos);
 }
 

libraries/animation/src/AnimUtil.cpp

@@ -9,7 +9,9 @@
 //
 
 #include "AnimUtil.h"
-#include "GLMHelpers.h"
+#include <GLMHelpers.h>
+#include <NumericalConstants.h>
+#include <DebugDraw.h>
 
 // TODO: use restrict keyword
 // TODO: excellent candidate for simd vectorization.
@@ -107,3 +109,39 @@ AnimPose boneLookAt(const glm::vec3& target, const AnimPose& bone) {
                      glm::vec4(bone.trans(), 1.0f));
     return AnimPose(lookAt);
 }
+
+// This will attempt to determine the proper body facing of a characters body
+// assumes headRot is z-forward and y-up.
+// and returns a bodyRot that is also z-forward and y-up
+glm::quat computeBodyFacingFromHead(const glm::quat& headRot, const glm::vec3& up) {
+
+    glm::vec3 bodyUp = glm::normalize(up);
+
+    // initially take the body facing from the head.
+    glm::vec3 headUp = headRot * Vectors::UNIT_Y;
+    glm::vec3 headForward = headRot * Vectors::UNIT_Z;
+    const float THRESHOLD = cosf(glm::radians(30.0f));
+
+    glm::vec3 bodyForward = headForward;
+
+    float dot = glm::dot(headForward, bodyUp);
+
+    if (dot < -THRESHOLD) { // head is looking down
+        // the body should face in the same direction as the top the head.
+        bodyForward = headUp;
+    } else if (dot > THRESHOLD) {  // head is looking upward
+        // the body should face away from the top of the head.
+        bodyForward = -headUp;
+    }
+
+    // cancel out upward component
+    bodyForward = glm::normalize(bodyForward - dot * bodyUp);
+
+    glm::vec3 u, v, w;
+    generateBasisVectors(bodyForward, bodyUp, u, v, w);
+
+    // create matrix from orthogonal basis vectors
+    glm::mat4 bodyMat(glm::vec4(w, 0.0f), glm::vec4(v, 0.0f), glm::vec4(u, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f));
+
+    return glmExtractRotation(bodyMat);
+}

libraries/animation/src/AnimUtil.h

@@ -33,4 +33,9 @@ inline glm::quat safeLerp(const glm::quat& a, const glm::quat& b, float alpha) {
 
 AnimPose boneLookAt(const glm::vec3& target, const AnimPose& bone);
 
+// This will attempt to determine the proper body facing of a characters body
+// assumes headRot is z-forward and y-up.
+// and returns a bodyRot that is also z-forward and y-up
+glm::quat computeBodyFacingFromHead(const glm::quat& headRot, const glm::vec3& up);
+
 #endif

libraries/shared/src/GLMHelpers.cpp

@@ -574,8 +574,9 @@ void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& seconda
     vAxisOut = glm::cross(wAxisOut, uAxisOut);
 }
 
+// assumes z-forward and y-up
 glm::vec2 getFacingDir2D(const glm::quat& rot) {
-    glm::vec3 facing3D = rot * Vectors::UNIT_NEG_Z;
+    glm::vec3 facing3D = rot * Vectors::UNIT_Z;
     glm::vec2 facing2D(facing3D.x, facing3D.z);
     const float ALMOST_ZERO = 0.0001f;
     if (glm::length(facing2D) < ALMOST_ZERO) {
@@ -585,8 +586,9 @@ glm::vec2 getFacingDir2D(const glm::quat& rot) {
     }
 }
 
+// assumes z-forward and y-up
 glm::vec2 getFacingDir2D(const glm::mat4& m) {
-    glm::vec3 facing3D = transformVectorFast(m, Vectors::UNIT_NEG_Z);
+    glm::vec3 facing3D = transformVectorFast(m, Vectors::UNIT_Z);
     glm::vec2 facing2D(facing3D.x, facing3D.z);
     const float ALMOST_ZERO = 0.0001f;
     if (glm::length(facing2D) < ALMOST_ZERO) {

libraries/shared/src/GLMHelpers.h

@@ -250,7 +250,10 @@ glm::vec3 transformVectorFull(const glm::mat4& m, const glm::vec3& v);
 void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& secondaryAxis,
                           glm::vec3& uAxisOut, glm::vec3& vAxisOut, glm::vec3& wAxisOut);
 
+// assumes z-forward and y-up
 glm::vec2 getFacingDir2D(const glm::quat& rot);
+
+// assumes z-forward and y-up
 glm::vec2 getFacingDir2D(const glm::mat4& m);
 
 inline bool isNaN(const glm::vec3& value) { return isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }