IK.C

Go to the documentation of this file.

/* file IK.C    for IK animation   */

#define MODULE_IK 1

#include "animate.h"


void AssignIKchain(node *Np, long frame){
  return;
}

void RemoveIKchain(node *Np, long frame){
  return;
}

void RemoveAllIKchains(node *Np, long frame){
  return;
}


#if 0
static void IterateTowardsGoal(HWND hWnd,int xi, int yi){
 double *J,*Jt,*dTheta,*I,*dI,*Ji;
 double x[3],dx[3];
 long Nv1,Nv2,n=0;
 if(Lv < 0)return;
 if(debug != NULL)fprintf(debug,"Test No. %ld\n",++version);
 Nv1=Nv-1;
 Nv1=Lv; 
 Nv2=Nv1-Sv;
 SaveLast();
 // Define the goal point for the end of the linkage
 x[0]=TVlist[Nv1].p[0]+(double)xi;
 x[1]=TVlist[Nv1].p[1]+(double)yi;
 x[2]=TVlist[Nv1].p[2]+(double)0.0;
 VECSUB(x,TVlist[Nv1].p,dx)
 if(debug != NULL)fprintf(debug,"Initial |dx| = %lf\n",Length(dx,3));
 if((dTheta=(double *)malloc((3*Nv2)*sizeof(double))) == NULL)return;
 if((J=(double *)malloc(3*(3*Nv2)*sizeof(double))) == NULL)return;
 if((Jt=(double *)malloc((3*Nv2)*3*sizeof(double))) == NULL)return;
 if((Ji=(double *)malloc((3*Nv2)*3*sizeof(double))) == NULL)return;
 if((dI=(double *)malloc(3*3*sizeof(double))) == NULL)return;
 if((I=(double *)malloc(3*3*sizeof(double))) == NULL)return;

 while(1){
   VECSUB(x,TVlist[Nv1].p,dx)
   if(++n > 50){
     if(debug != NULL)fprintf(debug,"*********** Out of iteration ******\n");
     // revert to last position (TODO)
     RevertToLast();
     MessageBeep(MB_OK);
     break;
   }
   CalculateJacobian(J,Nv2,Sv);
   CalculateTranspose(J,Jt,3,3*Nv2);
   MultiplyMatrix(J,3,3*Nv2,Jt,3*Nv2,3,I);
   if(InvertMatrix(I,3,dI)){
     // calculate the generalised inverse of the jacobian J (i.e. Ji)
     MultiplyMatrix(Jt,3*Nv2,3,dI,3,3,Ji);
     // StepOK returns a reduced dx so that the product [J][Ji] is
     // close to the identity matrix [I] and therefore can be used to
     // determine incremental angular rotations.
     if(debug != NULL)fprintf(debug,"Iteration %ld ",n);
     if(!StepOK(J,Ji,dx,3,3*Nv2,1.e-2,1.e-6)){
       MessageBox(NULL,"Dx too small",NULL,MB_OK);
       break;
     }
     else{
       // calculate the incremental rotations
       MultiplyMatrix(Ji,3*Nv2,3,dx,3,1,dTheta);
       // determine the new position of the linkage
       UpdateTheLinkage(Nv1,Nv,Sv,dTheta);
       if(Dist(TVlist[Nv1].p,x,3) < 3.0){ //
         if(debug != NULL)
           fprintf(debug,"converged in %ld iterations with |dx| = %lf\n",
                   n,Length(dx,3));
         break;   // converged to solution
       }
       else{
         if(debug != NULL)fprintf(debug,"linkage not yet at end point\n");
       }
     }
   }
   else{
     MessageBox(NULL,"Singular matrix",NULL,MB_OK);
   }
 }

 free(dTheta);
 free(Ji);
 free(I);
 free(dI);
 free(Jt);
 free(J);
 InvalidateRect(hWnd,NULL,FALSE);
 return;
}

