diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index d1fa5f1dc1..9571b44839 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -2103,10 +2103,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()) { diff --git a/interface/src/LeapManager.cpp b/interface/src/LeapManager.cpp index fe47e527fa..a941d0a221 100755 --- a/interface/src/LeapManager.cpp +++ b/interface/src/LeapManager.cpp @@ -7,6 +7,7 @@ // #include "LeapManager.h" +#include "avatar/Avatar.h" #include #include // needed for RTLD_LAZY #include @@ -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 fingerTips; - std::vector fingerRoots; - std::vector handPositions; - std::vector 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,195 @@ 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; + if (controllersExist()) { _listener->onFrame(*_controller); } + +#ifndef LEAP_STUBS + if (controllersExist()) { + gotRealData = true; + // 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. + + // After this many frames of no data, assume the digit is lost. + const int assumeLostAfterFrameCount = 10; + + // Increment our frame data counters + for (size_t i = 0; i < hand.getNumPalms(); ++i) { + PalmData& palm = hand.getPalms()[i]; + palm.incrementFramesWithoutData(); + if (palm.getFramesWithoutData() > assumeLostAfterFrameCount) { + palm.setActive(false); + } + for (size_t f = 0; f < palm.getNumFingers(); ++f) { + FingerData& finger = palm.getFingers()[f]; + finger.incrementFramesWithoutData(); + if (finger.getFramesWithoutData() > assumeLostAfterFrameCount) { + finger.setActive(false); + } + } + } + + size_t numLeapHands = frame.hands().count(); + std::vector palmAssignment(numLeapHands); + + // Look for matches + for (size_t index = 0; index < numLeapHands; ++index) { + PalmData* takeoverCandidate = NULL; + palmAssignment[index] = NULL; + Leap::Hand leapHand = frame.hands()[index]; + int id = leapHand.id(); + if (leapHand.isValid()) { + for (size_t i = 0; i < hand.getNumPalms() && palmAssignment[index] == NULL; ++i) { + PalmData& palm = hand.getPalms()[i]; + if (palm.getLeapID() == id) { + // Found hand with the same ID. We're set! + palmAssignment[index] = &palm; + palm.resetFramesWithoutData(); + } + else if (palm.getFramesWithoutData() > assumeLostAfterFrameCount) { + takeoverCandidate = &palm; + } + } + if (palmAssignment[index] == NULL) { + palmAssignment[index] = takeoverCandidate; + } + if (palmAssignment[index] == NULL) { + palmAssignment[index] = &hand.addNewPalm(); + } + } + } + + // Apply the assignments + for (size_t index = 0; index < numLeapHands; ++index) { + if (palmAssignment[index]) { + Leap::Hand leapHand = frame.hands()[index]; + PalmData& palm = *(palmAssignment[index]); + + palm.resetFramesWithoutData(); + palm.setLeapID(leapHand.id()); + palm.setActive(true); + 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)); + } + } + + // Look for fingers per palm + for (size_t i = 0; i < hand.getNumPalms(); ++i) { + PalmData& palm = hand.getPalms()[i]; + if (palm.isActive()) { + Leap::Hand leapHand = frame.hand(palm.getLeapID()); + if (leapHand.isValid()) { + int numLeapFingers = leapHand.fingers().count(); + std::vector fingerAssignment(numLeapFingers); + + + // Look for matches + for (size_t index = 0; index < numLeapFingers; ++index) { + FingerData* takeoverCandidate = NULL; + fingerAssignment[index] = NULL; + Leap::Finger leapFinger = leapHand.fingers()[index]; + int id = leapFinger.id(); + if (leapFinger.isValid()) { + for (size_t f = 0; f < palm.getNumFingers() && fingerAssignment[index] == NULL; ++f) { + FingerData& finger = palm.getFingers()[f]; + if (finger.getLeapID() == id) { + // Found hand with the same ID. We're set! + fingerAssignment[index] = &finger; + } + else if (finger.getFramesWithoutData() > assumeLostAfterFrameCount) { + takeoverCandidate = &finger; + } + } + // If we didn't find a match, but we found an unused finger, us it. + if (fingerAssignment[index] == NULL) { + fingerAssignment[index] = takeoverCandidate; + } + } + } + + // Apply the assignments + for (size_t index = 0; index < numLeapFingers; ++index) { + if (fingerAssignment[index]) { + Leap::Finger leapFinger = leapHand.fingers()[index]; + FingerData& finger = *(fingerAssignment[index]); + + finger.resetFramesWithoutData(); + finger.setLeapID(leapFinger.id()); + finger.setActive(true); + 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)); + } + } + } + } + } + } +#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 +251,6 @@ bool LeapManager::controllersExist() { #endif } -const std::vector& LeapManager::getFingerTips() { - if (controllersExist()) { - return _listener->fingerTips; - } - else { - static std::vector 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& LeapManager::getFingerRoots() { - if (controllersExist()) { - return _listener->fingerRoots; - } - else { - static std::vector 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& LeapManager::getHandPositions() { - if (controllersExist()) { - return _listener->handPositions; - } - else { - static std::vector 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& LeapManager::getHandNormals() { - if (controllersExist()) { - return _listener->handNormals; - } - else { - static std::vector 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 diff --git a/interface/src/LeapManager.h b/interface/src/LeapManager.h index e6ac304677..11dbefe849 100755 --- a/interface/src/LeapManager.h +++ b/interface/src/LeapManager.h @@ -13,6 +13,7 @@ #include #include +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& getFingerTips(); diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp index 69d8ddfaac..97594d0f50 100755 --- a/interface/src/avatar/Hand.cpp +++ b/interface/src/avatar/Hand.cpp @@ -95,6 +95,7 @@ void Hand::render(bool lookingInMirror) { glEnable(GL_DEPTH_TEST); glEnable(GL_RESCALE_NORMAL); + renderFingerTrails(); renderHandSpheres(); } @@ -173,44 +174,35 @@ void Hand::renderHandSpheres() { glPopMatrix(); } -void Hand::setLeapFingers(const std::vector& fingerTips, - const std::vector& fingerRoots) { - // TODO: add id-checking here to increase finger stability - - size_t fingerIndex = 0; +void Hand::renderFingerTrails() { + // Draw the finger root cones 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); + if (palm.isActive()) { + for (size_t f = 0; f < palm.getNumFingers(); ++f) { + FingerData& finger = palm.getFingers()[f]; + int numPositions = finger.getTrailNumPositions(); + if (numPositions > 0) { + glBegin(GL_TRIANGLE_STRIP); + for (int t = 0; t < numPositions; ++t) + { + const glm::vec3& center = finger.getTrailPosition(t); + const float halfWidth = 0.001f; + const glm::vec3 edgeDirection(1.0f, 0.0f, 0.0f); + glm::vec3 edge0 = center + edgeDirection * halfWidth; + glm::vec3 edge1 = center - edgeDirection * halfWidth; + float alpha = 1.0f - ((float)t / (float)(numPositions - 1)); + glColor4f(1.0f, 0.0f, 0.0f, alpha); + glVertex3fv((float*)&edge0); + glVertex3fv((float*)&edge1); + } + glEnd(); + } } } } } -void Hand::setLeapHands(const std::vector& handPositions, - const std::vector& 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) { diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h index fb6b863458..f37046ed3a 100755 --- a/interface/src/avatar/Hand.h +++ b/interface/src/avatar/Hand.h @@ -42,10 +42,6 @@ public: void render(bool lookingInMirror); void setBallColor (glm::vec3 ballColor ) { _ballColor = ballColor; } - void setLeapFingers (const std::vector& fingerTips, - const std::vector& fingerRoots); - void setLeapHands (const std::vector& handPositions, - const std::vector& handNormals); void updateFingerParticles(float deltaTime); void setRaveGloveActive(bool active) { _isRaveGloveActive = active; } @@ -74,6 +70,7 @@ private: // private methods void renderRaveGloveStage(); void renderHandSpheres(); + void renderFingerTrails(); void calculateGeometry(); }; diff --git a/libraries/avatars/src/HandData.cpp b/libraries/avatars/src/HandData.cpp index 32e83b352d..7bb6eebdc0 100755 --- a/libraries/avatars/src/HandData.cpp +++ b/libraries/avatars/src/HandData.cpp @@ -13,15 +13,22 @@ HandData::HandData(AvatarData* owningAvatar) : _baseOrientation(0.0f, 0.0f, 0.0f, 1.0f), _owningAvatarData(owningAvatar) { - for (int i = 0; i < 2; ++i) { - _palms.push_back(PalmData(this)); - } + // Start with two palms + addNewPalm(); + addNewPalm(); +} + +PalmData& HandData::addNewPalm() { + _palms.push_back(PalmData(this)); + return _palms.back(); } PalmData::PalmData(HandData* owningHandData) : _rawPosition(0, 0, 0), _rawNormal(0, 1, 0), _isActive(false), +_leapID(LEAPID_INVALID), +_numFramesWithoutData(0), _owningHandData(owningHandData) { for (int i = 0; i < NUM_FINGERS_PER_HAND; ++i) { @@ -33,9 +40,13 @@ FingerData::FingerData(PalmData* owningPalmData, HandData* owningHandData) : _tipRawPosition(0, 0, 0), _rootRawPosition(0, 0, 0), _isActive(false), +_leapID(LEAPID_INVALID), +_numFramesWithoutData(0), _owningPalmData(owningPalmData), _owningHandData(owningHandData) { + const int standardTrailLength = 30; + setTrailLength(standardTrailLength); } void HandData::encodeRemoteData(std::vector& fingerVectors) { @@ -80,3 +91,66 @@ void HandData::decodeRemoteData(const std::vector& fingerVectors) { } } +void HandData::setFingerTrailLength(unsigned int length) { + 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]; + finger.setTrailLength(length); + } + } +} + +void HandData::updateFingerTrails() { + 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]; + finger.updateTrail(); + } + } +} + +void FingerData::setTrailLength(unsigned int length) { + _tipTrailPositions.resize(length); + _tipTrailCurrentStartIndex = 0; + _tipTrailCurrentValidLength = 0; +} + +void FingerData::updateTrail() { + if (_tipTrailPositions.size() == 0) + return; + + if (_isActive) { + // Add the next point in the trail. + _tipTrailCurrentStartIndex--; + if (_tipTrailCurrentStartIndex < 0) + _tipTrailCurrentStartIndex = _tipTrailPositions.size() - 1; + + _tipTrailPositions[_tipTrailCurrentStartIndex] = getTipPosition(); + + if (_tipTrailCurrentValidLength < _tipTrailPositions.size()) + _tipTrailCurrentValidLength++; + } + else { + // It's not active, so just shorten the trail. + if (_tipTrailCurrentValidLength > 0) + _tipTrailCurrentValidLength--; + } +} + +int FingerData::getTrailNumPositions() { + return _tipTrailCurrentValidLength; +} + +const glm::vec3& FingerData::getTrailPosition(int index) { + if (index >= _tipTrailCurrentValidLength) { + static glm::vec3 zero(0,0,0); + return zero; + } + int posIndex = (index + _tipTrailCurrentStartIndex) % _tipTrailCurrentValidLength; + return _tipTrailPositions[posIndex]; +} + + + diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h index 65b32ff5c6..d2b5cae90d 100755 --- a/libraries/avatars/src/HandData.h +++ b/libraries/avatars/src/HandData.h @@ -20,6 +20,7 @@ class FingerData; class PalmData; const int NUM_FINGERS_PER_HAND = 5; +const int LEAPID_INVALID = -1; class HandData { public: @@ -39,6 +40,10 @@ public: std::vector& getPalms() { return _palms; } size_t getNumPalms() { return _palms.size(); } + PalmData& addNewPalm(); + + void setFingerTrailLength(unsigned int length); + void updateFingerTrails(); // Use these for sending and receiving hand data void encodeRemoteData(std::vector& fingerVectors); @@ -65,15 +70,31 @@ 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); + void updateTrail(); + + int getTrailNumPositions(); + const glm::vec3& getTrailPosition(int index); + + void incrementFramesWithoutData() { _numFramesWithoutData++; } + void resetFramesWithoutData() { _numFramesWithoutData = 0; } + int getFramesWithoutData() const { return _numFramesWithoutData; } 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 + int _numFramesWithoutData; // after too many frames without data, this tracked object assumed lost. + std::vector _tipTrailPositions; + int _tipTrailCurrentStartIndex; + int _tipTrailCurrentValidLength; PalmData* _owningPalmData; HandData* _owningHandData; }; @@ -86,19 +107,27 @@ 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& 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; } + void incrementFramesWithoutData() { _numFramesWithoutData++; } + void resetFramesWithoutData() { _numFramesWithoutData = 0; } + int getFramesWithoutData() const { return _numFramesWithoutData; } + private: std::vector _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 + int _numFramesWithoutData; // after too many frames without data, this tracked object assumed lost. HandData* _owningHandData; };