From a12b90c65becd64ca9d78cf75ac123b6c49abf5b Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 17 Apr 2014 13:34:37 -0700
Subject: [PATCH] Add head translation to Faceplus.  Closes #2684.

---
 interface/src/Application.cpp      |  1 +
 interface/src/devices/Faceplus.cpp | 44 ++++++++++++++++++++++++------
 interface/src/devices/Faceplus.h   | 14 +++++++---
 3 files changed, 47 insertions(+), 12 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index bd7a82b439..978e457ae9 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -3020,6 +3020,7 @@ void Application::resetSensors() {
     _mouseX = _glWidget->width() / 2;
     _mouseY = _glWidget->height() / 2;
 
+	_faceplus.reset();
     _faceshift.reset();
     _visage.reset();
 
diff --git a/interface/src/devices/Faceplus.cpp b/interface/src/devices/Faceplus.cpp
index f7f2f1f1bd..f06433b663 100644
--- a/interface/src/devices/Faceplus.cpp
+++ b/interface/src/devices/Faceplus.cpp
@@ -40,9 +40,16 @@ void Faceplus::init() {
     updateEnabled();
 }
 
-void Faceplus::setState(const glm::quat& headRotation, float estimatedEyePitch, float estimatedEyeYaw,
-        const QVector<float>& blendshapeCoefficients) {
-    _headRotation = headRotation;
+void Faceplus::reset() {
+	if (_enabled) {
+		QMetaObject::invokeMethod(_reader, "reset");
+	}
+}
+
+void Faceplus::setState(const glm::vec3& headTranslation, const glm::quat& headRotation,
+		float estimatedEyePitch, float estimatedEyeYaw, const QVector<float>& blendshapeCoefficients) {
+    _headTranslation = headTranslation;
+	_headRotation = headRotation;
     _estimatedEyePitch = estimatedEyePitch;
     _estimatedEyeYaw = estimatedEyeYaw;
     _blendshapeCoefficients = blendshapeCoefficients;
@@ -149,7 +156,7 @@ FaceplusReader::~FaceplusReader() {
 
 void FaceplusReader::init() {
 #ifdef HAVE_FACEPLUS
-    if (!faceplus_init("VGA")) {
+    if (!faceplus_init("hHD")) {
         qDebug() << "Failed to initialized Faceplus.";
         return;
     }
@@ -190,7 +197,8 @@ void FaceplusReader::init() {
         }
     }
     _blendshapeCoefficients.resize(maxIndex + 1);
-    
+    _referenceInitialized = false;
+
     QMetaObject::invokeMethod(this, "update", Qt::QueuedConnection);
 #endif
 }
@@ -202,10 +210,24 @@ void FaceplusReader::shutdown() {
 
 void FaceplusReader::update() {
 #ifdef HAVE_FACEPLUS
-    if (!(faceplus_synchronous_track() && faceplus_current_output_vector(_outputVector.data()))) {
+    float x, y, rotation, scale;
+	if (!(faceplus_synchronous_track() && faceplus_current_face_location(&x, &y, &rotation, &scale) && !glm::isnan(x) &&
+			faceplus_current_output_vector(_outputVector.data()))) {
         QMetaObject::invokeMethod(this, "update", Qt::QueuedConnection);
         return;
     }
+	if (!_referenceInitialized) {
+		_referenceX = x;
+		_referenceY = y;
+		_referenceScale = scale;
+		_referenceInitialized = true;
+	}
+	const float TRANSLATION_SCALE = 10.0f;
+	const float REFERENCE_DISTANCE = 10.0f;
+	float depthScale = _referenceScale / scale;
+	float z = REFERENCE_DISTANCE * (depthScale - 1.0f);
+	glm::vec3 headTranslation((x - _referenceX) * depthScale * TRANSLATION_SCALE,
+		(y - _referenceY) * depthScale * TRANSLATION_SCALE, z);
     glm::quat headRotation(glm::radians(glm::vec3(-_outputVector.at(_headRotationIndices[0]),
         _outputVector.at(_headRotationIndices[1]), -_outputVector.at(_headRotationIndices[2]))));
     float estimatedEyePitch = (_outputVector.at(_leftEyeRotationIndices[0]) +
@@ -221,10 +243,16 @@ void FaceplusReader::update() {
         }
     }
 
-    QMetaObject::invokeMethod(Application::getInstance()->getFaceplus(), "setState", Q_ARG(const glm::quat&, headRotation),
-        Q_ARG(float, estimatedEyePitch), Q_ARG(float, estimatedEyeYaw), Q_ARG(const QVector<float>&, _blendshapeCoefficients));
+    QMetaObject::invokeMethod(Application::getInstance()->getFaceplus(), "setState", Q_ARG(const glm::vec3&, headTranslation),
+		Q_ARG(const glm::quat&, headRotation), Q_ARG(float, estimatedEyePitch), Q_ARG(float, estimatedEyeYaw),
+		Q_ARG(const QVector<float>&, _blendshapeCoefficients));
 
     QMetaObject::invokeMethod(this, "update", Qt::QueuedConnection);
 #endif
 }
 
+void FaceplusReader::reset() {
+#ifdef HAVE_FACEPLUS
+	_referenceInitialized = false;
+#endif
+}
diff --git a/interface/src/devices/Faceplus.h b/interface/src/devices/Faceplus.h
index 2b9219f3fd..f819dadca3 100644
--- a/interface/src/devices/Faceplus.h
+++ b/interface/src/devices/Faceplus.h
@@ -30,11 +30,12 @@ public:
     virtual ~Faceplus();
     
     void init();
-    
+    void reset();
+
     bool isActive() const { return _active; }
     
-    Q_INVOKABLE void setState(const glm::quat& headRotation, float estimatedEyePitch, float estimatedEyeYaw,
-        const QVector<float>& blendshapeCoefficients);
+    Q_INVOKABLE void setState(const glm::vec3& headTranslation, const glm::quat& headRotation,
+		float estimatedEyePitch, float estimatedEyeYaw, const QVector<float>& blendshapeCoefficients);
     
 public slots:
 
@@ -63,6 +64,7 @@ public:
     Q_INVOKABLE void init();
     Q_INVOKABLE void shutdown();
     Q_INVOKABLE void update();
+	Q_INVOKABLE void reset();
 
 private:
     
@@ -72,7 +74,11 @@ private:
     int _headRotationIndices[3];
     int _leftEyeRotationIndices[2];
     int _rightEyeRotationIndices[2];
-    QVector<float> _blendshapeCoefficients;
+    float _referenceX;
+	float _referenceY;
+	float _referenceScale;
+	bool _referenceInitialized;
+	QVector<float> _blendshapeCoefficients;
 #endif
 };