Clean file formating

- Clean the classes declaration and body for
DeviceTracker
MotionTracker
ControllerScriptingInterface / InputController
Leapmotion

- final bug remaining in the LeapOfFaith..js
THe orientation assigned to the skeleton joints doesn't work even though
all looks good.

examples/leapOfFaith.js

@@ -28,7 +28,7 @@ function quatToString( q ) {
 }
 
 function printSpatialEvent( label, spatialEvent ) {
-    if ( label == "RightHand" ) {
+    if ( label == "RightHandIndex1" ) {
         var dataString = label + " " +
              /*vec3ToString( spatialEvent.locTranslation ) + " " +
              quatToString( spatialEvent.locRotation ) + " " +*/
@@ -38,7 +38,7 @@ function printSpatialEvent( label, spatialEvent ) {
     }
 }
 
-function avatarToWorld( apos ) {
+function avatarToWorldPos( apos ) {
 
     // apply offset ?
     var offset = { x: 0, y: 0.5, z: -0.5 };   
@@ -49,6 +49,21 @@ function avatarToWorld( apos ) {
     return wpos;
 }
 
+function controlerToSkeletonOri( isRightSide, crot ) {
+ /*   var front = Quat.getFront( crot );
+    var right = Quat.getRight( crot );
+    var up = Quat.getUp( crot );
+*/
+    var qrootoffset = Quat.angleAxis( -180, {x:0, y:1, z:0});
+    var qoffset = Quat.angleAxis( -( 2 * isRightSide - 1) * 90, {x:0, y:1, z:0});
+
+   return Quat.multiply( qrootoffset, Quat.multiply( crot, qoffset ) );
+  // return Quat.multiply( crot, qoffset );
+   // return Quat.multiply( qrootoffset, crot );
+   return ( crot );
+}
+
+
 var jointParticles = [];
 function updateJointParticle( joint, pos, look ) {
   /*  print( "debug 1" );
@@ -75,15 +90,24 @@ function updateJointParticle( joint, pos, look ) {
     }*/
 }
 
+function evalArmBoneLook( isRightSide, bone ) {
+    return { c: { red: (255 * ( 1 - isRightSide )),
+                  green: 255 * ( ((bone)) / 2 ),
+                  blue: (255 * isRightSide) },
+             r: 3 ,
+             side: isRightSide };
+}
+
 function evalFingerBoneLook( isRightSide, finger, bone ) {
     return { c: { red: (255 * ( 1 - isRightSide )),
                   green: 255 * ( ((bone - 1)) / 3 ),
                   blue: (255 * isRightSide) },
-             r: (5 + (5 - (finger-1))) / 10.0  };
+             r: (5 + (5 - (finger-1))) / 10.0,
+             side: isRightSide };
 }
 
 var leapJoints = [
-    { n: "LeftHand",        l: { c: { red: 255, green: 0, blue: 0 },     r: 3 } },
+    { n: "LeftHand",        l: evalArmBoneLook( 0, 0) },
     
     { n: "LeftHandThumb2",  l: evalFingerBoneLook( 0, 1, 2) },
     { n: "LeftHandThumb3",  l: evalFingerBoneLook( 0, 1, 3) },
@@ -109,7 +133,7 @@ var leapJoints = [
     { n: "LeftHandPinky3",  l: evalFingerBoneLook( 0, 5, 3) },
     { n: "LeftHandPinky4",  l: evalFingerBoneLook( 0, 5, 4) },   
 
-    { n: "RightHand",        l: { c: { red: 0, green: 0, blue: 255 }, r: 3 } },
+    { n: "RightHand",        l: evalArmBoneLook( 1, 0) },
 
     { n: "RightHandThumb2",  l: evalFingerBoneLook( 1, 1, 2) },
     { n: "RightHandThumb3",  l: evalFingerBoneLook( 1, 1, 3) },
@@ -140,9 +164,10 @@ var leapJoints = [
 function onSpatialEventHandler( jointName, look ) {
     var _jointName = jointName;
     var _look = look;
+    var _side = look.side;
     return (function( spatialEvent ) {
-        MyAvatar.setJointData(_jointName, spatialEvent.absRotation);
-        updateJointParticle(_jointName, avatarToWorld( spatialEvent.absTranslation ), _look );       
+        MyAvatar.setJointData(_jointName, controlerToSkeletonOri( _side, spatialEvent.absRotation ));
+        updateJointParticle(_jointName, avatarToWorldPos( spatialEvent.absTranslation ), _look );       
         printSpatialEvent(_jointName, spatialEvent );
     });
 }

interface/src/devices/DeviceTracker.cpp

@@ -14,18 +14,14 @@
 // The singleton managing the connected devices
 DeviceTracker::Singleton DeviceTracker::Singleton::_singleton;
 
-int DeviceTracker::init()
-{
-
+int DeviceTracker::init() {
     return Singleton::get()->_devicesMap.size();
 }
-int DeviceTracker::getNumDevices()
-{
+int DeviceTracker::getNumDevices() {
     return Singleton::get()->_devicesMap.size();
 }
 
-int DeviceTracker::getDeviceIndex( const Name& name )
-{
+int DeviceTracker::getDeviceIndex( const Name& name ) {
     auto deviceIt = Singleton::get()->_devicesMap.find( name );
     if ( deviceIt != Singleton::get()->_devicesMap.end() )
         return (*deviceIt).second;
@@ -33,13 +29,11 @@ int DeviceTracker::getDeviceIndex( const Name& name )
         return -1;
 }
 
-DeviceTracker* DeviceTracker::getDevice( const Name& name )
-{
+DeviceTracker* DeviceTracker::getDevice( const Name& name ) {
     return getDevice( getDeviceIndex( name ) );
 }
 
-DeviceTracker* DeviceTracker::getDevice( int deviceNum )
-{
+DeviceTracker* DeviceTracker::getDevice( int deviceNum ) {
     if ( (deviceNum >= 0) && ( deviceNum < Singleton::get()->_devicesVector.size() ) ) {
         return Singleton::get()->_devicesVector[ deviceNum ];
     } else {
@@ -47,8 +41,7 @@ DeviceTracker* DeviceTracker::getDevice( int deviceNum )
     }
 }
 
-int DeviceTracker::registerDevice( const Name& name, DeviceTracker* device )
-{
+int DeviceTracker::registerDevice( const Name& name, DeviceTracker* device ) {
     if ( !device )
         return -1;
     int index = getDeviceIndex( name );
@@ -81,11 +74,9 @@ DeviceTracker::~DeviceTracker()
 {
 }
 
-bool DeviceTracker::isConnected() const
-{
+bool DeviceTracker::isConnected() const {
     return false;
 }
 
-void DeviceTracker::update()
-{
+void DeviceTracker::update() {
 }

interface/src/devices/Leapmotion.cpp

@@ -177,8 +177,29 @@ const float LEAP_Y = 0.3f;  // meters
 const float LEAP_Z = 0.3f;  // meters
 #endif
 
+const int PALMROOT_NUM_JOINTS = 3;
 const int FINGER_NUM_JOINTS = 4;
-const int HAND_NUM_JOINTS = FINGER_NUM_JOINTS*5+1;
+const int HAND_NUM_JOINTS = FINGER_NUM_JOINTS*5+PALMROOT_NUM_JOINTS;
+
+// find the index of a joint from
+// the side: true = right
+// the finger & the bone:
+//             finger in [0..4] : bone in  [0..3] a finger phalange
+//             [-1]   up the hand branch : bone in [0..2] <=> [ hand, forearm, arm]
+// the bone:
+MotionTracker::Index evalJointIndex( bool isRightSide, int finger, int bone ) {
+
+    MotionTracker::Index offset = 1                                     // start after root
+                                + (int(isRightSide) * HAND_NUM_JOINTS)  // then offset for side
+                                + PALMROOT_NUM_JOINTS;                  // then add the arm/forearm/hand chain
+    if ( finger > 0 ) {
+        // from there go down in the correct finger and bone
+        return offset + (finger * FINGER_NUM_JOINTS) + bone;
+    } else {
+        // or go back up for the correct root bone
+        return offset - 1 - bone;
+    }
+}
 
 Leapmotion::Leapmotion() :
      MotionTracker(),
@@ -196,9 +217,14 @@ Leapmotion::Leapmotion() :
 
     // Create the Leapmotion joint hierarchy
     {
-        std::vector< Semantic > hands;
-        hands.push_back( "Left" );
-        hands.push_back( "Right" );
+        std::vector< Semantic > sides;
+        sides.push_back( "Left" );
+        sides.push_back( "Right" );
+
+        std::vector< Semantic > rootBones;
+        rootBones.push_back( "Arm" );
+        rootBones.push_back( "ForeArm" );
+        rootBones.push_back( "Hand" );
 
         std::vector< Semantic > fingers;
         fingers.push_back( "Thumb" );
@@ -219,13 +245,18 @@ Leapmotion::Leapmotion() :
         fingerBones.push_back( "4" );
 
         std::vector< Index > palms;
-        for ( int h = 0; h < hands.size(); h++ ) {
-            Index rootJoint = addJoint( hands[h] + "Hand", 0 );
+        for ( int s = 0; s < sides.size(); s++ ) {
+            Index rootJoint = 0;
+            for ( int rb = 0; rb < rootBones.size(); rb++ ) {
+                rootJoint = addJoint( sides[s] + rootBones[rb], rootJoint );
+            }
+
+            // capture the hand index for debug
             palms.push_back( rootJoint );
 
             for ( int f = 0; f < fingers.size(); f++ ) {
                 for ( int b = 0; b < fingerBones.size(); b++ ) {
-                    rootJoint  = addJoint( hands[h] + "Hand" + fingers[f] + fingerBones[b], rootJoint );
+                    rootJoint  = addJoint( sides[s] + "Hand" + fingers[f] + fingerBones[b], rootJoint );
                 }
             }
         }
@@ -248,14 +279,15 @@ void Leapmotion::init() {
 #ifdef HAVE_LEAPMOTION
 glm::quat quatFromLeapBase( float sideSign, const Leap::Matrix& basis ) {
 
+    // fix the handness to right and always...
     glm::vec3 xAxis = glm::normalize( sideSign * glm::vec3( basis.xBasis.x, basis.xBasis.y, basis.xBasis.z) );
-    glm::vec3 yAxis = glm::normalize( sideSign * glm::vec3( basis.yBasis.x, basis.yBasis.y, basis.yBasis.z) );
-    glm::vec3 zAxis = glm::normalize( sideSign * glm::vec3( basis.zBasis.x, basis.zBasis.y, basis.zBasis.z) );
-    zAxis = glm::normalize( glm::cross( xAxis, yAxis ) );
-    yAxis = glm::normalize( glm::cross( zAxis, xAxis ) );
-    glm::quat orientation = /*glm::inverse*/(glm::quat_cast(glm::mat3(xAxis, yAxis, zAxis)));
-  //  orientation = glm::normalize( orientation );
-    return glm::quat();
+    glm::vec3 yAxis = glm::normalize( glm::vec3( basis.yBasis.x, basis.yBasis.y, basis.yBasis.z) );
+    glm::vec3 zAxis = glm::normalize( glm::vec3( basis.zBasis.x, basis.zBasis.y, basis.zBasis.z) );
+
+    xAxis = glm::normalize( glm::cross( yAxis, zAxis ) );
+
+    glm::quat orientation = (glm::quat_cast(glm::mat3(xAxis, yAxis, zAxis)));
+  //  return glm::quat();
     return orientation;
 }
 glm::quat quatFromLeapBase( float sideSign, const Leap::Vector& dir, const Leap::Vector& normal ) {
@@ -301,29 +333,47 @@ void Leapmotion::update() {
     glm::quat handOri;
     if ( !frame.hands().isEmpty() ) {
         for ( int handNum = 0; handNum < frame.hands().count(); handNum++ ) {
-            // Get the first hand
+
             const Leap::Hand hand = frame.hands()[handNum];
-            int side = ( hand.isRight() ? -1 : 1 );
-            Index handIndex = 1 + ((1 - side)/2) * HAND_NUM_JOINTS;
+            int side = ( hand.isRight() ? 1 : -1 );
+            Index handIndex = evalJointIndex( (side > 0), -1, 0 );
 
-            glm::vec3 pos = vec3FromLeapVector(hand.palmPosition());
-           // glm::quat ori = quatFromLeapBase(float(side), hand.basis() );
-            glm::quat ori = quatFromLeapBase(float(side), hand.direction(), hand.palmNormal() );
+            // only in SDK 2.0.3
+          /*  Leap::Arm arm = hand.arm();
+            if ( arm.isValid() ) {
+                glm::vec3 pos = vec3FromLeapVector(arm.wristPosition());
+                glm::quat ori = quatFromLeapBase(float(side), arm.basis() );
+            //     glm::quat ori = quatFromLeapBase(float(side), hand.direction(), hand.palmNormal() );
 
-            JointTracker* palmJoint = editJointTracker( handIndex );
-            palmJoint->editLocFrame().setTranslation( pos );
-            palmJoint->editLocFrame().setRotation( ori );
-            palmJoint->editAbsFrame().setTranslation( pos );
-            palmJoint->editAbsFrame().setRotation( ori );
-            palmJoint->activeFrame();
+                JointTracker* wrist = editJointTracker( evalJointIndex( (side > 0), -1, 1 ) );
+                //  palmJoint->editLocFrame().setTranslation( pos );
+                //  palmJoint->editLocFrame().setRotation( ori );
+                wrist->editAbsFrame().setTranslation( pos );
+                wrist->editAbsFrame().setRotation( ori );
+                wrist->activeFrame();
 
-            // Transform the measured position into body frame.  
-            glm::vec3 neck = _leapBasePos;
-            // Zeroing y component of the "neck" effectively raises the measured position a little bit.
-            //neck.y = 0.f;
-            pos = _leapBaseOri * (pos - neck);
+                pos = vec3FromLeapVector(arm.elbowPosition());
+                JointTracker* elbow = editJointTracker( evalJointIndex( (side > 0), -1, 2 ) );
+                //  palmJoint->editLocFrame().setTranslation( pos );
+                //  palmJoint->editLocFrame().setRotation( ori );
+                elbow->editAbsFrame().setTranslation( pos );
+                elbow->editAbsFrame().setRotation( ori );
+                elbow->activeFrame();
+            }*/
 
+            JointTracker* palmJoint = nullptr;
+            {
+                glm::vec3 pos = vec3FromLeapVector(hand.palmPosition());
+                glm::quat ori = quatFromLeapBase(float(side), hand.basis() );
+           //     glm::quat ori = quatFromLeapBase(float(side), hand.direction(), hand.palmNormal() );
 
+                palmJoint = editJointTracker( evalJointIndex( (side > 0), -1, 0 ) );
+              //  palmJoint->editLocFrame().setTranslation( pos );
+              //  palmJoint->editLocFrame().setRotation( ori );
+                palmJoint->editAbsFrame().setTranslation( pos );
+                palmJoint->editAbsFrame().setRotation( ori );
+                palmJoint->activeFrame();
+            }
             // Check if the hand has any fingers
             const Leap::FingerList fingers = hand.fingers();
             if (!fingers.isEmpty()) {
@@ -333,7 +383,8 @@ void Leapmotion::update() {
                     JointTracker* parentJointTracker = palmJoint;
 
                     // surprisingly, Leap::Finger::Type start at 0 for thumb a until 4 for the pinky
-                    Index fingerIndex = handIndex + 1 + Index(fingers[i].type()) * FINGER_NUM_JOINTS;
+                    Index fingerIndex = evalJointIndex( (side > 0), int(fingers[i].type()), 0 );
+                    //handIndex + 1 + Index(fingers[i].type()) * FINGER_NUM_JOINTS;
 
                     // let's update the finger's joints
                     for ( int b = 0; b < FINGER_NUM_JOINTS; b++ ) {
@@ -346,7 +397,7 @@ void Leapmotion::update() {
                             ljointTracker->editAbsFrame().setTranslation( vec3FromLeapVector( bp ) );
                             ljointTracker->editAbsFrame().setRotation(quatFromLeapBase( float(side), bone.basis() ) );
                           //  ljointTracker->editAbsFrame().setRotation(quatFromLeapBase( float(side), bone.direction(), bone.basis() ) );
-                            ljointTracker->updateLocFromAbsTransform( parentJointTracker );
+                          //  ljointTracker->updateLocFromAbsTransform( parentJointTracker );
                             ljointTracker->activeFrame();
                         }
                         parentJointTracker = ljointTracker;

interface/src/devices/MotionTracker.cpp

@@ -76,13 +76,11 @@ MotionTracker::~MotionTracker()
 {
 }
 
-bool MotionTracker::isConnected() const
-{
+bool MotionTracker::isConnected() const {
     return false;
 }
 
-MotionTracker::Index MotionTracker::addJoint( const Semantic& semantic, Index parent )
-{
+MotionTracker::Index MotionTracker::addJoint( const Semantic& semantic, Index parent ) {
     // Check the parent
     if ( int(parent) < 0 )
         return INVALID_PARENT;
@@ -100,16 +98,14 @@ MotionTracker::Index MotionTracker::addJoint( const Semantic& semantic, Index pa
     return newIndex;
 }
 
-MotionTracker::Index MotionTracker::findJointIndex( const Semantic& semantic ) const
-{
+MotionTracker::Index MotionTracker::findJointIndex( const Semantic& semantic ) const {
     auto jointIt = _jointsMap.find( semantic );
     if ( jointIt != _jointsMap.end() )
         return (*jointIt).second;
     return INVALID_SEMANTIC;
 }
 
-void MotionTracker::updateAllAbsTransform()
-{
+void MotionTracker::updateAllAbsTransform() {
     _jointsArray[0].updateAbsFromLocTransform( 0 );
 
     // Because we know the hierarchy is stored from root down the branches let's just traverse and update
@@ -147,8 +143,7 @@ MotionTracker::JointTracker::JointTracker( const JointTracker& tracker ) :
     _lastUpdate( tracker._lastUpdate )
 {
 }
-void MotionTracker::JointTracker::updateAbsFromLocTransform(const JointTracker* parentJoint)
-{
+void MotionTracker::JointTracker::updateAbsFromLocTransform(const JointTracker* parentJoint) {
     if ( parentJoint ) {
         //editAbsFrame()._transform = glm::mult( parentJoint->getAbsFrame()._transform, getLocFrame()._transform );
         editAbsFrame()._transform = ( parentJoint->getAbsFrame()._transform * getLocFrame()._transform );
@@ -157,8 +152,7 @@ void MotionTracker::JointTracker::updateAbsFromLocTransform(const JointTracker*
     }
 }
 
-void MotionTracker::JointTracker::updateLocFromAbsTransform(const JointTracker* parentJoint)
-{
+void MotionTracker::JointTracker::updateLocFromAbsTransform(const JointTracker* parentJoint) {
     if ( parentJoint ) {
       //  glm::mat4 ip = glm::inverse( glm::mat4( parentJoint->getAbsFrame()._transform ) );
         glm::mat4 ip = glm::inverse( parentJoint->getAbsFrame()._transform );
@@ -186,19 +180,16 @@ void MotionTracker::Frame::setRotation( const glm::quat& rotation )
     _transform[2] = glm::vec4( rot[2], 0.f );
 }
 
-void MotionTracker::Frame::getRotation( glm::quat& rotation ) const
-{
+void MotionTracker::Frame::getRotation( glm::quat& rotation ) const {
    // rotation = glm::quat_cast( glm::mat3( _transform[0], _transform[1], _transform[2] ) );
     rotation = glm::quat_cast(  _transform );
 }
 
-void MotionTracker::Frame::setTranslation( const glm::vec3& translation )
-{
+void MotionTracker::Frame::setTranslation( const glm::vec3& translation ) {
     _transform[3] = glm::vec4( translation, 1.f );
 }
 
-void MotionTracker::Frame::getTranslation( glm::vec3& translation ) const
-{
+void MotionTracker::Frame::getTranslation( glm::vec3& translation ) const {
     translation = glm::vec3( _transform[3] );
 }
 

interface/src/scripting/ControllerScriptingInterface.cpp

@@ -269,13 +269,13 @@ AbstractInputController* ControllerScriptingInterface::createInputController( co
     } else {
 
         // Look for matching category
+        // TODO in this current implementation, we just pick the first device assuming there is one ( normally the LeapMotion)
+        // in the near future we need to change that to a real mapping between the device names and the cateory
         int categoryID = 0;
         MotionTracker* motionTracker = dynamic_cast< MotionTracker* > ( DeviceTracker::getDevice( categoryID ) );
-        if ( motionTracker )
-        {
+        if ( motionTracker ) {
             int trackerID = motionTracker->findJointIndex( tracker.toStdString() );
-            if ( trackerID > 0 )
-            {
+            if ( trackerID > 0 ) {
                 AbstractInputController* inputController = new InputController(categoryID,trackerID, this );
 
                 _inputControllers.insert( InputControllerMap::value_type( inputController->getKey(), inputController ) );