From 1f2fe5ddee553a0a8e5772f382160dde7a4ebffe Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Fri, 15 Nov 2013 18:06:32 -0800
Subject: [PATCH 01/16] Rudimentary Sixense (Razer Hydra) support.

---
 interface/CMakeLists.txt      | 4 ++++
 interface/src/Application.cpp | 8 ++++++++
 interface/src/Application.h   | 4 ++++
 interface/src/avatar/Hand.cpp | 2 +-
 4 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt
index 4709bf234d..6629398bf7 100644
--- a/interface/CMakeLists.txt
+++ b/interface/CMakeLists.txt
@@ -15,6 +15,7 @@ set(LEAP_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/Leap)
 set(MOTIONDRIVER_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/MotionDriver)
 set(PORTAUDIO_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/PortAudio)
 set(OPENCV_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/OpenCV)
+set(SIXENSE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/Sixense)
 set(UVCCAMERACONTROL_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/external/UVCCameraControl)
 
 if (APPLE)
@@ -95,6 +96,7 @@ find_package(Leap)
 find_package(MotionDriver)
 find_package(OpenCV)
 find_package(OpenNI)
+find_package(Sixense)
 find_package(UVCCameraControl)
 find_package(ZLIB)
 
@@ -125,6 +127,7 @@ include_directories(
   ${LEAP_INCLUDE_DIRS}
   ${MOTIONDRIVER_INCLUDE_DIRS}
   ${OPENCV_INCLUDE_DIRS}
+  ${SIXENSE_INCLUDE_DIRS}
 )
 
 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -isystem ${OPENCV_INCLUDE_DIRS}")
@@ -135,6 +138,7 @@ target_link_libraries(
   ${MOTIONDRIVER_LIBRARIES}
   ${OPENCV_LIBRARIES} 
   ${ZLIB_LIBRARIES}
+  ${SIXENSE_LIBRARIES}
 )
 
 if (APPLE)
diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 628a91c69b..ca56301c8e 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -2205,6 +2205,13 @@ void Application::updateLeap(float deltaTime) {
     LeapManager::nextFrame(_myAvatar);
 }
 
