/* FILE: 	besl.c
   DATE:	8.92
   DESCRIPTION: Computes the transformation a list of pairs.
                Uses the algorithm in Besl et al. (see ref).
                The eigenvectors are computed from the package
                eispack (file eigen.f).
   PARAMETERS: Xx,Xy,Xz: Arrays of x,y,z coordinates of one object
               Px,Py,Pz: Arrays of x,y,z coordinates of second object
               N: size (number of pairs of points).
               R: rotation matrix.
               dispx,dispy,dispz: displacements.
   USES: 
   DIST DATE: 6.1994

*/
#include <stdio.h>

typedef struct { float x,y,z; }triplet;

double sqrt();




besl(Xx,Xy,Xz,Px,Py,Pz,N,R,dispx,dispy,dispz)
double Xx[],Xy[],Xz[];
double Px[],Py[],Pz[];   
int N;
double R[9];
double *dispx, *dispy,*dispz;
{
  float q0,q1,q2,q3;		/* Quaternion of the max eigenvalue*/
  triplet CenterP, CenterX;	/*  Centers Of Mass */

  triplet SIGMApx[3];		/*  Cross-covariance matrix */
                /*  REPRESENTED AS: */
		/* SIGMApx[0].x   SIGMApx[0].y   SIGMApx[0].z */
		/* SIGMApx[1].x   SIGMApx[1].y   SIGMApx[1].z */
		/* SIGMApx[2].x   SIGMApx[2].y   SIGMApx[2].z */
  triplet  T;   /* stores the displacements */

  /* AntiSymetric matrix A  Computed as SIGMApx - SIGMApx trasposed*/

  /* triplet DELTA;  [A23, A31, A12] of A 
                     (SIGMApx-SIGMApx transposed) */

  double Q[16];			/* Symmetric matrix Q */

  double w[4],z[16];		/* OUTPUT of eigen */
  int ierr;


 

      CenterOfMass(Xx,Xy,Xz,N,&CenterX);
      CenterOfMass(Px,Py,Pz,N,&CenterP);


      ComputeSIGMApx(Xx,Xy,Xz,Px,Py,Pz,N,CenterP,CenterX,SIGMApx);



      ComputeQ(SIGMApx,Q);

      /*printf("Calling to eigen\n"); */
      eigen_(&ierr,Q,w,z);
      /*printf("After eigen\n"); */

       q0=z[12]; q1=z[13];q2=z[14];q3=z[15];
       
       CheckOk(q0,q1,q2,q3,ierr);
       
       
       ComputeR(q0,q1,q2,q3,R);

       ComputeT(CenterX,CenterP,R,&T);

       *dispx=T.x; *dispy=T.y; *dispz=T.z;
}




CenterOfMass(ax,ay,az,N,center)
double ax[],ay[],az[];
int N;
triplet * center;
{
 int i;
 float suXx=0,suXy=0,suXz=0;
 for(i=0;i<N;++i)
 {
  suXx+=ax[i];
  suXy+=ay[i];
  suXz+=az[i];
 };
 center->x = suXx/N;
 center->y = suXy/N;
 center->z = suXz/N;
}




ComputeSIGMApx(Xx,Xy,Xz,Px,Py,Pz,N,CenterP,CenterX,SIGMApx)
double Xx[],Xy[],Xz[];
double Px[],Py[],Pz[];   
int N;
triplet CenterP, CenterX, SIGMApx[];
{
 int i;
 float sum11=0,sum12=0,sum13=0;
 float sum21=0,sum22=0,sum23=0;
 float sum31=0,sum32=0,sum33=0;

 for (i=0;i<N;++i)
 {

    sum11 += Px[i] * Xx[i];
    sum12 += Px[i] * Xy[i];
    sum13 += Px[i] * Xz[i];

    sum21 += Py[i] * Xx[i];
    sum22 += Py[i] * Xy[i];
    sum23 += Py[i] * Xz[i];

    sum31 += Pz[i] * Xx[i];
    sum32 += Pz[i] * Xy[i];
    sum33 += Pz[i] * Xz[i];

 };

 SIGMApx[0].x =  sum11/N - CenterP.x * CenterX.x;
 SIGMApx[0].y =  sum12/N - CenterP.x * CenterX.y;
 SIGMApx[0].z =  sum13/N - CenterP.x * CenterX.z;

 SIGMApx[1].x =  sum21/N - CenterP.y * CenterX.x;
 SIGMApx[1].y =  sum22/N - CenterP.y * CenterX.y;
 SIGMApx[1].z =  sum23/N - CenterP.y * CenterX.z;
 
 SIGMApx[2].x =  sum31/N - CenterP.z * CenterX.x;
 SIGMApx[2].y =  sum32/N - CenterP.z * CenterX.y;
 SIGMApx[2].z =  sum33/N - CenterP.z * CenterX.z;

}



