Merge pull request #2870 from ey6es/priority

Basic support for joysticks using SDL, shoehorn PrioVR position/joysticks into palm data.

interface/CMakeLists.txt

@@ -136,6 +136,7 @@ find_package(Faceplus)
 find_package(Faceshift)
 find_package(LibOVR)
 find_package(PrioVR)
+find_package(SDL)
 find_package(Sixense)
 find_package(Visage)
 find_package(ZLIB)
@@ -193,6 +194,13 @@ if (PRIOVR_FOUND AND NOT DISABLE_PRIOVR)
     target_link_libraries(${TARGET_NAME} "${PRIOVR_LIBRARIES}")
 endif (PRIOVR_FOUND AND NOT DISABLE_PRIOVR)
 
+# and with SDL for joysticks
+if (SDL_FOUND AND NOT DISABLE_SDL)
+    add_definitions(-DHAVE_SDL)
+    include_directories(SYSTEM "${SDL_INCLUDE_DIR}")
+    target_link_libraries(${TARGET_NAME} "${SDL_LIBRARY}")
+endif (SDL_FOUND AND NOT DISABLE_SDL)
+
 # and with qxmpp for chat
 if (QXMPP_FOUND AND NOT DISABLE_QXMPP)
     add_definitions(-DHAVE_QXMPP -DQXMPP_STATIC)

interface/src/Application.cpp

