From 33f9acde67e8ac1a7707e9880c5662de271a3eb1 Mon Sep 17 00:00:00 2001
From: amantley <amantley@googlemail.com>
Date: Tue, 17 Jul 2018 17:42:11 -0700
Subject: [PATCH] latest

---
 interface/src/avatar/MyAvatar.cpp | 20 +++++++++++++++++++-
 interface/src/avatar/MyAvatar.h   |  2 ++
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index d084ca69f9..9eaf42d25d 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -89,9 +89,10 @@ const float MyAvatar::ZOOM_MAX = 25.0f;
 const float MyAvatar::ZOOM_DEFAULT = 1.5f;
 const float MIN_SCALE_CHANGED_DELTA = 0.001f;
 const int MODE_READINGS_RING_BUFFER_SIZE = 500;
+const int HEAD_FACING_RING_BUFFER_SIZE = 250;
 const float CENTIMETERS_PER_METER = 100.0f;
 
-//#define DEBUG_DRAW_HMD_MOVING_AVERAGE
+#define DEBUG_DRAW_HMD_MOVING_AVERAGE
 
 MyAvatar::MyAvatar(QThread* thread) :
     Avatar(thread),
@@ -112,6 +113,7 @@ MyAvatar::MyAvatar(QThread* thread) :
     _hmdSensorMatrix(),
     _hmdSensorOrientation(),
     _hmdSensorPosition(),
+    _headFacingBuffer(HEAD_FACING_RING_BUFFER_SIZE),
     _recentModeReadings(MODE_READINGS_RING_BUFFER_SIZE),
     _bodySensorMatrix(),
     _goToPending(false),
@@ -434,11 +436,25 @@ void MyAvatar::update(float deltaTime) {
     _recentModeReadings.insert(newHeightReadingInCentimeters);
     setCurrentStandingHeight(computeStandingHeightMode(getControllerPoseInAvatarFrame(controller::Action::HEAD)));
     setAverageHeadRotation(computeAverageHeadRotation(getControllerPoseInAvatarFrame(controller::Action::HEAD)));
+    
+    glm::vec2 bodyFacing = getFacingDir2D(_bodySensorMatrix);
+    _headFacingBuffer.insert(_headControllerFacing);
+    //qCDebug(interfaceapp) << "the body facing is " << -bodyFacing.x << " " << -bodyFacing.y << " the head controller facing is " << _headControllerFacing.x << " " << _headControllerFacing.y;
+    
+    float sumHeadFacingX = 0.0f;
+    float sumHeadFacingY = 0.0f;
+    for (auto headFacingIterator = _headFacingBuffer.begin(); headFacingIterator != _headFacingBuffer.end(); ++headFacingIterator) {
+        sumHeadFacingX += (*headFacingIterator).x;
+        sumHeadFacingY += (*headFacingIterator).y;
+    }
+    _averageFacing.x = sumHeadFacingX / (float)_headFacingBuffer.getNumEntries();
+    _averageFacing.y= sumHeadFacingY / (float)_headFacingBuffer.getNumEntries();
 
 #ifdef DEBUG_DRAW_HMD_MOVING_AVERAGE
     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 worldFacingAverage = transformVectorFast(getSensorToWorldMatrix(), glm::vec3(_averageFacing.x, 0.0f, _averageFacing.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));
@@ -3478,6 +3494,8 @@ bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, co
     const float FOLLOW_ROTATION_THRESHOLD = cosf(myAvatar.getRotationThreshold());
     glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix);
     return glm::dot(-myAvatar.getHeadControllerFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD;
+
+    //return glm::dot(myAvatar._averageFacing, -bodyFacing) < FOLLOW_ROTATION_THRESHOLD;
 }
 
 bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 819d5b0066..828ca48e32 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -1546,6 +1546,8 @@ private:
     // cache head controller pose in sensor space
     glm::vec2 _headControllerFacing;  // facing vector in xz plane (sensor space)
     glm::vec2 _headControllerFacingMovingAverage { 0.0f, 0.0f };   // facing vector in xz plane (sensor space)
+    RingBufferHistory<vec2> _headFacingBuffer;
+    vec2 _averageFacing { 0.0f, 0.0f };
     glm::quat _averageHeadRotation { 0.0f, 0.0f, 0.0f, 1.0f };
 
     float _currentStandingHeight { 0.0f };