/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2012 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt
-----------------------------------------------------------------------------
*/

struct ObjPointableStr = [
  PPOINT_obj                    : SO3_OBJECT,
  PPOINT_body                   : SO3_PHYSICBODY,
  PPOINT_joint                  : SO3_PHYSICCONTRAINT,
  PPOINT_iMode                  : I,
  PPOINT_initPos                : [[F F F] [F F F F] [F F F F] [F F F]],
  PPOINT_lPhalanges             : [ObjPointableStr r1],
  PPOINT_bAutoHide              : I,
  PPOINT_bState                 : I
  
  ]mkObjPointableStr;;

struct ObjhandStr = [
  PHAND_obj                    : SO3_OBJECT,
  PHAND_iMode                  : I,
  PHAND_initPos                : [[F F F] [F F F F] [F F F F]],
  PHAND_lFingers               : [ObjPointableStr r1],
  PHAND_iNbFingers             : I,
  PHAND_iGrabState             : I,
  PHAND_iPinchState            : I,
  PHAND_bAutoHide              : I,
  PHAND_pinchJoint             : SO3_PHYSICCONTRAINT,
  PHAND_hPinchBody             : SO3_PHYSICBODY,
  PHAND_pinchBody              : SO3_PHYSICBODY,
  PHAND_fMass                  : F,
  PHAND_bfingerState           : [[I [I I]] r1],
  PHAND_bState                 : I
  
  ]mkObjhandStr;;

struct ObjleapStr = [
  PLEAP_inst                   : PInstance,
  PLEAP_objRef                 : SO3_OBJECT,
  PLEAP_device                 : ObjLeapMotion,
  PLEAP_lHands                 : [ObjhandStr r1],
  PLEAP_lTools                 : [ObjPointableStr r1],
  PLEAP_vOffset                : [F F],
  PLEAP_vCoef                  : [F F F],
  PLEAP_iMode                  : I,
  PLEAP_bSonsAsFingers         : I,
  PLEAP_bMirror                : I,
  PLEAP_bTopView               : I,
  PLEAP_bPhysicGrab            : I,
  PLEAP_lLastCircleId          : [[I [S [F F] F]] r1],
  PLEAP_lLastSwipeId           : [[I [S [F F] F]] r1],
  PLEAP_iLastSwipeTick         : I,
  PLEAP_bConnected             : I,
  PLEAP_bLScreenPos            : I,
  PLEAP_bRScreenPos            : I,
  PLEAP_bLHandPos              : I,
  PLEAP_bRHandPos              : I,
  PLEAP_bLHandVel              : I,
  PLEAP_bRHandVel              : I,
  PLEAP_bLToolPos              : I,
  PLEAP_bRToolPos              : I,
  PLEAP_bLToolVel              : I,
  PLEAP_bRToolVel              : I,
  PLEAP_bPointDist             : I,
  PLEAP_bPointRot              : I

  ]mkObjleapStr;;


var lFingersNames = "Thumb"::"Index"::"Middle"::"Ring"::"Pinky"::nil;;

fun deleteOb(inst, obstr)=
  setPluginInstanceCbScenePreRenderPhysic inst nil;
  _DSleapMotionDevice obstr.PLEAP_device;
  
  while (obstr.PLEAP_lHands != nil) do
  (
    let hd obstr.PLEAP_lHands -> handstr in
    let handstr.PHAND_initPos -> [pos quat ihgquat] in
    (
      SO3ObjectSetPosition handstr.PHAND_obj pos;
      SO3ObjectSetOrientation handstr.PHAND_obj quat;
      
      V3DphysEnableBody SO3SceneNodeGetBody handstr.PHAND_obj 1;
      SO3ObjectSetVisible handstr.PHAND_obj 1 1;
      SO3PhysicsContraintDestroy handstr.PHAND_pinchJoint;
      SO3BodyDestroy handstr.PHAND_hPinchBody;
      
      while (handstr.PHAND_lFingers != nil) do
      (
        let hd handstr.PHAND_lFingers -> pstr in
        let pstr.PPOINT_initPos -> [pos quat gquat _] in
        (
          SO3ObjectSetPosition pstr.PPOINT_obj pos;
          SO3ObjectSetOrientation pstr.PPOINT_obj quat;
          
          
          V3DphysEnableBody SO3SceneNodeGetBody pstr.PPOINT_obj 1;
          SO3ObjectSetVisible pstr.PPOINT_obj 1 1;
          
          if (pstr.PPOINT_body == nil) then nil else
          (
            let SO3BodyGetMaterial pstr.PPOINT_body -> mat1 in
            let V3DgetAvatarMaterial c3dXsession -> mat2 in
            if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
            (
              SO3PhysicsMaterialSetDefaultCollidable mat1 mat1 1;
              SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 1;
            );
            
            SO3BodyDestroy pstr.PPOINT_body;
            SO3PhysicsContraintDestroy pstr.PPOINT_joint;
          );
          
          while (pstr.PPOINT_lPhalanges != nil) do
          (
            let hd pstr.PPOINT_lPhalanges -> npstr in
            let npstr.PPOINT_initPos -> [ipos ipquat gquat _] in
            (
              SO3ObjectSetPosition npstr.PPOINT_obj ipos;
              SO3ObjectSetOrientation npstr.PPOINT_obj ipquat;
              
              V3DphysEnableBody SO3SceneNodeGetBody npstr.PPOINT_obj 1;
              SO3ObjectSetVisible npstr.PPOINT_obj 1 1;
              
              if (npstr.PPOINT_body == nil) then nil else
              (
                let SO3BodyGetMaterial npstr.PPOINT_body -> mat1 in
                let V3DgetAvatarMaterial c3dXsession -> mat2 in
                if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
                (
                  SO3PhysicsMaterialSetDefaultCollidable mat1 mat1 1;
                  SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 1;
                );
                
                SO3BodyDestroy npstr.PPOINT_body;
                SO3PhysicsContraintDestroy npstr.PPOINT_joint;
              );
            );
            
            set pstr.PPOINT_lPhalanges = tl pstr.PPOINT_lPhalanges;
          );
        );
        
        set handstr.PHAND_lFingers = tl handstr.PHAND_lFingers;
      );

      set obstr.PLEAP_lHands = tl obstr.PLEAP_lHands;
    );
  );
  0;;


fun usePhysic(node, mode)=
  let SO3SceneNodeGetBody node -> body in
  let SO3BodyGetMassMatrix body -> [mass _] in
    if ((mode != 0) || (body == nil) || (SO3BodyGetFluid body) || !(V3DphysGetState c3dXsession) || (mass == 0.0)) then 0 else 1;;


fun getSign(f)=
  if f >=. 0.0 then 
    1.0
  else
    -.1.0;;


fun switchVectorXY(vec)=
  let vec -> [x y z] in
    [y x z];;


fun switchVectorXZ(vec)=
  let vec -> [x y z] in
    [z y x];;


fun switchVectorYZ(vec)=
  let vec -> [x y z] in
    [x z y];;


fun isCollideBody(body)=
  if ((SO3BodyGetType body) == 1) then 1 else 0;;


fun isAnInvalidBody(handstr, body)=
  let SO3BodyGetMassMatrix body -> [mass _] in
  if ((mass == 0.0) || (SO3BodyGetIgnoreCollision body) || (body == (SO3SceneNodeGetBody handstr.PHAND_obj)) || (isCollideBody body)) then 1 else
  //check if this is an hand body
  let 0 -> ret in
  (
    let handstr.PHAND_lFingers -> lfingers in
    while ((lfingers != nil) && !ret) do
    (
      let hd lfingers -> npstr in
      (
        let npstr.PPOINT_lPhalanges -> lphalanges in
        while ((lphalanges != nil) && !ret) do
        (
          let hd lphalanges -> nnpstr in
          let SO3SceneNodeGetBody nnpstr.PPOINT_obj -> bd in
          if (bd != body) then nil else
            set ret = 1;
          
          set lphalanges = tl lphalanges;
        );
        
        let SO3SceneNodeGetBody npstr.PPOINT_obj -> bd in
        if (bd != body) then nil else
          set ret = 1;
      );
        
      set lfingers = tl lfingers;
    );
    ret;
  );;


// ignore avatar bodies and disabled bodies
fun getValidRay(lray, handstr)=
  let nil -> body in
  let 0.0 -> dist in
  let [0.0 1.0 0.0] -> normal in
  (
    while ((lray != nil) && (body == nil)) do
    (
      let hd lray -> [tbody tdist tnormal] in
      if (((SO3BodyGetType tbody) & 1) || ((SO3BodyGetType tbody) & 4) || (SO3BodyGetIgnoreCollision tbody)) || (isAnInvalidBody handstr tbody) || (SO3BodyGetFluid tbody) then nil else
      (
        set body = tbody;
        set dist = tdist;
        set normal = tnormal;
      );
      set lray = tl lray;
    );    
    [body dist normal]
  );;


fun inverseVector(vec)=
  multiplyVectorF vec [(-.1.0) (-.1.0) (-.1.0)];;


fun getPinchThumbBody(handstr)=
  let nth_list handstr.PHAND_lFingers 0 -> npstr in
  let nth_list npstr.PPOINT_lPhalanges ((sizelist npstr.PPOINT_lPhalanges) - 1) -> nnpstr in
  let SO3SceneNodeGetBody nnpstr.PPOINT_obj -> pbody in
  let if (pbody == nil) then SO3SceneNodeGetBody npstr.PPOINT_obj else pbody -> pbody in
  let if (pbody == nil) then SO3SceneNodeGetBody handstr.PHAND_obj else pbody -> pbody in
    pbody;;