+void Application::updateSixense() {
+    bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
+    PerformanceWarning warn(showWarnings, "Application::updateSixense()");
+    
+    _sixenseManager.update();
+}
+
 void Application::updateSerialDevices(float deltaTime) {
     bool showWarnings = Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
     PerformanceWarning warn(showWarnings, "Application::updateSerialDevices()");
@@ -2412,6 +2419,7 @@ void Application::update(float deltaTime) {
     updateMouseVoxels(deltaTime, mouseRayOrigin, mouseRayDirection, distance, face); // UI/UX related to voxels
     updateHandAndTouch(deltaTime); // Update state for touch sensors
     updateLeap(deltaTime); // Leap finger-sensing device
+    updateSixense(); // Razer Hydra controllers
     updateSerialDevices(deltaTime); // Read serial port interface devices
     updateAvatar(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...
diff --git a/interface/src/Application.h b/interface/src/Application.h
index 78530ecea2..945de07303 100644
--- a/interface/src/Application.h
+++ b/interface/src/Application.h
@@ -50,6 +50,7 @@
 #include "avatar/HandControl.h"
 #include "devices/Faceshift.h"
 #include "devices/SerialInterface.h"
+#include "devices/SixenseManager.h"
 #include "devices/Webcam.h"
 #include "renderer/AmbientOcclusionEffect.h"
 #include "renderer/GeometryCache.h"
@@ -242,6 +243,7 @@ private:
         glm::vec3& eyePosition);
     void updateHandAndTouch(float deltaTime);
     void updateLeap(float deltaTime);
+    void updateSixense();
     void updateSerialDevices(float deltaTime);
     void updateThreads(float deltaTime);
     void updateMyAvatarSimulation(float deltaTime);
@@ -337,6 +339,8 @@ private:
     
     Faceshift _faceshift;
     
+    SixenseManager _sixenseManager;
+    
     Camera _myCamera;                  // My view onto the world
     Camera _viewFrustumOffsetCamera;   // The camera we use to sometimes show the view frustum from an offset mode
     Camera _mirrorCamera;              // Cammera for mirror view
diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 2ee8aad4ae..ae22752acb 100755
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -264,7 +264,7 @@ void Hand::renderLeapHands() {
     for (size_t i = 0; i < getNumPalms(); ++i) {
         PalmData& palm = getPalms()[i];
         if (palm.isActive()) {
-            const float palmThickness = 0.002f;
+            const float palmThickness = 0.02f;
             glColor4f(handColor.r, handColor.g, handColor.b, 0.25);
             glm::vec3 tip = palm.getPosition();
             glm::vec3 root = palm.getPosition() + palm.getNormal() * palmThickness;

From 9c401607b101784884e251005c6127098d8adcb8 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Fri, 15 Nov 2013 18:47:25 -0800
Subject: [PATCH 02/16] Missed the actual files.

---
 cmake/modules/FindSixense.cmake          | 44 ++++++++++++++++++++
 interface/src/devices/SixenseManager.cpp | 53 ++++++++++++++++++++++++
 interface/src/devices/SixenseManager.h   | 22 ++++++++++
 3 files changed, 119 insertions(+)
 create mode 100644 cmake/modules/FindSixense.cmake
 create mode 100644 interface/src/devices/SixenseManager.cpp
 create mode 100644 interface/src/devices/SixenseManager.h

diff --git a/cmake/modules/FindSixense.cmake b/cmake/modules/FindSixense.cmake
new file mode 100644
index 0000000000..b8692d4fb5
--- /dev/null
+++ b/cmake/modules/FindSixense.cmake
@@ -0,0 +1,44 @@
+#  Try to find the Sixense controller library
+#
+#  You must provide a SIXENSE_ROOT_DIR which contains lib and include directories
+#
+#  Once done this will define
+#
+#  SIXENSE_FOUND - system found Sixense
+#  SIXENSE_INCLUDE_DIRS - the Sixense include directory
+#  SIXENSE_LIBRARIES - Link this to use Sixense
+#
+#  Created on 11/15/2013 by Andrzej Kapolka
+#  Copyright (c) 2013 High Fidelity
+#
+
+if (SIXENSE_LIBRARIES AND SIXENSE_INCLUDE_DIRS)
+  # in cache already
+  set(SIXENSE_FOUND TRUE)
+else (SIXENSE_LIBRARIES AND SIXENSE_INCLUDE_DIRS)
+  find_path(SIXENSE_INCLUDE_DIRS sixense.h ${SIXENSE_ROOT_DIR}/include)
+
+  if (APPLE)
+    find_library(SIXENSE_LIBRARIES libsixense.dylib ${SIXENSE_ROOT_DIR}/lib/MacOS/)
+  elseif (UNIX)
+    find_library(SIXENSE_LIBRARIES libsixense_x64.so ${SIXENSE_ROOT_DIR}/lib/UNIX/)
+  endif ()
+
+  if (SIXENSE_INCLUDE_DIRS AND SIXENSE_LIBRARIES)
+     set(SIXENSE_FOUND TRUE)
+  endif (SIXENSE_INCLUDE_DIRS AND SIXENSE_LIBRARIES)
+ 
+  if (SIXENSE_FOUND)
+    if (NOT SIXENSE_FIND_QUIETLY)
+      message(STATUS "Found Sixense: ${SIXENSE_LIBRARIES}")
+    endif (NOT SIXENSE_FIND_QUIETLY)
+  else (SIXENSE_FOUND)
+    if (SIXENSE_FIND_REQUIRED)
+      message(FATAL_ERROR "Could not find Sixense")
+    endif (SIXENSE_FIND_REQUIRED)
+  endif (SIXENSE_FOUND)
+
+  # show the SIXENSE_INCLUDE_DIRS and SIXENSE_LIBRARIES variables only in the advanced view
+  mark_as_advanced(SIXENSE_INCLUDE_DIRS SIXENSE_LIBRARIES)
+
+endif (SIXENSE_LIBRARIES AND SIXENSE_INCLUDE_DIRS)
diff --git a/interface/src/devices/SixenseManager.cpp b/interface/src/devices/SixenseManager.cpp
new file mode 100644
index 0000000000..24110da227
--- /dev/null
+++ b/interface/src/devices/SixenseManager.cpp
@@ -0,0 +1,53 @@
+//
+//  Sixense.cpp
+//  interface
+//
+//  Created by Andrzej Kapolka on 11/15/13.
+//  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
+//
+
+#include "sixense.h"
+
+#include "Application.h"
+#include "SixenseManager.h"
+
+SixenseManager::SixenseManager() {
+    sixenseInit();
+}
+
+SixenseManager::~SixenseManager() {
+    sixenseExit();
+}
+    
+void SixenseManager::update() {
+    if (sixenseGetNumActiveControllers() == 0) {
+        return;
+    }
+    Hand& hand = Application::getInstance()->getAvatar()->getHand();
+    hand.getPalms().clear();
+    
+    int maxControllers = sixenseGetMaxControllers();
+    for (int i = 0; i < maxControllers; i++) {
+        if (!sixenseIsControllerEnabled(i)) {
+            continue;
+        }
+        sixenseControllerData data;
+        sixenseGetNewestData(i, &data);
+        PalmData palm(&hand);
+        palm.setActive(true);
+        glm::vec3 position(-data.pos[0], data.pos[1], -data.pos[2]);
+        palm.setRawPosition(position);
+        glm::quat rotation(data.rot_quat[3], -data.rot_quat[0], data.rot_quat[1], -data.rot_quat[2]);
+        palm.setRawNormal(rotation * glm::vec3(0.0f, -1.0f, 0.0f));
+        FingerData finger(&palm, &hand);
+        finger.setActive(true);
+        finger.setRawRootPosition(position);
+        finger.setRawTipPosition(position + rotation * glm::vec3(0.0f, 0.0f, 100.0f));
+        palm.getFingers().clear();
+        palm.getFingers().push_back(finger);
+        palm.getFingers().push_back(finger);
+        palm.getFingers().push_back(finger);
+        hand.getPalms().push_back(palm);
+    }   
+}
+
diff --git a/interface/src/devices/SixenseManager.h b/interface/src/devices/SixenseManager.h
new file mode 100644
index 0000000000..90b8cf988f
--- /dev/null
+++ b/interface/src/devices/SixenseManager.h
@@ -0,0 +1,22 @@
+//
+//  SixenseManager.h
+//  interface
+//
+//  Created by Andrzej Kapolka on 11/15/13.
+//  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
+//
+
+#ifndef __interface__SixenseManager__
+#define __interface__SixenseManager__
+
+/// Handles interaction with the Sixense SDK (e.g., Razer Hydra).
+class SixenseManager {
+public:
+    
+    SixenseManager();
+    ~SixenseManager();
+    
+    void update();
+};
+
+#endif /* defined(__interface__SixenseManager__) */

From 0231ed133cae4af1ae2df74e9c62f9d3e7773a4b Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 18 Nov 2013 12:09:52 -0800
Subject: [PATCH 03/16] Optional compilation for Sixense.

---
 cmake/modules/FindSixense.cmake          |  4 ++--
 interface/CMakeLists.txt                 | 10 ++++++++--
 interface/external/Sixense/readme.txt    | 11 +++++++++++
 interface/src/devices/SixenseManager.cpp | 25 ++++++++++++++++++++----
 4 files changed, 42 insertions(+), 8 deletions(-)
 create mode 100644 interface/external/Sixense/readme.txt

diff --git a/cmake/modules/FindSixense.cmake b/cmake/modules/FindSixense.cmake
index b8692d4fb5..cf23b2218c 100644
--- a/cmake/modules/FindSixense.cmake
+++ b/cmake/modules/FindSixense.cmake
@@ -19,9 +19,9 @@ else (SIXENSE_LIBRARIES AND SIXENSE_INCLUDE_DIRS)
   find_path(SIXENSE_INCLUDE_DIRS sixense.h ${SIXENSE_ROOT_DIR}/include)
 
   if (APPLE)
-    find_library(SIXENSE_LIBRARIES libsixense.dylib ${SIXENSE_ROOT_DIR}/lib/MacOS/)
+    find_library(SIXENSE_LIBRARIES libsixense_x64.dylib ${SIXENSE_ROOT_DIR}/lib/osx_x64/release_dll)
   elseif (UNIX)
-    find_library(SIXENSE_LIBRARIES libsixense_x64.so ${SIXENSE_ROOT_DIR}/lib/UNIX/)
+    find_library(SIXENSE_LIBRARIES libsixense_x64.so ${SIXENSE_ROOT_DIR}/lib/linux_x64/release)
   endif ()
 
   if (SIXENSE_INCLUDE_DIRS AND SIXENSE_LIBRARIES)
diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt
index 6629398bf7..735989dfbf 100644
--- a/interface/CMakeLists.txt
+++ b/interface/CMakeLists.txt
@@ -108,6 +108,14 @@ if (OPENNI_FOUND AND NOT DISABLE_OPENNI)
     target_link_libraries(${TARGET_NAME} ${OPENNI_LIBRARIES})
 endif (OPENNI_FOUND AND NOT DISABLE_OPENNI)
 
+# likewise with Sixense library for Razer Hydra
+if (SIXENSE_FOUND AND NOT DISABLE_SIXENSE)
+    add_definitions(-DHAVE_SIXENSE)
+    include_directories(SYSTEM ${SIXENSE_INCLUDE_DIRS})
+    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -isystem ${SIXENSE_INCLUDE_DIRS}")
+    target_link_libraries(${TARGET_NAME} ${SIXENSE_LIBRARIES})
+endif (SIXENSE_FOUND AND NOT DISABLE_SIXENSE)
+
 qt5_use_modules(${TARGET_NAME} Core Gui Multimedia Network OpenGL Svg WebKit WebKitWidgets)
 
 # include headers for interface and InterfaceConfig.
@@ -127,7 +135,6 @@ include_directories(
   ${LEAP_INCLUDE_DIRS}
   ${MOTIONDRIVER_INCLUDE_DIRS}
   ${OPENCV_INCLUDE_DIRS}
-  ${SIXENSE_INCLUDE_DIRS}
 )
 
 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -isystem ${OPENCV_INCLUDE_DIRS}")
@@ -138,7 +145,6 @@ target_link_libraries(
   ${MOTIONDRIVER_LIBRARIES}
   ${OPENCV_LIBRARIES} 
   ${ZLIB_LIBRARIES}
-  ${SIXENSE_LIBRARIES}
 )
 
 if (APPLE)
diff --git a/interface/external/Sixense/readme.txt b/interface/external/Sixense/readme.txt
new file mode 100644
index 0000000000..4672765b25
--- /dev/null
+++ b/interface/external/Sixense/readme.txt
@@ -0,0 +1,11 @@
+
+Instructions for adding the Sixense driver to Interface
+Andrzej Kapolka, November 18, 2013
+
+NOTE: Without doing step 2, you will crash at program start time.
+
+1. Copy the Sixense sdk folders (lib, include) into the interface/external/Sixense folder. This readme.txt should be there as well.
+
+2. IMPORTANT: Copy the file interface/external/Leap/lib/libc++/libLeap.dylib to /usr/lib
+
+3. Delete your build directory, run cmake and build, and you should be all set.
diff --git a/interface/src/devices/SixenseManager.cpp b/interface/src/devices/SixenseManager.cpp
index 24110da227..96481db4c8 100644
--- a/interface/src/devices/SixenseManager.cpp
+++ b/interface/src/devices/SixenseManager.cpp
@@ -6,20 +6,27 @@
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 //
 
-#include "sixense.h"
+#ifdef HAVE_SIXENSE
+    #include "sixense.h"
+#endif
 
 #include "Application.h"
 #include "SixenseManager.h"
 
 SixenseManager::SixenseManager() {
+#ifdef HAVE_SIXENSE
     sixenseInit();
+#endif
 }
 
 SixenseManager::~SixenseManager() {
+#ifdef HAVE_SIXENSE
     sixenseExit();
+#endif
 }
     
 void SixenseManager::update() {
+#ifdef HAVE_SIXENSE
     if (sixenseGetNumActiveControllers() == 0) {
         return;
     }
@@ -33,21 +40,31 @@ void SixenseManager::update() {
         }
         sixenseControllerData data;
         sixenseGetNewestData(i, &data);
+        
+        // set palm position and normal based on Hydra position/orientation
         PalmData palm(&hand);
         palm.setActive(true);
         glm::vec3 position(-data.pos[0], data.pos[1], -data.pos[2]);
         palm.setRawPosition(position);
         glm::quat rotation(data.rot_quat[3], -data.rot_quat[0], data.rot_quat[1], -data.rot_quat[2]);
-        palm.setRawNormal(rotation * glm::vec3(0.0f, -1.0f, 0.0f));
+        const glm::vec3 PALM_VECTOR(0.0f, -1.0f, 0.0f);
+        palm.setRawNormal(rotation * PALM_VECTOR);
+        
+        // initialize the "finger" based on the direction
         FingerData finger(&palm, &hand);
         finger.setActive(true);
         finger.setRawRootPosition(position);
-        finger.setRawTipPosition(position + rotation * glm::vec3(0.0f, 0.0f, 100.0f));
+        const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, 100.0f);
+        finger.setRawTipPosition(position + rotation * FINGER_VECTOR);
+        
+        // three fingers indicates to the skeleton that we have enough data to determine direction
         palm.getFingers().clear();
         palm.getFingers().push_back(finger);
         palm.getFingers().push_back(finger);
         palm.getFingers().push_back(finger);
+        
         hand.getPalms().push_back(palm);
-    }   
+    }
+#endif
 }
 

From 6d9bd6d2b0e1e392c95dacd7065ada05579f8dba Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 18 Nov 2013 12:16:25 -0800
Subject: [PATCH 04/16] Gitignore additions.

---
 .gitignore | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/.gitignore b/.gitignore
index b90059c479..62686287bc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -35,5 +35,9 @@ interface/external/Leap/lib/
 interface/external/Leap/samples/
 interface/external/Leap/util/
 
+# Ignore Sixense
+interface/external/Sixense/include/
+interface/external/Sixense/lib/
 
-
+# Ignore interfaceCache for Linux users
+interface/interfaceCache/

From 83d89cdc30b0beeb866f537bec6ad1b276997582 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 18 Nov 2013 12:34:38 -0800
Subject: [PATCH 05/16] Path fix for readme.

---
 interface/external/Sixense/readme.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/interface/external/Sixense/readme.txt b/interface/external/Sixense/readme.txt
index 4672765b25..dd6cbdd96c 100644
--- a/interface/external/Sixense/readme.txt
+++ b/interface/external/Sixense/readme.txt
@@ -6,6 +6,6 @@ NOTE: Without doing step 2, you will crash at program start time.
 
 1. Copy the Sixense sdk folders (lib, include) into the interface/external/Sixense folder. This readme.txt should be there as well.
 
-2. IMPORTANT: Copy the file interface/external/Leap/lib/libc++/libLeap.dylib to /usr/lib
+2. IMPORTANT: Copy the file interface/external/Sixense/lib/osx_x64/release_dll/libsixense_x64.dylib to /usr/lib
 
 3. Delete your build directory, run cmake and build, and you should be all set.

From f59b403103dda931b0036255c2c00b68b1fbcd2f Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 18 Nov 2013 16:20:00 -0800
Subject: [PATCH 06/16] Drive avatar with Sixense joysticks and triggers.

---
 interface/src/avatar/Avatar.h            |  4 +++-
 interface/src/avatar/MyAvatar.cpp        | 29 ++++++++++++++----------
 interface/src/avatar/MyAvatar.h          |  5 ++--
 interface/src/devices/SixenseManager.cpp | 19 +++++++++++++++-
 4 files changed, 41 insertions(+), 16 deletions(-)

diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h
index 4258ce6f14..aad3e4ce72 100755
--- a/interface/src/avatar/Avatar.h
+++ b/interface/src/avatar/Avatar.h
@@ -100,7 +100,9 @@ enum DriveKeys {
     UP,
     DOWN,
     ROT_LEFT, 
-    ROT_RIGHT, 
+    ROT_RIGHT,
+    ROT_UP,
+    ROT_DOWN,
     MAX_DRIVE_KEYS
 };
 
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 3ce54d7032..3343f5c920 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -27,6 +27,7 @@ using namespace std;
 
 const glm::vec3 DEFAULT_UP_DIRECTION(0.0f, 1.0f, 0.0f);
 const float YAW_MAG = 500.0;
+const float PITCH_MAG = 100.0f;
 const float COLLISION_RADIUS_SCALAR = 1.2; // pertains to avatar-to-avatar collisions
 const float COLLISION_BALL_FORCE = 200.0; // pertains to avatar-to-avatar collisions
 const float COLLISION_BODY_FORCE = 30.0; // pertains to avatar-to-avatar collisions
@@ -42,6 +43,7 @@ MyAvatar::MyAvatar(Node* owningNode) :
     _mousePressed(false),
     _bodyPitchDelta(0.0f),
     _bodyRollDelta(0.0f),
+    _mousePitchDelta(0.0f),
     _shouldJump(false),
     _gravity(0.0f, -1.0f, 0.0f),
     _distanceToNearestAvatar(std::numeric_limits<float>::max()),
@@ -57,7 +59,7 @@ MyAvatar::MyAvatar(Node* owningNode) :
     _moveTargetStepCounter(0)
 {
     for (int i = 0; i < MAX_DRIVE_KEYS; i++) {
-        _driveKeys[i] = false;
+        _driveKeys[i] = 0.0f;
     }
 
     _collisionRadius = _height * COLLISION_RADIUS_SCALE;   
@@ -395,8 +397,8 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
     
     } else {
         if (!_leadingAvatar) {
-            _head.setMousePitch(pitchFromTouch);
-            _head.setPitch(pitchFromTouch);
+            _head.setMousePitch(pitchFromTouch + _mousePitchDelta);
+            _head.setPitch(pitchFromTouch + _mousePitchDelta);
         }
         _head.getVideoFace().clearFrame();
         
@@ -408,7 +410,7 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
         _head.setLeanForward(glm::mix(_head.getLeanForward(), 0.0f, RESTORE_RATE));
         return;
     }
-    _head.setMousePitch(pitchFromTouch);
+    _head.setMousePitch(pitchFromTouch + _mousePitchDelta);
 
     if (webcam->isActive()) {
         estimatedPosition = webcam->getEstimatedPosition();
@@ -707,14 +709,17 @@ void MyAvatar::updateThrust(float deltaTime, Transmitter * transmitter) {
     const float THRUST_JUMP = 120.f;
     
     //  Add Thrusts from keyboard
-    if (_driveKeys[FWD]) {_thrust += _scale * THRUST_MAG_FWD * _thrustMultiplier * deltaTime * front;}
-    if (_driveKeys[BACK]) {_thrust -= _scale * THRUST_MAG_BACK *  _thrustMultiplier * deltaTime * front;}
-    if (_driveKeys[RIGHT]) {_thrust += _scale * THRUST_MAG_LATERAL * _thrustMultiplier * deltaTime * right;}
-    if (_driveKeys[LEFT]) {_thrust -= _scale * THRUST_MAG_LATERAL * _thrustMultiplier * deltaTime * right;}
-    if (_driveKeys[UP]) {_thrust += _scale * THRUST_MAG_UP * _thrustMultiplier * deltaTime * up;}
-    if (_driveKeys[DOWN]) {_thrust -= _scale * THRUST_MAG_DOWN * _thrustMultiplier * deltaTime * up;}
-    if (_driveKeys[ROT_RIGHT]) {_bodyYawDelta -= YAW_MAG * deltaTime;}
-    if (_driveKeys[ROT_LEFT]) {_bodyYawDelta += YAW_MAG * deltaTime;}
+    _thrust += _driveKeys[FWD] * _scale * THRUST_MAG_FWD * _thrustMultiplier * deltaTime * front;
+    _thrust -= _driveKeys[BACK] * _scale * THRUST_MAG_BACK *  _thrustMultiplier * deltaTime * front;
+    _thrust += _driveKeys[RIGHT] * _scale * THRUST_MAG_LATERAL * _thrustMultiplier * deltaTime * right;
+    _thrust -= _driveKeys[LEFT] * _scale * THRUST_MAG_LATERAL * _thrustMultiplier * deltaTime * right;
+    _thrust += _driveKeys[UP] * _scale * THRUST_MAG_UP * _thrustMultiplier * deltaTime * up;
+    _thrust -= _driveKeys[DOWN] * _scale * THRUST_MAG_DOWN * _thrustMultiplier * deltaTime * up;
+    _bodyYawDelta -= _driveKeys[ROT_RIGHT] * YAW_MAG * deltaTime;
+    _bodyYawDelta += _driveKeys[ROT_LEFT] * YAW_MAG * deltaTime;
+    const float MAX_PITCH = 90.0f;
+    _mousePitchDelta = min(_mousePitchDelta + _driveKeys[ROT_UP] * PITCH_MAG * deltaTime, MAX_PITCH);
+    _mousePitchDelta = max(_mousePitchDelta - _driveKeys[ROT_DOWN] * PITCH_MAG * deltaTime, -MAX_PITCH);
     
     //  If thrust keys are being held down, slowly increase thrust to allow reaching great speeds
     if (_driveKeys[FWD] || _driveKeys[BACK] || _driveKeys[RIGHT] || _driveKeys[LEFT] || _driveKeys[UP] || _driveKeys[DOWN]) {
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 790510536f..9398a0d52d 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -54,7 +54,7 @@ public:
     void loadData(QSettings* settings);
 
     //  Set what driving keys are being pressed to control thrust levels
-    void setDriveKeys(int key, bool val) { _driveKeys[key] = val; };
+    void setDriveKeys(int key, float val) { _driveKeys[key] = val; };
     bool getDriveKeys(int key) { return _driveKeys[key]; };
     void jump() { _shouldJump = true; };
 
@@ -66,8 +66,9 @@ private:
     bool _mousePressed;
     float _bodyPitchDelta;
     float _bodyRollDelta;
+    float _mousePitchDelta;
     bool _shouldJump;
-    int _driveKeys[MAX_DRIVE_KEYS];
+    float _driveKeys[MAX_DRIVE_KEYS];
     glm::vec3 _gravity;
     float _distanceToNearestAvatar; // How close is the nearest avatar?
     Avatar* _interactingOther;
diff --git a/interface/src/devices/SixenseManager.cpp b/interface/src/devices/SixenseManager.cpp
index 96481db4c8..fcb795eaed 100644
--- a/interface/src/devices/SixenseManager.cpp
+++ b/interface/src/devices/SixenseManager.cpp
@@ -30,7 +30,8 @@ void SixenseManager::update() {
     if (sixenseGetNumActiveControllers() == 0) {
         return;
     }
-    Hand& hand = Application::getInstance()->getAvatar()->getHand();
+    MyAvatar* avatar = Application::getInstance()->getAvatar();
+    Hand& hand = avatar->getHand();
     hand.getPalms().clear();
     
     int maxControllers = sixenseGetMaxControllers();
@@ -41,6 +42,22 @@ void SixenseManager::update() {
         sixenseControllerData data;
         sixenseGetNewestData(i, &data);
         
+        // drive avatar with joystick and triggers
+        if (data.controller_index) {
+            avatar->setDriveKeys(ROT_LEFT, qMax(0.0f, -data.joystick_x));
+            avatar->setDriveKeys(ROT_RIGHT, qMax(0.0f, data.joystick_x));
+            avatar->setDriveKeys(ROT_UP, qMax(0.0f, data.joystick_y));
+            avatar->setDriveKeys(ROT_DOWN, qMax(0.0f, -data.joystick_y));
+            avatar->setDriveKeys(UP, data.trigger);
+            
+        } else {
+            avatar->setDriveKeys(FWD, qMax(0.0f, data.joystick_y));
+            avatar->setDriveKeys(BACK, qMax(0.0f, -data.joystick_y));
+            avatar->setDriveKeys(LEFT, qMax(0.0f, -data.joystick_x));
+            avatar->setDriveKeys(RIGHT, qMax(0.0f, data.joystick_x));
+            avatar->setDriveKeys(DOWN, data.trigger);
+        }
+        
         // set palm position and normal based on Hydra position/orientation
         PalmData palm(&hand);
         palm.setActive(true);

From 5e933b08153efe3dafb65e66d266d4aad47ec0c5 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 18 Nov 2013 18:06:01 -0800
Subject: [PATCH 07/16] Working on basic Faceshift drive.

---
 interface/src/Menu.cpp              |  2 ++
 interface/src/Menu.h                |  2 ++
 interface/src/devices/Faceshift.cpp | 15 +++++++++++++++
 3 files changed, 19 insertions(+)

diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp
index 69ea9d010e..9f2b665af9 100644
--- a/interface/src/Menu.cpp
+++ b/interface/src/Menu.cpp
@@ -333,6 +333,7 @@ Menu::Menu() :
                                            false,
                                            appInstance->getFaceshift(),
                                            SLOT(setTCPEnabled(bool)));
+    addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::FaceshiftDrive, 0, false);
     addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::ChatCircling, 0, true);
 
     QMenu* webcamOptionsMenu = developerMenu->addMenu("Webcam Options");
@@ -361,6 +362,7 @@ Menu::Menu() :
 
     addCheckableActionToQMenuAndActionHash(raveGloveOptionsMenu, MenuOption::SimulateLeapHand);
     addCheckableActionToQMenuAndActionHash(raveGloveOptionsMenu, MenuOption::DisplayLeapHands, 0, true);
+    addCheckableActionToQMenuAndActionHash(raveGloveOptionsMenu, MenuOption::LeapDrive, 0, false);
     addCheckableActionToQMenuAndActionHash(raveGloveOptionsMenu, MenuOption::TestRaveGlove);
 
     QMenu* trackingOptionsMenu = developerMenu->addMenu("Tracking Options");
diff --git a/interface/src/Menu.h b/interface/src/Menu.h
index dd1789689b..9a2ded7154 100644
--- a/interface/src/Menu.h
+++ b/interface/src/Menu.h
@@ -172,6 +172,7 @@ namespace MenuOption {
     const QString DontFadeOnVoxelServerChanges = "Don't Fade In/Out on Voxel Server Changes";
     const QString HeadMouse = "Head Mouse";
     const QString FaceMode = "Cycle Face Mode";
+    const QString FaceshiftDrive = "Faceshift Drive";
     const QString FaceshiftTCP = "Faceshift (TCP)";
     const QString FalseColorByDistance = "FALSE Color By Distance";
     const QString FalseColorBySource = "FALSE Color By Source";
@@ -198,6 +199,7 @@ namespace MenuOption {
     const QString GoHome = "Go Home";
     const QString Gravity = "Use Gravity";
     const QString ParticleCloud = "Particle Cloud";
+    const QString LeapDrive = "Leap Drive";
     const QString LodTools = "LOD Tools";
     const QString Log = "Log";
     const QString Login = "Login";
diff --git a/interface/src/devices/Faceshift.cpp b/interface/src/devices/Faceshift.cpp
index 9a45810cbe..0e8aefd0f8 100644
--- a/interface/src/devices/Faceshift.cpp
+++ b/interface/src/devices/Faceshift.cpp
@@ -10,6 +10,7 @@
 
 #include <SharedUtil.h>
 
+#include "Application.h"
 #include "Faceshift.h"
 #include "Menu.h"
 #include "Util.h"
@@ -80,6 +81,20 @@ void Faceshift::update() {
     }
     _estimatedEyePitch = eulers.x - _longTermAverageEyePitch;
     _estimatedEyeYaw = eulers.y - _longTermAverageEyeYaw;
+    
+    // if Faceshift drive is enabled, set the avatar drive based on the head position
+    if (!Menu::getInstance()->isOptionChecked(MenuOption::FaceshiftDrive)) {
+        return;
+    }
+    MyAvatar* avatar = Application::getInstance()->getAvatar();
+    const float DRIVE_SCALE = 1.0f;
+    const float DEAD_ZONE = 0.1f;
+    avatar->setDriveKeys(FWD, qMax(0.0f, -_headTranslation.z * DRIVE_SCALE - DEAD_ZONE));
+    avatar->setDriveKeys(BACK, qMax(0.0f, _headTranslation.z * DRIVE_SCALE - DEAD_ZONE));
+    avatar->setDriveKeys(LEFT, qMax(0.0f, -_headTranslation.x * DRIVE_SCALE - DEAD_ZONE));
+    avatar->setDriveKeys(RIGHT, qMax(0.0f, _headTranslation.x * DRIVE_SCALE - DEAD_ZONE));
+    avatar->setDriveKeys(UP, qMax(0.0f, _headTranslation.y * DRIVE_SCALE - DEAD_ZONE));
+    avatar->setDriveKeys(DOWN, qMax(0.0f, -_headTranslation.y * DRIVE_SCALE - DEAD_ZONE));
 }
 
 void Faceshift::reset() {

From df8fbf8f72c468b88794436fd293a338d80816ca Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 19 Nov 2013 11:09:02 -0800
Subject: [PATCH 08/16] More general, angle-driven form of driving based on
 lean.

---
 interface/src/Menu.cpp              |  2 +-
 interface/src/Menu.h                |  2 +-
 interface/src/avatar/MyAvatar.cpp   | 24 +++++++++++++++++++++---
 interface/src/devices/Faceshift.cpp | 14 --------------
 4 files changed, 23 insertions(+), 19 deletions(-)

diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp
index 9f2b665af9..647b387332 100644
--- a/interface/src/Menu.cpp
+++ b/interface/src/Menu.cpp
@@ -243,6 +243,7 @@ Menu::Menu() :
                                            MenuOption::TurnWithHead,
                                            0,
                                            true);
+    addCheckableActionToQMenuAndActionHash(viewMenu, MenuOption::MoveWithLean, 0, false);
     addCheckableActionToQMenuAndActionHash(viewMenu, MenuOption::HeadMouse, 0, false);
     
     
@@ -333,7 +334,6 @@ Menu::Menu() :
                                            false,
                                            appInstance->getFaceshift(),
                                            SLOT(setTCPEnabled(bool)));
-    addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::FaceshiftDrive, 0, false);
     addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::ChatCircling, 0, true);
 
     QMenu* webcamOptionsMenu = developerMenu->addMenu("Webcam Options");
diff --git a/interface/src/Menu.h b/interface/src/Menu.h
index 9a2ded7154..d35deb3500 100644
--- a/interface/src/Menu.h
+++ b/interface/src/Menu.h
@@ -172,7 +172,6 @@ namespace MenuOption {
     const QString DontFadeOnVoxelServerChanges = "Don't Fade In/Out on Voxel Server Changes";
     const QString HeadMouse = "Head Mouse";
     const QString FaceMode = "Cycle Face Mode";
-    const QString FaceshiftDrive = "Faceshift Drive";
     const QString FaceshiftTCP = "Faceshift (TCP)";
     const QString FalseColorByDistance = "FALSE Color By Distance";
     const QString FalseColorBySource = "FALSE Color By Source";
@@ -207,6 +206,7 @@ namespace MenuOption {
     const QString LookAtVectors = "Look-at Vectors";
     const QString LowRes = "Lower Resolution While Moving";
     const QString Mirror = "Mirror";
+    const QString MoveWithLean = "Move with Lean";
     const QString NewVoxelCullingMode = "New Voxel Culling Mode";
     const QString NudgeVoxels = "Nudge";
     const QString OcclusionCulling = "Occlusion Culling";
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 3343f5c920..98394747c7 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -446,12 +446,30 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
     _head.setRoll(estimatedRotation.z * AVATAR_HEAD_ROLL_MAGNIFY);
         
     //  Update torso lean distance based on accelerometer data
-    const float TORSO_LENGTH = _scale * 0.5f;
+    const float TORSO_LENGTH = 0.5f;
+    glm::vec3 relativePosition = estimatedPosition - glm::vec3(0.0f, -TORSO_LENGTH, 0.0f);
     const float MAX_LEAN = 45.0f;
-    _head.setLeanSideways(glm::clamp(glm::degrees(atanf(estimatedPosition.x * _leanScale / TORSO_LENGTH)),
+    _head.setLeanSideways(glm::clamp(glm::degrees(atanf(relativePosition.x * _leanScale / relativePosition.y)),
         -MAX_LEAN, MAX_LEAN));
-    _head.setLeanForward(glm::clamp(glm::degrees(atanf(estimatedPosition.z * _leanScale / TORSO_LENGTH)),
+    _head.setLeanForward(glm::clamp(glm::degrees(atanf(relativePosition.z * _leanScale / relativePosition.y)),
         -MAX_LEAN, MAX_LEAN));
+    
+    // if Faceshift drive is enabled, set the avatar drive based on the head position
+    if (!Menu::getInstance()->isOptionChecked(MenuOption::MoveWithLean)) {
+        return;
+    }
+    const float ANGULAR_DRIVE_SCALE = 0.1f;
+    const float ANGULAR_DEAD_ZONE = 0.5f;
+    setDriveKeys(FWD, qMax(0.0f, _head.getLeanForward() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE));
+    setDriveKeys(BACK, qMax(0.0f, -_head.getLeanForward() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE));
+    setDriveKeys(LEFT, qMax(0.0f, _head.getLeanSideways() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE));
+    setDriveKeys(RIGHT, qMax(0.0f, -_head.getLeanSideways() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE));
+    
+    const float LINEAR_DRIVE_SCALE = 10.0f;
+    const float LINEAR_DEAD_ZONE = 0.1f;
+    float torsoDelta = glm::length(relativePosition) - TORSO_LENGTH;
+    setDriveKeys(UP, qMax(0.0f, torsoDelta * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE));
+    setDriveKeys(DOWN, qMax(0.0f, -torsoDelta * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE));
 }
 
 static TextRenderer* textRenderer() {
diff --git a/interface/src/devices/Faceshift.cpp b/interface/src/devices/Faceshift.cpp
index 0e8aefd0f8..6264ac26b5 100644
--- a/interface/src/devices/Faceshift.cpp
+++ b/interface/src/devices/Faceshift.cpp
@@ -81,20 +81,6 @@ void Faceshift::update() {
     }
     _estimatedEyePitch = eulers.x - _longTermAverageEyePitch;
     _estimatedEyeYaw = eulers.y - _longTermAverageEyeYaw;
-    
-    // if Faceshift drive is enabled, set the avatar drive based on the head position
-    if (!Menu::getInstance()->isOptionChecked(MenuOption::FaceshiftDrive)) {
-        return;
-    }
-    MyAvatar* avatar = Application::getInstance()->getAvatar();
-    const float DRIVE_SCALE = 1.0f;
-    const float DEAD_ZONE = 0.1f;
-    avatar->setDriveKeys(FWD, qMax(0.0f, -_headTranslation.z * DRIVE_SCALE - DEAD_ZONE));
-    avatar->setDriveKeys(BACK, qMax(0.0f, _headTranslation.z * DRIVE_SCALE - DEAD_ZONE));
-    avatar->setDriveKeys(LEFT, qMax(0.0f, -_headTranslation.x * DRIVE_SCALE - DEAD_ZONE));
-    avatar->setDriveKeys(RIGHT, qMax(0.0f, _headTranslation.x * DRIVE_SCALE - DEAD_ZONE));
-    avatar->setDriveKeys(UP, qMax(0.0f, _headTranslation.y * DRIVE_SCALE - DEAD_ZONE));
-    avatar->setDriveKeys(DOWN, qMax(0.0f, -_headTranslation.y * DRIVE_SCALE - DEAD_ZONE));
 }
 
 void Faceshift::reset() {

From 1e7866ad0217af4298232a940503a0c4d9f3b657 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 19 Nov 2013 11:51:32 -0800
Subject: [PATCH 09/16] Clamped values, adjusted them.

---
 interface/src/avatar/MyAvatar.cpp | 24 +++++++++++++-----------
 1 file changed, 13 insertions(+), 11 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 98394747c7..9c60836040 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -459,17 +459,19 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
         return;
     }
     const float ANGULAR_DRIVE_SCALE = 0.1f;
-    const float ANGULAR_DEAD_ZONE = 0.5f;
-    setDriveKeys(FWD, qMax(0.0f, _head.getLeanForward() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE));
-    setDriveKeys(BACK, qMax(0.0f, -_head.getLeanForward() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE));
-    setDriveKeys(LEFT, qMax(0.0f, _head.getLeanSideways() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE));
-    setDriveKeys(RIGHT, qMax(0.0f, -_head.getLeanSideways() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE));
-    
-    const float LINEAR_DRIVE_SCALE = 10.0f;
-    const float LINEAR_DEAD_ZONE = 0.1f;
-    float torsoDelta = glm::length(relativePosition) - TORSO_LENGTH;
-    setDriveKeys(UP, qMax(0.0f, torsoDelta * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE));
-    setDriveKeys(DOWN, qMax(0.0f, -torsoDelta * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE));
+    const float ANGULAR_DEAD_ZONE = 0.3f;
+    setDriveKeys(FWD, glm::clamp(-_head.getLeanForward() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
+    setDriveKeys(BACK, glm::clamp(_head.getLeanForward() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
+    setDriveKeys(LEFT, glm::clamp(_head.getLeanSideways() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
+    setDriveKeys(RIGHT, glm::clamp(-_head.getLeanSideways() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
+
+    // only consider going up if we're not going in any of the four horizontal directions
+    if (_driveKeys[FWD] == 0.0f && _driveKeys[BACK] == 0 && _driveKeys[LEFT] == 0.0f && _driveKeys[RIGHT] == 0.0f) {
+        const float LINEAR_DRIVE_SCALE = 5.0f;
+        const float LINEAR_DEAD_ZONE = 0.95f;
+        float torsoDelta = glm::length(relativePosition) - TORSO_LENGTH;
+        setDriveKeys(UP, glm::clamp(torsoDelta * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
+    }
 }
 
 static TextRenderer* textRenderer() {

From 3d82fe65b46e03ce305fc0b038a82287d1cbe807 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 19 Nov 2013 11:53:57 -0800
Subject: [PATCH 10/16] Forgot this part.

---
 interface/src/avatar/MyAvatar.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 9c60836040..7ca01b65b1 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -449,9 +449,9 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
     const float TORSO_LENGTH = 0.5f;
     glm::vec3 relativePosition = estimatedPosition - glm::vec3(0.0f, -TORSO_LENGTH, 0.0f);
     const float MAX_LEAN = 45.0f;
-    _head.setLeanSideways(glm::clamp(glm::degrees(atanf(relativePosition.x * _leanScale / relativePosition.y)),
+    _head.setLeanSideways(glm::clamp(glm::degrees(atanf(relativePosition.x * _leanScale / TORSO_LENGTH)),
         -MAX_LEAN, MAX_LEAN));
-    _head.setLeanForward(glm::clamp(glm::degrees(atanf(relativePosition.z * _leanScale / relativePosition.y)),
+    _head.setLeanForward(glm::clamp(glm::degrees(atanf(relativePosition.z * _leanScale / TORSO_LENGTH)),
         -MAX_LEAN, MAX_LEAN));
     
     // if Faceshift drive is enabled, set the avatar drive based on the head position

From ced1fca2e43c5b54a6f0b121de12afd6623b48ff Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 19 Nov 2013 11:57:01 -0800
Subject: [PATCH 11/16] Missed another spot.

---
 interface/src/avatar/MyAvatar.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 7ca01b65b1..3106bb4b34 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -466,7 +466,7 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
     setDriveKeys(RIGHT, glm::clamp(-_head.getLeanSideways() * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
 
     // only consider going up if we're not going in any of the four horizontal directions
-    if (_driveKeys[FWD] == 0.0f && _driveKeys[BACK] == 0 && _driveKeys[LEFT] == 0.0f && _driveKeys[RIGHT] == 0.0f) {
+    if (_driveKeys[FWD] == 0.0f && _driveKeys[BACK] == 0.0f && _driveKeys[LEFT] == 0.0f && _driveKeys[RIGHT] == 0.0f) {
         const float LINEAR_DRIVE_SCALE = 5.0f;
         const float LINEAR_DEAD_ZONE = 0.95f;
         float torsoDelta = glm::length(relativePosition) - TORSO_LENGTH;

From bc523092babd0ee6457bf578bf5866797aa8d06e Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 19 Nov 2013 11:59:25 -0800
Subject: [PATCH 12/16] Sniffing glue, wrong week, etc.

---
 interface/src/avatar/MyAvatar.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 3106bb4b34..e754054fec 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -471,6 +471,9 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
         const float LINEAR_DEAD_ZONE = 0.95f;
         float torsoDelta = glm::length(relativePosition) - TORSO_LENGTH;
         setDriveKeys(UP, glm::clamp(torsoDelta * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
+    
+    } else {
+        setDriveKeys(UP, 0.0f);
     }
 }
 

From 38c2abd6d89696ca885acab0e048f2747a2b70c4 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 19 Nov 2013 15:10:02 -0800
Subject: [PATCH 13/16] First stab at Leap driving.

---
 interface/src/Application.cpp         |  3 +-
 interface/src/devices/LeapManager.cpp | 84 +++++++++++++++++++++++++--
 interface/src/devices/LeapManager.h   |  4 +-
 3 files changed, 84 insertions(+), 7 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index ca56301c8e..08a4e96da6 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -2202,7 +2202,7 @@ void Application::updateLeap(float deltaTime) {
 
     LeapManager::enableFakeFingers(Menu::getInstance()->isOptionChecked(MenuOption::SimulateLeapHand));
     _myAvatar.getHand().setRaveGloveActive(Menu::getInstance()->isOptionChecked(MenuOption::TestRaveGlove));
-    LeapManager::nextFrame(_myAvatar);
+    LeapManager::nextFrame();
 }
 
 void Application::updateSixense() {
@@ -4100,6 +4100,7 @@ void Application::resetSensors() {
     }
     _webcam.reset();
     _faceshift.reset();
+    LeapManager::reset();
     QCursor::setPos(_headMouseX, _headMouseY);
     _myAvatar.reset();
     _myTransmitter.resetLevels();
diff --git a/interface/src/devices/LeapManager.cpp b/interface/src/devices/LeapManager.cpp
index c3dd993c16..adb14d41a0 100644
--- a/interface/src/devices/LeapManager.cpp
+++ b/interface/src/devices/LeapManager.cpp
@@ -6,12 +6,16 @@
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 //
 
-#include "LeapManager.h"
-#include "avatar/Avatar.h"
-#include <Leap.h>
 #include <dlfcn.h>     // needed for RTLD_LAZY
+
 #include <sstream>
 
+#include <Leap.h>
+
+#include "Application.h"
+#include "LeapManager.h"
+#include "Menu.h"
+
 // Uncomment the next line to use Leap-smoothed stabilized (slower) data.
 //#define USE_STABILIZED_DATA
 
@@ -19,6 +23,7 @@ bool LeapManager::_libraryExists = false;
 bool LeapManager::_doFakeFingers = false;
 Leap::Controller* LeapManager::_controller = NULL;
 HifiLeapListener* LeapManager::_listener = NULL;
+glm::vec3 LeapManager::_baseDrivePosition;
 
 class HifiLeapListener  : public Leap::Listener {
 public:
@@ -44,6 +49,25 @@ void LeapManager::initialize() {
 #endif
 }
 
+static PalmData* getRightmostPalm() {
+    PalmData* rightmostPalm = NULL;
+    Hand& hand = Application::getInstance()->getAvatar()->getHand();
+    for (int i = 0; i < hand.getNumPalms(); i++) {
+        PalmData* palm = &hand.getPalms()[i];
+        if (palm->isActive() && (rightmostPalm == NULL || palm->getRawPosition().x > rightmostPalm->getRawPosition().x)) {
+            rightmostPalm = palm;
+        } 
+    }
+    return rightmostPalm;
+}
+
+void LeapManager::reset() {
+    PalmData* rightmostPalm = getRightmostPalm();
+    if (rightmostPalm != NULL) {
+        _baseDrivePosition = rightmostPalm->getRawPosition();
+    }
+}
+
 void LeapManager::terminate() {
     delete _listener;
     delete _controller;
@@ -51,9 +75,10 @@ void LeapManager::terminate() {
     _controller = NULL;
 }
 
-void LeapManager::nextFrame(Avatar& avatar) {
+void LeapManager::nextFrame() {
     // Apply the frame data directly to the avatar.
-    Hand& hand = avatar.getHand();
+    MyAvatar* avatar = Application::getInstance()->getAvatar();
+    Hand& hand = avatar->getHand();
     
     // If we actually get valid Leap data, this will be set to true;
     bool gotRealData = false;
@@ -244,6 +269,55 @@ void LeapManager::nextFrame(Avatar& avatar) {
         }
     }
     hand.updateFingerTrails();
+    
+    // if Leap drive is enabled, drive avatar based on Leap input
+    if (!Menu::getInstance()->isOptionChecked(MenuOption::LeapDrive)) {
+        return;
+    }
+    glm::vec3 relativePosition;
+    glm::vec3 eulerAngles;
+    PalmData* rightmostPalm = getRightmostPalm();
+    if (rightmostPalm != NULL) {
+        relativePosition = rightmostPalm->getRawPosition() - _baseDrivePosition;
+        
+        glm::vec3 directionSum;
+        int activeFingerCount = 0;
+        for (int i = 0; i < rightmostPalm->getNumFingers(); i++) {
+            FingerData& finger = rightmostPalm->getFingers()[i];
+            glm::vec3 fingerVector = finger.getTipRawPosition() - rightmostPalm->getRawPosition();
+            if (finger.isActive() && glm::length(fingerVector) > EPSILON) {
+                directionSum += glm::normalize(fingerVector);
+                activeFingerCount++;
+            }
+        }
+        const int MIN_DIRECTION_FINGER_COUNT = 3;
+        glm::vec3 right;
+        if (activeFingerCount >= MIN_DIRECTION_FINGER_COUNT) {
+            right = glm::normalize(glm::cross(glm::normalize(directionSum), -rightmostPalm->getRawNormal()));
+                
+        } else {
+            right = glm::normalize(glm::cross(IDENTITY_FRONT, -rightmostPalm->getRawNormal()));
+        }
+        eulerAngles = safeEulerAngles(glm::quat_cast(glm::mat3(right, -rightmostPalm->getRawNormal(),
+            glm::cross(right, -rightmostPalm->getRawNormal()))));
+    }
+    const float LINEAR_DRIVE_SCALE = 0.1f;
+    const float LINEAR_DEAD_ZONE = 0.3f;
+    avatar->setDriveKeys(FWD, glm::clamp(-relativePosition.z * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
+    avatar->setDriveKeys(BACK, glm::clamp(relativePosition.z * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
+    avatar->setDriveKeys(LEFT, glm::clamp(-relativePosition.x * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
+    avatar->setDriveKeys(RIGHT, glm::clamp(relativePosition.x * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
+    avatar->setDriveKeys(UP, glm::clamp(relativePosition.y * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
+    avatar->setDriveKeys(DOWN, glm::clamp(-relativePosition.y * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
+    
+    const float ANGULAR_DRIVE_SCALE = 0.1f;
+    const float ANGULAR_DEAD_ZONE = 0.3f;
+    avatar->setDriveKeys(ROT_LEFT, glm::clamp(glm::max(eulerAngles.y, eulerAngles.z) * ANGULAR_DRIVE_SCALE -
+        ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
+    avatar->setDriveKeys(ROT_RIGHT, glm::clamp(glm::max(-eulerAngles.y, -eulerAngles.z) * ANGULAR_DRIVE_SCALE -
+        ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
+    avatar->setDriveKeys(ROT_UP, glm::clamp(eulerAngles.x * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
+    avatar->setDriveKeys(ROT_DOWN, glm::clamp(-eulerAngles.x * ANGULAR_DRIVE_SCALE - ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
 }
 
 void LeapManager::enableFakeFingers(bool enable) {
diff --git a/interface/src/devices/LeapManager.h b/interface/src/devices/LeapManager.h
index 11dbefe849..4544d5afcf 100755
--- a/interface/src/devices/LeapManager.h
+++ b/interface/src/devices/LeapManager.h
@@ -21,7 +21,7 @@ namespace Leap {
 
 class LeapManager {
 public:
-    static void nextFrame(Avatar& avatar);      // called once per frame to get new Leap data
+    static void nextFrame();                    // called once per frame to get new Leap data
     static bool controllersExist();             // Returns true if there's at least one active Leap plugged in
     static void enableFakeFingers(bool enable); // put fake data in if there's no Leap plugged in
     static const std::vector<glm::vec3>& getFingerTips();
@@ -30,6 +30,7 @@ public:
     static const std::vector<glm::vec3>& getHandNormals();
     static std::string statusString();
     static void initialize();
+    static void reset();
     static void terminate();
     
 private:
@@ -37,6 +38,7 @@ private:
     static bool _doFakeFingers;
     static Leap::Controller* _controller;
     static HifiLeapListener* _listener;
+    static glm::vec3 _baseDrivePosition;
 };
 
 #endif /* defined(__hifi__LeapManager__) */

From 80e6606dd24974af20c17cafd53bb7095891199e Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 19 Nov 2013 15:31:04 -0800
Subject: [PATCH 14/16] Adjusted constants, added reasonable starting base
 position.

---
 interface/src/devices/LeapManager.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/interface/src/devices/LeapManager.cpp b/interface/src/devices/LeapManager.cpp
index adb14d41a0..b0f6f6077f 100644
--- a/interface/src/devices/LeapManager.cpp
+++ b/interface/src/devices/LeapManager.cpp
@@ -23,7 +23,7 @@ bool LeapManager::_libraryExists = false;
 bool LeapManager::_doFakeFingers = false;
 Leap::Controller* LeapManager::_controller = NULL;
 HifiLeapListener* LeapManager::_listener = NULL;
-glm::vec3 LeapManager::_baseDrivePosition;
+glm::vec3 LeapManager::_baseDrivePosition(0.0f, 150.0f, 0.0f); // experimentally derived
 
 class HifiLeapListener  : public Leap::Listener {
 public:
@@ -301,8 +301,8 @@ void LeapManager::nextFrame() {
         eulerAngles = safeEulerAngles(glm::quat_cast(glm::mat3(right, -rightmostPalm->getRawNormal(),
             glm::cross(right, -rightmostPalm->getRawNormal()))));
     }
-    const float LINEAR_DRIVE_SCALE = 0.1f;
-    const float LINEAR_DEAD_ZONE = 0.3f;
+    const float LINEAR_DRIVE_SCALE = 0.01f;
+    const float LINEAR_DEAD_ZONE = 0.25f;
     avatar->setDriveKeys(FWD, glm::clamp(-relativePosition.z * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
     avatar->setDriveKeys(BACK, glm::clamp(relativePosition.z * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
     avatar->setDriveKeys(LEFT, glm::clamp(-relativePosition.x * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
@@ -310,8 +310,8 @@ void LeapManager::nextFrame() {
     avatar->setDriveKeys(UP, glm::clamp(relativePosition.y * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
     avatar->setDriveKeys(DOWN, glm::clamp(-relativePosition.y * LINEAR_DRIVE_SCALE - LINEAR_DEAD_ZONE, 0.0f, 1.0f));
     
-    const float ANGULAR_DRIVE_SCALE = 0.1f;
-    const float ANGULAR_DEAD_ZONE = 0.3f;
+    const float ANGULAR_DRIVE_SCALE = 0.05f;
+    const float ANGULAR_DEAD_ZONE = 0.75f;
     avatar->setDriveKeys(ROT_LEFT, glm::clamp(glm::max(eulerAngles.y, eulerAngles.z) * ANGULAR_DRIVE_SCALE -
         ANGULAR_DEAD_ZONE, 0.0f, 1.0f));
     avatar->setDriveKeys(ROT_RIGHT, glm::clamp(glm::max(-eulerAngles.y, -eulerAngles.z) * ANGULAR_DRIVE_SCALE -

From 69b5b81f349f0916fd15db9176fc3f9772d26ec4 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 19 Nov 2013 16:31:24 -0800
Subject: [PATCH 15/16] Limit combined pitch.

---
 interface/src/avatar/MyAvatar.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index e754054fec..5cb2ba491f 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -370,6 +370,8 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter) {
 
 }
 
+const float MAX_PITCH = 90.0f;
+
 //  Update avatar head rotation with sensor data
 void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHead) {
     Faceshift* faceshift = Application::getInstance()->getFaceshift();
@@ -377,6 +379,7 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
     Webcam* webcam = Application::getInstance()->getWebcam();
     glm::vec3 estimatedPosition, estimatedRotation;
     
+    float combinedPitch = glm::clamp(pitchFromTouch + _mousePitchDelta, -MAX_PITCH, MAX_PITCH);
     if (faceshift->isActive()) {
         estimatedPosition = faceshift->getHeadTranslation();
         estimatedRotation = safeEulerAngles(faceshift->getHeadRotation());
@@ -397,8 +400,8 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
     
     } else {
         if (!_leadingAvatar) {
-            _head.setMousePitch(pitchFromTouch + _mousePitchDelta);
-            _head.setPitch(pitchFromTouch + _mousePitchDelta);
+            _head.setMousePitch(combinedPitch);
+            _head.setPitch(combinedPitch);
         }
         _head.getVideoFace().clearFrame();
         
@@ -410,7 +413,7 @@ void MyAvatar::updateFromGyrosAndOrWebcam(float pitchFromTouch, bool turnWithHea
         _head.setLeanForward(glm::mix(_head.getLeanForward(), 0.0f, RESTORE_RATE));
         return;
     }
-    _head.setMousePitch(pitchFromTouch + _mousePitchDelta);
+    _head.setMousePitch(combinedPitch);
 
     if (webcam->isActive()) {
         estimatedPosition = webcam->getEstimatedPosition();
@@ -740,7 +743,6 @@ void MyAvatar::updateThrust(float deltaTime, Transmitter * transmitter) {
     _thrust -= _driveKeys[DOWN] * _scale * THRUST_MAG_DOWN * _thrustMultiplier * deltaTime * up;
     _bodyYawDelta -= _driveKeys[ROT_RIGHT] * YAW_MAG * deltaTime;
     _bodyYawDelta += _driveKeys[ROT_LEFT] * YAW_MAG * deltaTime;
-    const float MAX_PITCH = 90.0f;
     _mousePitchDelta = min(_mousePitchDelta + _driveKeys[ROT_UP] * PITCH_MAG * deltaTime, MAX_PITCH);
     _mousePitchDelta = max(_mousePitchDelta - _driveKeys[ROT_DOWN] * PITCH_MAG * deltaTime, -MAX_PITCH);
     

From 4e952b0f994d3e0c6cfa3ed92800e34b46dae0a5 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 19 Nov 2013 16:40:29 -0800
Subject: [PATCH 16/16] Clamp pitch from touch when adjusting it.

---
 interface/src/Application.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 08a4e96da6..8481ba7908 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -2189,8 +2189,10 @@ void Application::updateHandAndTouch(float deltaTime) {
         float TOUCH_YAW_SCALE = -0.25f;
         float TOUCH_PITCH_SCALE = -12.5f;
         float FIXED_TOUCH_TIMESTEP = 0.016f;
+        const float MAX_PITCH = 90.0f;
         _yawFromTouch += ((_touchAvgX - _lastTouchAvgX) * TOUCH_YAW_SCALE * FIXED_TOUCH_TIMESTEP);
-        _pitchFromTouch += ((_touchAvgY - _lastTouchAvgY) * TOUCH_PITCH_SCALE * FIXED_TOUCH_TIMESTEP);
+        _pitchFromTouch = glm::clamp(_pitchFromTouch + (_touchAvgY - _lastTouchAvgY) * TOUCH_PITCH_SCALE *
+            FIXED_TOUCH_TIMESTEP, -MAX_PITCH, MAX_PITCH);
         _lastTouchAvgX = _touchAvgX;
         _lastTouchAvgY = _touchAvgY;
     }