From 2bec8678639e2aff14f83063be6e2154c34ee983 Mon Sep 17 00:00:00 2001
From: amantley <amantley@googlemail.com>
Date: Fri, 24 Aug 2018 16:29:08 -0700
Subject: [PATCH] added the sanity check to see if the shoulders have flipped

---
 interface/src/avatar/MyAvatar.cpp | 19 ++++++++++++++++---
 interface/src/avatar/MyAvatar.h   |  2 +-
 2 files changed, 17 insertions(+), 4 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index a1e30144ee..ae02eacf75 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -458,9 +458,11 @@ void MyAvatar::update(float deltaTime) {
     glm::vec3 handHipAzimuthSensorSpace = transformVectorFast(worldToSensorMat, glm::vec3(handHipAzimuthWorldSpace.x, 0.0f, handHipAzimuthWorldSpace.z));
     glm::vec2 normedHandHip = glm::normalize(glm::vec2(handHipAzimuthSensorSpace.x, handHipAzimuthSensorSpace.z));
     glm::vec2 headFacingPlusHandHipAzimuthMix = lerp(normedHandHip, _headControllerFacing, 0.5f);
-    //qCDebug(interfaceapp) << "the hand hip azimuth " << normedHandHip << "the head look at" << _headControllerFacing;
+    // if head facing dot mid point hands facing is close to -1 and the hands midpoint is close to -1 you then flip
+
+    // qCDebug(interfaceapp) << "the hand hip azimuth " << normedHandHip << "the head look at" << _headControllerFacing;
     _headControllerFacingMovingAverage = lerp(_headControllerFacingMovingAverage, headFacingPlusHandHipAzimuthMix, tau);
-    qCDebug(interfaceapp) << "the hand hip azimuth average " << _headControllerFacingMovingAverage;
+    // qCDebug(interfaceapp) << "the hand hip azimuth average " << _headControllerFacingMovingAverage;
 
     if (_smoothOrientationTimer < SMOOTH_TIME_ORIENTATION) {
         _rotationChanged = usecTimestampNow();
@@ -831,11 +833,22 @@ void MyAvatar::computeHandAzimuth() {
 
     // qCDebug(interfaceapp) << "the right hand in avatar space" << rightHandRigSpace;
     if (leftHandPoseAvatarSpace.isValid() && rightHandPoseAvatarSpace.isValid()) {
+        vec2 oldAzimuthReading = _hipToHandController;
         _hipToHandController = lerp(glm::normalize(glm::vec2(rightHandPoseAvatarSpace.translation.x, rightHandPoseAvatarSpace.translation.z)), glm::normalize(glm::vec2(leftHandPoseAvatarSpace.translation.x, leftHandPoseAvatarSpace.translation.z)), 0.5f);
+        
+        //if they are 180 apart
+        float cosForwardAngle = glm::dot(_hipToHandController, oldAzimuthReading);
+        float cosBackwardAngle = glm::dot(_hipToHandController, -oldAzimuthReading);
+        if (cosBackwardAngle > cosForwardAngle) {
+            // this means we have lost continuity with the current chest position
+            _hipToHandController = -_hipToHandController;
+        }
+
+        
         //need to use easing function here.
         //var rotateAngle = ((Math.cos((leftRightMidpoint / 180.0) * Math.PI) + 2.0) / 3.0) * leftRightMidpoint;
     } else {
-        _hipToHandController = glm::vec2(0.0f, 1.0f);
+        // _hipToHandController = glm::vec2(0.0f, -1.0f);
     }
 }
 
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 177b453267..ea5059bf39 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -1663,7 +1663,7 @@ private:
     glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f };   // facing vector in xz plane (sensor space)
     glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f };
 
-    glm::vec2 _hipToHandController;  // spine2 facing vector in xz plane (avatar space)
+    glm::vec2 _hipToHandController { 0.0f, -1.0f };  // spine2 facing vector in xz plane (avatar space)
 
     float _currentStandingHeight { 0.0f };
     bool _resetMode { true };