/*============= Mathematica program RCRCR.mff ==================
  <*
     (* Denavit-Hartenberg matrices *)
     T1={{C1,-S1,0},{S1,C1,0},{0,0,1}};                                  (* eq(2)  *)
     T2={{C2,-S2,0},{S2,C2,0},{0,0,1}};
     T3={{C3,-S3,0},{S3,C3,0},{0,0,1}};
     T4={{C4,-S4,0},{S4,C4,0},{0,0,1}};
     T5={{C5,-S5,0},{S5,C5,0},{0,0,1}};
     A1={{1,0,0},{0,Ca1,-Sa1},{0,Sa1,Ca1}};                              (* eq(3)  *)
     A2={{1,0,0},{0,Ca2,-Sa2},{0,Sa2,Ca2}};
     A3={{1,0,0},{0,Ca3,-Sa3},{0,Sa3,Ca3}};
     A4={{1,0,0},{0,Ca4,-Sa4},{0,Sa4,Ca4}};
     A5={{1,0,0},{0,Ca5,-Sa5},{0,Sa5,Ca5}};

     (* I/O loop-closure displacement equation *)
     IO=A1.T1.A5.T5.A4-Transpose[T4.A3.T3.A2.T2]                         (* eq(4)  *)
     eq1=Expand[IO[[1,1]]]
     eq2=Expand[IO[[1,2]]]
     eq3=Expand[IO[[1,3]]]
     eq4=Expand[IO[[2,1]]]
     eq5=Expand[IO[[2,2]]]
     eq6=Expand[IO[[2,3]]]
     eq7=Expand[IO[[3,1]]]
     eq8=Expand[IO[[3,2]]]
     eq9=Expand[IO[[3,3]]]

     (* Get dual coefficients for element (3,3) in eq(5) *) 
     BBB=Coefficient[eq9,C3,1];                                          (* eq(8)  *)
     CCC=Coefficient[eq9,S5,1];                                          (* eq(9)  *)
     DDD=Coefficient[eq9,C5,1];                                          (* eq(10) *)
     AAA=Expand[eq9-BBB*C3-CCC*S5-DDD*C5];                               (* eq(7)  *)

     (* Real and dual number substitution *) 
     AA=Ax+e*Ay; BB=Bx+e*By; CC=Cx+e*Cy; DD=Dx+e*Dy; 
     CC3=c3-e*d3*s3; SS5=s5+e*d5*c5; CC5=c5-e*d5*s5; 

     (* Separate real and dual parst of eq9 *) 
     eq9=Expand[AA+BB*CC3+CC*SS5+DD*CC5];                                (* eq(6) *)
     Req9=Coefficient[eq9,e,0];                                          (* eq(11) *)
     Deq9=Coefficient[eq9,e,1];                                          (* eq(12) *)

     (* Solve for c3 and s3 from Req9 and Deq9 *) 
     temp=Solve[Req9==0,c3]; 
     c3=c3 /. temp; c3=c3[[1]]; 
     temp=Solve[Deq9==0,s3]; 
     s3=s3 /. temp; s3=s3[[1]]; 

     (* Substitue half angle tangent for s5 and c5 *) 
     s5=2*X/(1+X*X); c5=(1-X*X)/(1+X*X);                                 (* eq(13) *)
     s3=Together[Expand[s3]]*(1+X*X);                                    (* eq(14) *)
     c3=Together[Expand[c3]]*(1+X*X);                                    (* eq(15) *)

     (* Get constants multiplied by X from s3 and c3 *)
     EEE=Together[Coefficient[s3,X,2]];                                  (* eq(18) *)
     FFF=Together[Coefficient[s3,X,1]];                                  (* eq(19) *)
     GGG=Together[Coefficient[s3,X,0]];                                  (* eq(20) *)
     HHH=Together[Coefficient[c3,X,2]];                                  (* eq(21) *)
     III=Together[Coefficient[c3,X,1]];                                  (* eq(22) *)
     JJJ=Together[Coefficient[c3,X,0]];                                  (* eq(23) *)

     (* Use c3^2 + s3^2 -1 = 0 to form a 4th order polynomial *)
     s3=(EE*X*X+FF*X+GG); c3=(HH*X*X+II*X+JJ);                           (* eq(16-17) *)
     poly=Together[Expand[c3*c3+s3*s3-(1+X*X)*(1+X*X)]];                 (* eq(25) *)
     p4=Together[Coefficient[poly,X,4]];                                 (* eq(26) *)
     p3=Together[Coefficient[poly,X,3]];                                 (* eq(27) *)
     p2=Together[Coefficient[poly,X,2]];                                 (* eq(28) *)
     p1=Together[Coefficient[poly,X,1]];                                 (* eq(29) *)
     p0=Together[Coefficient[poly,X,0]];                                 (* eq(40) *)

     (* Use element (1,3) and (2,3) to find theta2 *)
     temp=Coefficient[eq3,S2,0]; AA1=-Coefficient[temp,C2,0];            (* eq(35) *)
     CC1=Coefficient[eq3,S2,1];  DD1=Coefficient[eq3,C2,1];              (* eq(35) *)
     temp=Coefficient[eq6,S2,0]; BB1=-Coefficient[temp,C2,0];            (* eq(36) *)
     EE1=Coefficient[eq6,S2,1];  FF1=Coefficient[eq6,C2,1];              (* eq(36) *)

     (* Use element (3,1) and (3,2) to find theta4 *)
     temp=Coefficient[eq7,S4,0]; AA2=-Coefficient[temp,C4,0];            (* eq(40) *)
     CC2=Coefficient[eq7,S4,1];  DD2=Coefficient[eq7,C4,1];              (* eq(40) *)
     temp=Coefficient[eq8,S4,0]; BB2=-Coefficient[temp,C4,0];            (* eq(41) *)
     EE2=Coefficient[eq8,S4,1];  FF2=Coefficient[eq8,C4,1];              (* eq(41) *)
  *> 
===============================================================*/ 
void RCRCR(double dual a[5],double d[3],double dual theta1 
           double dual theta2[4],theta3[4],theta4[4],theta5[4]) 
{ 
  #define Power pow
  int j; 
  double t1,d1,d3,d5,X,EE,FF,GG,HH,II,JJ,
         SIN3,COS3, Ax,Ay,Bx,By,Cx,Cy,Dx,Dy; 
  double dual T1,T2,T3,T4,T5,a1,a2,a3,a4,a5,AA,BB,CC,DD,AA1,
              BB1,CC1,DD1,EE1,FF1,AA2,BB2,CC2,DD2,EE2,FF2; 
  double complex roots[4],p[5]; 

  T1=theta1; 
  a1=a[0]; a2=a[1]; a3=a[2]; a4=a[3]; a5=a[4]; 
  d1=d[0]; d3=d[1]; d5=d[2]; 
  t1=real(T1); 
  AA=<*AAA*>;  BB=<*BBB*>;                                               /* eq(7-8) */
  CC=<*CCC*>;  DD=<*DDD*>;                                               /* eq(9-10) */
  Ax=real(AA); Ay=dual(AA); Bx=real(BB); By=dual(BB);                    
  Cx=real(CC); Cy=dual(CC); Dx=real(DD); Dy=dual(DD);
  EE=<*EEE*>;                                                            /* eq(18) */
  FF=<*FFF*>;                                                            /* eq(19) */
  GG=<*GGG*>;                                                            /* eq(20) */
  HH=<*HHH*>; II=<*III*>;  JJ=<*JJJ*>;                                   /* eq(21-23) */ 
  p[4]=<*p4*>;                                                           /* eq(26) */
  p[3]=<*p3*>;                                                           /* eq(27) */
  p[2]=<*p2*>;                                                           /* eq(28) */
  p[1]=<*p1*>;                                                           /* eq(29) */
  p[0]=<*p0*>;                                                           /* eq(30) */
  zroots(p,4,roots,1);  /* solve polynomial equation */
  for(j=0;j<=3;j++) { 
    X=real(roots[j]);
    T5=dual(2*atan(X), d5);                                              /* eq(31) */
    SIN3=(<*s3*>)/(1+X*X);                                               /* eq(16) */
    COS3=(<*c3*>)/(1+X*X);                                               /* eq(17) */
    T3=dual(2*atan(SIN3/(1+COS3)), d3);                                  /* eq(32) */
    AA1=<*AA1*>; 
    BB1=<*BB1*>; 
    CC1=<*CC1*>; DD1=<*DD1*>; 
    EE1=<*EE1*>; FF1=<*FF1*>; 
    T2=2*atan((AA1*FF1-BB1*DD1)/(CC1*FF1-DD1*EE1+BB1*CC1-AA1*EE1));      /* eq(37) */
    AA2=<*AA2*>;
    BB2=<*BB2*>; 
    CC2=<*CC2*>; DD2=<*DD2*>; 
    EE2=<*EE2*>; FF2=<*FF2*>; 
    T4=2*atan((AA2*FF2-BB2*DD2)/(CC2*FF2-DD2*EE2+BB2*CC2-AA2*EE2));      /* eq(42) */
    theta2[j]=T2; theta3[j]=T3; theta4[j]=T4; theta5[j]=T5;
  } 
}