fun getFingerTipSourceAndDir(obstr, pstr, ljoints, cquat, hquat, hdir, fquat, fvec)=
  if ((SO3ObjectGetType pstr.PPOINT_obj) != SO3_TYPE_BONE) then
  (
    let normalizeVectorF (SO3MathsQuatGetDirection fquat [0.0 0.0 (-.1.0)]) -> dest in
    let addVectorF fvec (multiplyVectorF dest [0.01 0.01 0.01]) -> src in
      [src dest];
  )
  else
  (  
    let if obstr.PLEAP_bMirror then [(-.1.0) (-.1.0) (1.0)] else [(-.1.0) (-.1.0) (-.1.0)] -> vdir in
    let if obstr.PLEAP_bTopView then multiplyVectorF [(-.1.0) (-.1.0) 1.0] vdir else vdir -> vdir in
    let nth_list ljoints 0 -> mcp in
    let multiplyVectorF mcp vdir -> mcp in
    
    let nth_list ljoints 1 -> pip in
    let multiplyVectorF pip vdir -> pip in
  
    let nth_list ljoints 2 -> dip in
    let multiplyVectorF dip vdir -> dip in
    
    let nth_list ljoints 3 -> tip in
    let multiplyVectorF tip vdir -> tip in
    
    let normalizeVectorF (subVectorF pip mcp) -> mcpdir in
    let SO3MathsQuatAdd (SO3MathsGetRotationTo normalizeVectorF hdir mcpdir) hquat -> mcpq in
    let if obstr.PLEAP_bTopView then SO3MathsQuatAdd [sqrt(0.5) 0.0 0.0 sqrt(0.5)] mcpq else mcpq -> mcpq in

    
    let normalizeVectorF (subVectorF dip pip) -> pipdir in
    let SO3MathsQuatAdd (SO3MathsGetRotationTo normalizeVectorF mcpdir pipdir) mcpq -> pipq in
    
    let normalizeVectorF (subVectorF tip dip) -> dipdir in
    let SO3MathsQuatAdd (SO3MathsGetRotationTo normalizeVectorF pipdir dipdir) pipq -> dipq in
    
    let nth_list pstr.PPOINT_lPhalanges 0 -> npipstr in
    let npipstr.PPOINT_initPos -> [_ ipipquat pipgquat _] in
    let nth_list pstr.PPOINT_lPhalanges 1 -> npdipstr in
    let npdipstr.PPOINT_initPos -> [_ idipquat dipgquat _] in
    
    let SO3MathsQuatAdd cquat SO3MathsQuatAdd dipq dipgquat -> dipfq in
    let pstr.PPOINT_initPos -> [ifpos _ _ _] in
    let (SO3ObjectGetGlobalPosition npdipstr.PPOINT_obj) -> src in
    let SO3MathsQuatGetDirection dipfq ifpos -> odir in
    let addVectorF src (multiplyVectorF odir [0.1 0.1 0.1]) -> src in
    let normalizeVectorF odir -> dest in
    (
      [src dest];
    );
  );;


fun updateFingerSub(obstr, handstr, pstr, lquat, quat, cvel, comega, vnbsec, nbframes, stiffness, id, index)=
  let pstr.PPOINT_initPos -> [ipos _ gquat initdir] in
  if (usePhysic pstr.PPOINT_obj pstr.PPOINT_iMode) then
  (
    let SO3BodyGetMassMatrix pstr.PPOINT_body -> [mass vmassmat] in
    let [mass mass mass] -> vmass in
    
    // TODO remove the hand gquat and gpos, and add the current hand quat and vec from calculation 
    let SO3ObjectGetGlobalPosition pstr.PPOINT_obj -> gvec in
    let addVectorF (SO3ObjectGetGlobalPosition SO3ObjectGetParent pstr.PPOINT_obj) SO3MathsQuatGetDirection (SO3ObjectGetGlobalOrientation SO3ObjectGetParent pstr.PPOINT_obj) ipos -> fvec in
    let if (id == 0) then 1.0 else stiffness -> stiffness in
    (    
      SO3BodySetGravityEnable pstr.PPOINT_body 0;
      SO3ObjectSetPosition pstr.PPOINT_obj ipos; // force init pos to avoid strange behavior

      if (id != 0) then
      (
        V3DphysEnableBody pstr.PPOINT_body 1;
        SO3BodyMoveTo pstr.PPOINT_body fvec stiffness;
        SO3BodyRotateTo pstr.PPOINT_body quat stiffness;
        //SO3BodySetOmega pstr.PPOINT_body [0.0 0.0 0.0];  
      )
      else
      (
        let SO3MathsQuatToEulerPYR lquat -> [p y r] in
        let if (p >. 0.0) then 1.0 else (-.1.0) -> pdir in
        let if (y >. 0.0) then 1.0 else (-.1.0) -> ydir in
        let if (r >. 0.0) then 1.0 else (-.1.0) -> rdir in
        let if index != 0 then 
              [(if ((SO3MathsRadianToDegree (absf p)) >. 90.0) then SO3MathsDegreeToRadian 90.0 *. pdir else p) (if ((SO3MathsRadianToDegree (absf y)) >. 20.0) then SO3MathsDegreeToRadian 20.0 *. ydir else y) (if ((SO3MathsRadianToDegree (absf r)) >. 0.0) then SO3MathsDegreeToRadian 0.0 else r)]
            else
              [p y r] //[(if ((SO3MathsRadianToDegree (absf p)) >. 45.0) then SO3MathsDegreeToRadian 45.0 *. pdir else p) (if ((SO3MathsRadianToDegree (absf y)) >. 60.0) then SO3MathsDegreeToRadian 60.0 *. ydir else y) (if ((SO3MathsRadianToDegree (absf r)) >. 60.0) then SO3MathsDegreeToRadian 60.0 *. rdir else r)]
        -> [p y r] in
        (
//          if (index !=  1) then nil else
//            addLogMessageVector3F "f rot : " [SO3MathsRadianToDegree p SO3MathsRadianToDegree y SO3MathsRadianToDegree r];
          SO3PhysicsContraintSetBallAndSocketYawAngle pstr.PPOINT_joint y;
          SO3PhysicsContraintSetBallAndSocketPitchAngle pstr.PPOINT_joint p;
          SO3PhysicsContraintSetBallAndSocketRollAngle pstr.PPOINT_joint r;
        );
      );
    );
    0;
  )
  else
  (
    //SO3ObjectSetPosition pstr.PPOINT_obj ipos;
    SO3ObjectSetGlobalOrientation pstr.PPOINT_obj quat;
    0;
  );
  0;;


