Merge pull request #2842 from AndrewMeadows/master

Revert "Merge pull request #2834 from andrew/thermonuclear"

interface/src/BuckyBalls.cpp

@@ -60,13 +60,15 @@ BuckyBalls::BuckyBalls() {
 
 void BuckyBalls::grab(PalmData& palm, float deltaTime) {
     float penetration;
-    glm::vec3 fingerTipPosition = palm.getFingerTipPosition();
+    glm::vec3 diff;
+    FingerData& finger = palm.getFingers()[0];   //  Sixense has only one finger
+    glm::vec3 fingerTipPosition = finger.getTipPosition();
 
     if (palm.getControllerButtons() & BUTTON_FWD) {
         if (!_bballIsGrabbed[palm.getSixenseID()]) {
             // Look for a ball to grab
             for (int i = 0; i < NUM_BBALLS; i++) {
-                glm::vec3 diff = _bballPosition[i] - fingerTipPosition;
+                diff = _bballPosition[i] - fingerTipPosition;
                 penetration = glm::length(diff) - (_bballRadius[i] + COLLISION_RADIUS);
                 if (penetration < 0.f) {
                     _bballIsGrabbed[palm.getSixenseID()] = i;
@@ -75,7 +77,7 @@ void BuckyBalls::grab(PalmData& palm, float deltaTime) {
         }
         if (_bballIsGrabbed[palm.getSixenseID()]) {
             //  If ball being grabbed, move with finger
-            glm::vec3 diff = _bballPosition[_bballIsGrabbed[palm.getSixenseID()]] - fingerTipPosition;
+            diff = _bballPosition[_bballIsGrabbed[palm.getSixenseID()]] - fingerTipPosition;
             penetration = glm::length(diff) - (_bballRadius[_bballIsGrabbed[palm.getSixenseID()]] + COLLISION_RADIUS);
             _bballPosition[_bballIsGrabbed[palm.getSixenseID()]] -= glm::normalize(diff) * penetration;
             glm::vec3 fingerTipVelocity = palm.getTipVelocity();

interface/src/avatar/Avatar.cpp

@@ -609,6 +609,18 @@ bool Avatar::findParticleCollisions(const glm::vec3& particleCenter, float parti
             const PalmData* palm = handData->getPalm(i);
             if (palm && palm->hasPaddle()) {
                 // create a disk collision proxy where the hand is
+                glm::vec3 fingerAxis(0.0f);
+                for (size_t f = 0; f < palm->getNumFingers(); ++f) {
+                    const FingerData& finger = (palm->getFingers())[f];
+                    if (finger.isActive()) {
+                        // compute finger axis
+                        glm::vec3 fingerTip = finger.getTipPosition();
+                        glm::vec3 fingerRoot = finger.getRootPosition();
+                        fingerAxis = glm::normalize(fingerTip - fingerRoot);
+                        break;
+                    }
+                }
+
                 int jointIndex = -1;
                 glm::vec3 handPosition;
                 if (i == 0) {
@@ -619,10 +631,8 @@ bool Avatar::findParticleCollisions(const glm::vec3& particleCenter, float parti
                     _skeletonModel.getRightHandPosition(handPosition);
                     jointIndex = _skeletonModel.getRightHandJointIndex();
                 }
-
-                glm::vec3 fingerAxis = palm->getFingerDirection();
                 glm::vec3 diskCenter = handPosition + HAND_PADDLE_OFFSET * fingerAxis;
-                glm::vec3 diskNormal = palm->getPalmDirection();
+                glm::vec3 diskNormal = palm->getNormal();
                 const float DISK_THICKNESS = 0.08f;
 
                 // collide against the disk

interface/src/avatar/Hand.cpp

@@ -176,7 +176,8 @@ void Hand::renderHandTargets(bool isMine) {
             if (!palm.isActive()) {
                 continue;
             }
-            glm::vec3 targetPosition = palm.getFingerTipPosition();
+            glm::vec3 targetPosition;
+            palm.getBallHoldPosition(targetPosition);
             glPushMatrix();
             glTranslatef(targetPosition.x, targetPosition.y, targetPosition.z);
         
@@ -196,20 +197,59 @@ void Hand::renderHandTargets(bool isMine) {
     for (size_t i = 0; i < getNumPalms(); ++i) {
         PalmData& palm = getPalms()[i];
         if (palm.isActive()) {
-            glColor4f(handColor.r, handColor.g, handColor.b, alpha);
+            for (size_t f = 0; f < palm.getNumFingers(); ++f) {
-            glm::vec3 tip = palm.getFingerTipPosition();
+                FingerData& finger = palm.getFingers()[f];
-            glm::vec3 root = palm.getPosition();
+                if (finger.isActive()) {
-            Avatar::renderJointConnectingCone(root, tip, PALM_FINGER_ROD_RADIUS, PALM_FINGER_ROD_RADIUS);
+                    glColor4f(handColor.r, handColor.g, handColor.b, alpha);
-            //  Render sphere at palm/finger root
+                    glm::vec3 tip = finger.getTipPosition();
-            glm::vec3 offsetFromPalm = root + palm.getPalmDirection() * PALM_DISK_THICKNESS;
+                    glm::vec3 root = finger.getRootPosition();
-            Avatar::renderJointConnectingCone(root, offsetFromPalm, PALM_DISK_RADIUS, 0.0f);
+                    Avatar::renderJointConnectingCone(root, tip, PALM_FINGER_ROD_RADIUS, PALM_FINGER_ROD_RADIUS);
-            glPushMatrix();
+                    //  Render sphere at palm/finger root
-            glTranslatef(root.x, root.y, root.z);
+                    glm::vec3 palmNormal = root + palm.getNormal() * PALM_DISK_THICKNESS;
-            glutSolidSphere(PALM_BALL_RADIUS, 20.0f, 20.0f);
+                    Avatar::renderJointConnectingCone(root, palmNormal, PALM_DISK_RADIUS, 0.0f);
-            glPopMatrix();
+                    glPushMatrix();
+                    glTranslatef(root.x, root.y, root.z);
+                    glutSolidSphere(PALM_BALL_RADIUS, 20.0f, 20.0f);
+                    glPopMatrix();
+
+                }
+            }
         }
     }
 
+    /*
+    // Draw the hand paddles
+    int MAX_NUM_PADDLES = 2; // one for left and one for right
+    glColor4f(handColor.r, handColor.g, handColor.b, 0.3f);
+    for (int i = 0; i < MAX_NUM_PADDLES; i++) {
+        const PalmData* palm = getPalm(i);
+        if (palm) {
+            // compute finger axis
+            glm::vec3 fingerAxis(0.f);
+            for (size_t f = 0; f < palm->getNumFingers(); ++f) {
+                const FingerData& finger = (palm->getFingers())[f];
+                if (finger.isActive()) {
+                    glm::vec3 fingerTip = finger.getTipPosition();
+                    glm::vec3 fingerRoot = finger.getRootPosition();
+                    fingerAxis = glm::normalize(fingerTip - fingerRoot);
+                    break;
+                }
+            }
+            // compute paddle position
+            glm::vec3 handPosition;
+            if (i == SIXENSE_CONTROLLER_ID_LEFT_HAND) {
+                _owningAvatar->getSkeletonModel().getLeftHandPosition(handPosition);
+            } else if (i == SIXENSE_CONTROLLER_ID_RIGHT_HAND) {
+                _owningAvatar->getSkeletonModel().getRightHandPosition(handPosition);
+            }
+            glm::vec3 tip = handPosition + HAND_PADDLE_OFFSET * fingerAxis;
+            glm::vec3 root = tip + palm->getNormal() * HAND_PADDLE_THICKNESS;
+            // render a very shallow cone as the paddle
+            Avatar::renderJointConnectingCone(root, tip, HAND_PADDLE_RADIUS, 0.f);
+        }
+    }
+     */
+
     glDepthMask(GL_TRUE);
     glEnable(GL_DEPTH_TEST);
 

interface/src/avatar/SkeletonModel.cpp

@@ -33,7 +33,7 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
         return; // only simulate for own avatar
     }
 
-    // find the left and rightmost active palms
+    // find the left and rightmost active Leap palms
     int leftPalmIndex, rightPalmIndex;
     Hand* hand = _owningAvatar->getHand();
     hand->getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
@@ -42,7 +42,7 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
 
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     if (leftPalmIndex == -1) {
-        // palms are not yet set, use mouse
+        // no Leap data; set hands from mouse
         if (_owningAvatar->getHandState() == HAND_STATE_NULL) {
             restoreRightHandPosition(HAND_RESTORATION_RATE);
         } else {
@@ -159,13 +159,29 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     } else {
         getJointRotation(jointIndex, palmRotation, true);
     }
-    palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getPalmDirection()) * palmRotation;
+    palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation;
     
+    // sort the finger indices by raw x, get the average direction
+    QVector<IndexValue> fingerIndices;
+    glm::vec3 direction;
+    for (size_t i = 0; i < palm.getNumFingers(); i++) {
+        glm::vec3 fingerVector = palm.getFingers()[i].getTipPosition() - palm.getPosition();
+        float length = glm::length(fingerVector);
+        if (length > EPSILON) {
+            direction += fingerVector / length;
+        }
+        fingerVector = glm::inverse(palmRotation) * fingerVector * -sign;
+        IndexValue indexValue = { (int)i, atan2f(fingerVector.z, fingerVector.x) };
+        fingerIndices.append(indexValue);
+    }
+    qSort(fingerIndices.begin(), fingerIndices.end());
+
     // rotate forearm according to average finger direction
-    // NOTE: we're doing this in the avatar local frame, so we DON'T want to use Palm::getHandDirection() 
+    float directionLength = glm::length(direction);
-    // which returns the world-frame.
+    const unsigned int MIN_ROTATION_FINGERS = 3;
-    glm::vec3 direction = palm.getRawRotation() * glm::vec3(0.0f, 0.0f, 1.0f);
+    if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
-    palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
+        palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
+    }
 
     // set hand position, rotation
     if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {

interface/src/devices/SixenseManager.cpp

@@ -22,9 +22,9 @@ const int CALIBRATION_STATE_Z = 3;
 const int CALIBRATION_STATE_COMPLETE = 4;
 
 // default (expected) location of neck in sixense space
-const float NECK_X = 0.25f; // meters
+const float NECK_X = 250.f;   // millimeters
-const float NECK_Y = 0.3f;  // meters
+const float NECK_Y = 300.f;   // millimeters
-const float NECK_Z = 0.3f;  // meters
+const float NECK_Z = 300.f;   // millimeters
 #endif
 
 SixenseManager::SixenseManager() {
@@ -106,11 +106,8 @@ void SixenseManager::update(float deltaTime) {
         palm->setControllerButtons(data->buttons);
         palm->setTrigger(data->trigger);
         palm->setJoystick(data->joystick_x, data->joystick_y);
- 
-        // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters.
-        glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
-        position *= METERS_PER_MILLIMETER;
 
+        glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
         // Transform the measured position into body frame.  
         glm::vec3 neck = _neckBase;
         // Zeroing y component of the "neck" effectively raises the measured position a little bit.
@@ -120,12 +117,15 @@ void SixenseManager::update(float deltaTime) {
         //  Rotation of Palm
         glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]);
         rotation = glm::angleAxis(PI, glm::vec3(0.f, 1.f, 0.f)) * _orbRotation * rotation;
+        const glm::vec3 PALM_VECTOR(0.0f, -1.0f, 0.0f);
+        glm::vec3 newNormal = rotation * PALM_VECTOR;
+        palm->setRawNormal(newNormal);
         palm->setRawRotation(rotation);
         
         //  Compute current velocity from position change
         glm::vec3 rawVelocity;
         if (deltaTime > 0.f) {
-            rawVelocity = (position - palm->getRawPosition()) / deltaTime; 
+            rawVelocity = (position - palm->getRawPosition()) / deltaTime / 1000.f;
         } else {
             rawVelocity = glm::vec3(0.0f);
         }
@@ -140,17 +140,29 @@ void SixenseManager::update(float deltaTime) {
             _amountMoved = glm::vec3(0.0f);
         }
         
-        // Store the one fingertip in the palm structure so we can track velocity
+        // initialize the "finger" based on the direction
-        const float FINGER_LENGTH = 0.3f;   //  meters
+        FingerData finger(palm, hand);
+        finger.setActive(true);
+        finger.setRawRootPosition(position);
+        const float FINGER_LENGTH = 300.0f;   //  Millimeters
         const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
         const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
+        finger.setRawTipPosition(position + rotation * FINGER_VECTOR);
+        
+        // Store the one fingertip in the palm structure so we can track velocity
         glm::vec3 oldTipPosition = palm->getTipRawPosition();
         if (deltaTime > 0.f) {
-            palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
+            palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime / 1000.f);
         } else {
             palm->setTipVelocity(glm::vec3(0.f));
         }
         palm->setTipPosition(newTipPosition);
+        
+        // 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);
     }
 
     if (numActiveControllers == 2) {
@@ -159,7 +171,7 @@ void SixenseManager::update(float deltaTime) {
 
     // if the controllers haven't been moved in a while, disable
     const unsigned int MOVEMENT_DISABLE_SECONDS = 3;
-    if (usecTimestampNow() - _lastMovement > (MOVEMENT_DISABLE_SECONDS * USECS_PER_SECOND)) {
+    if (usecTimestampNow() - _lastMovement > (MOVEMENT_DISABLE_SECONDS * 1000 * 1000)) {
         for (std::vector<PalmData>::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) {
             it->setActive(false);
         }
@@ -176,8 +188,8 @@ void SixenseManager::update(float deltaTime) {
 // (4) move arms a bit forward (Z)
 // (5) release BUTTON_FWD on both hands
 
-const float MINIMUM_ARM_REACH = 0.3f; // meters
+const float MINIMUM_ARM_REACH = 300.f;   // millimeters
-const float MAXIMUM_NOISE_LEVEL = 0.05f; // meters
+const float MAXIMUM_NOISE_LEVEL = 50.f;  // millimeters
 const quint64 LOCK_DURATION = USECS_PER_SECOND / 4;     // time for lock to be acquired
 
 void SixenseManager::updateCalibration(const sixenseControllerData* controllers) {
@@ -217,17 +229,14 @@ void SixenseManager::updateCalibration(const sixenseControllerData* controllers)
         return;
     }
 
-    // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters.
     const float* pos = dataLeft->pos;
     glm::vec3 positionLeft(pos[0], pos[1], pos[2]);
-    positionLeft *= METERS_PER_MILLIMETER;
     pos = dataRight->pos;
     glm::vec3 positionRight(pos[0], pos[1], pos[2]);
-    positionRight *= METERS_PER_MILLIMETER;
 
     if (_calibrationState == CALIBRATION_STATE_IDLE) {
         float reach = glm::distance(positionLeft, positionRight);
-        if (reach > 2.0f * MINIMUM_ARM_REACH) {
+        if (reach > 2.f * MINIMUM_ARM_REACH) {
             qDebug("started: sixense calibration");
             _averageLeft = positionLeft;
             _averageRight = positionRight;

interface/src/scripting/ControllerScriptingInterface.cpp

@@ -198,9 +198,9 @@ glm::vec3 ControllerScriptingInterface::getSpatialControlNormal(int controlIndex
     if (palmData) {
         switch (controlOfPalm) {
             case PALM_SPATIALCONTROL:
-                return palmData->getPalmDirection();
+                return palmData->getNormal();
             case TIP_SPATIALCONTROL:
-                return palmData->getFingerDirection();
+                return palmData->getNormal(); // currently the tip doesn't have a unique normal, use the palm normal
         }
     }
     return glm::vec3(0); // bad index

libraries/avatars/src/HandData.cpp

@@ -26,8 +26,8 @@ HandData::HandData(AvatarData* owningAvatar) :
     addNewPalm();
 }
 
-glm::vec3 HandData::worldToLocalVector(const glm::vec3& worldVector) const {
+glm::vec3 HandData::worldVectorToLeapVector(const glm::vec3& worldVector) const {
-    return glm::inverse(getBaseOrientation()) * worldVector;
+    return glm::inverse(getBaseOrientation()) * worldVector / LEAP_UNIT_SCALE;
 }
 
 PalmData& HandData::addNewPalm()  {
@@ -66,21 +66,69 @@ void HandData::getLeftRightPalmIndices(int& leftPalmIndex, int& rightPalmIndex)
 PalmData::PalmData(HandData* owningHandData) :
 _rawRotation(0.f, 0.f, 0.f, 1.f),
 _rawPosition(0.f),
+_rawNormal(0.f, 1.f, 0.f),
 _rawVelocity(0.f),
 _rotationalVelocity(0.f),
 _totalPenetration(0.f),
 _controllerButtons(0),
 _isActive(false),
+_leapID(LEAPID_INVALID),
 _sixenseID(SIXENSEID_INVALID),
 _numFramesWithoutData(0),
 _owningHandData(owningHandData),
 _isCollidingWithVoxel(false),
 _isCollidingWithPalm(false),
-_collisionlessPaddleExpiry(0) {
+_collisionlessPaddleExpiry(0)
+{
+    for (int i = 0; i < NUM_FINGERS_PER_HAND; ++i) {
+        _fingers.push_back(FingerData(this, owningHandData));
+    }
 }
 
 void PalmData::addToPosition(const glm::vec3& delta) {
-    _rawPosition += _owningHandData->worldToLocalVector(delta);
+    // convert to Leap coordinates, then add to palm and finger positions
+    glm::vec3 leapDelta = _owningHandData->worldVectorToLeapVector(delta);
+    _rawPosition += leapDelta;
+    for (size_t i = 0; i < getNumFingers(); i++) {
+        FingerData& finger = _fingers[i];
+        if (finger.isActive()) {
+            finger.setRawTipPosition(finger.getTipRawPosition() + leapDelta);
+            finger.setRawRootPosition(finger.getRootRawPosition() + leapDelta);
+        }
+    }
+}
+
+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 = 10;
+    setTrailLength(standardTrailLength);
+}
+
+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();
+        }
+    }
 }
 
 bool HandData::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, glm::vec3& penetration, 
@@ -109,20 +157,54 @@ glm::vec3 HandData::getBasePosition() const {
     return _owningAvatarData->getPosition();
 }
 
-glm::vec3 PalmData::getFingerTipPosition() const {
+void FingerData::setTrailLength(unsigned int length) {
-    glm::vec3 fingerOffset(0.0f, 0.0f, 0.3f);
+    _tipTrailPositions.resize(length);
-    glm::vec3 palmOffset(0.0f, -0.08f, 0.0f);
+    _tipTrailCurrentStartIndex = 0;
-    return getPosition() + _owningHandData->localToWorldDirection(_rawRotation * (fingerOffset + palmOffset));
+    _tipTrailCurrentValidLength = 0;
 }
 
-glm::vec3 PalmData::getFingerDirection() const {
+void FingerData::updateTrail() {
-    const glm::vec3 LOCAL_FINGER_DIRECTION(0.0f, 0.0f, 1.0f);
+    if (_tipTrailPositions.size() == 0)
-    return _owningHandData->localToWorldDirection(_rawRotation * LOCAL_FINGER_DIRECTION);
+        return;
+    
+    if (_isActive) {
+        // Add the next point in the trail.
+        _tipTrailCurrentStartIndex--;
+        if (_tipTrailCurrentStartIndex < 0)
+            _tipTrailCurrentStartIndex = _tipTrailPositions.size() - 1;
+        
+        _tipTrailPositions[_tipTrailCurrentStartIndex] = getTipPosition();
+        
+        if (_tipTrailCurrentValidLength < (int)_tipTrailPositions.size())
+            _tipTrailCurrentValidLength++;
+    }
+    else {
+        // It's not active, so just kill the trail.
+        _tipTrailCurrentValidLength = 0;
+    }
 }
 
-glm::vec3 PalmData::getPalmDirection() const {
+int FingerData::getTrailNumPositions() {
-    const glm::vec3 LOCAL_PALM_DIRECTION(0.0f, -1.0f, 0.0f);
+    return _tipTrailCurrentValidLength;
-    return _owningHandData->localToWorldDirection(_rawRotation * LOCAL_PALM_DIRECTION);
+}
+
+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];
+}
+
+void PalmData::getBallHoldPosition(glm::vec3& position) const { 
+    const float BALL_FORWARD_OFFSET = 0.08f;    // put the ball a bit forward of fingers
+    position = BALL_FORWARD_OFFSET * getNormal(); 
+    if (_fingers.size() > 0) {
+        position += _fingers[0].getTipPosition();
+    } else {
+        position += getPosition();
+    }
 }
 
 

libraries/avatars/src/HandData.h

@@ -21,6 +21,7 @@
 #include "SharedUtil.h"
 
 class AvatarData;
+class FingerData;
 class PalmData;
 
 const int NUM_HANDS = 2;
@@ -30,6 +31,8 @@ const int NUM_FINGERS = NUM_HANDS * NUM_FINGERS_PER_HAND;
 const int LEAPID_INVALID = -1;
 const int SIXENSEID_INVALID = -1;
 
+const float LEAP_UNIT_SCALE = 0.001f; ///< convert mm to meters
+
 const int SIXENSE_CONTROLLER_ID_LEFT_HAND = 0;
 const int SIXENSE_CONTROLLER_ID_RIGHT_HAND = 1;
 
@@ -38,16 +41,17 @@ public:
     HandData(AvatarData* owningAvatar);
     virtual ~HandData() {}
     
+    // These methods return the positions in Leap-relative space.
+    // To convert to world coordinates, use Hand::leapPositionToWorldPosition.
+
     // position conversion
-    glm::vec3 localToWorldPosition(const glm::vec3& localPosition) {
+    glm::vec3 leapPositionToWorldPosition(const glm::vec3& leapPosition) {
-        return getBasePosition() + getBaseOrientation() * localPosition;
+        return getBasePosition() + getBaseOrientation() * (leapPosition * LEAP_UNIT_SCALE);
     }
-
+    glm::vec3 leapDirectionToWorldDirection(const glm::vec3& leapDirection) {
-    glm::vec3 localToWorldDirection(const glm::vec3& localVector) {
+        return getBaseOrientation() * leapDirection;
-        return getBaseOrientation() * localVector;
+    }
-    } 
+    glm::vec3 worldVectorToLeapVector(const glm::vec3& worldVector) const;
-
-    glm::vec3 worldToLocalVector(const glm::vec3& worldVector) const;
 
     std::vector<PalmData>& getPalms() { return _palms; }
     const std::vector<PalmData>& getPalms() const { return _palms; }
@@ -59,6 +63,9 @@ public:
     /// both is not found.
     void getLeftRightPalmIndices(int& leftPalmIndex, int& rightPalmIndex) const;
 
+    void setFingerTrailLength(unsigned int length);
+    void updateFingerTrails();
+
     /// Checks for penetration between the described sphere and the hand.
     /// \param penetratorCenter the center of the penetration test sphere
     /// \param penetratorRadius the radius of the penetration test sphere
@@ -82,23 +89,71 @@ private:
     HandData& operator= (const HandData&);
 };
 
+class FingerData {
+public:
+    FingerData(PalmData* owningPalmData, HandData* owningHandData);
+
+    glm::vec3        getTipPosition()     const { return _owningHandData->leapPositionToWorldPosition(_tipRawPosition); }
+    glm::vec3        getRootPosition()    const { return _owningHandData->leapPositionToWorldPosition(_rootRawPosition); }
+    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
+    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<glm::vec3> _tipTrailPositions;
+    int                    _tipTrailCurrentStartIndex;
+    int                    _tipTrailCurrentValidLength;
+    PalmData* _owningPalmData;
+    HandData* _owningHandData;
+};
 
 class PalmData {
 public:
     PalmData(HandData* owningHandData);
-    glm::vec3 getPosition() const { return _owningHandData->localToWorldPosition(_rawPosition); }
+    glm::vec3 getPosition() const { return _owningHandData->leapPositionToWorldPosition(_rawPosition); }
-    glm::vec3 getVelocity() const { return _owningHandData->localToWorldDirection(_rawVelocity); }
+    glm::vec3 getNormal() const { return _owningHandData->leapDirectionToWorldDirection(_rawNormal); }
+    glm::vec3 getVelocity() const { return _owningHandData->leapDirectionToWorldDirection(_rawVelocity); }
 
     const glm::vec3& getRawPosition() const { return _rawPosition; }
+    const glm::vec3& getRawNormal()   const { return _rawNormal; }
     bool isActive() const { return _isActive; }
+    int getLeapID() const { return _leapID; }
     int getSixenseID() const { return _sixenseID; }
 
+
+    std::vector<FingerData>& getFingers() { return _fingers; }
+    const std::vector<FingerData>& getFingers() const { return _fingers; }
+    size_t getNumFingers() const { return _fingers.size(); }
+
     void setActive(bool active) { _isActive = active; }
+    void setLeapID(int id) { _leapID = id; }
     void setSixenseID(int id) { _sixenseID = id; }
 
     void setRawRotation(const glm::quat rawRotation) { _rawRotation = rawRotation; };
     glm::quat getRawRotation() const { return _rawRotation; }
     void setRawPosition(const glm::vec3& pos)  { _rawPosition = pos; }
+    void setRawNormal(const glm::vec3& normal) { _rawNormal = normal; }
     void setRawVelocity(const glm::vec3& velocity) { _rawVelocity = velocity; }
     const glm::vec3& getRawVelocity()  const { return _rawVelocity; }
     void addToPosition(const glm::vec3& delta);
@@ -107,11 +162,11 @@ public:
     void resolvePenetrations() { addToPosition(-_totalPenetration); _totalPenetration = glm::vec3(0.f); }
     
     void setTipPosition(const glm::vec3& position) { _tipPosition = position; }
-    const glm::vec3 getTipPosition() const { return _owningHandData->localToWorldPosition(_tipPosition); }
+    const glm::vec3 getTipPosition() const { return _owningHandData->leapPositionToWorldPosition(_tipPosition); }
     const glm::vec3& getTipRawPosition() const { return _tipPosition; }
 
     void setTipVelocity(const glm::vec3& velocity) { _tipVelocity = velocity; }
-    const glm::vec3 getTipVelocity() const { return _owningHandData->localToWorldDirection(_tipVelocity); }
+    const glm::vec3 getTipVelocity() const { return _owningHandData->leapDirectionToWorldDirection(_tipVelocity); }
     const glm::vec3& getTipRawVelocity() const { return _tipVelocity; }
     
     void incrementFramesWithoutData() { _numFramesWithoutData++; }
@@ -143,14 +198,11 @@ public:
     /// Store position where the palm holds the ball.
     void getBallHoldPosition(glm::vec3& position) const;
 
-    // return world-frame:
-    glm::vec3 getFingerTipPosition() const;
-    glm::vec3 getFingerDirection() const;
-    glm::vec3 getPalmDirection() const;
-
 private:
+    std::vector<FingerData> _fingers;
     glm::quat _rawRotation;
     glm::vec3 _rawPosition;
+    glm::vec3 _rawNormal;
     glm::vec3 _rawVelocity;
     glm::vec3 _rotationalVelocity;
     glm::quat _lastRotation;
@@ -164,6 +216,7 @@ private:
     float _joystickX, _joystickY;
     
     bool      _isActive;             // This has current valid data
+    int       _leapID;               // the Leap's serial id for this tracked object
     int       _sixenseID;            // Sixense controller ID for this palm
     int       _numFramesWithoutData; // after too many frames without data, this tracked object assumed lost.
     HandData* _owningHandData;

libraries/shared/src/SharedUtil.h

@@ -54,7 +54,6 @@ static const float SQUARE_ROOT_OF_3 = (float)sqrt(3.f);
 static const float METERS_PER_DECIMETER  = 0.1f;
 static const float METERS_PER_CENTIMETER = 0.01f;
 static const float METERS_PER_MILLIMETER = 0.001f;
-static const float MILLIMETERS_PER_METER = 1000.0f;
 static const quint64 USECS_PER_MSEC = 1000;
 static const quint64 MSECS_PER_SECOND = 1000;
 static const quint64 USECS_PER_SECOND = USECS_PER_MSEC * MSECS_PER_SECOND;