static void CalculateJacobian(double *J,
                              long n,
                              long ns
                             ){
 long k,l;
 vector ax,ay,az,dp,Jx,Jy,Jz;
 for(l=0,k=0;k<n;k++){
   VECCOPY(TVlist[k+ns].ax,ax)
   VECCOPY(TVlist[k+ns].ay,ay)
   VECCOPY(TVlist[k+ns].az,az)
   VECSUB(TVlist[n+ns].p,TVlist[k+ns].p,dp)
   CROSS(ax,dp,Jx)
   CROSS(ay,dp,Jy)
   CROSS(az,dp,Jz)
   J[l]       = Jx[0];
   J[l+n*3]   = Jx[1];
   J[l+n*3*2] = Jx[2];
   l++;
   J[l]       = Jy[0];
   J[l+n*3]   = Jy[1];
   J[l+n*3*2] = Jy[2];
   l++;
   J[l]       = Jz[0];
   J[l+n*3]   = Jz[1];
   J[l+n*3*2] = Jz[2];
   l++;
 }
#if 0
 l=3*2;  // lock link 2
 for(k=l;k<l+3;k++){
   J[k]=0.0;
   J[k+n*3]=0.0;
   J[k+n*3*2]=0.0;
 }
#endif
#if 0
 for(l=0,k=0;k<n;k++){ // damping
   J[l]        *= (1.0+2.0*(double)(k));
   J[l+n*3]    *= (1.0+2.0*(double)(k));
   J[l+n*3*2]  *= (1.0+2.0*(double)(k));
   l++;
   J[l]        *= (1.0+2.0*(double)(k));
   J[l+n*3]    *= (1.0+2.0*(double)(k));
   J[l+n*3*2]  *= (1.0+2.0*(double)(k));
   l++;
   J[l]        *= (1.0+2.0*(double)(k));
   J[l+n*3]    *= (1.0+2.0*(double)(k));
   J[l+n*3*2]  *= (1.0+2.0*(double)(k));
   l++;
 }
#endif
 return;
}

static void UpdateTheLinkage(long Nv1, long Nv, long Ns, double *dTheta){
 long i,j,l;
 double thetaX,thetaY,thetaZ;
 vector ax,ay,az,baseZero={0.0,0.0,0.0};
 for(i=Ns,l=0;i<Nv1;i++,l+=3){
   // extract the incremental rotations and axis vectors around which
   // they are to be applied.
   thetaX = *(dTheta+l);
   thetaY = *(dTheta+l+1);
   thetaZ = *(dTheta+l+2);
   VECCOPY(TVlist[i].ax,ax)
   VECCOPY(TVlist[i].ay,ay)
   VECCOPY(TVlist[i].az,az)
   // apply the rotation to all the vertex locations and base vectors
   // further down the linkage (including the end point)
   for(j=i+1;j<Nv;j++){
     RotateVectorRound(TVlist[j].p,TVlist[i].p,ax,thetaX);
     RotateVectorRound(TVlist[j].p,TVlist[i].p,ay,thetaY);
     RotateVectorRound(TVlist[j].p,TVlist[i].p,az,thetaZ);
     RotateVectorRound(TVlist[j].ax,baseZero,ax,thetaX);
     RotateVectorRound(TVlist[j].ax,baseZero,ay,thetaY);
     RotateVectorRound(TVlist[j].ax,baseZero,az,thetaZ);
     RotateVectorRound(TVlist[j].ay,baseZero,ax,thetaX);
     RotateVectorRound(TVlist[j].ay,baseZero,ay,thetaY);
     RotateVectorRound(TVlist[j].ay,baseZero,az,thetaZ);
     RotateVectorRound(TVlist[j].az,baseZero,ax,thetaX);
     RotateVectorRound(TVlist[j].az,baseZero,ay,thetaY);
     RotateVectorRound(TVlist[j].az,baseZero,az,thetaZ);
     TVlist[j].x=(int)TVlist[j].p[0];
     TVlist[j].y=(int)TVlist[j].p[1];
     TVlist[j].z=(int)TVlist[j].p[2]; // should be 0 in case of pseudo 2D
   }
 }
 return;
}

static BOOL StepOK(double *J, double *Ji, double *dx,
                   long n, long m,
                   double e, double dmin){
 // n=3 for 3D Jacobian, m=(# linkages - 1)*3 (no angular constraints apply)
 BOOL result=TRUE;
 long i;
 double *I,*N,norm=0.0;
 if((I=(double *)malloc(n*n*sizeof(double))) == NULL)return FALSE;
 if((N=(double *)malloc(n*sizeof(double))) == NULL)return FALSE;
 MultiplyMatrix(J,n,m,Ji,m,n,I);
 if(debug != NULL){
   fprintf(debug,"\nInverse matrix\n");
   for(i=0;i<n;i++){
     long j;
     for(j=0;j<3;j++){
       fprintf(debug,"%lf ",I[i*n+j]);
     }
     fprintf(debug,"\n");
   }
 }
 for(i=0;i<n;i++){
   I[i*n+i] -= 1.0;
 }
 while(1){
   MultiplyMatrix(I,n,n,dx,n,1,N);
   norm=0.0;
   for(i=0;i<n;i++){
     norm += N[i] * N[i];
   }
   if(debug != NULL)fprintf(debug,"Norm = %lf\n",norm);
   if(norm < e)break;
   for(i=0;i<n;i++)dx[i] *= 0.5;
   if(Length(dx,n) < dmin){
      result=FALSE;
      break;
   }
 }
 free(N);
 free(I);
 return result;
}