fun updateFingers(obstr, handstr, lfingers, phand, pcam, vnbsec, nbframes)=
  /*let 0 -> i in
  let sizelist lfingers -> size in
  let "" -> ids in
  (
    while (i < size) do
    (
      let nth_list lfingers i -> [_ _ _ id] in
        set ids = strcatn ids::" "::(itoa id)::nil;
      set i = i + 1;
    );
    addLogMessage ids;
  );*/
  
  let phand -> [hquat hdir hnormal cvel comega] in
  let pcam -> [cquat cpos] in
  let obstr.PLEAP_vOffset -> [yoffset zoffset] in
  let 0 -> j in
  let handstr.PHAND_lFingers -> hlfingers in
  while (lfingers != nil) do
  (
    let hd lfingers -> fingerpos in 
    let fingerpos -> [[fx fy fz] [fax fay faz] [sx sy] ljoints id] in
    let multiplyVectorF [fx fy fz] obstr.PLEAP_vCoef -> [fx fy fz] in
    let if obstr.PLEAP_bMirror then [fx fy (-.fz)] else [fx fy fz] -> [fx fy fz] in
    let if obstr.PLEAP_bMirror then [fax (-.fay) +. PIf (-.faz)] else [fax fay faz] -> [fax fay faz] in
    let if obstr.PLEAP_bTopView then [(-.fx) (-.fz) (-.fy)] else [fx fy fz] -> [fx fy fz] in
    let if obstr.PLEAP_bTopView then [(-.fax +. 1.91986218) fay (-.faz +. PIf)] else [fax fay faz] -> [fax fay faz] in
    
    let hd hlfingers -> pstr in
    let pstr.PPOINT_initPos -> [ipos iquat gquat initdir] in
    let SO3MathsQuatToEulerPYR iquat -> [iax iay iaz] in
    let SO3MathsEulerPYRToQuat [fax (-.fay) iaz] -> fquat in
    let SO3MathsQuatAdd cquat fquat -> fquat in
    let SO3MathsQuatGetDirection cquat [fx (fy +. yoffset) (fz +. zoffset)] -> npos in
    let (addVectorF npos cpos) -> fvec in
    (
      if ((handstr.PHAND_pinchJoint != nil) || !obstr.PLEAP_bPhysicGrab || (j > 1)) then nil else
      let 0.10 -> dist in
      let getFingerTipSourceAndDir obstr pstr ljoints cquat hquat hdir fquat fvec -> [src dest] in
      let getValidRay (SO3PhysicsRayCastExt (V3DgetSession c3dXsession) src dest dist) handstr -> [rbody cdist rnormal] in
      if (rbody != nil) then
      (
        if (((handstr.PHAND_pinchBody != nil) && (handstr.PHAND_pinchBody != rbody))) then 
          set handstr.PHAND_pinchBody = nil
        else
          set handstr.PHAND_pinchBody = rbody;        
        0;
      )
      else
      (
        set handstr.PHAND_pinchBody = nil;
        0;
      );

      if (((SO3ObjectGetType pstr.PPOINT_obj) != SO3_TYPE_BONE) && (usePhysic pstr.PPOINT_obj pstr.PPOINT_iMode) && (pstr.PPOINT_bState)) then
      (
        let SO3SceneNodeGetBody pstr.PPOINT_obj -> body in
        if (vectorIsZeroF [fx fy fz]) then
        (
          SO3BodySetVelocity body [0.0 0.0 0.0];
          SO3BodySetOmega body [0.0 0.0 0.0];
          
          if (!pstr.PPOINT_bAutoHide) then nil else
          (
            SO3ObjectSetVisible pstr.PPOINT_obj 0 1;
            V3DphysEnableBody body 0;
          );
          
          set pstr.PPOINT_bState = 0;
          0;
        )
        else
        (
          if (!pstr.PPOINT_bAutoHide) then nil else
          (
            SO3ObjectSetVisible pstr.PPOINT_obj 1 1;
            V3DphysEnableBody body 1;
          );     
          
          let SO3BodyGetMassMatrix body -> [mass vmassmat] in
          let [mass mass mass] -> vmass in
          let SO3ObjectGetGlobalPosition pstr.PPOINT_obj -> gvec in
          (
            SO3BodyMoveTo body fvec 0.3;
            SO3BodyRotateTo body fquat 0.6;
            
            //SO3BodySetVelocity body cvel;
            //SO3BodySetOmega body [0.0 0.0 0.0];
            
            //SO3BodySetVelocity body addVectorF hvel cvel;
            //SO3BodySetOmega body omega;
            
            SO3BodySetGravityEnable body 0;
          );
          
          set pstr.PPOINT_bState = 1;
          0;
        );
      )
      else
      (
        if (vectorIsZeroF [fx fy fz]) then
        (
          let SO3SceneNodeGetBody pstr.PPOINT_obj -> body in
          if (body == nil) then nil else
          (
            SO3BodySetVelocity body [0.0 0.0 0.0];
            SO3BodySetOmega body [0.0 0.0 0.0];
            V3DphysEnableBody body 0;
            
            let pstr.PPOINT_initPos -> [ippos ipquat gquat _] in
            (
              SO3ObjectSetPosition pstr.PPOINT_obj ippos;
              SO3ObjectSetOrientation pstr.PPOINT_obj ipquat;
            );
          );
          
          if (!pstr.PPOINT_bAutoHide) then nil else
          (
            SO3ObjectSetVisible pstr.PPOINT_obj 0 1;
          );
          
          //SO3ObjectSetPosition pstr.PPOINT_obj ipos;
          //SO3ObjectSetOrientation pstr.PPOINT_obj iquat;
          
          let pstr.PPOINT_lPhalanges -> lphalanges in
          while (lphalanges != nil) do
          (
            let hd lphalanges -> npstr in
            let npstr.PPOINT_initPos -> [ippos ipquat gquat _] in
            (
              SO3BodySetVelocity npstr.PPOINT_body [0.0 0.0 0.0];
              SO3BodySetOmega npstr.PPOINT_body [0.0 0.0 0.0];
              V3DphysEnableBody npstr.PPOINT_body 0;
              SO3ObjectSetOrientation npstr.PPOINT_obj ipquat;
              SO3ObjectSetPosition npstr.PPOINT_obj ippos;
            );
            
            set lphalanges = tl lphalanges;
          );
          
          set pstr.PPOINT_bState = 0;
          0;
        )
        else
        (
          if ((SO3ObjectGetType pstr.PPOINT_obj) == SO3_TYPE_BONE) then
          (
            if (!pstr.PPOINT_bState) then 
            (
              let nth_list pstr.PPOINT_lPhalanges 0 -> npipstr in
              let npipstr.PPOINT_initPos -> [_ ipipquat pipgquat _] in
              let nth_list pstr.PPOINT_lPhalanges 1 -> npdipstr in
              let npdipstr.PPOINT_initPos -> [_ idipquat dipgquat _] in
              (
                let SO3SceneNodeGetBody pstr.PPOINT_obj -> body in
                let pstr.PPOINT_initPos -> [ippos ipquat gquat _] in
                if (body == nil) then nil else
                (
                  SO3ObjectSetOrientation pstr.PPOINT_obj ipquat;
                  SO3ObjectSetPosition pstr.PPOINT_obj ippos;
                  SO3BodySetVelocity body [0.0 0.0 0.0];
                  SO3BodySetOmega body [0.0 0.0 0.0];
                  V3DphysEnableBody body 0;
                );
                
                let pstr.PPOINT_lPhalanges -> lphalanges in
                while (lphalanges != nil) do
                (
                  let hd lphalanges -> npstr in
                  let npstr.PPOINT_initPos -> [ippos ipquat gquat _] in
                  (
                    SO3BodySetVelocity npstr.PPOINT_body [0.0 0.0 0.0];
                    SO3BodySetOmega npstr.PPOINT_body [0.0 0.0 0.0];
                    V3DphysEnableBody npstr.PPOINT_body 0;
                    SO3ObjectSetOrientation npstr.PPOINT_obj ipquat;
                    SO3ObjectSetPosition npstr.PPOINT_obj ippos;
                  );
                  
                  set lphalanges = tl lphalanges;
                );
              );
              0;
            )
            else
            (
              let if obstr.PLEAP_bMirror then [(-.1.0) (-.1.0) (1.0)] else [(-.1.0) (-.1.0) (-.1.0)] -> vdir in
              let if obstr.PLEAP_bTopView then multiplyVectorF [(-.1.0) (-.1.0) 1.0] vdir else vdir -> vdir in
              let nth_list ljoints 0 -> mcp in
              let multiplyVectorF mcp vdir -> mcp in
              
              let nth_list ljoints 1 -> pip in
              let multiplyVectorF pip vdir -> pip in
            
              let nth_list ljoints 2 -> dip in
              let multiplyVectorF dip vdir -> dip in
              
              let nth_list ljoints 3 -> tip in
              let multiplyVectorF tip vdir -> tip in
              
              let normalizeVectorF (subVectorF pip mcp) -> mcpdir in
              let SO3MathsQuatAdd (SO3MathsGetRotationTo normalizeVectorF hdir mcpdir) hquat -> mcpq in
              
              let normalizeVectorF (subVectorF dip pip) -> pipdir in
              let SO3MathsQuatAdd (SO3MathsGetRotationTo normalizeVectorF mcpdir pipdir) mcpq -> pipq in
              
              let normalizeVectorF (subVectorF tip dip) -> dipdir in
              let SO3MathsQuatAdd (SO3MathsGetRotationTo normalizeVectorF pipdir dipdir) pipq -> dipq in
              
              let nth_list pstr.PPOINT_lPhalanges 0 -> npipstr in
              let npipstr.PPOINT_initPos -> [_ ipipquat pipgquat _] in
              let nth_list pstr.PPOINT_lPhalanges 1 -> npdipstr in
              let npdipstr.PPOINT_initPos -> [_ idipquat dipgquat _] in
              
              let if obstr.PLEAP_bTopView then SO3MathsQuatAdd [sqrt(0.5) 0.0 0.0 sqrt(0.5)] mcpq else mcpq -> mcpq in
              let if obstr.PLEAP_bTopView then SO3MathsQuatAdd [sqrt(0.5) 0.0 0.0 sqrt(0.5)] pipq else pipq -> pipq in
              let if obstr.PLEAP_bTopView then SO3MathsQuatAdd [sqrt(0.5) 0.0 0.0 sqrt(0.5)] dipq else dipq -> dipq in
              let SO3MathsQuatAdd cquat SO3MathsQuatAdd mcpq gquat -> mcpfq in
              let SO3MathsQuatAdd cquat SO3MathsQuatAdd pipq pipgquat -> pipfq in
              let SO3MathsQuatAdd cquat SO3MathsQuatAdd dipq dipgquat -> dipfq in
              
              let SO3MathsGetRotationTo [0.0 0.0 1.0] initdir -> initquat in
              let SO3MathsGetRotationTo [0.0 0.0 1.0] normalizeVectorF SO3MathsQuatGetDirection quatInverse hquat (subVectorF pip mcp) -> mclq in
              //let SO3MathsQuatDiff mclq initquat -> mclq in
              (
                let if (obstr.PLEAP_bPhysicGrab) then 1.0 else
                    (
                      let 0.02 -> dist in
                      let pstr.PPOINT_initPos -> [ifpos _ _ _] in
                      let (SO3ObjectGetGlobalPosition npdipstr.PPOINT_obj) -> src in
                      let SO3MathsQuatGetDirection dipfq ifpos -> odir in
                      let addVectorF src (multiplyVectorF odir [0.1 0.1 0.1]) -> src in
                      let normalizeVectorF odir -> dest in
                      let getValidRay (SO3PhysicsRayCastExt (V3DgetSession c3dXsession) src dest dist) handstr -> [rbody cdist rnormal] in
                        if (rbody == nil) then 0.3 else minf ((cdist *. 10.0) +. 0.001) 0.3;
                    )
                -> stiffness in
                (
                  updateFingerSub obstr handstr pstr mclq mcpfq cvel comega vnbsec nbframes stiffness 0 j;
                  updateFingerSub obstr handstr npipstr nil pipfq cvel comega vnbsec nbframes stiffness 1 j;
                  updateFingerSub obstr handstr npdipstr nil dipfq cvel comega vnbsec nbframes stiffness 2 j;
                );
              );
              0;
            );
          )
          else
          (
            if (!pstr.PPOINT_bAutoHide) then nil else
            (
              SO3ObjectSetVisible pstr.PPOINT_obj 1 1;
            );

            SO3ObjectSetGlobalOrientation pstr.PPOINT_obj fquat;
            SO3ObjectSetGlobalPosition pstr.PPOINT_obj fvec;
            0;
          );
          
          set pstr.PPOINT_bState = 1;
          0;
        );
      );
    );
    
    set hlfingers = tl hlfingers;
    set lfingers = tl lfingers;
    set j = j + 1;
  );
  0;;


