Merge pull request #920 from ey6es/master

Added UDP support to Faceshift interface, eye widening based on "eye open" coefficients.

interface/src/Menu.cpp

@@ -370,11 +370,11 @@ Menu::Menu() :
                                            SLOT(setDepthOnly(bool)));
     
     addCheckableActionToQMenuAndActionHash(developerMenu,
-                                           MenuOption::Faceshift,
+                                           MenuOption::FaceshiftTCP,
                                            0,
                                            false,
                                            appInstance->getFaceshift(),
-                                           SLOT(setEnabled(bool)));
+                                           SLOT(setTCPEnabled(bool)));
                                            
     QMenu* audioDebugMenu = developerMenu->addMenu("Audio Debugging Tools");
     addCheckableActionToQMenuAndActionHash(audioDebugMenu, MenuOption::EchoAudio);

interface/src/Menu.h

@@ -137,7 +137,7 @@ namespace MenuOption {
     const QString ExportVoxels = "Export Voxels";
     const QString HeadMouse = "Head Mouse";
     const QString FaceMode = "Cycle Face Mode";
-    const QString Faceshift = "Faceshift";
+    const QString FaceshiftTCP = "Faceshift (TCP)";
     const QString FalseColorByDistance = "FALSE Color By Distance";
     const QString FalseColorBySource = "FALSE Color By Source";
     const QString FalseColorEveryOtherVoxel = "FALSE Color Every Other Randomly";

interface/src/avatar/Head.cpp

@@ -726,7 +726,7 @@ void Head::renderEyeBalls() {
         float angle = -67.5f - 50.0f * _leftEyeBlink;
         glRotatef(angle, 1, 0, 0);
         Application::getInstance()->getGeometryCache()->renderHemisphere(15, 10);
-        glRotatef(glm::mix(-angle, 180.0f, _leftEyeBlink), 1, 0, 0);
+        glRotatef(glm::mix(-angle, 180.0f, max(0.0f, _leftEyeBlink)), 1, 0, 0);
         Application::getInstance()->getGeometryCache()->renderHemisphere(15, 10);
     }
     glPopMatrix();
@@ -740,7 +740,7 @@ void Head::renderEyeBalls() {
         float angle = -67.5f - 50.0f * _rightEyeBlink; 
         glRotatef(angle, 1, 0, 0);
         Application::getInstance()->getGeometryCache()->renderHemisphere(15, 10);
-        glRotatef(glm::mix(-angle, 180.0f, _rightEyeBlink), 1, 0, 0);
+        glRotatef(glm::mix(-angle, 180.0f, max(0.0f, _rightEyeBlink)), 1, 0, 0);
         Application::getInstance()->getGeometryCache()->renderHemisphere(15, 10);
     }
     glPopMatrix();

interface/src/devices/Faceshift.cpp

@@ -8,33 +8,50 @@
 
 #include <QTimer>
 
+#include <SharedUtil.h>
+
 #include "Faceshift.h"
 
 using namespace fs;
 using namespace std;
 
+const quint16 FACESHIFT_PORT = 33433;
+
 Faceshift::Faceshift() :
-    _enabled(false),
+    _tcpEnabled(false),
+    _lastMessageReceived(0),
     _eyeGazeLeftPitch(0.0f),
     _eyeGazeLeftYaw(0.0f),
     _eyeGazeRightPitch(0.0f),
     _eyeGazeRightYaw(0.0f),
     _leftBlink(0.0f),
     _rightBlink(0.0f),
-    _leftBlinkIndex(-1),
+    _leftBlinkIndex(0), // see http://support.faceshift.com/support/articles/35129-export-of-blendshapes
-    _rightBlinkIndex(-1),
+    _rightBlinkIndex(1),
+    _leftEyeOpenIndex(8),
+    _rightEyeOpenIndex(9),
     _browHeight(0.0f),
-    _browUpCenterIndex(-1),
+    _browUpCenterIndex(16),
     _mouthSize(0.0f),
-    _jawOpenIndex(-1),
+    _jawOpenIndex(21),
     _longTermAverageEyePitch(0.0f),
     _longTermAverageEyeYaw(0.0f),
     _estimatedEyePitch(0.0f),
     _estimatedEyeYaw(0.0f)
 {
-    connect(&_socket, SIGNAL(connected()), SLOT(noteConnected()));
+    connect(&_tcpSocket, SIGNAL(connected()), SLOT(noteConnected()));
-    connect(&_socket, SIGNAL(error(QAbstractSocket::SocketError)), SLOT(noteError(QAbstractSocket::SocketError)));
+    connect(&_tcpSocket, SIGNAL(error(QAbstractSocket::SocketError)), SLOT(noteError(QAbstractSocket::SocketError)));
-    connect(&_socket, SIGNAL(readyRead()), SLOT(readFromSocket()));
+    connect(&_tcpSocket, SIGNAL(readyRead()), SLOT(readFromSocket()));
+    
+    connect(&_udpSocket, SIGNAL(readyRead()), SLOT(readPendingDatagrams()));
+    
+    _udpSocket.bind(FACESHIFT_PORT);
+}
+
+bool Faceshift::isActive() const {
+    const uint64_t ACTIVE_TIMEOUT_USECS = 1000000;
+    return (_tcpSocket.state() == QAbstractSocket::ConnectedState ||
+        (usecTimestampNow() - _lastMessageReceived) < ACTIVE_TIMEOUT_USECS) && _tracking;
 }
 
 void Faceshift::update() {
@@ -51,28 +68,27 @@ void Faceshift::update() {
 }
 
 void Faceshift::reset() {
-    if (isActive()) {
+    if (_tcpSocket.state() == QAbstractSocket::ConnectedState) {
         string message;
         fsBinaryStream::encode_message(message, fsMsgCalibrateNeutral());
         send(message);
     }
 }
 
-void Faceshift::setEnabled(bool enabled) {
+void Faceshift::setTCPEnabled(bool enabled) {
-    if ((_enabled = enabled)) {
+    if ((_tcpEnabled = enabled)) {
         connectSocket();
     
     } else {
-        _socket.disconnectFromHost();
+        _tcpSocket.disconnectFromHost();
     }
 }
 
 void Faceshift::connectSocket() {
-    if (_enabled) {
+    if (_tcpEnabled) {
         qDebug("Faceshift: Connecting...\n");
     
-        const quint16 FACESHIFT_PORT = 33433;
+        _tcpSocket.connectToHost("localhost", FACESHIFT_PORT);
-        _socket.connectToHost("localhost", FACESHIFT_PORT);
         _tracking = false;
     }
 }
@@ -87,16 +103,32 @@ void Faceshift::noteConnected() {
 }
 
 void Faceshift::noteError(QAbstractSocket::SocketError error) {
-    qDebug() << "Faceshift: " << _socket.errorString() << "\n";
+    qDebug() << "Faceshift: " << _tcpSocket.errorString() << "\n";
     
     // reconnect after a delay
-    if (_enabled) {
+    if (_tcpEnabled) {
         QTimer::singleShot(1000, this, SLOT(connectSocket()));
     }
 }
 
+void Faceshift::readPendingDatagrams() {
+    QByteArray buffer;
+    while (_udpSocket.hasPendingDatagrams()) {
+        buffer.resize(_udpSocket.pendingDatagramSize());
+        _udpSocket.readDatagram(buffer.data(), buffer.size());
+        receive(buffer);
+    }
+}
+
 void Faceshift::readFromSocket() {
-    QByteArray buffer = _socket.readAll();
+    receive(_tcpSocket.readAll());
+}
+
+void Faceshift::send(const std::string& message) {
+    _tcpSocket.write(message.data(), message.size());
+}
+
+void Faceshift::receive(const QByteArray& buffer) {
     _stream.received(buffer.size(), buffer.constData());
     fsMsgPtr msg;
     for (fsMsgPtr msg; (msg = _stream.get_message()); ) {
@@ -114,11 +146,12 @@ void Faceshift::readFromSocket() {
                     _eyeGazeRightPitch = -data.m_eyeGazeRightPitch;
                     _eyeGazeRightYaw = data.m_eyeGazeRightYaw;
                     
+                    const float EYE_OPEN_SCALE = 0.5f;
                     if (_leftBlinkIndex != -1) {
-                        _leftBlink = data.m_coeffs[_leftBlinkIndex];
+                        _leftBlink = data.m_coeffs[_leftBlinkIndex] - data.m_coeffs[_leftEyeOpenIndex] * EYE_OPEN_SCALE;
                     }
                     if (_rightBlinkIndex != -1) {
-                        _rightBlink = data.m_coeffs[_rightBlinkIndex];
+                        _rightBlink = data.m_coeffs[_rightBlinkIndex] - data.m_coeffs[_rightEyeOpenIndex] * EYE_OPEN_SCALE;
                     }
                     if (_browUpCenterIndex != -1) {
                         _browHeight = data.m_coeffs[_browUpCenterIndex];
@@ -137,7 +170,13 @@ void Faceshift::readFromSocket() {
                     
                     } else if (names[i] == "EyeBlink_R") {
                         _rightBlinkIndex = i;
-                        
+                    
+                    } else if (names[i] == "EyeOpen_L") {
+                        _leftEyeOpenIndex = i;
+                    
+                    } else if (names[i] == "EyeOpen_R") {
+                        _rightEyeOpenIndex = i;
+                    
                     } else if (names[i] == "BrowsU_C") {
                         _browUpCenterIndex = i;    
                         
@@ -151,8 +190,5 @@ void Faceshift::readFromSocket() {
                 break;
         }
     }
-}
+    _lastMessageReceived = usecTimestampNow();
-
-void Faceshift::send(const std::string& message) {
-    _socket.write(message.data(), message.size());
 }

interface/src/devices/Faceshift.h

@@ -10,6 +10,7 @@
 #define __interface__Faceshift__
 
 #include <QTcpSocket>
+#include <QUdpSocket>
 
 #include <glm/glm.hpp>
 #include <glm/gtc/quaternion.hpp>
@@ -24,7 +25,7 @@ public:
 
     Faceshift();
 
-    bool isActive() const { return _socket.state() == QAbstractSocket::ConnectedState && _tracking; }
+    bool isActive() const;
 
     const glm::quat& getHeadRotation() const { return _headRotation; }
     const glm::vec3& getHeadTranslation() const { return _headTranslation; }
@@ -50,23 +51,27 @@ public:
 
 public slots:
     
-    void setEnabled(bool enabled);
+    void setTCPEnabled(bool enabled);
 
 private slots:
 
     void connectSocket();
     void noteConnected();
     void noteError(QAbstractSocket::SocketError error);
+    void readPendingDatagrams();
     void readFromSocket();        
     
 private:
     
     void send(const std::string& message);
+    void receive(const QByteArray& buffer);
     
-    QTcpSocket _socket;
+    QTcpSocket _tcpSocket;
+    QUdpSocket _udpSocket;
     fs::fsBinaryStream _stream;
-    bool _enabled;
+    bool _tcpEnabled;
     bool _tracking;
+    uint64_t _lastMessageReceived;
     
     glm::quat _headRotation;
     glm::vec3 _headTranslation;
@@ -83,6 +88,9 @@ private:
     int _leftBlinkIndex;
     int _rightBlinkIndex;
     
+    int _leftEyeOpenIndex;
+    int _rightEyeOpenIndex;
+    
     float _browHeight;
     
     int _browUpCenterIndex;