/* Rigid body motion, tracing angular momentum, both on body and in
   lab....
   
   compile using "cc -o main main.c -lm"
   
   run with ./main

problems:

- crushes if you give lengths of inertia ellipsoid axes <=0

- probably also similar problem with omega

- when change velocity of integration the integration loop stops

- need to clean up the main menu of geomview from all these 
body_curves

*/

#include <math.h>
#include <stdio.h>

float I[3]; /* Inertia tensor eigenvalues */

FILE *gv, *tk, *cf ; 
int body_curve_num=0;
 
# define DIM        12
# define LENGTH     1000

# define  sqr(x)  ((x)*(x))
# define min(x,y,z) ((x)<(y)? ((x)<(z)? (x):(z)) : ((y)<(z)? (y):(z)))

float  body_curve[LENGTH][3], lab_curve[LENGTH][3];


# include "../lib/full_euler_equations.c" 
# include "../lib/runge_kutta_step.c"

/*********************************************************************
 ******************            main        ****************************
 **********************************************************************/

main()   
     
{float y[12],t, dt=.05 ,v , L[3]; 
 int curve_length=0, i; char command;

 void start_geomview(void);
 void scale_I(void);
 void update_ellipsoid_shape(void);
 void scale_angular_velocity(float*);
 void update_angular_momentum(float*);
 void update_geomview(float*,int);
 void quit(void);

 /*  make pipes to Geomview and the  Tk interface */

 gv=popen("geomview  -c - -wpos 500,500@285,70", "w");


 
 tk=popen("/usr/local/etc/httpd/htdocs/talleres/escuela_verano/mecanica_clasica/rigid_body/tk","r");
 
 start_geomview();
 
 /* set  initial rotation matrix to identity matrix */
 y[3] = 1; y[4] = 0; y[5] = 0; 
 y[6] = 0; y[7]= 1; y[8]= 0; 
 y[9]= 0; y[10]= 0; y[11]= 1; 
 
 /* set  initial eigenvalues of the inertia tensor */
 
 I[0]=  1.; I[1]=1./4. ; I[2]=1./9.; 

 /* scale inertia tensor so that longests axes of inertia elipsiod has 
 length=1 */
 
 scale_I();
 
 /* tell geomview to draw the corresponding inertia tensor */
 
  update_ellipsoid_shape();
 
 /* set initial angular velocity */
 
 y[0]=.2; y[1]=1.; y[2]=.2;  
 
 /* scale angular velocity so that its tip lies on the inertia
    ellipsoid */
 
 scale_angular_velocity(y);

 /* tell geomview to draw ang vel vector and some other stuff */
 update_geomview(y,0);

 /* tell geomview to draw the corresponding angular momentum vector
    and the poinsoit invariant plane (it is the tangent plane to the
    ellipsoid at the pt thhe ang vel vect touches it; the ang mom vect is
    normal to this plane) */
 
 update_angular_momentum(y);

 /* tell geomview to adjust size of picture  */
 fprintf(gv,"(look world)"); fflush(gv);

 while(1) {
   switch (fgetc(tk)) {
     
   case 'v': /* change velocity of integration */
     fscanf(tk, "%f", &v);
     dt=v/20.;
     break;

   case 'y': /* change initial ang velocity */
     fscanf(tk, "%f %f %f",y,y+1,y+2);
     scale_angular_velocity(y);
     update_angular_momentum(y);
     if (body_curve_num>0) 
       {body_curve_num++; curve_length=0;
       update_geomview(y,curve_length);}
     break;
     
   case 'L':  /* change lengths of axes of inertia tensor */
     fscanf(tk, "%f %f %f",L, L+1, L+2);
     for (i=0;i<3;I[i]=1./sqr(L[i]), i++);
     scale_I();
     update_ellipsoid_shape();
     scale_angular_velocity(y);
     update_angular_momentum(y); 
     if (body_curve_num>0) 
       {for (i=0;i<=body_curve_num;i++)
	 fprintf(gv,"(geometry body_curve%i)",i);
       curve_length=body_curve_num=0;
       update_geomview(y,curve_length);};
     break;
     
   case 'q': 
     quit();
     
   case 'g': /* start integrfation loop */
     command='g';
     if (body_curve_num==0) {body_curve_num=1;};
     for(t=0.; (command=='g') && (curve_length<LENGTH); t+=dt,curve_length++)
       {runge_kutta_step(t, y, dt);
       update_geomview(y, curve_length);
       cf=fopen("/tmp/command_file", "r");
       fscanf(cf,"%c",&command) ;
       fclose(cf);
       if (command=='q') quit();
       if (command=='v') 
	 {fscanf(tk, "%c %f", &command, &v); 
	 dt=v/20.;
	 command='g';
	 };
       }; 
     break;
   };
 };
}

