Read Faceshift head translation/rotation.

interface/src/avatar/MyAvatar.cpp

@@ -333,16 +333,25 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter, float gyroCam
 //  Update avatar head rotation with sensor data
 void MyAvatar::updateFromGyrosAndOrWebcam(bool gyroLook,
                                           float pitchFromTouch) {
+    Faceshift* faceshift = Application::getInstance()->getFaceshift();
     SerialInterface* gyros = Application::getInstance()->getSerialHeadSensor();
     Webcam* webcam = Application::getInstance()->getWebcam();
     glm::vec3 estimatedPosition, estimatedRotation;
-    if (gyros->isActive()) {
+    
+    if (faceshift->isActive()) {
+        estimatedPosition = faceshift->getHeadTranslation();
+        estimatedRotation = safeEulerAngles(faceshift->getHeadRotation());
+    
+    } else if (gyros->isActive()) {
         estimatedRotation = gyros->getEstimatedRotation();
+    
     } else if (webcam->isActive()) {
         estimatedRotation = webcam->getEstimatedRotation();
+    
     } else if (_leadingAvatar) {
         _head.getFace().clearFrame();
         return;
+    
     } else {
         _head.setMousePitch(pitchFromTouch);
         _head.setPitch(pitchFromTouch);

interface/src/devices/Faceshift.cpp

@@ -0,0 +1,51 @@
+//
+//  Faceshift.cpp
+//  interface
+//
+//  Created by Andrzej Kapolka on 9/3/13.
+//  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
+//
+
+#include "Faceshift.h"
+
+using namespace fs;
+
+Faceshift::Faceshift() : _enabled(false) {
+    connect(&_socket, SIGNAL(readyRead()), SLOT(readFromSocket()));
+    connect(&_socket, SIGNAL(error(QAbstractSocket::SocketError)), SLOT(connectSocket()));
+}
+
+void Faceshift::setEnabled(bool enabled) {
+    if (_enabled = enabled) {
+        connectSocket();
+    
+    } else {
+        _socket.disconnectFromHost();
+    }
+}
+
+void Faceshift::connectSocket() {
+    if (_enabled) {
+        const quint16 FACESHIFT_PORT = 33433;
+        _socket.connectToHost("localhost", FACESHIFT_PORT);
+    }
+}
+
+void Faceshift::readFromSocket() {
+    QByteArray buffer = _socket.readAll();
+    _stream.received(buffer.size(), buffer.constData());
+    fsMsgPtr msg;
+    for (fsMsgPtr msg; msg = _stream.get_message(); ) {
+        switch (msg->id()) {
+            case fsMsg::MSG_OUT_TRACKING_STATE:
+                const fsTrackingData& data = static_cast<fsMsgTrackingState*>(msg.get())->tracking_data();
+                if (data.m_trackingSuccessful) {
+                    _headRotation = glm::quat(data.m_headRotation.w, data.m_headRotation.x,
+                        data.m_headRotation.y, data.m_headRotation.z);
+                    _headTranslation = glm::vec3(data.m_headTranslation.x, data.m_headTranslation.y, data.m_headTranslation.z);
+                }
+                break;
+        }
+    }
+}
+

interface/src/devices/Faceshift.h

@@ -0,0 +1,51 @@
+//
+//  Faceshift.h
+//  interface
+//
+//  Created by Andrzej Kapolka on 9/3/13.
+//  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
+//
+
+#ifndef __interface__Faceshift__
+#define __interface__Faceshift__
+
+#include <QTcpSocket>
+
+#include <glm/glm.hpp>
+#include <glm/gtc/quaternion.hpp>
+
+#include <fsbinarystream.h>
+
+/// Handles interaction with the Faceshift software, which provides head position/orientation and facial features.
+class Faceshift : public QObject {
+    Q_OBJECT
+
+public:
+
+    Faceshift();
+
+    bool isActive() const { return _socket.state() == QAbstractSocket::ConnectedState; }
+
+    const glm::quat& getHeadRotation() const { return _headRotation; }
+    const glm::vec3& getHeadTranslation() const { return _headTranslation; }
+
+public slots:
+    
+    void setEnabled(bool enabled);
+
+private slots:
+
+    void connectSocket();
+    void readFromSocket();        
+    
+private:
+    
+    QTcpSocket _socket;
+    fs::fsBinaryStream _stream;
+    bool _enabled;
+    
+    glm::quat _headRotation;
+    glm::vec3 _headTranslation;
+};
+
+#endif /* defined(__interface__Faceshift__) */