import solids;
import tube;
import graph3;
import palette;
size(8cm);

currentprojection=
  perspective(camera=(13.3596389245356,8.01038090435314,14.4864483364785),
              up=(-0.0207054323419367,-0.00472438375047319,0.0236460907598947),
              target=(-1.06042550499095,2.68154529985845,0.795007562120261));

defaultpen(fontsize(6pt));

// draw coordinates and frames
// axis1 is defined by z axis of TBase
// axis2 is defined by z axis of TEnd
void DrawFrame(transform3 TBase, transform3 TEnd, string s)
{
  triple p1,v1,p2,v2;
  p1=TBase*O;
  v1=TBase*Z-p1;
  p2=TEnd*O;
  v2=TEnd*Z-p2;
  triple n=cross(v1,v2);

  real[][] A=
    {
     {v1.x,-v2.x,-n.x},
     {v1.y,-v2.y,-n.y},
     {v1.z,-v2.z,-n.z}
    };

  triple vb=p2-p1;

  real[] b={vb.x,vb.y,vb.z};

  // Get the extention along vector v1 and v2,
  // so, we can get the common normal between two axis
  real[] x=solve(A,b);

  real s1=x[0];
  real s2=x[1];

  // get foot of a perpendicular on both axies
  triple foot1=p1+s1*v1;
  triple foot2=p2+s2*v2;

  // draw two axis
  triple axis_a,axis_b;
  axis_a=p1+s1*v1*1.5;
  axis_b=p1-s1*v1*1.5;
  draw(axis_a--axis_b);

  axis_a=p2+s2*v2*1.5;
  axis_b=p2-s2*v2*1.5;
  draw(axis_a--axis_b);

  // draw "a"(common normal)
  draw(Label("$a_{"+s+"}$"),foot1--foot2,linewidth(1pt));

  // draw the coordinates frame
  triple dx,dy,dz,org;
  real length=0.8;

  org=foot1;
  dx =length*unit(foot2-foot1); // define the x axis of the frame on "a"
  dz =length*unit(v1);          // define the z axis which is along axis1
  dy =length*unit(cross(dz,dx));

  draw(Label("$X_{"+s+"}$",1,align=-dy-dz),org--(org+dx),red+linewidth(1.5pt),
       Arrow3(8));
  draw(Label("$Y_{"+s+"}$",1,align=2dy-dz-dx),org--(org+dy),
       green+linewidth(1.5pt),  Arrow3(8));
  draw(Label("$Z_{"+s+"}$",1,align=-2dx-dy),org--(org+dz),
       blue+linewidth(1.5pt),   Arrow3(8));

  dot(Label("$O_{"+s+"}$",align =-dx-dz,black),org,black); // origin

}

void DrawLink(transform3 TBase, transform3 TEnd, pen objStyle,string s)
{
  real h=1;
  real r=0.5;
  path3 generator=(0.5*r,0,h)--(r,0,h)--(r,0,0)--(0.5*r,0,0);
  revolution vase=revolution(O,generator,0,360);
  surface objSurface=surface(vase);

  render render=render(merge=true);

  // draw two cylinders
  draw(TBase*objSurface,objStyle,render);
  draw(TEnd*shift((0,0,-h+1e-5))*objSurface,objStyle,render);

  // draw the link between two cylinders
  triple pStart=TBase*(0.5*h*Z);
  triple pEnd  =TEnd*(-0.5*h*Z);
  triple pControl1=0.25*(pEnd-pStart)+TBase*(0,0,h);
  triple pControl2=-0.25*(pEnd-pStart)+TEnd*(0,0,-h);
  path3 p=pStart..controls pControl1 and pControl2..pEnd;
  draw(tube(p,scale(0.2)*unitsquare),objStyle,render);
}

// t1 and t2 define the starting frame and ending frame of the first link(i-1)
transform3 t1=shift((0,0,1));
transform3 t2=shift((0,0,-1))*rotate(-20,Y)*shift((0,3,2));
// as, the two links were connected, so t2 is also the starting frame of link(i)
// t3 defines the ending frame of link(i)
transform3 t3=t2*rotate(40,Z)*shift((0,3,1.5))*rotate(-15,Y)*shift(-1.5*Z);

// draw link(i-1)
DrawLink(t1,t2,palegreen,"i-1");
DrawFrame(t1,t2,"i-1");
// draw link(i)
DrawLink(t2,t3,lightmagenta,"i");
DrawFrame(t2,t3,"i");


// draw angle alpha, which is the angle between axis(i-1) and axis(i)
triple p0=(0,0,-1);
triple p1=(0,0,2.3);
triple p2=shift((0,0,-1))*rotate(-20,Y)*(0,0,4);
draw(p0--p2,cyan);
draw("$\alpha_{i-1}$",arc(p0,p1,p2,Y,CW),ArcArrow3(3));


// draw angle theta, which is the angle between a_i and a_{i-1}
transform3 tx=shift((0,0,-1))*rotate(-20,Y)*shift((0,3,0));
p0=tx*O;
p1=tx*(0,3,0);
p2=tx*rotate(40,Z)*(0,3,0);
draw(p0--p1,cyan);
draw(p0--p2,cyan);

triple p1a=tx*(0,1.5,0);
draw("$\theta_{i}$",arc(p0,p1a,p2),ArcArrow3(3));

// draw d_{i-1}
triple org_i   =t2*shift((0,0,1.5))*O;
draw(Label("$d_{i}$",0.13),p0--org_i,linewidth(1pt));