Merge pull request #614 from machinelevel/dev4

(changed per requests and re-merged) Two Leap-related crashes fixed, plu...

interface/src/Application.cpp

@@ -956,6 +956,8 @@ void Application::terminate() {
     // Close serial port
     // close(serial_fd);
     
+    LeapManager::terminate();
+    
     if (_settingsAutosave->isChecked()) {
         saveSettings();
         _settings->sync();
@@ -1648,6 +1650,8 @@ void Application::init() {
         QMetaObject::invokeMethod(_fullScreenMode, "trigger", Qt::QueuedConnection);
     }
     
+    LeapManager::initialize();
+    
     gettimeofday(&_timerStart, NULL);
     gettimeofday(&_lastTimeIdle, NULL);
 
@@ -1806,7 +1810,8 @@ void Application::update(float deltaTime) {
     
     // Leap finger-sensing device
     LeapManager::nextFrame();
-    _myAvatar.getHand().setLeapFingers(LeapManager::getFingerPositions());
+    _myAvatar.getHand().setLeapFingers(LeapManager::getFingerTips(), LeapManager::getFingerRoots());
+    _myAvatar.getHand().setLeapHands(LeapManager::getHandPositions(), LeapManager::getHandNormals());
     
      //  Read serial port interface devices
     if (_serialHeadSensor.isActive()) {

interface/src/Avatar.h

@@ -107,6 +107,7 @@ public:
 
     //getters
     bool             isInitialized             ()                const { return _initialized;}
+    bool             isMyAvatar                ()                const { return _owningAgent == NULL; }
     const Skeleton&  getSkeleton               ()                const { return _skeleton;}
     float            getHeadYawRate            ()                const { return _head.yawRate;}
     float            getBodyYaw                ()                const { return _bodyYaw;}    
@@ -155,13 +156,13 @@ public:
     void writeAvatarDataToFile();
     void readAvatarDataFromFile();
 
+    static void renderJointConnectingCone(glm::vec3 position1, glm::vec3 position2, float radius1, float radius2);
+
 private:
     // privatize copy constructor and assignment operator to avoid copying
     Avatar(const Avatar&);
     Avatar& operator= (const Avatar&);
     
-    bool isMyAvatar() const { return _owningAgent == NULL; }
-
     struct AvatarBall
     {
         AvatarJointID    parentJoint;    // the skeletal joint that serves as a reference for determining the position
@@ -237,7 +238,6 @@ private:
     void applyHardCollision(const glm::vec3& penetration, float elasticity, float damping);
     void applyCollisionWithOtherAvatar( Avatar * other, float deltaTime );
     void checkForMouseRayTouching();
-    void renderJointConnectingCone(glm::vec3 position1, glm::vec3 position2, float radius1, float radius2);
 };
 
 #endif

interface/src/Hand.cpp

@@ -21,21 +21,19 @@ Hand::Hand(Avatar* owningAvatar) :
     _owningAvatar(owningAvatar),
     _renderAlpha(1.0),
     _lookingInMirror(false),
-    _ballColor(0.0, 0.4, 0.0),
+    _ballColor(0.0, 0.0, 0.4),
     _position(0.0, 0.4, 0.0),
     _orientation(0.0, 0.0, 0.0, 1.0)
 {
 }
 
 void Hand::init() {
-    _numLeapBalls = 0;    
+    // Different colors for my hand and others' hands
-    for (int b = 0; b < MAX_AVATAR_LEAP_BALLS; b++) {
+    if (_owningAvatar && _owningAvatar->isMyAvatar()) {
-        _leapBall[b].position       = glm::vec3(0.0, 0.0, 0.0);
+        _ballColor = glm::vec3(0.0, 0.4, 0.0);
-        _leapBall[b].velocity       = glm::vec3(0.0, 0.0, 0.0);
-        _leapBall[b].radius         = 0.01;
-        _leapBall[b].touchForce     = 0.0;
-        _leapBall[b].isCollidable   = true;
     }
+    else
+        _ballColor = glm::vec3(0.0, 0.0, 0.4);
 }
 
 void Hand::reset() {
@@ -44,20 +42,27 @@ void Hand::reset() {
 void Hand::simulate(float deltaTime, bool isMine) {
 }
 
+glm::vec3 Hand::leapPositionToWorldPosition(const glm::vec3& leapPosition) {
+    float unitScale = 0.001; // convert mm to meters
+    return _position + _orientation * (leapPosition * unitScale);
+}
+
 void Hand::calculateGeometry() {
-    glm::vec3 offset(0.1, -0.1, -0.15);  // place the hand in front of the face where we can see it
+    glm::vec3 offset(0.2, -0.2, -0.3);  // place the hand in front of the face where we can see it
     
     Head& head = _owningAvatar->getHead();
     _position = head.getPosition() + head.getOrientation() * offset;
     _orientation = head.getOrientation();
 
-    _numLeapBalls = _fingerPositions.size();
+    int numLeapBalls = _fingerTips.size() + _fingerRoots.size();
+    _leapBalls.resize(numLeapBalls);
     
-    float unitScale = 0.001; // convert mm to meters
+    for (int i = 0; i < _fingerTips.size(); ++i) {
-    for (int b = 0; b < _numLeapBalls; b++) {
+        _leapBalls[i].rotation = _orientation;
-        glm::vec3 pos = unitScale * _fingerPositions[b] + offset;
+        _leapBalls[i].position = leapPositionToWorldPosition(_fingerTips[i]);
-        _leapBall[b].rotation = _orientation;
+        _leapBalls[i].radius         = 0.01;
-        _leapBall[b].position = _position + _orientation * pos;
+        _leapBalls[i].touchForce     = 0.0;
+        _leapBalls[i].isCollidable   = true;
     }
 }
 
@@ -78,23 +83,52 @@ void Hand::render(bool lookingInMirror) {
 void Hand::renderHandSpheres() {
     glPushMatrix();
     // Draw the leap balls
-    for (int b = 0; b < _numLeapBalls; b++) {
+    for (size_t i = 0; i < _leapBalls.size(); i++) {
         float alpha = 1.0f;
         
         if (alpha > 0.0f) {
-            glColor4f(_ballColor.r, _ballColor.g, _ballColor.b, alpha); // Just to test
+            glColor4f(_ballColor.r, _ballColor.g, _ballColor.b, alpha);
             
             glPushMatrix();
-            glTranslatef(_leapBall[b].position.x, _leapBall[b].position.y, _leapBall[b].position.z);
+            glTranslatef(_leapBalls[i].position.x, _leapBalls[i].position.y, _leapBalls[i].position.z);
-            glutSolidSphere(_leapBall[b].radius, 20.0f, 20.0f);
+            glutSolidSphere(_leapBalls[i].radius, 20.0f, 20.0f);
             glPopMatrix();
         }
     }
+    
+    // Draw the finger root cones
+    if (_fingerTips.size() == _fingerRoots.size()) {
+        for (size_t i = 0; i < _fingerTips.size(); ++i) {
+            glColor4f(_ballColor.r, _ballColor.g, _ballColor.b, 0.5);
+            glm::vec3 tip = leapPositionToWorldPosition(_fingerTips[i]);
+            glm::vec3 root = leapPositionToWorldPosition(_fingerRoots[i]);
+            Avatar::renderJointConnectingCone(root, tip, 0.001, 0.003);
+        }
+    }
+
+    // Draw the palms
+    if (_handPositions.size() == _handNormals.size()) {
+        for (size_t i = 0; i < _handPositions.size(); ++i) {
+            glColor4f(_ballColor.r, _ballColor.g, _ballColor.b, 0.25);
+            glm::vec3 tip = leapPositionToWorldPosition(_handPositions[i]);
+            glm::vec3 root = leapPositionToWorldPosition(_handPositions[i] + (_handNormals[i] * 2.0f));
+            Avatar::renderJointConnectingCone(root, tip, 0.05, 0.03);
+        }
+    }
+
     glPopMatrix();
 }
 
-void Hand::setLeapFingers(const std::vector<glm::vec3>& fingerPositions) {
+void Hand::setLeapFingers(const std::vector<glm::vec3>& fingerTips,
-    _fingerPositions = fingerPositions;
+                          const std::vector<glm::vec3>& fingerRoots) {
+    _fingerTips = fingerTips;
+    _fingerRoots = fingerRoots;
+}
+
+void Hand::setLeapHands(const std::vector<glm::vec3>& handPositions,
+                          const std::vector<glm::vec3>& handNormals) {
+    _handPositions = handPositions;
+    _handNormals = handNormals;
 }
 
 

interface/src/Hand.h

@@ -16,6 +16,7 @@
 #include "InterfaceConfig.h"
 #include "SerialInterface.h"
 #include <SharedUtil.h>
+#include <vector>
 
 
 class Avatar;
@@ -41,11 +42,13 @@ public:
     void render(bool lookingInMirror);
 
     void setBallColor      (glm::vec3 ballColor         ) { _ballColor          = ballColor;          }
-    void setLeapFingers    (const std::vector<glm::vec3>& fingerPositions);
+    void setLeapFingers    (const std::vector<glm::vec3>& fingerTips,
+                            const std::vector<glm::vec3>& fingerRoots);
+    void setLeapHands      (const std::vector<glm::vec3>& handPositions,
+                            const std::vector<glm::vec3>& handNormals);
 
     // getters
-    int              getNumLeapBalls           ()                const { return _numLeapBalls;}
+    const glm::vec3& getLeapBallPosition       (int ball)       const { return _leapBalls[ball].position;}
-    const glm::vec3& getLeapBallPosition       (int which)       const { return _leapBall[which].position;}
 
 private:
     // disallow copies of the Hand, copy of owning Avatar is disallowed too
@@ -58,12 +61,12 @@ private:
     glm::vec3   _ballColor;
     glm::vec3   _position;
     glm::quat   _orientation;
-    int         _numLeapBalls;
+    std::vector<HandBall>	_leapBalls;
-    HandBall	_leapBall[ MAX_AVATAR_LEAP_BALLS ];
     
     // private methods
     void renderHandSpheres();
     void calculateGeometry();
+    glm::vec3 leapPositionToWorldPosition(const glm::vec3& leapPosition);
 };
 
 #endif

interface/src/LeapManager.cpp

@@ -11,25 +11,43 @@
 #include <dlfcn.h>     // needed for RTLD_LAZY
 #include <sstream>
 
-bool LeapManager::_isInitialized = false;
 bool LeapManager::_libraryExists = false;
-Leap::Controller* LeapManager::_controller = 0;
+Leap::Controller* LeapManager::_controller = NULL;
-HifiLeapListener* LeapManager::_listener = 0;
+HifiLeapListener* LeapManager::_listener = NULL;
 
 class HifiLeapListener  : public Leap::Listener {
 public:
     Leap::Frame lastFrame;
-    std::vector<glm::vec3> fingerPositions;
+    std::vector<glm::vec3> fingerTips;
+    std::vector<glm::vec3> fingerRoots;
+    std::vector<glm::vec3> handPositions;
+    std::vector<glm::vec3> handNormals;
     
     virtual void onFrame(const Leap::Controller& controller) {
 #ifndef LEAP_STUBS
         Leap::Frame frame = controller.frame();
         int numFingers = frame.fingers().count();
-        fingerPositions.resize(numFingers);
+        fingerTips.resize(numFingers);
+        fingerRoots.resize(numFingers);
         for (int i = 0; i < numFingers; ++i) {
             const Leap::Finger& thisFinger = frame.fingers()[i];
-            const Leap::Vector pos = thisFinger.tipPosition();
+            const Leap::Vector pos = thisFinger.stabilizedTipPosition();
-            fingerPositions[i] = glm::vec3(pos.x, pos.y, pos.z);
+            fingerTips[i] = glm::vec3(pos.x, pos.y, pos.z);
+
+            const Leap::Vector root = pos - thisFinger.direction() * thisFinger.length();
+            fingerRoots[i] = glm::vec3(root.x, root.y, root.z);
+        }
+
+        int numHands = frame.hands().count();
+        handPositions.resize(numHands);
+        handNormals.resize(numHands);
+        for (int i = 0; i < numHands; ++i) {
+            const Leap::Hand& thisHand = frame.hands()[i];
+            const Leap::Vector pos = thisHand.palmPosition();
+            handPositions[i] = glm::vec3(pos.x, pos.y, pos.z);
+            
+            const Leap::Vector norm = thisHand.palmNormal();
+            handNormals[i] = glm::vec3(norm.x, norm.y, norm.z);
         }
         lastFrame = frame;
 #endif
@@ -38,28 +56,61 @@ public:
 };
 
 void LeapManager::initialize() {
-    if (!_isInitialized) {
 #ifndef LEAP_STUBS
     if (dlopen("/usr/lib/libLeap.dylib", RTLD_LAZY)) {
         _libraryExists = true;
         _controller = new Leap::Controller();
         _listener = new HifiLeapListener();
-            _controller->addListener(*_listener);
     }
 #endif
-        _isInitialized = true;
+}
-    }
+
+void LeapManager::terminate() {
+    delete _listener;
+    delete _controller;
+    _listener = NULL;
+    _controller = NULL;
 }
 
 void LeapManager::nextFrame() {
-    initialize();
     if (_listener && _controller)
         _listener->onFrame(*_controller);
 }
 
-const std::vector<glm::vec3>& LeapManager::getFingerPositions() {
+const std::vector<glm::vec3>& LeapManager::getFingerTips() {
-    if (_listener)
+    if (_listener) {
-        return _listener->fingerPositions;
+        return _listener->fingerTips;
+    }
+    else {
+        static std::vector<glm::vec3> empty;
+        return empty;
+    }
+}
+
+const std::vector<glm::vec3>& LeapManager::getFingerRoots() {
+    if (_listener) {
+        return _listener->fingerRoots;
+    }
+    else {
+        static std::vector<glm::vec3> empty;
+        return empty;
+    }
+}
+
+const std::vector<glm::vec3>& LeapManager::getHandPositions() {
+    if (_listener) {
+        return _listener->handPositions;
+    }
+    else {
+        static std::vector<glm::vec3> empty;
+        return empty;
+    }
+}
+
+const std::vector<glm::vec3>& LeapManager::getHandNormals() {
+    if (_listener) {
+        return _listener->handNormals;
+    }
     else {
         static std::vector<glm::vec3> empty;
         return empty;
@@ -69,7 +120,6 @@ const std::vector<glm::vec3>& LeapManager::getFingerPositions() {
 std::string LeapManager::statusString() {
     std::stringstream leapString;
 #ifndef LEAP_STUBS
-    if (_isInitialized) {
     if (!_libraryExists)
         leapString << "Leap library at /usr/lib/libLeap.dylib does not exist.";
     else if (!_controller || !_listener || !_controller->devices().count())
@@ -81,7 +131,6 @@ std::string LeapManager::statusString() {
             leapString << " pos: " << pos.x << " " << pos.y << " " << pos.z;
         }
     }
-    }
 #endif
     return leapString.str();
 }

interface/src/LeapManager.h

@@ -21,19 +21,18 @@ namespace Leap {
 class LeapManager {
 public:
     static void nextFrame();  // called once per frame to get new Leap data
-    static const std::vector<glm::vec3>& getFingerPositions();
+    static const std::vector<glm::vec3>& getFingerTips();
+    static const std::vector<glm::vec3>& getFingerRoots();
+    static const std::vector<glm::vec3>& getHandPositions();
+    static const std::vector<glm::vec3>& getHandNormals();
     static std::string statusString();
+    static void initialize();
+    static void terminate();
     
 private:
-    static void initialize();
-    static bool _isInitialized;            // We've looked for the library and hooked it up if it's there.
     static bool _libraryExists;            // The library is present, so we won't crash if we call it.
     static Leap::Controller* _controller;
     static HifiLeapListener* _listener;
-    
-
-
-
 };
 
 #endif /* defined(__hifi__LeapManager__) */

libraries/avatars/src/AvatarData.cpp

@@ -125,13 +125,31 @@ int AvatarData::getBroadcastData(unsigned char* destinationBuffer) {
     *destinationBuffer++ = bitItems;
     
     // leap hand data
-    const std::vector<glm::vec3>& fingerPositions = _handData->getFingerPositions();
+    // In order to make the hand data version-robust, hand data packing is just a series of vec3's,
-    *destinationBuffer++ = (unsigned char)fingerPositions.size();
+    // with conventions. If a client doesn't know the conventions, they can just get the vec3's
-    for (size_t i = 0; i < fingerPositions.size(); ++i)
+    // and render them as balls, or ignore them, without crashing or disrupting anyone.
-    {
+    // Current convention:
-        destinationBuffer += packFloatScalarToSignedTwoByteFixed(destinationBuffer, fingerPositions[i].x, 4);
+    //    Zero or more fingetTip positions, followed by the same number of fingerRoot positions
-        destinationBuffer += packFloatScalarToSignedTwoByteFixed(destinationBuffer, fingerPositions[i].y, 4);
+    
-        destinationBuffer += packFloatScalarToSignedTwoByteFixed(destinationBuffer, fingerPositions[i].z, 4);
+    const std::vector<glm::vec3>& fingerTips = _handData->getFingerTips();
+    const std::vector<glm::vec3>& fingerRoots = _handData->getFingerRoots();
+    size_t numFingerVectors = fingerTips.size() + fingerRoots.size();
+    if (numFingerVectors > 255)
+        numFingerVectors = 0; // safety. We shouldn't ever get over 255, so consider that invalid.
+
+    *destinationBuffer++ = (unsigned char)numFingerVectors;
+    
+    if (numFingerVectors > 0) {
+        for (size_t i = 0; i < fingerTips.size(); ++i) {
+            destinationBuffer += packFloatScalarToSignedTwoByteFixed(destinationBuffer, fingerTips[i].x, 4);
+            destinationBuffer += packFloatScalarToSignedTwoByteFixed(destinationBuffer, fingerTips[i].y, 4);
+            destinationBuffer += packFloatScalarToSignedTwoByteFixed(destinationBuffer, fingerTips[i].z, 4);
+        }
+        for (size_t i = 0; i < fingerRoots.size(); ++i) {
+            destinationBuffer += packFloatScalarToSignedTwoByteFixed(destinationBuffer, fingerRoots[i].x, 4);
+            destinationBuffer += packFloatScalarToSignedTwoByteFixed(destinationBuffer, fingerRoots[i].y, 4);
+            destinationBuffer += packFloatScalarToSignedTwoByteFixed(destinationBuffer, fingerRoots[i].z, 4);
+        }
     }
     
     return destinationBuffer - bufferStart;
@@ -229,18 +247,20 @@ int AvatarData::parseData(unsigned char* sourceBuffer, int numBytes) {
     // leap hand data
     if (sourceBuffer - startPosition < numBytes)    // safety check
     {
-        std::vector<glm::vec3> fingerPositions = _handData->getFingerPositions();
+        std::vector<glm::vec3> fingerTips = _handData->getFingerTips();
-        unsigned int numFingers = *sourceBuffer++;
+        std::vector<glm::vec3> fingerRoots = _handData->getFingerRoots();
-        if (numFingers > MAX_AVATAR_LEAP_BALLS)    // safety check
+        unsigned int numFingerVectors = *sourceBuffer++;
-            numFingers = 0;
+        unsigned int numFingerTips = numFingerVectors / 2;
-        fingerPositions.resize(numFingers);
+        unsigned int numFingerRoots = numFingerVectors - numFingerTips;
-        for (size_t i = 0; i < numFingers; ++i)
+        fingerTips.resize(numFingerTips);
-        {
+        fingerRoots.resize(numFingerRoots);
-            sourceBuffer += unpackFloatScalarFromSignedTwoByteFixed((int16_t*) sourceBuffer, &(fingerPositions[i].x), 4);
+        for (size_t i = 0; i < numFingerTips; ++i) {
-            sourceBuffer += unpackFloatScalarFromSignedTwoByteFixed((int16_t*) sourceBuffer, &(fingerPositions[i].y), 4);
+            sourceBuffer += unpackFloatScalarFromSignedTwoByteFixed((int16_t*) sourceBuffer, &(fingerTips[i].x), 4);
-            sourceBuffer += unpackFloatScalarFromSignedTwoByteFixed((int16_t*) sourceBuffer, &(fingerPositions[i].z), 4);
+            sourceBuffer += unpackFloatScalarFromSignedTwoByteFixed((int16_t*) sourceBuffer, &(fingerTips[i].y), 4);
+            sourceBuffer += unpackFloatScalarFromSignedTwoByteFixed((int16_t*) sourceBuffer, &(fingerTips[i].z), 4);
         }
-        _handData->setFingerPositions(fingerPositions);
+        _handData->setFingerTips(fingerTips);
+        _handData->setFingerRoots(fingerRoots);
     }
     
     return sourceBuffer - startPosition;

libraries/avatars/src/HandData.h

@@ -14,20 +14,27 @@
 
 #include <glm/glm.hpp>
 
-#define MAX_AVATAR_LEAP_BALLS 10
-
 class AvatarData;
 
 class HandData {
 public:
     HandData(AvatarData* owningAvatar);
     
-    const std::vector<glm::vec3>& getFingerPositions() const { return _fingerPositions; }
+    const std::vector<glm::vec3>& getFingerTips() const { return _fingerTips; }
-    void setFingerPositions(const std::vector<glm::vec3>& fingerPositions) { _fingerPositions = fingerPositions; }
+    const std::vector<glm::vec3>& getFingerRoots() const { return _fingerRoots; }
+    const std::vector<glm::vec3>& getHandPositions() const { return _handPositions; }
+    const std::vector<glm::vec3>& getHandNormals() const { return _handNormals; }
+    void setFingerTips(const std::vector<glm::vec3>& fingerTips) { _fingerTips = fingerTips; }
+    void setFingerRoots(const std::vector<glm::vec3>& fingerRoots) { _fingerRoots = fingerRoots; }
+    void setHandPositions(const std::vector<glm::vec3>& handPositons) { _handPositions = handPositons; }
+    void setHandNormals(const std::vector<glm::vec3>& handNormals) { _handNormals = handNormals; }
     
     friend class AvatarData;
 protected:
-    std::vector<glm::vec3> _fingerPositions;
+    std::vector<glm::vec3> _fingerTips;
+    std::vector<glm::vec3> _fingerRoots;
+    std::vector<glm::vec3> _handPositions;
+    std::vector<glm::vec3> _handNormals;
     AvatarData* _owningAvatarData;
 private:
     // privatize copy ctor and assignment operator so copies of this object cannot be made