From a68af15b8271af84e51add5714c6da683143f686 Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@highfidelity.io>
Date: Tue, 12 Aug 2014 14:26:14 -0700
Subject: [PATCH] DDE Face tracker tweaks to start adding long term averaging

---
 interface/src/devices/DdeFaceTracker.cpp | 47 +++++++++++++++++++++---
 interface/src/devices/DdeFaceTracker.h   |  4 ++
 2 files changed, 46 insertions(+), 5 deletions(-)

diff --git a/interface/src/devices/DdeFaceTracker.cpp b/interface/src/devices/DdeFaceTracker.cpp
index 3dc5a818cc..ae5beb8c85 100644
--- a/interface/src/devices/DdeFaceTracker.cpp
+++ b/interface/src/devices/DdeFaceTracker.cpp
@@ -178,6 +178,31 @@ float rescaleCoef(float ddeCoef) {
     return (ddeCoef - DDE_MIN_RANGE) / (DDE_MAX_RANGE - DDE_MIN_RANGE);
 }
 
+const int MIN = 0;
+const int AVG = 1;
+const int MAX = 2;
+const float LONG_TERM_AVERAGE = 0.999f;
+
+
+void resetCoefficient(float * coefficient, float currentValue) {
+    coefficient[MIN] = coefficient[MAX] = coefficient[AVG] = currentValue;
+}
+
+float updateAndGetCoefficient(float * coefficient, float currentValue, bool scaleToRange = false) {
+    coefficient[MIN] = (currentValue < coefficient[MIN]) ? currentValue : coefficient[MIN];
+    coefficient[MAX] = (currentValue > coefficient[MAX]) ? currentValue : coefficient[MAX];
+    coefficient[AVG] = LONG_TERM_AVERAGE * coefficient[AVG] + (1.f - LONG_TERM_AVERAGE) * currentValue;
+    if (coefficient[MAX] > coefficient[MIN]) {
+        if (scaleToRange) {
+            return glm::clamp((currentValue - coefficient[AVG]) / (coefficient[MAX] - coefficient[MIN]), 0.f, 1.f);
+        } else {
+            return glm::clamp(currentValue - coefficient[AVG], 0.f, 1.f);
+        }
+    } else {
+        return 0.f;
+    }
+}
+
 void DdeFaceTracker::decodePacket(const QByteArray& buffer) {
     if(buffer.size() > MIN_PACKET_SIZE) {
         Packet packet;
@@ -189,14 +214,17 @@ void DdeFaceTracker::decodePacket(const QByteArray& buffer) {
         memcpy(&translation, packet.translation, sizeof(packet.translation));
         glm::quat rotation;
         memcpy(&rotation, &packet.rotation, sizeof(packet.rotation));
-        if (_reset) {
+        if (_reset || (_lastReceiveTimestamp == 0)) {
             memcpy(&_referenceTranslation, &translation, sizeof(glm::vec3));
             memcpy(&_referenceRotation, &rotation, sizeof(glm::quat));
+            
+            resetCoefficient(_rightEye, packet.expressions[0]);
+            resetCoefficient(_leftEye, packet.expressions[1]);
             _reset = false;
         }
 
         // Compute relative translation
-        float LEAN_DAMPING_FACTOR = 40;
+        float LEAN_DAMPING_FACTOR = 200.0f;
         translation -= _referenceTranslation;
         translation /= LEAN_DAMPING_FACTOR;
         translation.x *= -1;
@@ -208,10 +236,19 @@ void DdeFaceTracker::decodePacket(const QByteArray& buffer) {
         _headTranslation = translation;
         _headRotation = rotation;
         
+        if (_lastReceiveTimestamp == 0) {
+            //  On first packet, reset coefficients
+        }
+        
         // Set blendshapes
-        float BLINK_MAGNIFIER = 2.0f;
-        _blendshapeCoefficients[_leftBlinkIndex] = rescaleCoef(packet.expressions[1]) * BLINK_MAGNIFIER;
-        _blendshapeCoefficients[_rightBlinkIndex] = rescaleCoef(packet.expressions[0]) * BLINK_MAGNIFIER;
+        float EYE_MAGNIFIER = 4.0f;
+
+        float rightEye = (updateAndGetCoefficient(_rightEye, packet.expressions[0])) * EYE_MAGNIFIER;
+        _blendshapeCoefficients[_rightBlinkIndex] = rightEye;
+        float leftEye = (updateAndGetCoefficient(_leftEye, packet.expressions[1])) * EYE_MAGNIFIER;
+        _blendshapeCoefficients[_leftBlinkIndex] = leftEye;
+
+        //  Right eye = packet.expressions[0];
         
         float leftBrow = 1.0f - rescaleCoef(packet.expressions[14]);
         if (leftBrow < 0.5f) {
diff --git a/interface/src/devices/DdeFaceTracker.h b/interface/src/devices/DdeFaceTracker.h
index 96707ac94c..68d9e7d487 100644
--- a/interface/src/devices/DdeFaceTracker.h
+++ b/interface/src/devices/DdeFaceTracker.h
@@ -84,6 +84,10 @@ private:
     int _mouthSmileRightIndex;
     
     int _jawOpenIndex;
+    
+    float _leftEye[3];
+    float _rightEye[3];
+    
 };
 
 #endif // hifi_DdeFaceTracker_h
\ No newline at end of file