fun cbNavPreRender(inst, sessionstr, etime, obstr)=
  let maxf 0.000001 ((itof etime) /. 1000000.0) -> nbsec in  
  //base on physic update framerate
  let (itof SO3WorldGetFPS sessionstr.V3D_session) -> pfps in
  let nbsec *. pfps -> nbfr in
  let [nbfr nbfr nbfr] -> nbframes in
  let [nbsec nbsec nbsec] -> vnbsec in
  let obstr.PLEAP_vOffset -> [yoffset zoffset] in
  let obstr.PLEAP_iMode -> mode in
  //camera space
  let if (mode == 1) then V3DgetObjectByName c3dXsession "Current camera" else
    if (mode == 2) then
      V3DgetObjectByName c3dXsession "Current camera shell"
    else if (mode == 3) then
      obstr.PLEAP_objRef else nil -> camera in
  
  let (SO3ObjectGetParent sessionstr.V3D_defaultCamera) -> parent in
  // the navigation shell should be the 2nd parent
  let if (mode != 2) then camera else if ((SO3ObjectGetParent parent) == nil || (SO3ObjectGetParent parent) == SO3ObjectGetSceneRoot (V3DgetSession sessionstr)) then
    parent
  else
    (SO3ObjectGetParent parent)
  -> camera in
  let if (camera == nil) then [0.0 0.0 0.0] else SO3ObjectGetGlobalPosition camera -> cpos in
  let if (camera == nil) then [0.0 0.0 0.0 1.0] else SO3ObjectGetGlobalOrientation camera -> cquat in
  let if (sessionstr.V3D_defaultCamera == camera) then (V3DgetCameraByType sessionstr camera 2) else camera -> cobj in
  let (SO3SceneNodeGetBody cobj) -> cbody in
  let if (cbody != nil) then SO3BodyGetVelocity cbody else [0.0 0.0 0.0] -> cvel in
  let if (cbody != nil) then SO3BodyGetOmega cbody else [0.0 0.0 0.0] -> comega in
  (
    let 0 -> i in
    let if obstr.PLEAP_bMirror then revertlist obstr.PLEAP_lHands else obstr.PLEAP_lHands -> lhandstr in
    while (lhandstr != nil) do
    (
      let hd lhandstr -> handstr in
      let _GetLeapMotionHandExt obstr.PLEAP_device i 0 -> hand in
      (
        //hide the hand
        if (hand == nil) then
        (
          if (!handstr.PHAND_bState) then nil else
          (
            if (!handstr.PHAND_bAutoHide) then nil else
            (
              SO3ObjectSetVisible handstr.PHAND_obj 0 1;    
              V3DphysEnableBody SO3SceneNodeGetBody handstr.PHAND_obj 0;
            );
            
            set handstr.PHAND_bState = 0;
            
            let handstr.PHAND_lFingers -> lhfingers in
            while (lhfingers != nil) do
            (
              let hd lhfingers -> pstr in
              (
                if (!pstr.PPOINT_bAutoHide) then nil else
                (
                  SO3ObjectSetVisible pstr.PPOINT_obj 0 1;  
                  V3DphysEnableBody SO3SceneNodeGetBody pstr.PPOINT_obj 1;
                );
                
                set pstr.PPOINT_bState = 0;
              );
              
              set lhfingers = tl lhfingers;
            );
            
            let switch obstr.PLEAP_lLastCircleId i -> [psid [radius sdir] progress] in
            if (psid == nil) then nil else
            (
              if (sdir <. 0.0) then
                SendPluginEvent obstr.PLEAP_inst "Circle left end" (strcatn (itoa i)::" "::(ftoa radius)::" "::(ftoa progress)::nil) nil
              else
                SendPluginEvent obstr.PLEAP_inst "Circle right end" (strcatn (itoa i)::" "::(ftoa radius)::" "::(ftoa progress)::nil) nil;
            );
            
            let switch obstr.PLEAP_lLastSwipeId i -> [psid [sdx sdy] speed] in
            if (psid == nil) then nil else
            (
              if (sdx <. -.0.6) then
                SendPluginEvent obstr.PLEAP_inst "Swipe left end" (strcatn (itoa i)::" "::(ftoa speed)::" "::(ftoa 1.0)::nil) nil
              else if (sdx >. 0.6) then
                SendPluginEvent obstr.PLEAP_inst "Swipe right end" (strcatn (itoa i)::" "::(ftoa speed)::" "::(ftoa 1.0)::nil) nil
              else if (sdy <. -.0.6) then
                SendPluginEvent obstr.PLEAP_inst "Swipe down end" (strcatn (itoa i)::" "::(ftoa speed)::" "::(ftoa 1.0)::nil) nil
              else if (sdy >. 0.6) then
                SendPluginEvent obstr.PLEAP_inst "Swipe up end" (strcatn (itoa i)::" "::(ftoa speed)::" "::(ftoa 1.0)::nil) nil
              else nil;
            );
            
            set obstr.PLEAP_lLastCircleId = remove_idx_from_list obstr.PLEAP_lLastCircleId i;
            set obstr.PLEAP_lLastSwipeId = remove_idx_from_list obstr.PLEAP_lLastSwipeId i;
            
            SendPluginEvent obstr.PLEAP_inst (if (i == 0) then "First hand lost" else "Second hand lost") nil nil;
            
            if (!handstr.PHAND_iGrabState) then nil else
            (
              SendPluginEvent obstr.PLEAP_inst "Ungrab" (itoa i) nil;
              set handstr.PHAND_iGrabState = 0;
            );
            
            if (!handstr.PHAND_iPinchState) then nil else
            (
              if (!obstr.PLEAP_bPhysicGrab || (handstr.PHAND_pinchBody == nil && handstr.PHAND_pinchJoint == nil)) then nil else
              (
                SO3PhysicsContraintDestroy handstr.PHAND_pinchJoint;
                SO3BodyDestroy handstr.PHAND_hPinchBody;
                
                cbPlugGeneric 22 SO3ObjectGetName (SO3BodyGetSceneNode handstr.PHAND_pinchBody) nil;
                set handstr.PHAND_pinchBody = nil;
                set handstr.PHAND_hPinchBody = nil;
                set handstr.PHAND_pinchJoint = nil;
              );
              
              SendPluginEvent obstr.PLEAP_inst "Unpinch" (itoa i) nil;
              set handstr.PHAND_iPinchState = 0;
            );
            
            set handstr.PHAND_iNbFingers = 0;
            
            let 0 -> k in
            while (handstr.PHAND_bfingerState != nil) do
            (
              let hd handstr.PHAND_bfingerState -> [id [lstate _]] in
              if (!lstate) then nil else
                SendPluginEvent obstr.PLEAP_inst strcatn (if (i == 0) then "First" else "Second")::" "::(nth_list lFingersNames id)::" "::"lost"::nil nil nil;

              set handstr.PHAND_bfingerState = tl handstr.PHAND_bfingerState;
              set k = k + 1;
            );
          );
          0;
        )
        else
        (
          let _GetLeapMotionHandFingersWithJoints obstr.PLEAP_device i 0 -> lfingers in
          let hand -> [pos orient velocity dir normal confidence] in
          let multiplyVectorF pos obstr.PLEAP_vCoef -> [x y z] in
          
          let orient -> [ax ay az] in
          let if obstr.PLEAP_bMirror then [x y (-.z)] else [x y z] -> [x y z] in
          let if obstr.PLEAP_bTopView then [(-.x) (-.z) (-.y)] else [x y z] -> [x y z] in
          let if obstr.PLEAP_bMirror then [(-.1.0) (-.1.0) (1.0)] else [(-.1.0) (-.1.0) (-.1.0)] -> vdir in
          let if obstr.PLEAP_bTopView then multiplyVectorF [(-.1.0) (-.1.0) 1.0] vdir else vdir -> vdir in
          let normalizeVectorF multiplyVectorF dir vdir -> hdir in
          let multiplyVectorF normal vdir -> hnormal in
          let SO3MathsGetRotationToAxis hdir hnormal -> hquat in
          let SO3MathsQuatGetDirection cquat [x (y +. yoffset) (z +. zoffset)] -> npos in
          let (addVectorF npos cpos) -> hvec in
          let SO3MathsQuatAdd cquat if obstr.PLEAP_bTopView then SO3MathsQuatAdd [sqrt(0.5) 0.0 0.0 sqrt(0.5)] hquat else hquat -> chquat in
          (
            //count valid finger
            let lfingers -> cplfingers in
            let 0 -> nbvfingers in
            (
              let 0 -> k in
              while (cplfingers != nil) do
              (
                let hd cplfingers -> fingerpos in 
                let fingerpos -> [[fx fy fz] [fax fay faz] [sx sy] ljoints id] in
                let if ((zeroVectorF [fx fy fz]) || ((sx == 0.0) && (sy == 0.0))) then 0 else 1 -> ivisible in
                let switch handstr.PHAND_bfingerState k -> fstate in
                let fstate -> [lstate _] in
                (
                  if (lstate == nil) then
                  (
                    set handstr.PHAND_bfingerState = lcat [k [ivisible 0]]::nil handstr.PHAND_bfingerState;
                    if (!ivisible) then nil else
                      SendPluginEvent obstr.PLEAP_inst strcatn (if (i == 0) then "First" else "Second")::" "::(nth_list lFingersNames k)::" "::(if ivisible then "found" else "lost")::nil nil nil;
                    0;
                  )
                  else if (ivisible == lstate) then nil else
                  (
                    mutate fstate <- [ivisible _];

                    SendPluginEvent obstr.PLEAP_inst strcatn (if (i == 0) then "First" else "Second")::" "::(nth_list lFingersNames k)::" "::(if ivisible then "found" else "lost")::nil nil nil;
                    0;
                  );
                  
                  set nbvfingers = nbvfingers + (vectorIsZeroF [fx fy fz]);
                );
                
                set cplfingers = tl cplfingers;
                set k = k + 1;
              );
              
              if (!handstr.PHAND_iGrabState && ((_GetLeapMotionGrabStrength obstr.PLEAP_device i) >. 0.5)) then
              (
                SendPluginEvent obstr.PLEAP_inst "Grab" (itoa i) nil;
                set handstr.PHAND_iGrabState = 1;
              )
              else if (handstr.PHAND_iGrabState && ((_GetLeapMotionGrabStrength obstr.PLEAP_device i) <. 0.3)) then
              (
                SendPluginEvent obstr.PLEAP_inst "Ungrab" (itoa i) nil;
                set handstr.PHAND_iGrabState = 0;
              )
              else nil;
              
              if (!handstr.PHAND_iPinchState && ((_GetLeapMotionPinchStrength obstr.PLEAP_device i) >. 0.5)) then
              (
                if (!obstr.PLEAP_bPhysicGrab || handstr.PHAND_pinchBody == nil || handstr.PHAND_pinchJoint != nil) then nil else
                (
                  if ((SO3SceneNodeGetBody handstr.PHAND_obj) != nil) then nil else
                  (
                    set handstr.PHAND_hPinchBody = SO3BodyCreateEllipsoid handstr.PHAND_obj [0.02 0.02 0.02];
                    
                    let SO3BodyGetMassMatrix handstr.PHAND_pinchBody -> [gmass _] in
                      SO3BodySetMass handstr.PHAND_hPinchBody gmass;
                    SO3BodySetAutoSleep handstr.PHAND_hPinchBody 0;
                    SO3BodySetGravityEnable handstr.PHAND_hPinchBody 0;
                    SO3BodySetIgnoreCollision handstr.PHAND_hPinchBody 1;
                  );
                  
                  let SO3ObjectGetGlobalPosition SO3BodyGetSceneNode handstr.PHAND_pinchBody -> jpos in
                    set handstr.PHAND_pinchJoint = SO3PhysicsContraintCreateSlider (V3DgetSession c3dXsession) (SO3SceneNodeGetBody handstr.PHAND_obj) /*(getPinchThumbBody handstr)*/ handstr.PHAND_pinchBody jpos [0.0 1.0 0.0] 1 0;
                  SO3PhysicsContraintSetSliderLimits handstr.PHAND_pinchJoint 0.0 0.0;
                  SO3PhysicsContraintSetCollisionState handstr.PHAND_pinchJoint 0;
                  cbPlugGeneric 21 SO3ObjectGetName (SO3BodyGetSceneNode handstr.PHAND_pinchBody) nil;
                );
                
                SendPluginEvent obstr.PLEAP_inst "Pinch" (itoa i) nil;
                set handstr.PHAND_iPinchState = 1;
              )
              else if (handstr.PHAND_iPinchState && ((_GetLeapMotionPinchStrength obstr.PLEAP_device i) <. 0.3)) then
              (
                if (!obstr.PLEAP_bPhysicGrab || (handstr.PHAND_pinchBody == nil && handstr.PHAND_pinchJoint == nil)) then nil else
                (
                  SO3PhysicsContraintDestroy handstr.PHAND_pinchJoint;
                  SO3BodyDestroy handstr.PHAND_hPinchBody;
                  cbPlugGeneric 22 SO3ObjectGetName (SO3BodyGetSceneNode handstr.PHAND_pinchBody) nil;
                  set handstr.PHAND_pinchBody = nil;
                  set handstr.PHAND_hPinchBody = nil;
                  set handstr.PHAND_pinchJoint = nil;
                );
                
                SendPluginEvent obstr.PLEAP_inst "Unpinch" (itoa i) nil;
                set handstr.PHAND_iPinchState = 0;
              )
              else nil;
              
              set handstr.PHAND_iNbFingers = nbvfingers;
            );
            
            if ((usePhysic handstr.PHAND_obj handstr.PHAND_iMode) && (handstr.PHAND_bState || !handstr.PHAND_bAutoHide)) then
            let SO3SceneNodeGetBody handstr.PHAND_obj -> body in
            (
              //physic
              let SO3BodyGetMassMatrix body -> [mass vmassmat] in
              let handstr.PHAND_fMass -> mass in
              let [mass mass mass] -> vmass in
              let SO3ObjectGetGlobalPosition handstr.PHAND_obj -> gvec in
              (
                SO3BodyMoveTo body hvec 0.3;
                SO3BodyRotateTo body chquat 0.6;
            
                //SO3BodySetVelocity body cvel;
                //SO3BodySetOmega body [0.0 0.0 0.0];                
                SO3BodySetGravityEnable body 0;
                
                //if (!obstr.PLEAP_bSonsAsFingers) then nil else
                  updateFingers obstr handstr lfingers [hquat hdir hnormal cvel comega] [cquat cpos] vnbsec nbframes;
              );
              0;
            )
            else // no physic
            (
              SO3ObjectSetGlobalPosition handstr.PHAND_obj hvec;
              SO3ObjectSetGlobalOrientation handstr.PHAND_obj chquat;
              
              let SO3SceneNodeGetBody handstr.PHAND_obj -> body in
              if (body == nil) then nil else
              (
                SO3BodySetVelocity body [0.0 0.0 0.0];
                SO3BodySetOmega body [0.0 0.0 0.0];
                V3DphysEnableBody body 0;
              );

              //if (!obstr.PLEAP_bSonsAsFingers) then nil else
                updateFingers obstr handstr lfingers [hquat hdir hnormal cvel comega] [cquat cpos] vnbsec nbframes;
            );
            
            if (handstr.PHAND_bState) then nil else
            (
              set handstr.PHAND_bState = 1;
              
              let SO3SceneNodeGetBody handstr.PHAND_obj -> body in
              if (!handstr.PHAND_bAutoHide) then nil else
              (
                SO3ObjectSetVisible handstr.PHAND_obj 1 !obstr.PLEAP_bSonsAsFingers;
                V3DphysEnableBody body 1;
              );
              
              SendPluginEvent obstr.PLEAP_inst (if (i == 0) then "First hand found" else "Second hand found") nil nil;
            );
            
            if (!obstr.PLEAP_bLHandVel && !obstr.PLEAP_bRHandVel) then nil else
            let velocity -> [vx vy vz] in
            let if obstr.PLEAP_bTopView then [vx vy vz] else [vx (-.vz) vy] -> [vx vy vz] in
              SendPluginEvent obstr.PLEAP_inst (if (i == 0) then "First hand velocity" else "Second hand velocity") strcatn (ftoa vx)::" "::(ftoa vy)::" "::(ftoa vz)::nil nil;
            
            if (!obstr.PLEAP_bLHandPos && !obstr.PLEAP_bRHandPos) then nil else
            let hvec -> [hvx hvy hvz] in
            let SO3MathsQuatToEulerPYR chquat -> [hax hay haz] in
              SendPluginEvent obstr.PLEAP_inst (if (i == 0) then "First hand position" else "Second hand position") strcatn (ftoa hvx)::" "::(ftoa hvy)::" "::(ftoa hvz)::"\n"::(ftoa (SO3MathsRadianToDegree hax))::" "::(ftoa (SO3MathsRadianToDegree hay))::" "::(ftoa (SO3MathsRadianToDegree haz))::nil nil;
          );
          0;
        );
      );
      
      set lhandstr = tl lhandstr;
      set i = i + 1;
    );
    
    
    //Tools
    let obstr.PLEAP_lTools -> ltools in
    let 0 -> i in
    while (ltools != nil) do
    (
      let hd ltools -> toolstr in
      let _GetLeapMotionTool obstr.PLEAP_device i 0 -> tool in
      (
        //hide the hand
        if (tool == nil) then
        (       
          if (!toolstr.PPOINT_bState) then nil else
          (
            if (!toolstr.PPOINT_bAutoHide) then nil else
            (
              SO3ObjectSetVisible toolstr.PPOINT_obj 0 1;
              V3DphysEnableBody SO3SceneNodeGetBody toolstr.PPOINT_obj 0;  
            );
            
            set toolstr.PPOINT_bState = 0;

            SendPluginEvent obstr.PLEAP_inst (if (i == 0) then "First tool lost" else "Second tool lost") nil nil;
          );
          0;
        )
        else
        (
          let tool -> [pos orient velocity] in
          let multiplyVectorF pos obstr.PLEAP_vCoef -> [x y z] in
          let orient -> [ax ay az] in
          let if obstr.PLEAP_bMirror then [x y (-.z)] else [x y z] -> [x y z] in
          let if obstr.PLEAP_bMirror then [ax (-.ay) +. PIf (-.az)] else [ax ay az] -> [ax ay az] in
          let if obstr.PLEAP_bTopView then [(-.x) (-.z) (-.y)] else [x y z] -> [x y z] in
          let if obstr.PLEAP_bTopView then [(-.ax -. 1.91986218) (-.ay) (-.az +. PIf)] else [ax ay az] -> [ax ay az] in
          let SO3MathsEulerPYRToQuat [ax (-.ay) az] -> hquat in
          
          let SO3MathsQuatGetDirection cquat [x (y +. yoffset) (z +. zoffset)] -> npos in
          let (addVectorF npos cpos) -> hvec in
          let SO3MathsQuatAdd cquat hquat -> chquat in
          (
            if ((usePhysic toolstr.PPOINT_obj toolstr.PPOINT_iMode) && toolstr.PPOINT_bState) then
            let SO3SceneNodeGetBody toolstr.PPOINT_obj -> body in
            (
              //physic
              let SO3BodyGetMassMatrix body -> [mass vmassmat] in
              let [mass mass mass] -> vmass in
              let SO3ObjectGetGlobalPosition toolstr.PPOINT_obj -> gvec in
              (
                SO3BodyMoveTo body hvec 0.3;
                SO3BodyRotateTo body chquat 0.6;
                
                //SO3BodySetVelocity body cvel;
                //SO3BodySetOmega body [0.0 0.0 0.0];
                
                //SO3BodySetVelocity body addVectorF hvel cvel;
                //SO3BodySetOmega body omega;

                SO3BodySetGravityEnable body 0;
              );
              0;
            )
            else // no physic
            (
              SO3ObjectSetGlobalPosition toolstr.PPOINT_obj hvec;
              SO3ObjectSetGlobalOrientation toolstr.PPOINT_obj chquat;
              
              let SO3SceneNodeGetBody toolstr.PPOINT_obj -> body in
              if (body == nil) then nil else
              (
                SO3BodySetVelocity body [0.0 0.0 0.0];
                SO3BodySetOmega body [0.0 0.0 0.0];
              );
            );
            
            if (toolstr.PPOINT_bState) then nil else
            (
              set toolstr.PPOINT_bState = 1;
              
              if (!toolstr.PPOINT_bAutoHide) then nil else
              (
                SO3ObjectSetVisible toolstr.PPOINT_obj 1 1;
                V3DphysEnableBody SO3SceneNodeGetBody toolstr.PPOINT_obj 1;     
              );
               
              SendPluginEvent obstr.PLEAP_inst (if (i == 0) then "First tool found" else "Second tool found") nil nil;
            );
            
            if (!obstr.PLEAP_bLToolVel && !obstr.PLEAP_bRToolVel) then nil else
            let velocity -> [vx vy vz] in
              SendPluginEvent obstr.PLEAP_inst (if (i == 0) then "First tool velocity" else "Second tool velocity") strcatn (ftoa vx)::" "::(ftoa vy)::" "::(ftoa vz)::nil nil;
            
            if (!obstr.PLEAP_bLToolPos && !obstr.PLEAP_bRToolPos) then nil else
            let hvec -> [hvx hvy hvz] in
            let SO3MathsQuatToEulerPYR chquat -> [hax hay haz] in
              SendPluginEvent obstr.PLEAP_inst (if (i == 0) then "First tool position" else "Second tool position") strcatn (ftoa hvx)::" "::(ftoa hvy)::" "::(ftoa hvz)::"\n"::(ftoa (SO3MathsRadianToDegree hax))::" "::(ftoa (SO3MathsRadianToDegree hay))::" "::(ftoa (SO3MathsRadianToDegree haz))::nil nil;
          );
          0;
        );
      );
      
      set i = i + 1;
      set ltools = tl ltools;
    );
  );
  
  //hands without objs
  let 0 -> i in
  let nil -> hand in
  let nil -> lhandav in
  let nil -> lhandscreenav in
  (
    while (i < 2) do
    (
      set hand = _GetLeapMotionHand obstr.PLEAP_device i 0;
      let _GetLeapMotionHandFingers obstr.PLEAP_device i 1 -> lfingers in
      let 0 -> nbfingers in
      let [0.0 0.0 0.0] -> avvec in
      let [0.0 0.0] -> svec in
      (
        // screen pos      
        while (lfingers != nil) && (zeroVectorF avvec) do
        (
          let hd lfingers -> [fvec fdir [sx sy] id] in
          if (zeroVectorF fvec) || ((sx == 0.0) && (sy == 0.0)) then nil else
          let if obstr.PLEAP_bTopView then [(1.0 -. sx) (1.0 -. sy)] else [sx sy] -> [sx sy] in
          (
            set avvec = addVectorF avvec fvec;
            
            let svec -> [x y] in
              set svec = [(x +. sx) (y +. sy)];
            set nbfingers = nbfingers + 1;
          );
          set lfingers = tl lfingers;
        );
        
        if ((hand != nil) && (nbfingers != 0)) then
        (
          set avvec = divideVectorF avvec [(itof nbfingers) (itof nbfingers) (itof nbfingers)];
          set lhandav = lcat lhandav avvec::nil;
          
          let svec -> [nx ny] in
            set svec = [(nx /. (itof nbfingers)) (ny /. (itof nbfingers))];
          set lhandscreenav = lcat lhandscreenav svec::nil;
        )
        else
        (
          set lhandscreenav = lcat lhandscreenav [0.0 0.0]::nil;
        );
      );
      
      set i = i + 1;
    );
    
    if (!obstr.PLEAP_bLScreenPos && !obstr.PLEAP_bRScreenPos ) then nil else
    let 0 -> i in
    let sizelist lhandscreenav -> size in
    let V3DgetSessionView sessionstr -> viewstr in
    let V3DgetDefaultViewport viewstr -> viewportstr in
    let V3DgetViewportSize viewstr viewportstr -> [_ _ w h] in
    while (i < size) do
    (
      let nth_list lhandscreenav i -> [sx sy] in
      if (sx == 0.0 && sy == 0.0) then nil else
        let [(ftoi ((itof w) *. sx)) (ftoi ((itof h) *. (1.0 -. sy)))] -> [vx vy] in
          SendPluginEvent obstr.PLEAP_inst (if (i == 0) then "First hand screen pos" else "Second hand screen pos") strcatn (itoa vx)::" "::(itoa vy)::nil nil;
      
      set i = i + 1;
    );
    
    if ((sizelist lhandav) < 2) then nil else
    (
      let nth_list lhandav 0 -> [dx1 dy1 dz1] in
      let nth_list lhandav 1 -> [dx2 dy2 dz2] in
      let (absf(dx1 -. dx2)) -> dx in
      let (absf(dy1 -. dy2)) -> dy in
      let (absf(dz1 -. dz2)) -> dz in
      let strcatn (ftoa dx)::" "::(ftoa dy)::" "::(ftoa dz)::nil -> lv in

      let nth_list lhandscreenav 0 -> [sdx1 sdy1] in
      let nth_list lhandscreenav 1 -> [sdx2 sdy2] in
      let 0.0 -> ax in
      let SO3MathsRadianToDegree (atan2 (sdy1 -. sdy2) (sdx1 -. sdx2)) -> xang in
      let SO3MathsRadianToDegree (atan2 (dz1 -. dz2) (dx1 -. dx2)) -> zang in
      let strcatn (ftoa ax)::" "::(ftoa zang)::" "::(ftoa xang)::nil -> la in
      (
        SendPluginEvent obstr.PLEAP_inst "Points distance" lv nil;
        SendPluginEvent obstr.PLEAP_inst "Points rotation" la nil;
      );
    );
  );
  0;;