/*********************************************************************
 ****************** start_geomview ***********************************
 **********************************************************************/

void start_geomview()
     
{fprintf(gv,"
(progn
(geometry ellipsoid
  {INST transform :body_transform geom
  {INST geom </usr/local/etc/httpd/htdocs/talleres/escuela_verano/mecanica_clasica/rigid_body/nice_sphere transform :ellipsoid_transform}})
(geometry angular_momentum 
  {INST geom </usr/local/etc/httpd/htdocs/talleres/escuela_verano/mecanica_clasica/rigid_body/angular_momentum_geometry transform :angular_momentum_transform})
(geometry invariant_plane {: invariant_plane_geometry})
(geometry angular_velocity
  {INST transform :body_transform geom
  {INST geom </usr/local/etc/httpd/htdocs/talleres/escuela_verano/mecanica_clasica/rigid_body/angular_velocity_geometry 
   transform:angular_velocity_transform }})
(backcolor allcams .7 .5 1)
(normalization allgeoms none)
(bbox-draw world no)
)
",body_curve_num);

fflush(gv);
}


/*********************************************************************
 ***********************  scale_I  *******************************
 **********************************************************************/

void scale_I()
     
{float scale;
 
/* ----------- scale inertia tensor ------------------------------
 *  so that longest axis  of inertia ellipoid will have length = 1
 */
 
 scale=min(I[0],I[1],I[2])/2.; 
 
 I[0]=I[0]/scale;I[1]=I[1]/scale;I[2]=I[2]/scale;
}


/*********************************************************************
 ***********************  scale_angular_velocity *********************
 *********************************************************************/

void scale_angular_velocity(float *y) 
     
{float scale;
 
/* --------   scale angular velocity wrt body  ------------------------------
 *         so that its  tip lies on inertia ellipsoid 
 */
 
 scale=I[0]*sqr(y[0])+I[1]*sqr(y[1])+I[2]*sqr(y[2]); 
 scale=sqrt(scale/2.);
 
 y[0]=y[0]/scale;y[1]=y[1]/scale;y[2]=y[2]/scale;
}


/*********************************************************************
****************** update_angular_momentum     ***********************
**********************************************************************/

void update_angular_momentum(float *y)

{float scale, X[3],Y[3], M[3], a,b, c; int i;




/* ----- components of angular  momentum wrt body ------------ */

scale=(sqr(I[0]*y[0]) + sqr(I[1]*y[1]) + sqr(I[2]*y[2]))/2.;

for (i=0;i<3;i++) M[i]=y[i]*I[i]/scale;

/* ----- components of angular  momentum wrt lab ------------ */


a=y[3]*M[0]+y[4]*M[1]+y[5]*M[2];
b=y[6]*M[0]+y[7]*M[1]+y[8]*M[2];
c=y[9]*M[0]+y[10]*M[1]+y[11]*M[2];

/* ----- the invariant plane ------------ */

if (b==0. && c==0.) 
{X[0]=0.; X[1]=1.; X[2]=0.;
 Y[0]=0.; Y[1]=0.; Y[2]=1.; 
}
 else
   {scale=sqrt(b*b + c*c)/2.;
   X[0]=0.; X[1]=-c/scale; X[2]=b/scale;
   scale=sqrt( sqr( b*b + c*c) + sqr (a*b) + sqr (a*c))/2.;
   Y[0]= (b*b + c*c)/scale; Y[1]= -a*b/scale; Y[2]=-a*c/scale;
   };

 fprintf(gv,"(progn
(read transform 
{define angular_momentum_transform
{
  %f 0 0 0 
     0  %f 0 0
     0  0  %f 0
     0  0  0 1 
     }})
     ", a, b, c);


fprintf(gv,"
(read geometry
{define
invariant_plane_geometry
QUAD
%f %f %f \n", X[0]+a,X[1]+b,X[2]+c);
fprintf(gv,"%f %f %f \n",Y[0]+a,Y[1]+b,Y[2]+c);
fprintf(gv,"%f %f %f \n",-X[0]+a,-X[1]+b,-X[2]+c);
fprintf(gv,"%f %f %f \n",-Y[0]+a,-Y[1]+b,-Y[2]+c);
fprintf(gv,"}))");


fflush(gv);

}


/*********************************************************************
****************** update_ellipsoid_shape  ***********************
**********************************************************************/

void update_ellipsoid_shape()

{fprintf(gv,"
(read transform 
{define ellipsoid_transform
{ 
  %f 0 0 0\n", sqrt(2./I[0]));
fprintf(gv," 0 %f 0 0\n", sqrt(2./I[1]));
fprintf(gv," 0 0 %f 0\n", sqrt(2./I[2]));
fprintf(gv," 0 0 0  1
     }})
     ");

fflush(gv);

}

/*********************************************************************
****************** update_geomview  *****************************
**********************************************************************/

void update_geomview(float *y,int curve_length)


{int i;


/* update  curves */

lab_curve[curve_length][0]=y[3]*y[0]+y[4]*y[1]+y[5]*y[2];
lab_curve[curve_length][1]=y[6]*y[0]+y[7]*y[1]+y[8]*y[2];
lab_curve[curve_length][2]=y[9]*y[0]+y[10]*y[1]+y[11]*y[2];

for (i=0; i <3; i++) body_curve[curve_length][i]=y[i];

fprintf(gv,"
(progn
(geometry lab_curve {
VECT 1  %i 1 %i 1\n 
",  curve_length+1, curve_length+1);

for (i=0; i <=curve_length; i++)
fprintf(gv," %f  %f  %f ",lab_curve[i][0], lab_curve[i][1], lab_curve[i][2]);
fprintf(gv,"1 1 1 1 
})");

fprintf(gv,"
(geometry  body_curve%i\n", body_curve_num);
fprintf(gv,"
{INST transform :body_transform geom 
{VECT 1  %i 1 %i 1\n ", curve_length+1, curve_length+1);
for (i=0; i <=curve_length; i++)
fprintf(gv," %f  %f  %f\n",body_curve[i][0], body_curve[i][1], body_curve[i][2]);
fprintf(gv,".5 1 .5 1 }})");


/* update angular-velocity-vector */

fprintf(gv,"(read transform { define angular_velocity_transform { \n");
fprintf(gv," %f 0 0 0 \n ", y[0]);
fprintf(gv," 0  %f 0 0 \n ", y[1]);
fprintf(gv," 0  0  %f 0 \n ", y[2]);
fprintf(gv," 0  0  0 1 \n ");
fprintf(gv," }})");

/* now rotate the whole body */

fprintf(gv,"(read transform {define body_transform {\n");
fprintf(gv,"  %f %f %f 0 \n", y[3],y[6],y[9]);
fprintf(gv,"%f %f %f    0 \n",y[4],y[7],y[10] );
fprintf(gv,"%f %f %f    0 \n", y[5],y[8],y[11] );
fprintf(gv,"0 0 0 1 
}})\n");

fprintf(gv,")\n");

fflush(gv);


}

/*********************************************************************
******************            quit        ****************************
**********************************************************************/
void quit()

{fprintf(gv,"(exit)\n");
pclose(tk); 
fclose(cf);
system("rm -f /tmp/command_file");
printf("BYE\n"); 
exit(0);}