static void SaveLast(void){
 long i;
 for(i=0;i<Nv;i++){ // make record of last positions
   TVlist[i].lx=TVlist[i].x;
   TVlist[i].ly=TVlist[i].y;
   TVlist[i].lz=TVlist[i].z;
   VECCOPY(TVlist[i].p,TVlist[i].lp)
   VECCOPY(TVlist[i].ax,TVlist[i].lax)
   VECCOPY(TVlist[i].ay,TVlist[i].lay)
   VECCOPY(TVlist[i].az,TVlist[i].laz)
 }
 return;
}

static void RevertToLast(void){
 long i;
 for(i=0;i<Nv;i++){ // make record of last positions
   TVlist[i].x=TVlist[i].lx;
   TVlist[i].y=TVlist[i].ly;
   TVlist[i].z=TVlist[i].lz;
   VECCOPY(TVlist[i].lp,TVlist[i].p)
   VECCOPY(TVlist[i].lax,TVlist[i].ax)
   VECCOPY(TVlist[i].lay,TVlist[i].ay)
   VECCOPY(TVlist[i].laz,TVlist[i].az)
 }
 return;
}

static void SetupStart(void){
 int i;
 vector x0={1.0,0.0,0.0},y0={0.0,1.0,0.0},z0={0.0,0.0,1.0};
 if(Nv < 2)return;
 for(i=0;i<Nv;i++){
   TVlist[i].p[0]=(double)TVlist[i].x;
   TVlist[i].p[1]=(double)TVlist[i].y;
   TVlist[i].p[2]=0.0;
   VECCOPY(x0,TVlist[i].ax)
   VECCOPY(y0,TVlist[i].ay)
   VECCOPY(z0,TVlist[i].az)
 }
}

static void RotateVectorRound(vector v,
                              vector BaseOfRotation,
                              vector AxisOfRotation,
                              double theta){
 // modify the input vector to apply the rotational transformation
 double t[4][4];
 vector lv,lvv;
 VECSUB(v,BaseOfRotation,lv) // remove
 rotate_round_vector(theta,AxisOfRotation,t);  // get rotational transform
 mv4by1(t,lv,lvv);  // apply it
 VECSUM(lvv,BaseOfRotation,v)
 return;
}

static void rotz(double tr[4][4], double ang){
 short i,j;
 for(i=0;i<4;i++)
 for(j=0;j<4;j++){
  tr[i][j]=0.0;
  if(i == j)tr[i][j]=1.0;
 }
  tr[0][0]= cos(ang);
  tr[0][1]=-sin(ang);
  tr[1][0]= sin(ang);
  tr[1][1]= cos(ang);
  return;
}

static void roty(double tr[4][4], double ang){
 short i,j;
 for(i=0;i<4;i++)
 for(j=0;j<4;j++){
  tr[i][j]=0.0;
  if(i == j)tr[i][j]=1.0;
 }
  tr[0][0]= cos(ang);
  tr[0][2]=-sin(ang);
  tr[2][0]= sin(ang);
  tr[2][2]= cos(ang);
  return;
}

static void rotx(double tr[4][4], double ang){
 long i,j;
 for(i=0;i<4;i++)
 for(j=0;j<4;j++){
  tr[i][j]=0.0;
  if(i == j)tr[i][j]=1.0;
 }
  tr[1][1]= cos(ang);
  tr[1][2]=-sin(ang);
  tr[2][1]= sin(ang);
  tr[2][2]= cos(ang);
  return;
}

static void mv4by1(double t4[4][4], vector v1, vector v2){
 double xx,yy,zz;
 xx=t4[0][0]*v1[0]+t4[0][1]*v1[1]+t4[0][2]*v1[2]+t4[0][3];
 yy=t4[1][0]*v1[0]+t4[1][1]*v1[1]+t4[1][2]*v1[2]+t4[1][3];
 zz=t4[2][0]*v1[0]+t4[2][1]*v1[1]+t4[2][2]*v1[2]+t4[2][3];
 v2[0]=xx; v2[1]=yy; v2[2]=zz;
 return;
}