fun cbConnected(objleap, obstr)=
  set obstr.PLEAP_bConnected = 1;
  setPluginInstanceCbScenePreRenderPhysic obstr.PLEAP_inst mkfun4 @cbNavPreRender obstr;
  SendPluginEvent obstr.PLEAP_inst "Connected" nil nil;
  0;;


fun cbDisconnected(objleap, obstr)=
  set obstr.PLEAP_bConnected = 0;
  setPluginInstanceCbScenePreRenderPhysic obstr.PLEAP_inst nil;
  SendPluginEvent obstr.PLEAP_inst "Disconnected" nil nil;
  0;;


fun cbCircleGesture(leapobj, obstr, state, handid, pointid, dir, radius, progress)=
  let strcatn (itoa handid)::"."::(itoa pointid)::nil -> sid in
  let switch obstr.PLEAP_lLastCircleId handid -> [psid _ _] in
  (
    if ((state != 1) || (radius <. 20.0) || (psid != nil)) then nil else
    (
      if (dir <. 0.0) then
        SendPluginEvent obstr.PLEAP_inst "Circle left start" (strcatn (itoa handid)::" "::(ftoa radius)::" "::(ftoa progress)::nil) nil
      else
        SendPluginEvent obstr.PLEAP_inst "Circle right start" (strcatn (itoa handid)::" "::(ftoa radius)::" "::(ftoa progress)::nil) nil;
        
      set obstr.PLEAP_lLastCircleId = [handid [sid [radius dir] progress]]::obstr.PLEAP_lLastCircleId;
    );
    
    if ((state != 2) || (strcmp psid sid)) then nil else
    (
      if (dir <. 0.0) then
        SendPluginEvent obstr.PLEAP_inst "Circle left progress" (strcatn (itoa handid)::" "::(ftoa radius)::" "::(ftoa progress)::nil) nil
      else
        SendPluginEvent obstr.PLEAP_inst "Circle right progress" (strcatn (itoa handid)::" "::(ftoa radius)::" "::(ftoa progress)::nil) nil
    );
    
    // only get wide hand event
    if ((state != 3) || (strcmp psid sid)) then nil else
    (
      if (progress <. 0.8) then nil else
      (
        if (dir <. 0.0) then
          SendPluginEvent obstr.PLEAP_inst "Circle left end" (strcatn (itoa handid)::" "::(ftoa radius)::" "::(ftoa progress)::nil) nil
        else
          SendPluginEvent obstr.PLEAP_inst "Circle right end" (strcatn (itoa handid)::" "::(ftoa radius)::" "::(ftoa progress)::nil) nil;
      );
        
      set obstr.PLEAP_lLastCircleId = remove_idx_from_list obstr.PLEAP_lLastCircleId handid;
    );
  );
  0;;


