#debug "\n\nStarting IK Finger...\n\n"

//Some containers for numbers
#declare Length=array[3]{2,2,2}
#declare Rotate=array[3]{0,0,0}
#declare MaxAng=array[3]{90,90,90}
#declare Tip   =array[3]{<0,0,0>, <0,0,0>, <0,0,0>}
#declare Torque=array[3]{0,0,0}
#declare Right =array[3]{<0,0,0>, <0,0,0>, <0,0,0>}
#declare Force =array[3]{<0,0,0>, <0,0,0>, <0,0,0>}

#declare Delta=1;
#declare TargetBailout=0.5;
#declare TorqueBailout=0.001;
#declare MaxIterations=200;
#declare TargetReached=no;


//Some tools
#macro ShowVector(V1, V2)
  cone{V1, 0.05, V2, 0.0 texture{pigment{color rgb<1,1,0>}}}
#end

#macro Vector2Text(V)
  vstr(3,V,", ",4,2)
#end

#macro Number2Text(N)
  str(N,7,4)
#end

#declare Target=
  union{
    cone{0.06*x, 0, 1*x, 0.2}
    cone{0.06*x, 0, 1*x, 0.2 rotate 90*z}
    cone{0.06*x, 0, 1*x, 0.2 rotate 180*z}
    cone{0.06*x, 0, 1*x, 0.2 rotate 270*z}
    sphere{0, TargetBailout}
    texture{pigment{color rgbt<0,1,1,0.5>}}
  }

#macro Joint(length)
  merge{
    sphere{0, 0.2}
    cone{0, 0.2, length*x, 0.05}
    sphere{length*x, 0.05}
    cylinder{0, 50*x, 0.01}
  }
#end

#if (clock_on)
  #declare current_frame=(clock/clock_delta)+1;
#else
  #declare current_frame=1;
#end
  #debug concat("\n ******* ", Number2Text(current_frame), " *******\n")

#declare Joint0=object{Joint(Length[0]) texture{pigment{color
rgbt<1,0,0,0.5>}}}
#declare Joint1=object{Joint(Length[1]) texture{pigment{color
rgbt<0,1,0,0.5>}}}
#declare Joint2=object{Joint(Length[2]) texture{pigment{color
rgbt<0,0,1,0.5>}}}



//Calculates positions of each tip, and if the last one is close to the
target,
//  sets a variable to say so.
#macro CalcTipsFK()
  #declare Tip[0]=vrotate(Length[0]*x, Rotate[0]*z);
  #declare Tip[1]=Tip[0]+vrotate(Length[1]*x, (Rotate[0]+Rotate[1])*z);
  #declare Tip[2]=Tip[1]+vrotate(Length[2]*x,
(Rotate[0]+Rotate[1]+Rotate[2])*z);

  //object{ShowVector(Tip[0], Tip[1])}
  //object{ShowVector(Tip[1], Tip[2])}

  #if (vlength(TargetLocation-Tip[2])<TargetBailout)
    #declare TargetReached=yes;
    #debug "\n**Target Reached"
  #else
    #declare TargetReached=no;
  #end
#end


#declare TargetLocation=<2.5,3,0>;


//rotate the joint to point at the target
#macro ReorientTip2()
  CalcTipsFK()
  #declare Torque[2]=3*Delta;
  #while (Torque[2]>TorqueBailout)
    #if (TargetReached)
      #declare Torque[2]=0;
      #debug "TR"
    #else
      CalcTipsFK()
      #declare Right[2]=vrotate(<0,1,0>, (Rotate[0]+Rotate[1]+Rotate[2])*z);
      #declare Force[2]=TargetLocation-Tip[1];
      #declare Torque[2]=vdot(Right[2], Force[2])*Delta;
      #declare Rotate[2]=Rotate[2]+Torque[2];
      //#debug concat("\n  Torque[2]: ", Number2Text(Torque[2]/Delta))
    #end
  #end
#end



#macro ReorientTip1once()
  CalcTipsFK()
  #declare Right[1]=vrotate(<0,1,0>, (Rotate[0]+Rotate[1])*z);
  #declare Force[1]=TargetLocation-Tip[0];
  #declare Torque[1]=vdot(Right[1], Force[1])*Delta;
  #declare Rotate[1]=Rotate[1]+Torque[1];
  //#debug concat("\n  Torque[1]: ", Number2Text(Torque[1]))
#end


#declare Rotate[1]=90*clock;

  ReorientTip2()


/*
#declare i=0; #while (i<5)
  CalcTipsFK()
  ReorientTip1once()
  CalcTipsFK()
  ReorientTip2()
#declare i=i+1; #end
*/


CalcTipsFK()




object{Target translate TargetLocation}
object{Joint0 rotate (Rotate[0])*z}
object{Joint1 rotate (Rotate[0]+Rotate[1])*z translate Tip[0]}
object{Joint2 rotate (Rotate[0]+Rotate[1]+Rotate[2])*z translate Tip[1]}


#declare Camera_Location=<3, 3, -20>;
#declare Camera_Target=<3,3,0>;
#declare Camera_Angle=25;
#declare Use_AreaLights=no;
#declare Use_Rad=no;
#declare Ground_Height=-1;
#declare Draw_Sky=no;
#declare Draw_Ground=no;


#include "robot-testrig.inc"

#debug "\n\nEnd\n\n"

Post a reply to this message