static void m4by4(double t1[4][4], double t2[4][4], double tr[4][4]){
 short n=4,i,j,k;
 for(i=0;i<4;i++)
 for(j=0;j<4;j++){
  tr[i][j]=0.0;
  for(k=0;k<4;k++)tr[i][j]=tr[i][j]+t1[i][k]*t2[k][j];
 }
 return;
}

static void rotate_round_vector(double angle, vector v, double t[4][4]){
  double dx,dy,dz,phi,theta,dxy;
  double t1[4][4],t2[4][4],t3[4][4];
  dx=v[0]; dy=v[1]; dz=v[2];
  dxy=dx*dx+dy*dy;
  if(dxy < 0.00001){ /* vertical so just rotate about z  and return */
    if(dz > 0.0)rotz(t, angle);
    else        rotz(t,-angle);
    return;
  }
  dxy=sqrt(dxy);
  if     (dx == 0.0 && dy > 0.0)phi =  PI2;
  else if(dx == 0.0 && dy < 0.0)phi = -PI2;
  else phi = atan2(dy,dx);
  theta = atan2(dz,dxy);
  rotz(t3, -phi);
  roty(t2, -theta);
  m4by4(t2,t3,t1);
  rotx(t2, angle);
  m4by4(t2,t1,t3);
  roty(t2,theta);
  m4by4(t2,t3,t1);
  rotz(t2,phi);
  m4by4(t2,t1,t);
}

static double Length(double *dx, long n){
 long i;
 double l=0.0;
 for(i=0;i<n;i++)l += dx[i]*dx[i];
 return sqrt(l);
}

static double Dist(double *x, double *y, long n){
 long i;
 double d=0.0;
 for(i=0;i<n;i++)d += (x[i]-y[i])*(x[i]-y[i]);
 return sqrt(d);
}

static void CalculateTranspose(double *M, double *Mt, long n, long m){
 long i,j;                       // M  is "n" rows "m" columns  [n x m]
 for(i=0;i<n;i++){
   for(j=0;j<m;j++){
     *(Mt+j*n+i) = *(M+i*m+j);   // Mt[j*n+i] = M[i*m+j];
   }
 }
 return;
}

static void MultiplyMatrix(double *M1, long n1, long m1,
                           double *M2, long n2, long m2,
                           double *I){
 long   i,j,k,c;           // [n1 x m2][n2 x m2] result is [n1 x m2]
 double s;
 if(m1 != n2){MessageBox(NULL,"Bad Multiply",NULL,MB_OK); return;}
 c=n2;
 for(i=0;i<n1;i++){
   for(j=0;j<m2;j++){
     for(s=0.0,k=0;k<c;k++){
       s += (*(M1+i*m1+k)) * (*(M2+k*m2+j));
       //  s += M1[i*m1+k]*M2[k*m2+j];  //  M1[i][k] * M2[k][j]
     }
     *(I+i*m2+j) = s;                   //  I[i*m2+j]=s;   // I[i][j]
   }
 }
 return;
}

static BOOL InvertMatrix(double *A, long n, double *B){
 int i,j,k,imax,kp1;
 double del,amax,atmp,btmp,div,amult,eps=1.e-6;
 for(i=0;i<n;i++)for(j=0;j<n;j++){
   if(i == j)B[i*n+j]=1.0;
   else      B[i*n+j]=0.0;
 }
 del=1.0;
 for(k=0;k<n;k++){
   if(k < n-1){
     imax=k;
     amax=fabs(A[k*n+k]);
     kp1=k+1;
     for(i=kp1;i<n;i++){
       if(amax-fabs(A[i*n+k]) < 0.0){
         imax=i;
         amax=fabs(A[i*n+k]);
       }
     }
     if(imax != k){
       for(j=0;j<n;j++){
         atmp=A[imax*n+j];
         A[imax*n+j]=A[k*n+j];
         A[k*n+j]=atmp;
         btmp=B[imax*n+j];
         B[imax*n+j]=B[k*n+j];
         B[k*n+j]=btmp;
       }
       del = -del;
     }
   }
   if(fabs(A[k*n+k]) < eps)return FALSE;
   del=A[k*n+k]*del;
   div=1.0/A[k*n+k];
   for(j=0;j<n;j++){
     A[k*n+j]=A[k*n+j]*div;
     B[k*n+j]=B[k*n+j]*div;
   }
   for(i=0;i<n;i++){
     amult=A[i*n+k];
     if(i != k){
       for(j=0;j<n;j++){
         A[i*n+j]=A[i*n+j]-amult*A[k*n+j];
         B[i*n+j]=B[i*n+j]-amult*B[k*n+j];
       }
     }
   }
 }
 return TRUE;
}

#endif