fun cbSwipeGesture(leapobj, obstr, state, handid, pointid, dir, speed, progress)=
  let dir -> [dx dy dz] in
  let strcatn (itoa handid)::"."::(itoa pointid)::nil -> sid in
  let _tickcount - obstr.PLEAP_iLastSwipeTick -> tick in
  let switch obstr.PLEAP_lLastSwipeId handid -> [psid _ _] in
  (
    if ((state != 1) || (speed <. 60.0) || (psid != nil) || (tick < 200)) then nil else
    (
      if (dx <. -.0.6) then
        SendPluginEvent obstr.PLEAP_inst "Swipe left start" (strcatn (itoa handid)::" "::(ftoa speed)::nil) nil
      else if (dx >. 0.6) then
        SendPluginEvent obstr.PLEAP_inst "Swipe right start" (strcatn (itoa handid)::" "::(ftoa speed)::nil) nil
      else if (dy <. -.0.6) then
        SendPluginEvent obstr.PLEAP_inst "Swipe down start" (strcatn (itoa handid)::" "::(ftoa speed)::nil) nil
      else if (dy >. 0.6) then
        SendPluginEvent obstr.PLEAP_inst "Swipe up start" (strcatn (itoa handid)::" "::(ftoa speed)::nil) nil
      else nil;
      
      set obstr.PLEAP_lLastSwipeId = [handid [sid [dx dy] speed]]::obstr.PLEAP_lLastSwipeId;
    );
    
    if ((state != 2) || (strcmp psid sid)) then nil else
    (
      if (dx <. -.0.6) then
        SendPluginEvent obstr.PLEAP_inst "Swipe left progress" (strcatn (itoa handid)::" "::(ftoa speed)::" "::(ftoa progress)::nil) nil
      else if (dx >. 0.6) then
        SendPluginEvent obstr.PLEAP_inst "Swipe right progress" (strcatn (itoa handid)::" "::(ftoa speed)::" "::(ftoa progress)::nil) nil
      else if (dy <. -.0.6) then
        SendPluginEvent obstr.PLEAP_inst "Swipe down progress" (strcatn (itoa handid)::" "::(ftoa speed)::" "::(ftoa progress)::nil) nil
      else if (dy >. 0.6) then
        SendPluginEvent obstr.PLEAP_inst "Swipe up progress" (strcatn (itoa handid)::" "::(ftoa speed)::" "::(ftoa progress)::nil) nil
      else nil;
    );
    
    if ((state != 3) || (strcmp psid sid)) then nil else
    (
      //if (progress >. 150.0) then nil else
      (
        if (dx <. -.0.6) then
          SendPluginEvent obstr.PLEAP_inst "Swipe left end" (strcatn (itoa handid)::" "::(ftoa speed)::" "::(ftoa progress)::nil) nil
        else if (dx >. 0.6) then
          SendPluginEvent obstr.PLEAP_inst "Swipe right end" (strcatn (itoa handid)::" "::(ftoa speed)::" "::(ftoa progress)::nil) nil
        else if (dy <. -.0.6) then
          SendPluginEvent obstr.PLEAP_inst "Swipe down end" (strcatn (itoa handid)::" "::(ftoa speed)::" "::(ftoa progress)::nil) nil
        else if (dy >. 0.6) then
          SendPluginEvent obstr.PLEAP_inst "Swipe up end" (strcatn (itoa handid)::" "::(ftoa speed)::" "::(ftoa progress)::nil) nil
        else nil;
      );
      
      set obstr.PLEAP_lLastSwipeId = remove_idx_from_list obstr.PLEAP_lLastSwipeId handid;
    );
    
    set obstr.PLEAP_iLastSwipeTick = _tickcount;
  );
  0;;


fun cbKeyTapGesture(leapobj, obstr, handid, pointid, dir, pos)=
  let pos -> [x y z] in
  let dir -> [dx dy dz] in
    SendPluginEvent obstr.PLEAP_inst "Tap" (strcatn (itoa handid)::" "::(itoa pointid)::"\n"::(ftoa x)::" "::(ftoa y)::" "::(ftoa z)::"\n"::(ftoa dx)::" "::(ftoa dy)::" "::(ftoa dz)::nil) nil;
  0;;


fun cbScreenTapGesture(leapobj, obstr, handid, pointid, dir, pos)=
  let pos -> [x y z] in
  let dir -> [dx dy dz] in
    SendPluginEvent obstr.PLEAP_inst "Screen tap" (strcatn (itoa handid)::" "::(itoa pointid)::"\n"::(ftoa x)::" "::(ftoa y)::" "::(ftoa z)::"\n"::(ftoa dx)::" "::(ftoa dy)::" "::(ftoa dz)::nil) nil;
  0;;


fun cbEnable(inst, from, action, param, reply, obstr)=
  if (obstr.PLEAP_device != nil) then nil else
  let _CRleapMotionDevice _channel @cbConnected obstr @cbDisconnected obstr -> leapobj in
    set obstr.PLEAP_device = leapobj;
  
  _SetleapMotionHmdOptimization obstr.PLEAP_device obstr.PLEAP_bTopView;
  _CBLeapMotionCircleGesture obstr.PLEAP_device @cbCircleGesture obstr;
  _CBLeapMotionSwipeGesture obstr.PLEAP_device @cbSwipeGesture obstr;
  _CBLeapMotionKeyTapGesture obstr.PLEAP_device @cbKeyTapGesture obstr;
  _CBLeapMotionScreenTapGesture obstr.PLEAP_device @cbScreenTapGesture obstr;
  0;;


fun cbDisable(inst, from, action, param, reply, obstr)=
  setPluginInstanceCbScenePreRenderPhysic obstr.PLEAP_inst nil;
  _DSleapMotionDevice obstr.PLEAP_device;
  set obstr.PLEAP_device = nil;
  set obstr.PLEAP_lLastCircleId = nil;
  set obstr.PLEAP_lLastSwipeId = nil;
  0;;


fun cbMirror(inst, from, action, param, reply, obstr)=
  set obstr.PLEAP_bMirror = atoi param;
  0;;


fun cbTopView(inst, from, action, param, reply, obstr)=
  set obstr.PLEAP_bTopView = atoi param;
  _SetleapMotionHmdOptimization obstr.PLEAP_device obstr.PLEAP_bTopView;
  0;;


