/* A real iterative program in CH for RCCC mechanism
   created by H. H. Cheng and S. Thompson 

   The algorithm is described in 
   Uicker Jr., J. J., Denavit, J., Hartenberg, R. S.
   "An Iterative Method for the Displacement Analysis of Spatial Mechanisms",
   Journal of Applied Mechanics, June 1964, pp. 309-314.
*/

#include <linkage.h>     /* PI is defined in <linkage.h> */
#define D2R PI/180
#define R2D 180/PI
main(){
  int i, j, k, n=6, m=9, theta1;
  float alpha_a[8], theta_s[8], epsilon = 0.00001 ,MaxError= 1000000, error;
  array float v[m],BB[m][6],d[6],Bi[4][4],B1[4][4],Ai[4][4],Q[4][4];
  void a_i(float alpha,a,theta,s;array float A[:][:]) {                      /*(5)*/
     A[0][0] = 1; A[0][1] = 0; A[0][2] = 0; A[0][3] = 0;
     A[1][0] = a*cos(theta)            A[1][1] = cos(theta);
     A[1][2] = -sin(theta)*cos(alpha); A[1][3] = sin(theta)*sin(alpha)   
     A[2][0] = a*sin(theta)            A[2][1] = sin(theta); 
     A[2][2] = cos(theta)*cos(alpha);  A[2][3] = -cos(theta)*sin(alpha);
     A[3][0] = s; A[3][1] = 0;A[3][2] = sin(alpha); A[3][3] = cos(alpha); 
  }

  alpha_a[0]=30*D2R;  alpha_a[1]=2; alpha_a[2]=(55*D2R);  alpha_a[3]=4;
  alpha_a[4]=45*D2R;  alpha_a[5]=3; alpha_a[6]=(60*D2R);  alpha_a[7]=5; /* link parameters */
  theta_s[0]=0;       theta_s[1]=0; theta_s[2]=(100*D2R); theta_s[3]=0;
  theta_s[4]=100*D2R; theta_s[5]=0; theta_s[6]=(100*D2R); theta_s[7]=0; /* initial values */
  printf("  theta1     s1    theta2      s2     "
         "theta3     s3    theta4      s4\n");                  /* print heading */
  for(theta1=0;theta1<=360;theta1+=90) {                        /* 90 deg step for theta1 */
    theta_s[0] = theta1*D2R;                                    /* change deg to rad */
    do{
      for (k=0,i=2;i<=(n+1);i++;) { 
        Q=0;
        if (i%2==0){    /* Q used for rotational displacements */            /*(13)*/
          Q[1][2] = -1; 
          Q[2][1] = +1;
          k++;
        } 
        else 
          Q[3][0] = 1;  /* Q used for translational displacements */         /*(46)*/
        for (Bi=0,j=0;j<=3;j++) Bi[j][j] = 1; /* initialize to I matrix */
        for (j=0;j<=k*2-2;j+=2,Bi*=Ai)  
           a_i(alpha_a[j],alpha_a[j+1],theta_s[j],theta_s[j+1],Ai);          /*(18a)&(48)*/
        for (Bi*=Q,j=k*2;j<=n;j+=2,Bi*=Ai) 
           a_i(alpha_a[j],alpha_a[j+1],theta_s[j],theta_s[j+1],Ai);          /*(18a)&(48)*/
        BB[0][i-2] = Bi[1][0]; BB[1][i-2] = Bi[2][0]; BB[2][i-2] = Bi[3][0]; /*(39)*/
        BB[3][i-2] = Bi[1][1]; BB[4][i-2] = Bi[2][1]; BB[5][i-2] = Bi[3][1]; /*(39)*/
        BB[6][i-2] = Bi[2][2]; BB[7][i-2] = Bi[3][2]; BB[8][i-2] = Bi[3][3]; /*(40)*/
      }
      a_i(alpha_a[0],alpha_a[1],theta_s[0],theta_s[1],B1)                    /*(17a)*/
      for(i=2;i<=n;i+=2;B1*=Ai) 
         a_i(alpha_a[i],alpha_a[i+1],theta_s[i],theta_s[i+1],Ai);            /*(17a)*/
      v[0]= -B1[1][0]; v[1]=-B1[2][0]; v[2]= -B1[3][0];                      /*(39)*/
      v[3]=1-B1[1][1]; v[4]=-B1[2][1]; v[5]= -B1[3][1];                      /*(39)*/
      v[6]=1-B1[2][2]; v[7]=-B1[3][2]; v[8]=1-B1[3][3];                      /*(40)*/
      d = linearsolver(transpose(BB)*BB, transpose(BB)*v);                   /*(41)*/
      for (error=0,i=0;i<=n-1;i++) error += abs(d[i]);                       /*(52)*/
      for (i=2;i<=n+1;i++)  theta_s[i] += d[i-2]; /* update Theta and S */   /*(7) */
    }while ( error >= epsilon && error < MaxError); 
    if(error >= MaxError)                   /* diverged */
      printf("Error: the iterative algorithm diverges at theta1 = %d \n", theta1);
    else                                    /* converged, print results */
      printf("%8.2f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f\n",
      theta_s[0]*R2D,theta_s[1],theta_s[2]*R2D,theta_s[3],theta_s[4]*R2D,
      theta_s[5],theta_s[6]*R2D,theta_s[7]);
  }
}
/****  The output from the above program
  theta1     s1    theta2      s2     theta3     s3    theta4      s4
    0.00    0.000  149.679   -0.210   45.556   -2.693  144.209   -0.115
   90.00    0.000   54.512   -3.171   92.715   -1.513   81.114   -2.114
  180.00    0.000  -59.094   -0.301  142.648   -1.814   83.700   -0.173
  270.00    0.000 -157.692    1.136   92.714   -1.513  148.494   -0.515
  360.00    0.000 -210.320   -0.210   45.556   -2.693  144.209   -0.115
****/