Rave Glove Demo: Leap Rework (simplification and elimination of redundant data passing)

This version matches fingers based on Leap serialized ID's, avoiding finger-replacement-crosstalk.

interface/src/Application.cpp

@@ -2046,10 +2046,8 @@ void Application::update(float deltaTime) {
     
     // Leap finger-sensing device
     LeapManager::enableFakeFingers(_simulateLeapHand->isChecked() || _testRaveGlove->isChecked());
-    LeapManager::nextFrame();
     _myAvatar.getHand().setRaveGloveActive(_testRaveGlove->isChecked());
-    _myAvatar.getHand().setLeapFingers(LeapManager::getFingerTips(), LeapManager::getFingerRoots());
-    _myAvatar.getHand().setLeapHands(LeapManager::getHandPositions(), LeapManager::getHandNormals());
+    LeapManager::nextFrame(_myAvatar);
     
      //  Read serial port interface devices
     if (_serialHeadSensor.isActive()) {

interface/src/Hand.cpp

@@ -203,45 +203,6 @@ void Hand::renderFingerTrails() {
     }
 }
 
-void Hand::setLeapFingers(const std::vector<glm::vec3>& fingerTips,
-                          const std::vector<glm::vec3>& fingerRoots) {
-    // TODO: add id-checking here to increase finger stability
-    
-    size_t fingerIndex = 0;
-    for (size_t i = 0; i < getNumPalms(); ++i) {
-        PalmData& palm = getPalms()[i];
-        for (size_t f = 0; f < palm.getNumFingers(); ++f) {
-            FingerData& finger = palm.getFingers()[f];
-            if (fingerIndex < fingerTips.size()) {
-                finger.setActive(true);
-                finger.setRawTipPosition(fingerTips[fingerIndex]);
-                finger.setRawRootPosition(fingerRoots[fingerIndex]);
-                fingerIndex++;
-            }
-            else {
-                finger.setActive(false);
-            }
-        }
-    }
-    updateFingerTrails();
-}
-
-void Hand::setLeapHands(const std::vector<glm::vec3>& handPositions,
-                          const std::vector<glm::vec3>& handNormals) {
-    for (size_t i = 0; i < getNumPalms(); ++i) {
-        PalmData& palm = getPalms()[i];
-        if (i < handPositions.size()) {
-            palm.setActive(true);
-            palm.setRawPosition(handPositions[i]);
-            palm.setRawNormal(handNormals[i]);
-        }
-        else {
-            palm.setActive(false);
-        }
-    }
-}
-
-
 void Hand::updateFingerParticles(float deltaTime) {
 
     if (!_particleSystemInitialized) {

interface/src/Hand.h

@@ -42,10 +42,6 @@ public:
     void render(bool lookingInMirror);
 
     void setBallColor      (glm::vec3 ballColor         ) { _ballColor          = ballColor;          }
-    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);
     void updateFingerParticles(float deltaTime);
     void setRaveGloveActive(bool active) { _isRaveGloveActive = active; }
 

interface/src/LeapManager.cpp

@@ -7,6 +7,7 @@
 //
 
 #include "LeapManager.h"
+#include "Avatar.h"
 #include <Leap.h>
 #include <dlfcn.h>     // needed for RTLD_LAZY
 #include <sstream>
@@ -16,48 +17,15 @@ bool LeapManager::_doFakeFingers = false;
 Leap::Controller* LeapManager::_controller = NULL;
 HifiLeapListener* LeapManager::_listener = NULL;
 
-namespace {
-glm::vec3 fakeHandOffset(0.0f, 50.0f, 50.0f);
-}   // end anonymous namespace
-
 class HifiLeapListener  : public Leap::Listener {
 public:
     HifiLeapListener() {}
     virtual ~HifiLeapListener() {}
     
     Leap::Frame lastFrame;
-    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();
-        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.stabilizedTipPosition();
-            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;
+        lastFrame = controller.frame();
 #endif
     }
     
@@ -80,10 +48,192 @@ void LeapManager::terminate() {
     _controller = NULL;
 }
 
-void LeapManager::nextFrame() {
+void LeapManager::nextFrame(Avatar& avatar) {
+    // Apply the frame data directly to the avatar.
+    Hand& hand = avatar.getHand();
+    
+    // If we actually get valid Leap data, this will be set to true;
+    bool gotRealData = false;
+    // First, deactivate everything.
+    for (size_t i = 0; i < hand.getNumPalms(); ++i) {
+        PalmData& palm = hand.getPalms()[i];
+        palm.setActive(false);
+        for (size_t f = 0; f < palm.getNumFingers(); ++f) {
+            FingerData& finger = palm.getFingers()[f];
+            finger.setActive(false);
+        }
+    }
+
     if (controllersExist()) {
         _listener->onFrame(*_controller);
     }
+
+#ifndef LEAP_STUBS
+    if (controllersExist()) {
+        // Performance note:
+        //   This is a first pass.
+        //   Once all of this is stable, perfoamance may be improved ysing std::map or another
+        //   associative container to match serialized Leap ID's with available fingers.
+        //   That will make this code shorter and more efficient.
+        
+        // First, see which palms and fingers are still valid.
+        Leap::Frame& frame = _listener->lastFrame;
+        
+        // Note that this is O(n^2) at worst, but n is very small.
+
+        size_t numLeapHands = frame.hands().count();
+        std::vector<PalmData*> palmAssignment(numLeapHands);
+        // Look for matches
+        for (size_t index = 0; index < numLeapHands; ++index) {
+            palmAssignment[index] = NULL;
+            Leap::Hand leapHand = frame.hands()[index];
+            int id = leapHand.id();
+            if (leapHand.isValid()) {
+                for (size_t i = 0; i < hand.getNumPalms(); ++i) {
+                    PalmData& palm = hand.getPalms()[i];
+                    if (palm.getLeapID() == id) {
+                        // Found hand with the same ID. We're set!
+                        palmAssignment[index] = &palm;
+                        palm.setActive(true);
+                    }
+                }
+            }
+        }
+        // Fill empty slots
+        for (size_t index = 0; index < numLeapHands; ++index) {
+            if (palmAssignment[index] == NULL) {
+                Leap::Hand leapHand = frame.hands()[index];
+                if (leapHand.isValid()) {
+                    for (size_t i = 0; i < hand.getNumPalms() && palmAssignment[index] == NULL; ++i) {
+                        PalmData& palm = hand.getPalms()[i];
+                        if (!palm.isActive()) {
+                            // Found a free hand to use.
+                            palmAssignment[index] = &palm;
+                            palm.setActive(true);
+                            palm.setLeapID(leapHand.id());
+                        }
+                    }
+                }
+            }
+        }
+        // Apply the assignments
+        for (size_t index = 0; index < numLeapHands; ++index) {
+            if (palmAssignment[index]) {
+                Leap::Hand leapHand = frame.hands()[index];
+                PalmData& palm = *(palmAssignment[index]);
+                const Leap::Vector pos = leapHand.palmPosition();
+                const Leap::Vector normal = leapHand.palmNormal();
+                palm.setRawPosition(glm::vec3(pos.x, pos.y, pos.z));
+                palm.setRawNormal(glm::vec3(normal.x, normal.y, normal.z));
+            }
+        }
+
+        size_t numLeapFingers = frame.fingers().count();
+        std::vector<FingerData*> fingerAssignment(numLeapFingers);
+        // Look for matches
+        for (size_t index = 0; index < numLeapFingers; ++index) {
+            fingerAssignment[index] = NULL;
+            Leap::Finger leapFinger = frame.fingers()[index];
+            int id = leapFinger.id();
+            if (leapFinger.isValid()) {
+                for (size_t i = 0; i < hand.getNumPalms(); ++i) {
+                    PalmData& palm = hand.getPalms()[i];
+                    for (size_t f = 0; f < palm.getNumFingers(); ++f) {
+                        FingerData& finger = palm.getFingers()[f];
+                        if (finger.getLeapID() == id) {
+                            // Found finger with the same ID. We're set!
+                            fingerAssignment[index] = &finger;
+                            finger.setActive(true);
+                        }
+                    }
+                }
+            }
+        }
+        // Fill empty slots
+        for (size_t index = 0; index < numLeapFingers; ++index) {
+            if (fingerAssignment[index] == NULL) {
+                Leap::Finger leapFinger = frame.fingers()[index];
+                if (leapFinger.isValid()) {
+                    for (size_t i = 0; i < hand.getNumPalms() && fingerAssignment[index] == NULL; ++i) {
+                        PalmData& palm = hand.getPalms()[i];
+                        for (size_t f = 0; f < palm.getNumFingers() && fingerAssignment[index] == NULL; ++f) {
+                            FingerData& finger = palm.getFingers()[f];
+                            if (!finger.isActive()) {
+                                // Found a free finger to use.
+                                fingerAssignment[index] = &finger;
+                                finger.setActive(true);
+                                finger.setLeapID(leapFinger.id());
+                            }
+                        }
+                    }
+                }
+            }
+        }
+        // Apply the assignments
+        for (size_t index = 0; index < numLeapFingers; ++index) {
+            if (fingerAssignment[index]) {
+                Leap::Finger leapFinger = frame.fingers()[index];
+                FingerData& finger = *(fingerAssignment[index]);
+                const Leap::Vector tip = leapFinger.stabilizedTipPosition();
+                const Leap::Vector root = tip - leapFinger.direction() * leapFinger.length();
+                finger.setRawTipPosition(glm::vec3(tip.x, tip.y, tip.z));
+                finger.setRawRootPosition(glm::vec3(root.x, root.y, root.z));
+            }
+        }
+        gotRealData = true;
+    }
+#endif
+    if (!gotRealData) {
+        if (_doFakeFingers) {
+            // There's no real Leap data and we need to fake it.
+            for (size_t i = 0; i < hand.getNumPalms(); ++i) {
+                static const glm::vec3 fakeHandOffsets[] = {
+                    glm::vec3( -500.0f, 50.0f, 50.0f),
+                    glm::vec3(    0.0f, 50.0f, 50.0f)
+                };
+                static const glm::vec3 fakeHandFingerMirrors[] = {
+                    glm::vec3( -1.0f, 1.0f, 1.0f),
+                    glm::vec3(  1.0f, 1.0f, 1.0f)
+                };
+                static const glm::vec3 fakeFingerPositions[] = {
+                    glm::vec3( -60.0f, 0.0f, -40.0f),
+                    glm::vec3( -20.0f, 0.0f, -60.0f),
+                    glm::vec3(  20.0f, 0.0f, -60.0f),
+                    glm::vec3(  60.0f, 0.0f, -40.0f),
+                    glm::vec3( -50.0f, 0.0f,  30.0f)
+                };
+
+                PalmData& palm = hand.getPalms()[i];
+                palm.setActive(true);
+                // Simulated data
+                
+                palm.setRawPosition(glm::vec3( 0.0f, 0.0f, 0.0f) + fakeHandOffsets[i]);
+                palm.setRawNormal(glm::vec3(0.0f, 1.0f, 0.0f));
+
+                for (size_t f = 0; f < palm.getNumFingers(); ++f) {
+                    FingerData& finger = palm.getFingers()[f];
+                    finger.setActive(true);
+                    const float tipScale = 1.5f;
+                    const float rootScale = 0.75f;
+                    glm::vec3 fingerPos = fakeFingerPositions[f] * fakeHandFingerMirrors[i];
+                    finger.setRawTipPosition(fingerPos * tipScale + fakeHandOffsets[i]);
+                    finger.setRawRootPosition(fingerPos * rootScale + fakeHandOffsets[i]);
+                }
+            }
+        }
+        else {
+            // Just deactivate everything.
+            for (size_t i = 0; i < hand.getNumPalms(); ++i) {
+                PalmData& palm = hand.getPalms()[i];
+                palm.setActive(false);
+                for (size_t f = 0; f < palm.getNumFingers(); ++f) {
+                    FingerData& finger = palm.getFingers()[f];
+                    finger.setActive(false);
+                }
+            }
+        }
+    }
+    hand.updateFingerTrails();
 }
 
 void LeapManager::enableFakeFingers(bool enable) {
@@ -98,77 +248,6 @@ bool LeapManager::controllersExist() {
 #endif
 }
 
-const std::vector<glm::vec3>& LeapManager::getFingerTips() {
-    if (controllersExist()) {
-        return _listener->fingerTips;
-    }
-    else {
-        static std::vector<glm::vec3> stubData;
-        stubData.clear();
-        if (_doFakeFingers) {
-            // Simulated data
-            float scale = 1.5f;
-            stubData.push_back(glm::vec3( -60.0f, 0.0f, -40.0f) * scale + fakeHandOffset);
-            stubData.push_back(glm::vec3( -20.0f, 0.0f, -60.0f) * scale + fakeHandOffset);
-            stubData.push_back(glm::vec3(  20.0f, 0.0f, -60.0f) * scale + fakeHandOffset);
-            stubData.push_back(glm::vec3(  60.0f, 0.0f, -40.0f) * scale + fakeHandOffset);
-            stubData.push_back(glm::vec3( -50.0f, 0.0f,  30.0f) * scale + fakeHandOffset);
-        }
-        return stubData;
-    }
-}
-
-const std::vector<glm::vec3>& LeapManager::getFingerRoots() {
-    if (controllersExist()) {
-        return _listener->fingerRoots;
-    }
-    else {
-        static std::vector<glm::vec3> stubData;
-        stubData.clear();
-        if (_doFakeFingers) {
-            // Simulated data
-            float scale = 0.75f;
-            stubData.push_back(glm::vec3( -60.0f, 0.0f, -40.0f) * scale + fakeHandOffset);
-            stubData.push_back(glm::vec3( -20.0f, 0.0f, -60.0f) * scale + fakeHandOffset);
-            stubData.push_back(glm::vec3(  20.0f, 0.0f, -60.0f) * scale + fakeHandOffset);
-            stubData.push_back(glm::vec3(  60.0f, 0.0f, -40.0f) * scale + fakeHandOffset);
-            stubData.push_back(glm::vec3( -50.0f, 0.0f,  30.0f) * scale + fakeHandOffset);
-        }
-        return stubData;
-    }
-}
-
-const std::vector<glm::vec3>& LeapManager::getHandPositions() {
-    if (controllersExist()) {
-        return _listener->handPositions;
-    }
-    else {
-        static std::vector<glm::vec3> stubData;
-        stubData.clear();
-        if (_doFakeFingers) {
-            // Simulated data
-            glm::vec3 handOffset(0.0f, 50.0f, 50.0f);
-            stubData.push_back(glm::vec3( 0.0f, 0.0f, 0.0f) + fakeHandOffset);
-        }
-        return stubData;
-    }
-}
-
-const std::vector<glm::vec3>& LeapManager::getHandNormals() {
-    if (controllersExist()) {
-        return _listener->handNormals;
-    }
-    else {
-        static std::vector<glm::vec3> stubData;
-        stubData.clear();
-        if (_doFakeFingers) {
-            // Simulated data
-            stubData.push_back(glm::vec3(0.0f, 1.0f, 0.0f));
-        }
-        return stubData;
-    }
-}
-
 std::string LeapManager::statusString() {
     std::stringstream leapString;
 #ifndef LEAP_STUBS

interface/src/LeapManager.h

@@ -13,6 +13,7 @@
 #include <glm/glm.hpp>
 #include <string>
 
+class Avatar;
 class HifiLeapListener;
 namespace Leap {
     class Controller;
@@ -20,7 +21,7 @@ namespace Leap {
 
 class LeapManager {
 public:
-    static void nextFrame();                    // called once per frame to get new Leap data
+    static void nextFrame(Avatar& avatar);      // 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();

libraries/avatars/src/HandData.cpp

@@ -24,6 +24,7 @@ PalmData::PalmData(HandData* owningHandData) :
 _rawPosition(0, 0, 0),
 _rawNormal(0, 1, 0),
 _isActive(false),
+_leapID(-1),
 _owningHandData(owningHandData)
 {
     for (int i = 0; i < NUM_FINGERS_PER_HAND; ++i) {
@@ -35,6 +36,7 @@ FingerData::FingerData(PalmData* owningPalmData, HandData* owningHandData) :
 _tipRawPosition(0, 0, 0),
 _rootRawPosition(0, 0, 0),
 _isActive(false),
+_leapID(-1),
 _owningPalmData(owningPalmData),
 _owningHandData(owningHandData)
 {

libraries/avatars/src/HandData.h

@@ -68,8 +68,10 @@ public:
     const glm::vec3& getTipRawPosition()  const { return _tipRawPosition; }
     const glm::vec3& getRootRawPosition() const { return _rootRawPosition; }
     bool             isActive()           const { return _isActive; }
+    int              getLeapID()          const { return _leapID; }
 
     void setActive(bool active)                   { _isActive = active; }
+    void setLeapID(int id)                        { _leapID = id; }
     void setRawTipPosition(const glm::vec3& pos)  { _tipRawPosition = pos; }
     void setRawRootPosition(const glm::vec3& pos) { _rootRawPosition = pos; }
     void setTrailLength(unsigned int length);
@@ -81,7 +83,8 @@ public:
 private:
     glm::vec3 _tipRawPosition;
     glm::vec3 _rootRawPosition;
-    bool      _isActive;        // This has current valid data
+    bool      _isActive;            // This has current valid data
+    int       _leapID;              // the Leap's serial id for this tracked object
     std::vector<glm::vec3> _tipTrailPositions;
     int                    _tipTrailCurrentStartIndex;
     int                    _tipTrailCurrentValidLength;
@@ -97,11 +100,13 @@ public:
     const glm::vec3& getRawPosition() const { return _rawPosition; }
     const glm::vec3& getRawNormal()   const { return _rawNormal; }
     bool             isActive()       const { return _isActive; }
+    int              getLeapID()      const { return _leapID; }
 
     std::vector<FingerData>& getFingers()    { return _fingers; }
     size_t                   getNumFingers() { return _fingers.size(); }
 
     void setActive(bool active)                { _isActive = active; }
+    void setLeapID(int id)                     { _leapID = id; }
     void setRawPosition(const glm::vec3& pos)  { _rawPosition = pos; }
     void setRawNormal(const glm::vec3& normal) { _rawNormal = normal; }
 
@@ -109,7 +114,8 @@ private:
     std::vector<FingerData> _fingers;
     glm::vec3 _rawPosition;
     glm::vec3 _rawNormal;
-    bool      _isActive;        // This has current valid data
+    bool      _isActive;            // This has current valid data
+    int       _leapID;              // the Leap's serial id for this tracked object
     HandData* _owningHandData;
 };