fun cbGrabMode(inst, from, action, param, reply, obstr)=
  set obstr.PLEAP_bPhysicGrab = atoi param;
  0;;


fun cbSetOffset(inst, from, action, param, reply, obstr)=
  if (!strcmp "" (strtrim param)) || param == nil then nil else
  let strextr param -> lp in
  let [(atof (nth_list (hd lp) 0)) (atof (nth_list (hd lp) 1))] -> [dx dy] in 
  let obstr.PLEAP_vOffset -> [ox oy] in
    set obstr.PLEAP_vOffset = [dx if (dy == nil) then oy else dy];
  0;;


fun loadTools(inst, obstr)=
  let (getPluginInstanceParam inst "firstTool") -> objlname in
  let (getPluginInstanceParam inst "secondTool") -> objrname in
  let atoi (getPluginInstanceParam inst "firstToolAutoHide") -> objtlhide in
  let atoi (getPluginInstanceParam inst "secondToolAutoHide") -> objtrhide in
  let if objtlhide == nil then 1 else objtlhide -> objtlhide in
  let if objtrhide == nil then 1 else objtrhide -> objtrhide in

  let V3DgetObjectByName c3dXsession objlname -> fatherl in
  let V3DgetObjectTypeByName objlname -> iobjlmode in
  let SO3ObjectGetPosition fatherl -> clpos in
  let SO3ObjectGetOrientation fatherl -> clquat in
  let SO3ObjectGetGlobalOrientation fatherl -> glquat in
  
  let V3DgetObjectByName c3dXsession objrname -> fatherr in
  let V3DgetObjectTypeByName objrname -> iobjrmode in
  let SO3ObjectGetPosition fatherr -> crpos in
  let SO3ObjectGetOrientation fatherr -> crquat in
  let SO3ObjectGetGlobalOrientation fatherr -> grquat in
  
  let SO3ObjectGetChildren fatherl -> lchild in
  let SO3ObjectGetChildren fatherr -> rchild in
  let mkObjPointableStr [fatherl nil nil iobjlmode [clpos clquat glquat nil] nil objtlhide 0] -> ltoolstr in
  let mkObjPointableStr [fatherr nil nil iobjrmode [crpos crquat grquat nil] nil objtrhide 0] -> rtoolstr in
  (
    if (!ltoolstr.PPOINT_bAutoHide) then nil else
    (
      SO3ObjectSetVisible fatherl 0 1;
      V3DphysEnableBody SO3SceneNodeGetBody fatherl 0;  
    );    

    let SO3BodyGetMaterial SO3SceneNodeGetBody fatherl -> mat1 in
    let V3DgetAvatarMaterial c3dXsession -> mat2 in
    if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
      SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;   
    
    if (!rtoolstr.PPOINT_bAutoHide) then nil else
    (
      SO3ObjectSetVisible fatherr 0 1;
      V3DphysEnableBody SO3SceneNodeGetBody fatherr 0; 
    );    
    
    let SO3BodyGetMaterial SO3SceneNodeGetBody fatherr -> mat1 in
    let V3DgetAvatarMaterial c3dXsession -> mat2 in
    if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
      SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;

    set obstr.PLEAP_lTools = ltoolstr::rtoolstr::nil;
  );
  0;;
    

fun loadBone(handstr, npstr, ph)=
  let SO3SceneNodeGetBody handstr.PHAND_obj -> hbody in
  let SO3SceneNodeGetBody npstr.PPOINT_obj -> bbody in
  let SO3BodyGetMassMatrix hbody -> [mass _] in
  if (hbody == nil) then nil else
    if (bbody != nil) then nil else
    let if ((SO3ObjectGetParent npstr.PPOINT_obj) == nil) then handstr.PHAND_obj else (SO3ObjectGetParent npstr.PPOINT_obj) -> parent in
    let SO3SceneNodeGetBody parent -> pbody in
    (
      // search for a parent with physics
      if (pbody != nil) then nil else
      (
        while ((pbody == nil) && ((SO3ObjectGetParent parent) != nil)) do
        (
          set parent = SO3ObjectGetParent parent;
          set pbody = SO3SceneNodeGetBody parent;    
        );
        
        if (pbody != nil) then nil else
        (
          set parent = handstr.PHAND_obj;
          set pbody = SO3SceneNodeGetBody parent;
        ); 
      );
      
      let npstr.PPOINT_initPos -> [ipos iquat gquat _] in
      let hd SO3ObjectGetChildren npstr.PPOINT_obj -> son in
      let SO3ObjectGetGlobalPosition npstr.PPOINT_obj -> gpos in 
      let if (son == nil) then SO3ObjectGetGlobalPosition SO3ObjectGetParent npstr.PPOINT_obj else SO3ObjectGetGlobalPosition son -> ppos in
      let SO3ObjectGetGlobalOrientation npstr.PPOINT_obj -> gquat in
      let SO3MathsQuatGetDirection (quatInverse gquat) if (son == nil) then subVectorF gpos ppos else subVectorF ppos gpos -> ldir in
      
      let SO3ObjectGetOrientation npstr.PPOINT_obj -> lquat in      
      let ldir -> [ldx ldy ldz] in
      let [(absf ldx) /. 2.0 (absf ldy) /. 2.0 (absf ldz) /. 2.0] -> [ldx ldy ldz] in
      let [0.01 0.01 0.015] -> [mx my mz] in
      let [(maxf ldx mx) (maxf ldy my) (maxf ldz mz)] -> lpos in
      let lpos -> ssize in
      let multiplyVectorF ldir [0.5 0.5 0.5] -> soffset in
      (
        if (mass >=. 10.0) then nil else
        (
          SO3BodySetMass hbody 10.0;
          set mass = 10.0;
        );
        
        set npstr.PPOINT_body = SO3BodyCreateEllipsoidExt npstr.PPOINT_obj ssize soffset nil;
        SO3BodySetMass npstr.PPOINT_body mass /. 15.0;
        SO3BodySetMaterial npstr.PPOINT_body SO3BodyGetMaterial hbody;
        //SO3BodySetContiniousCollisionMode npstr.PPOINT_body 1;
        SO3BodySetCenterOfMass npstr.PPOINT_body [0.0 0.0 0.0];
        //SO3BodySetPositionOrientation npstr.PPOINT_body [0.0 0.0 (dist /. 2.0)] [0.0 0.0 0.0 1.0];
        //SO3BodySetCenterOfMass npstr.PPOINT_body [0.0 0.0 (-.dist /. 2.0)];
        
        set handstr.PHAND_fMass = handstr.PHAND_fMass +. mass /. 15.0;
        
        let SO3BodyGetMaterial pbody -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
        (
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat1 0;
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
        );
        
        let SO3ObjectGetGlobalPosition npstr.PPOINT_obj -> pos in
        let SO3MathsQuatGetDirection (SO3ObjectGetOrientation npstr.PPOINT_obj) [1.0 0.0 0.0] -> dir in
        (
          mutate npstr.PPOINT_initPos <- [_ _ _ (normalizeVectorF SO3MathsQuatGetDirection quatInverse (SO3ObjectGetGlobalOrientation handstr.PHAND_obj) (subVectorF gpos ppos))];
          set npstr.PPOINT_joint = if ph then SO3PhysicsContraintCreateHinge (V3DgetSession c3dXsession) npstr.PPOINT_body pbody pos dir 1 else SO3PhysicsContraintCreateBallAndSocket (V3DgetSession c3dXsession) npstr.PPOINT_body pbody pos 1;
          
          SO3PhysicsContraintSetHingeLimits npstr.PPOINT_joint (SO3MathsDegreeToRadian 4.0) (SO3MathsDegreeToRadian 85.0);
          SO3PhysicsContraintSetHingeMotorEnable npstr.PPOINT_joint 0;
          SO3PhysicsContraintSetBallAndSocketControlled npstr.PPOINT_joint 1;
          SO3PhysicsContraintSetBallAndSocketAngularVelocity npstr.PPOINT_joint 60.0;
          //SO3PhysicsContraintSetBallAndSocketLimits npstr.PPOINT_joint (SO3MathsDegreeToRadian 60.0) (SO3MathsDegreeToRadian -.60.0) (SO3MathsDegreeToRadian 60.0);
          SO3PhysicsContraintSetStiffness npstr.PPOINT_joint 1.0;
          
          SO3PhysicsContraintSetCollisionState npstr.PPOINT_joint 0;
        );
      );
    );
  0;;


fun loadPhalanges(handstr, pstr, obj, stage)=
  let SO3ObjectGetChildren obj -> lchild in
  let SO3ObjectGetGlobalPosition handstr.PHAND_obj -> ghpos in
  let hd lchild -> bone in
  if bone == nil then nil else
  (
    let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil handstr.PHAND_bAutoHide 0] -> npstr in
    let SO3SceneNodeGetBody bone -> body in
    (
      let SO3BodyGetMaterial body -> mat1 in
      let V3DgetAvatarMaterial c3dXsession -> mat2 in
      if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
        SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
      
      loadBone handstr npstr stage;
      
      set pstr.PPOINT_lPhalanges = lcat pstr.PPOINT_lPhalanges npstr::nil;
      loadPhalanges handstr pstr bone stage + 1;
    );
  );
  0;;