@@ -1990,7 +1990,8 @@ void Application::update(float deltaTime) {
     _myAvatar->updateLookAtTargetAvatar();
     updateMyAvatarLookAtPosition();
     _sixenseManager.update(deltaTime);
-    _prioVR.update();
+    _joystickManager.update();
+    _prioVR.update(deltaTime);
     updateMyAvatar(deltaTime); // Sample hardware, update view frustum if needed, and send avatar data to mixer/nodes
     updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process...
     _avatarManager.updateOtherAvatars(deltaTime); //loop through all the other avatars and simulate them...

interface/src/Application.h

@@ -58,6 +58,7 @@
 #include "avatar/MyAvatar.h"
 #include "devices/Faceplus.h"
 #include "devices/Faceshift.h"
+#include "devices/JoystickManager.h"
 #include "devices/PrioVR.h"
 #include "devices/SixenseManager.h"
 #include "devices/Visage.h"
@@ -196,6 +197,7 @@ public:
     FaceTracker* getActiveFaceTracker();
     SixenseManager* getSixenseManager() { return &_sixenseManager; }
     PrioVR* getPrioVR() { return &_prioVR; }
+    JoystickManager* getJoystickManager() { return &_joystickManager; }
     BandwidthMeter* getBandwidthMeter() { return &_bandwidthMeter; }
     QUndoStack* getUndoStack() { return &_undoStack; }
 
@@ -449,6 +451,7 @@ private:
 
     SixenseManager _sixenseManager;
     PrioVR _prioVR;
+    JoystickManager _joystickManager;
 
     Camera _myCamera;                  // My view onto the world
     Camera _viewFrustumOffsetCamera;   // The camera we use to sometimes show the view frustum from an offset mode

interface/src/devices/JoystickManager.cpp

@@ -0,0 +1,64 @@
+//
+//  JoystickManager.cpp
+//  interface/src/devices
+//
+//  Created by Andrzej Kapolka on 5/15/14.
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include <limits>
+
+#include <QtDebug>
+
+#include <glm/glm.hpp>
+
+#include "JoystickManager.h"
+
+using namespace std;
+
+JoystickManager::JoystickManager() {
+#ifdef HAVE_SDL
+    SDL_Init(SDL_INIT_JOYSTICK);
+    int joystickCount = SDL_NumJoysticks();
+    for (int i = 0; i < joystickCount; i++) {
+        SDL_Joystick* joystick = SDL_JoystickOpen(i);
+        if (joystick) {
+            JoystickState state = { SDL_JoystickName(i), QVector<float>(SDL_JoystickNumAxes(joystick)),
+                QVector<bool>(SDL_JoystickNumButtons(joystick)) };
+            _joystickStates.append(state);
+            _joysticks.append(joystick);
+        }
+    }
+#endif
+}
+
+JoystickManager::~JoystickManager() {
+#ifdef HAVE_SDL
+    foreach (SDL_Joystick* joystick, _joysticks) {
+        SDL_JoystickClose(joystick);
+    }
+    SDL_Quit();
+#endif
+}
+
+void JoystickManager::update() {
+#ifdef HAVE_SDL
+    SDL_JoystickUpdate();
+    
+    for (int i = 0; i < _joystickStates.size(); i++) {
+        SDL_Joystick* joystick = _joysticks.at(i);
+        JoystickState& state = _joystickStates[i];
+        for (int j = 0; j < state.axes.size(); j++) {
+            float value = glm::round(SDL_JoystickGetAxis(joystick, j) + 0.5f) / numeric_limits<short>::max();
+            const float DEAD_ZONE = 0.1f;
+            state.axes[j] = glm::abs(value) < DEAD_ZONE ? 0.0f : value;
+        }
+        for (int j = 0; j < state.buttons.size(); j++) {
+            state.buttons[j] = SDL_JoystickGetButton(joystick, j);
+        }
+    }
+#endif
+}

interface/src/devices/JoystickManager.h

@@ -0,0 +1,53 @@
+//
+//  JoystickManager.h
+//  interface/src/devices
+//
+//  Created by Andrzej Kapolka on 5/15/14.
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_JoystickManager_h
+#define hifi_JoystickManager_h
+
+#include <QObject>
+#include <QVector>
+
+#ifdef HAVE_SDL
+#include <SDL.h>
+#undef main
+#endif
+
+class JoystickState;
+
+/// Handles joystick input through SDL.
+class JoystickManager : public QObject {
+    Q_OBJECT
+
+public:
+    
+    JoystickManager();
+    virtual ~JoystickManager();
+    
+    const QVector<JoystickState>& getJoystickStates() const { return _joystickStates; }
+    
+    void update();
+
+private:
+    QVector<JoystickState> _joystickStates;
+    
+#ifdef HAVE_SDL
+    QVector<SDL_Joystick*> _joysticks;
+#endif
+};
+
+class JoystickState {
+public:
+    QString name;
+    QVector<float> axes;
+    QVector<bool> buttons;
+};
+
+#endif // hifi_JoystickManager_h

interface/src/devices/PrioVR.cpp

@@ -9,6 +9,7 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
+#include <QTimer>
 #include <QtDebug>
 
 #include <FBXReader.h>
@@ -17,6 +18,7 @@
 #include "PrioVR.h"
 #include "ui/TextRenderer.h"
 
+#ifdef HAVE_PRIOVR
 const unsigned int SERIAL_LIST[] = { 0x00000001, 0x00000000, 0x00000008, 0x00000009, 0x0000000A,
     0x0000000C, 0x0000000D, 0x0000000E, 0x00000004, 0x00000005, 0x00000010, 0x00000011 };
 const unsigned char AXIS_LIST[] = { 9, 43, 37, 37, 37, 13, 13, 13, 52, 52, 28, 28 };
@@ -25,7 +27,6 @@ const int LIST_LENGTH = sizeof(SERIAL_LIST) / sizeof(SERIAL_LIST[0]);
 const char* JOINT_NAMES[] = { "Neck", "Spine", "LeftArm", "LeftForeArm", "LeftHand", "RightArm",
     "RightForeArm", "RightHand", "LeftUpLeg", "LeftLeg", "RightUpLeg", "RightLeg" };  
 
-#ifdef HAVE_PRIOVR
 static int indexOfHumanIKJoint(const char* jointName) {
     for (int i = 0;; i++) {
         QByteArray humanIKJoint = HUMANIK_JOINTS[i];
@@ -37,6 +38,87 @@ static int indexOfHumanIKJoint(const char* jointName) {
         }
     }
 }
+
+static void setPalm(float deltaTime, int index) {
+    MyAvatar* avatar = Application::getInstance()->getAvatar();
+    Hand* hand = avatar->getHand();
+    PalmData* palm;
+    bool foundHand = false;
+    for (size_t j = 0; j < hand->getNumPalms(); j++) {
+        if (hand->getPalms()[j].getSixenseID() == index) {
+            palm = &(hand->getPalms()[j]);
+            foundHand = true;
+        }
+    }
+    if (!foundHand) {
+        PalmData newPalm(hand);
+        hand->getPalms().push_back(newPalm);
+        palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
+        palm->setSixenseID(index);
+    }
+    
+    palm->setActive(true);
+    
+    // Read controller buttons and joystick into the hand
+    if (!Application::getInstance()->getJoystickManager()->getJoystickStates().isEmpty()) {
+        const JoystickState& state = Application::getInstance()->getJoystickManager()->getJoystickStates().at(0);
+        if (state.axes.size() >= 4 && state.buttons.size() >= 4) {
+            if (index == SIXENSE_CONTROLLER_ID_LEFT_HAND) {
+                palm->setControllerButtons(state.buttons.at(1) ? BUTTON_FWD : 0);
+                palm->setTrigger(state.buttons.at(0) ? 1.0f : 0.0f);
+                palm->setJoystick(state.axes.at(0), -state.axes.at(1));
+                
+            } else {
+                palm->setControllerButtons(state.buttons.at(3) ? BUTTON_FWD : 0);
+                palm->setTrigger(state.buttons.at(2) ? 1.0f : 0.0f);
+                palm->setJoystick(state.axes.at(2), -state.axes.at(3)); 
+            }
+        }
+    }
+    
+    glm::vec3 position;
+    glm::quat rotation;
+    
+    Model* skeletonModel = &Application::getInstance()->getAvatar()->getSkeletonModel();
+    int jointIndex;
+    glm::quat inverseRotation = glm::inverse(Application::getInstance()->getAvatar()->getOrientation());
+    if (index == SIXENSE_CONTROLLER_ID_LEFT_HAND) {
+        jointIndex = skeletonModel->getLeftHandJointIndex();
+        skeletonModel->getJointRotation(jointIndex, rotation, true);      
+        rotation = inverseRotation * rotation * glm::quat(glm::vec3(0.0f, PI_OVER_TWO, 0.0f));
+        
+    } else {
+        jointIndex = skeletonModel->getRightHandJointIndex();
+        skeletonModel->getJointRotation(jointIndex, rotation, true);
+        rotation = inverseRotation * rotation * glm::quat(glm::vec3(0.0f, -PI_OVER_TWO, 0.0f));
+    }
+    skeletonModel->getJointPosition(jointIndex, position);
+    position = inverseRotation * (position - skeletonModel->getTranslation());
+    
+    palm->setRawRotation(rotation);
+    
+    //  Compute current velocity from position change
+    glm::vec3 rawVelocity;
+    if (deltaTime > 0.f) {
+        rawVelocity = (position - palm->getRawPosition()) / deltaTime; 
+    } else {
+        rawVelocity = glm::vec3(0.0f);
+    }
+    palm->setRawVelocity(rawVelocity);
+    palm->setRawPosition(position);
+    
+    // Store the one fingertip in the palm structure so we can track velocity
+    const float FINGER_LENGTH = 0.3f;   //  meters
+    const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
+    const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
+    glm::vec3 oldTipPosition = palm->getTipRawPosition();
+    if (deltaTime > 0.f) {
+        palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
+    } else {
+        palm->setTipVelocity(glm::vec3(0.f));
+    }
+    palm->setTipPosition(newTipPosition);
+}
 #endif
 
 PrioVR::PrioVR() {
@@ -78,7 +160,7 @@ glm::quat PrioVR::getTorsoRotation() const {
     return _jointRotations.size() > TORSO_ROTATION_INDEX ? _jointRotations.at(TORSO_ROTATION_INDEX) : glm::quat();
 }
 
-void PrioVR::update() {
+void PrioVR::update(float deltaTime) {
 #ifdef HAVE_PRIOVR
     if (!_skeletalDevice) {
         return;
@@ -96,6 +178,10 @@ void PrioVR::update() {
         _lastJointRotations[i] = _jointRotations.at(i);
         _jointRotations[i] = safeMix(lastRotation, _jointRotations.at(i), 0.5f);
     }
+    
+    // convert the joysticks into palm data
+    setPalm(deltaTime, SIXENSE_CONTROLLER_ID_LEFT_HAND);
+    setPalm(deltaTime, SIXENSE_CONTROLLER_ID_RIGHT_HAND);
 #endif
 }
 

interface/src/devices/PrioVR.h

@@ -43,7 +43,7 @@ public:
     const QVector<int>& getHumanIKJointIndices() const { return _humanIKJointIndices; }
     const QVector<glm::quat>& getJointRotations() const { return _jointRotations; }
     
-    void update();
+    void update(float deltaTime);
     void reset();
 
 private slots:

interface/src/scripting/ControllerScriptingInterface.cpp

@@ -74,7 +74,7 @@ bool ControllerScriptingInterface::isPrimaryButtonPressed() const {
             return true;
         }
     }
-
+    
     return false;
 }
 

libraries/fbx/src/FBXReader.cpp

@@ -585,6 +585,7 @@ const char* HUMANIK_JOINTS[] = {
     "LeftArm",
     "LeftForeArm",
     "LeftHand",
+    "Neck",
     "Spine",
     "Hips",
     "RightUpLeg",