/* INSTEAD OF COMPUTING THE WHOLE MATRIX, WE ONLY USE THE
   ELEMENTS THAT DEFINE DELTA: (2,3) (3,1) and (1,2) */

#define DELTAx (SIGMApx[1].z - SIGMApx[2].y)
#define DELTAy (SIGMApx[2].x - SIGMApx[0].z)
#define DELTAz (SIGMApx[0].y - SIGMApx[1].x)

ComputeQ(SIGMApx,Q)
triplet SIGMApx[3];
double Q[16];
{
 float tr=SIGMApx[0].x+SIGMApx[1].y+SIGMApx[2].z;

 Q[0]=tr;
 Q[4]= DELTAx;
 Q[8]= DELTAy;
 Q[12]= DELTAz;

 Q[1]=Q[4];
 Q[5]=SIGMApx[0].x+SIGMApx[0].x -tr;
 Q[9]=SIGMApx[0].y+SIGMApx[1].x;
 Q[13]=SIGMApx[0].z+SIGMApx[2].x;

 Q[2]= Q[8];
 Q[6]= Q[9];
 Q[10]=SIGMApx[1].y+SIGMApx[1].y-tr;
 Q[14]=SIGMApx[1].z+SIGMApx[2].y;

 Q[3]= Q[12];
 Q[7]= Q[13];
 Q[11]=Q[14];
 Q[15]=SIGMApx[2].z+SIGMApx[2].z-tr;
}



ComputeR(q0,q1,q2,q3,R)
float q0,q1,q2,q3;
double R[9];
{
 float q02=q0*q0,   q12=q1*q1,  q22=q2*q2,  q32=q3*q3;
 float q1q2=q1*q2,  q0q3=q0*q3, q1q3=q1*q3, q0q2=q0*q2;
 float q2q3=q2*q3,  q0q1=q0*q1;
 

 R[0] = q02+q12-q22-q32;
 R[1] = 2 * ( q1q2-q0q3);
 R[2] = 2 * ( q1q3+q0q2);

 R[3] = 2 * ( q1q2+q0q3); 
 R[4] = q02+q22-q12-q32;
 R[5] = 2 * ( q2q3- q0q1);

 R[6] = 2 * (q1q3 -q0q2);
 R[7] = 2 * (q2q3 + q0q1);
 R[8] = q02+q32-q12-q22;

}



ComputeT(CenterX,CenterP,r,T)
triplet CenterX,CenterP;
double r[9];
triplet *T;
{
 T->x = (float) (CenterX.x - (r[0]*CenterP.x+ r[1]*CenterP.y + r[2] *CenterP.z)); 
 T->y = (float) (CenterX.y - (r[3]*CenterP.x+ r[4]*CenterP.y + r[5] *CenterP.z));
 T->z=  (float) (CenterX.z - (r[6]*CenterP.x +r[7]*CenterP.y + r[8] *CenterP.z));

}


#define abso(a)  ((a)>0)?( (a) ): (-(a))

CheckOk(q0,q1,q2,q3,ierr)
float q0,q1,q2,q3;
int ierr;
{

  float nor;
  if (ierr)  printf("ERROR IN EIGENVALUE %d \n",ierr);
  nor= (float) sqrt( (double) (q0*q0+q1*q1+q2*q2+q3*q3)) -1; 
  if ( abso( nor) > .01) printf("ERROR, Q is not normalized\n");
}