fun loadHands(inst, obstr)=
  let (getPluginInstanceParam inst "firstHand") -> objlname in
  let (getPluginInstanceParam inst "secondHand") -> objrname in

  let atoi (getPluginInstanceParam inst "firstHandAutoHide") -> objlhide in
  let atoi (getPluginInstanceParam inst "secondHandAutoHide") -> objrhide in
  let if objlhide == nil then 1 else objlhide -> objlhide in
  let if objrhide == nil then 1 else objrhide -> objrhide in

  let V3DgetObjectByName c3dXsession objlname -> fatherl in
  let V3DgetObjectTypeByName objlname -> iobjlmode in
  let SO3ObjectGetPosition fatherl -> clpos in
  let SO3ObjectGetOrientation fatherl -> clquat in
  
  let V3DgetObjectByName c3dXsession objrname -> fatherr in
  let V3DgetObjectTypeByName objrname -> iobjrmode in
  let SO3ObjectGetPosition fatherr -> crpos in
  let SO3ObjectGetOrientation fatherr -> crquat in
    
  let SO3ObjectGetChildren fatherl -> lchild in
  let SO3ObjectGetChildren fatherr -> rchild in
  let SO3BodyGetMassMatrix SO3SceneNodeGetBody fatherl -> [lmass _] in 
  let SO3BodyGetMassMatrix SO3SceneNodeGetBody fatherr -> [rmass _] in
  let mkObjhandStr [fatherl iobjlmode [clpos clquat SO3ObjectGetGlobalOrientation fatherl] nil 0 0 0 objlhide nil nil nil lmass nil 0] -> lhandstr in
  let mkObjhandStr [fatherr iobjrmode [crpos crquat SO3ObjectGetGlobalOrientation fatherr] nil 0 0 0 objrhide nil nil nil rmass nil 0] -> rhandstr in
  (
    if (!lhandstr.PHAND_bAutoHide) then nil else
    (
      SO3ObjectSetVisible fatherl 0 1;
      V3DphysEnableBody SO3SceneNodeGetBody fatherl 0; 
    );

    let SO3BodyGetMaterial SO3SceneNodeGetBody fatherl -> mat1 in
    let V3DgetAvatarMaterial c3dXsession -> mat2 in
    if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
      SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;    
   
    if (!obstr.PLEAP_bSonsAsFingers) then
    (
      let (getPluginInstanceParam inst "lThumb") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil lhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!lhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
        
        loadBone lhandstr pstr 0;
        
        set lhandstr.PHAND_lFingers = lcat lhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges lhandstr pstr pstr.PPOINT_obj 1;
      );
      
      let (getPluginInstanceParam inst "lIndex") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil lhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!lhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
        
        loadBone lhandstr pstr 0;
        
        set lhandstr.PHAND_lFingers = lcat lhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges lhandstr pstr pstr.PPOINT_obj 1;
      );
      
      let (getPluginInstanceParam inst "lMiddle") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil lhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!lhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
        
        loadBone lhandstr pstr 0;
        
        set lhandstr.PHAND_lFingers = lcat lhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges lhandstr pstr pstr.PPOINT_obj 1;
      );
      
      let (getPluginInstanceParam inst "lRing") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil lhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!lhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
        
        loadBone lhandstr pstr 0;
        
        set lhandstr.PHAND_lFingers = lcat lhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges lhandstr pstr pstr.PPOINT_obj 1;
      );
      
      let (getPluginInstanceParam inst "lPinky") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil lhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!lhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
        
        loadBone lhandstr pstr 0;
        
        set lhandstr.PHAND_lFingers = lcat lhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges lhandstr pstr pstr.PPOINT_obj 1;
      );
      0;
    )
    else
    (
      let 0 -> i in
      let sizelist lchild -> size in
      while (i < size) do
      (
        let nth_list lchild i -> bone in
        let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil lhandstr.PHAND_bAutoHide 0] -> pstr in
        let SO3SceneNodeGetBody bone -> body in
        (          
          let SO3BodyGetMaterial body -> mat1 in
          let V3DgetAvatarMaterial c3dXsession -> mat2 in
          if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
            SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
          
          loadBone lhandstr pstr 0;
          
          if (!lhandstr.PHAND_bAutoHide) then nil else
          (
            SO3ObjectSetVisible bone 0 1;
            V3DphysEnableBody body 0; 
          );
          
          set lhandstr.PHAND_lFingers = lcat lhandstr.PHAND_lFingers pstr::nil;
          loadPhalanges lhandstr pstr pstr.PPOINT_obj 1;
        );
        
        set i = i + 1;
      );
      0;
    );
    
    if (!rhandstr.PHAND_bAutoHide) then nil else
    (
      SO3ObjectSetVisible fatherr 0 1;
      V3DphysEnableBody SO3SceneNodeGetBody fatherr 0;
    );
    
    let SO3BodyGetMaterial SO3SceneNodeGetBody fatherr -> mat1 in
    let V3DgetAvatarMaterial c3dXsession -> mat2 in
    if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
      SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
    
    if (!obstr.PLEAP_bSonsAsFingers) then
    (
      let (getPluginInstanceParam inst "rThumb") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil rhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!rhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
        
        loadBone rhandstr pstr 0;
        
        set rhandstr.PHAND_lFingers = lcat rhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges rhandstr pstr pstr.PPOINT_obj 1;
      );
      
      let (getPluginInstanceParam inst "rIndex") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil rhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!rhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
        
        loadBone rhandstr pstr 0;
        
        set rhandstr.PHAND_lFingers = lcat rhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges rhandstr pstr pstr.PPOINT_obj 1;
      );
      
      let (getPluginInstanceParam inst "rMiddle") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil rhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!rhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
        
        loadBone rhandstr pstr 0;
        
        set rhandstr.PHAND_lFingers = lcat rhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges rhandstr pstr pstr.PPOINT_obj 1;
      );
      
      let (getPluginInstanceParam inst "rRing") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil rhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!rhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
          
        loadBone rhandstr pstr 0;
        
        set rhandstr.PHAND_lFingers = lcat rhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges rhandstr pstr pstr.PPOINT_obj 1;
      );
      
      let (getPluginInstanceParam inst "rPinky") -> fobjname in
      let V3DgetObjectByName c3dXsession fobjname -> bone in
      let SO3SceneNodeGetBody bone -> body in
      let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil rhandstr.PHAND_bAutoHide 0] -> pstr in
      (
        if (!rhandstr.PHAND_bAutoHide) then nil else
        (
          V3DphysEnableBody body 0;
        );
        
        let SO3BodyGetMaterial body -> mat1 in
        let V3DgetAvatarMaterial c3dXsession -> mat2 in
        if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
          SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
          
        loadBone rhandstr pstr 0;
        
        set rhandstr.PHAND_lFingers = lcat rhandstr.PHAND_lFingers pstr::nil;
        loadPhalanges rhandstr pstr pstr.PPOINT_obj 1;
      );
      0;
    )
    else
    (
      let 0 -> i in
      let sizelist rchild -> size in
      while (i < size) do
      (
        let nth_list rchild i -> bone in
        let mkObjPointableStr [bone nil nil 0 [(SO3ObjectGetPosition bone) (SO3ObjectGetOrientation bone) (SO3ObjectGetGlobalOrientation bone) nil] nil rhandstr.PHAND_bAutoHide 0] -> pstr in
        let SO3SceneNodeGetBody bone -> body in
        (          
          let SO3BodyGetMaterial body -> mat1 in
          let V3DgetAvatarMaterial c3dXsession -> mat2 in
          if (mat1 == (V3DgetDefaultMaterial c3dXsession)) then nil else
            SO3PhysicsMaterialSetDefaultCollidable mat1 mat2 0;
          
          loadBone rhandstr pstr 0;
          
          if (!rhandstr.PHAND_bAutoHide) then nil else
          (
            SO3ObjectSetVisible bone 0 1;
            V3DphysEnableBody body 0;
          );
          
          set rhandstr.PHAND_lFingers = lcat rhandstr.PHAND_lFingers pstr::nil;
          loadPhalanges rhandstr pstr pstr.PPOINT_obj 1;
        );
        
        set i = i + 1;
      );
      0;
    );
    
    set obstr.PLEAP_lHands = lhandstr::rhandstr::nil;
  );
  0;;


fun newOb(inst)=
  let atoi(getPluginInstanceParam inst "init") -> init in
  let atof(getPluginInstanceParam inst "yoffset") -> yoffset in
  let if yoffset == nil then (-.0.20) else yoffset -> yoffset in
  let atof(getPluginInstanceParam inst "zoffset") -> zoffset in
  let if zoffset == nil then (-.0.20) else zoffset -> zoffset in
  let atof (getPluginInstanceParam inst "scaleX") -> ax in
  let atof (getPluginInstanceParam inst "scaleY") -> ay in
  let atof (getPluginInstanceParam inst "scaleZ") -> az in
  let if ax == nil then 1.0 else ax -> ax in
  let if ay == nil then 1.0 else ay -> ay in
  let if az == nil then 1.0 else az -> az in
   
  let atoi(getPluginInstanceParam inst "fingers") -> fingers in
  let if fingers == nil then 1 else fingers -> fingers in    
  let atoi(getPluginInstanceParam inst "mirrorx") -> mirrorx in
  let if mirrorx == nil then 0 else mirrorx -> mirrorx in
  let atoi(getPluginInstanceParam inst "topview") -> topview in
  let if topview == nil then 0 else topview -> topview in
  let atoi(getPluginInstanceParam inst "grab") -> grab in
  let if grab == nil then 0 else grab -> grab in
  let atoi(getPluginInstanceParam inst "imode") -> imode in
  let if imode == nil then 1 else imode -> imode in
  let (getPluginInstanceParam inst "objRef") -> objref in
  let V3DgetObjectByName c3dXsession objref -> fatherref in
  
  //disable not needed events in player
  let (IsInEditor inst) || IsEventLinked inst "First hand screen pos" -> blscreenpos in
  let (IsInEditor inst) || IsEventLinked inst "Second hand screen pos" -> brscreenpos in
  let (IsInEditor inst) || IsEventLinked inst "First hand position" -> blhpos in
  let (IsInEditor inst) || IsEventLinked inst "Second hand position" -> brhpos in
  let (IsInEditor inst) || IsEventLinked inst "First hand velocity" -> blhvel in
  let (IsInEditor inst) || IsEventLinked inst "Second hand velocity" -> brhvel in
  let (IsInEditor inst) || IsEventLinked inst "First tool position" -> bltpos in
  let (IsInEditor inst) || IsEventLinked inst "Second tool position" -> brtpos in
  let (IsInEditor inst) || IsEventLinked inst "First tool velocity" -> bltvel in
  let (IsInEditor inst) || IsEventLinked inst "Second tool velocity" -> brtvel in
  let (IsInEditor inst) || IsEventLinked inst "Points distance" -> bpdist in
  let (IsInEditor inst) || IsEventLinked inst "Points rotation" -> bprot in
  
  let mkObjleapStr [inst fatherref nil nil nil [yoffset zoffset] [ax ay az] imode fingers mirrorx topview grab nil nil 0 0 blscreenpos brscreenpos blhpos brhpos blhvel brhvel bltpos brtpos bltvel brtvel bpdist bprot] -> obstr in
  (
    loadHands inst obstr;
    loadTools inst obstr;
    
    PluginRegisterAction inst "Enable" mkfun6 @cbEnable obstr;
    PluginRegisterAction inst "Disable" mkfun6 @cbDisable obstr;
    PluginRegisterAction inst "Set mirror mode"  mkfun6 @cbMirror obstr;
    PluginRegisterAction inst "Set top view mode"  mkfun6 @cbTopView obstr;
    PluginRegisterAction inst "Set pinch grab mode"  mkfun6 @cbGrabMode obstr;
    PluginRegisterAction inst "Set offset"  mkfun6 @cbSetOffset obstr;
    setPluginInstanceCbDel inst mkfun2 @deleteOb obstr;

    if (!init) then nil else
      cbEnable inst nil nil nil nil obstr;
  );
  0;;


fun IniPlug(file)=
  PlugRegister @newOb nil;
  setPluginEditor @dynamicedit;
  0;;