Improve the LeapMotion support

- fixed a bug on indexing the bones of the hand in the update of the
LEapmotion

- Still struggling with the correct joint assignment in the javascript,
visiting HIFI later today

examples/leapOfFaith.js

@@ -28,7 +28,7 @@ function quatToString( q ) {
 }
 
 function printSpatialEvent( label, spatialEvent ) {
-    if ( label == "RightHandIndex1" ) {
+    if ( false ){//label == "RightHandIndex1" ) {
         var dataString = label + " " +
              /*vec3ToString( spatialEvent.locTranslation ) + " " +
              quatToString( spatialEvent.locRotation ) + " " +*/
@@ -49,44 +49,108 @@ function avatarToWorldPos( apos ) {
     return wpos;
 }
 
-function controlerToSkeletonOri( isRightSide, crot ) {
+function avatarToWorldQuat( aori) {
- /*   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 ) );
+    var wori = Quat.multiply(MyAvatar.orientation, aori);
+
+  //  var qoffset = Quat.angleAxis( -90, {x:0, y:1, z:0});
+  //  wori = Quat.multiply( wori, qoffset );
+
+    return wori;a
+}
+
+function controlerToSkeletonOri( jointName, isRightSide, event ) {
+ 
+  //  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 );
+ // return ( crot );
+
+  //return MyAvatar.getJointRotation( jointName );
+  //return Quat.fromPitchYawRollDegrees(0,0,30);
+
+  var qx = Quat.angleAxis( -90, {x:1, y:0, z:0});
+  var qy = Quat.angleAxis( 180, {x:0, y:1, z:0});
+  var q = Quat.multiply( qy, qx );
+
+  //  return q;
+
+ // return  Quat.multiply( event.locRotation, q );
+    var cq = jointControllers[0].c.getAbsRotation();
+  //  return cq;
+    //var q = spatialEvent.absRotation;
+
+   // MyAvatar.clearJointData( jointName );    
+    var qjointRef = MyAvatar.getJointRotation( jointName );
+    print( jointName + " " + quatToString( qjointRef ) );
+
+        var qjointRefI = Quat.inverse( qjointRef );
+        var qjoint = Quat.multiply( cq, qjointRefI );
+
+
+    print( quatToString( cq )  );
+
+    return  qjoint;
 }
 
 
 var jointParticles = [];
-function updateJointParticle( joint, pos, look ) {
+function updateJointParticle( joint, pos, ori, look ) {
-  /*  print( "debug 1" );
+ /*   print( "debug 1" );
     var jointID = jointParticles[ joint ];
     if ( jointID == null ) {
         print( "debug create " + joint );
 */
+        var radius = 0.005* look.r;
         var ballProperties = {
             position: pos,
             velocity: { x: 0, y: 0, z: 0},
             gravity: { x: 0, y: 0, z: 0 },
             damping: 0, 
-            radius : 0.005* look.r,
+            radius : radius,
             color: look.c,
-            lifetime: 0.05
+            lifetime: 0.1
         };
-        jointParticles[ joint ] = Particles.addParticle(ballProperties);
+        var atomPos = Particles.addParticle(ballProperties);
- /*   } else {
+/* 
-        print( "debug update " + joint );
+        // Zaxis
-        var prop = Particles.getParticleProperties( jointID );
+        var Zaxis = Vec3.multiply( Quat.getFront( ori ), - 1.5 * radius ) ;
-        prop.position = pos;
+        ballProperties.position = Vec3.sum(pos, Zaxis );
-        prop.lifetime = 1.0;
+        ballProperties.radius = 0.35* radius;
-        Particles.editParticle( jointID, prop );
+        ballProperties.color= { red: 255, green: 255, blue: 255 };
+
+        var atomZ = Particles.addParticle(ballProperties);
+
+        var up = Vec3.multiply( Quat.getUp( ori ), 1.5 * radius ) ;
+        ballProperties.position = Vec3.sum(pos, up) ;
+        ballProperties.radius = 0.35* radius;
+        ballProperties.color= { red: 0, green: 255, blue: 0 };
+
+        var atomY = Particles.addParticle(ballProperties);
+
+       var right = Vec3.multiply( Quat.getRight( ori ), 1.5 * radius ) ;
+        ballProperties.position = Vec3.sum(pos, right) ;
+        ballProperties.radius = 0.35* radius;
+        ballProperties.color= { red: 255, green: 0, blue: 225 };
+
+        var atomX = Particles.addParticle(ballProperties);
+*/
+    //    jointParticles[ joint ] = { p: atomPos, x: atomX, y: atomY, z: atomZ };
+/*
+    } else {
+        //print( "debug update " + joint );
+
+        var p = Particles.getParticleProperties( jointID.p );
+        p.position = pos;
+       // p.lifetime = 1.0;
+        Particles.editParticle( jointID.p, p );
+
+
     }*/
 }
 
@@ -113,7 +177,7 @@ var leapJoints = [
     { n: "LeftHandThumb3",  l: evalFingerBoneLook( 0, 1, 3) },
     { n: "LeftHandThumb4",  l: evalFingerBoneLook( 0, 1, 4) },
 
-    { n: "LeftHandIndex1",  l: evalFingerBoneLook( 0, 2, 1) },
+/*    { n: "LeftHandIndex1",  l: evalFingerBoneLook( 0, 2, 1) },
     { n: "LeftHandIndex2",  l: evalFingerBoneLook( 0, 2, 2) },
     { n: "LeftHandIndex3",  l: evalFingerBoneLook( 0, 2, 3) },
     { n: "LeftHandIndex4",  l: evalFingerBoneLook( 0, 2, 4) },
@@ -132,19 +196,19 @@ var leapJoints = [
     { n: "LeftHandPinky2",  l: evalFingerBoneLook( 0, 5, 2) },
     { n: "LeftHandPinky3",  l: evalFingerBoneLook( 0, 5, 3) },
     { n: "LeftHandPinky4",  l: evalFingerBoneLook( 0, 5, 4) },   
-
+*/
     { n: "RightHand",        l: evalArmBoneLook( 1, 0) },
-
+/*
     { n: "RightHandThumb2",  l: evalFingerBoneLook( 1, 1, 2) },
     { n: "RightHandThumb3",  l: evalFingerBoneLook( 1, 1, 3) },
     { n: "RightHandThumb4",  l: evalFingerBoneLook( 1, 1, 4) },
-
+*/
     { n: "RightHandIndex1",  l: evalFingerBoneLook( 1, 2, 1) },
     { n: "RightHandIndex2",  l: evalFingerBoneLook( 1, 2, 2) },
     { n: "RightHandIndex3",  l: evalFingerBoneLook( 1, 2, 3) },
     { n: "RightHandIndex4",  l: evalFingerBoneLook( 1, 2, 4) },
 
-    { n: "RightHandMiddle1",  l: evalFingerBoneLook( 1, 3, 1) },
+/*    { n: "RightHandMiddle1",  l: evalFingerBoneLook( 1, 3, 1) },
     { n: "RightHandMiddle2",  l: evalFingerBoneLook( 1, 3, 2) },
     { n: "RightHandMiddle3",  l: evalFingerBoneLook( 1, 3, 3) },
     { n: "RightHandMiddle4",  l: evalFingerBoneLook( 1, 3, 4) },
@@ -158,7 +222,7 @@ var leapJoints = [
     { n: "RightHandPinky2",  l: evalFingerBoneLook( 1, 5, 2) },
     { n: "RightHandPinky3",  l: evalFingerBoneLook( 1, 5, 3) },
     { n: "RightHandPinky4",  l: evalFingerBoneLook( 1, 5, 4) }, 
-
+    */
  ];
 
 function onSpatialEventHandler( jointName, look ) {
@@ -166,8 +230,13 @@ function onSpatialEventHandler( jointName, look ) {
     var _look = look;
     var _side = look.side;
     return (function( spatialEvent ) {
-        MyAvatar.setJointData(_jointName, controlerToSkeletonOri( _side, spatialEvent.absRotation ));
+
-        updateJointParticle(_jointName, avatarToWorldPos( spatialEvent.absTranslation ), _look );       
+        MyAvatar.setJointData(_jointName, controlerToSkeletonOri( _jointName, _side, spatialEvent ));
+
+        updateJointParticle(_jointName,
+            avatarToWorldPos( spatialEvent.absTranslation ),
+            avatarToWorldQuat( spatialEvent.absRotation ),
+            _look );       
         printSpatialEvent(_jointName, spatialEvent );
     });
 }
@@ -195,7 +264,9 @@ Script.update.connect(function(deltaTime) {
         for ( i in jointControllers ) {
             if ( jointControllers[i].c.isActive() ) {
                 var spatialEvent = { absTranslation: jointControllers[i].c.getAbsTranslation(),
-                                     absRotation: jointControllers[i].c.getAbsRotation() };
+                                     absRotation: jointControllers[i].c.getAbsRotation(),
+                                     locTranslation: jointControllers[i].c.getLocTranslation(),
+                                     locRotation: jointControllers[i].c.getLocRotation() };
                 jointControllers[i].h( spatialEvent );
             }
         }

interface/src/devices/Leapmotion.cpp

@@ -192,7 +192,7 @@ 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 ) {
+    if ( finger >= 0 ) {
         // from there go down in the correct finger and bone
         return offset + (finger * FINGER_NUM_JOINTS) + bone;
     } else {
@@ -368,8 +368,8 @@ void Leapmotion::update() {
            //     glm::quat ori = quatFromLeapBase(float(side), hand.direction(), hand.palmNormal() );
 
                 palmJoint = editJointTracker( evalJointIndex( (side > 0), -1, 0 ) );
-              //  palmJoint->editLocFrame().setTranslation( pos );
+                palmJoint->editLocFrame().setTranslation( pos );
-              //  palmJoint->editLocFrame().setRotation( ori );
+                palmJoint->editLocFrame().setRotation( ori );
                 palmJoint->editAbsFrame().setTranslation( pos );
                 palmJoint->editAbsFrame().setRotation( ori );
                 palmJoint->activeFrame();
@@ -384,7 +384,6 @@ void Leapmotion::update() {
 
                     // surprisingly, Leap::Finger::Type start at 0 for thumb a until 4 for the pinky
                     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++ ) {
@@ -397,7 +396,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.h

@@ -25,8 +25,7 @@ class MotionTracker : public DeviceTracker {
     Q_OBJECT
 public:
 
-    class Frame
+    class Frame {
-    {
     public:
         Frame();
 
@@ -46,8 +45,7 @@ public:
     static const Index INVALID_PARENT = -2;
 
 
-    class JointTracker
+    class JointTracker {
-    {
     public:
         typedef std::vector< JointTracker > vector;
         typedef std::map< Semantic, Index > map;