/* animate_joint_motion.c 
 *
 * (c) Motomu Nakashima
 *
*/

#include "togl.h"
#include <math.h>
#include <stdlib.h>
#include <tcl.h>
#include <tk.h>
#include <GL/glu.h>

#ifndef M_PI
#  define M_PI 3.14159265
#endif

static GLuint limit;
static GLuint count = 1;
static GLubyte polycolor[4] = {255, 255, 255, 255};

FILE *mdata;
FILE *gdata;
float xa[22],ya[22],za[22]
     ,xb[22],yb[22],zb[22]
     ,e1x[22],e1y[22],e1z[22]
     ,e2x[22],e2y[22],e2z[22]
     ,e3x[22],e3y[22],e3z[22]
     ,ra1[22],ra2[22],rb1[22],rb2[22],len[22],bu[10];

float xs2ra,ys2ra,zs2ra,xs2la,ys2la,zs2la;
float xnb2ha,ynb2ha,znb2ha;
float xh2rl,yh2rl,zh2rl,xh2ll,yh2ll,zh2ll;
float xtj2tra,ytj2tra,ztj2tra,xtj2tla,ytj2tla,ztj2tla;
float xsh2rf,ysh2rf,zsh2rf,xsh2lf,ysh2lf,zsh2lf;
float xfj2ra,yfj2ra,zfj2ra,xfj2la,yfj2la,zfj2la;
float xuaj2ra,yuaj2ra,zuaj2ra,xuaj2la,yuaj2la,zuaj2la;
int segnum[1000],axisnum[1000];
float axisval[1000][1000];
float xvec,yvec,zvec;

GLfloat x1d[22][1000],y1d[22][1000],z1d[22][1000]
       ,x2d[22][1000],y2d[22][1000],z2d[22][1000]
       ,v3x[22][1000],v3y[22][1000],v3z[22][1000];

GLfloat Rotx,Roty,Rotz,fscale;

static GLint time = 0;
GLint divt = 16;

GLint numl2,numb,divl,numline;
numb = 21;
GLfloat stlen, muki;



static void vecrot( float x, float y, float z, float rx, float ry, float rz )
{
      float x1, y1, z1;
      float x2, y2, z2;
      float x3, y3, z3;

      x1 =  x;
      y1 =  y * cos(rx) - z * sin(rx);
      z1 =  y * sin(rx) + z * cos(rx);

      x2 =  x1 * cos(ry) + z1 * sin(ry);
      y2 =  y1;
      z2 = -x1 * sin(ry) + z1 * cos(ry);

      x3 =  x2 * cos(rz) - y2 * sin(rz);
      y3 =  x2 * sin(rz) + y2 * cos(rz);
      z3 =  z2;

      xvec = x3;
      yvec = y3;
      zvec = z3;
}


static void display( struct Togl *togl )
{
   GLint i,j,k,ja;
   GLfloat scale;
   GLfloat xw1,xw2,xw3,xw4,xw5,xw6,yw1,yw2,yw3,yw4,yw5,yw6;
   GLfloat water_width;
   GLfloat vtx,vty,vtz;
   GLfloat v1x,v1y,v1z,v2x,v2y,v2z,v3xd,v3yd,v3zd,vabs;
   float theta;

   /* Initialization for OpenGL */
   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
   glColor3f(1.0, 0.9, 0.0); /* yellow */
   glPushMatrix();
   glRotatef( -Rotx, 1.0, 0.0, 0.0 );
   glRotatef( -Roty, 0.0, 1.0, 0.0 );
   glRotatef( -Rotz, 0.0, 0.0, 1.0 );
   glRotatef( 180.0, 0.0, 1.0, 0.0 );
   glRotatef(  90.0, 1.0, 0.0, 0.0 );
   glRotatef( -90.0, 0.0, 0.0, 1.0 );
   glRotatef( -90.0, 0.0, 1.0, 0.0 );
   glEnable(GL_DEPTH_TEST);

   scale = 1.3;

   /* First, directions of elements are given */
   for (j=1;j<=numb;j++) {
      e1x[j] = 1.0;
      e1y[j] = 0.0;
      e1z[j] = 0.0;

      e2x[j] = 0.0;
      e2y[j] = 1.0;
      e2z[j] = 0.0;

      e3x[j] = 0.0;
      e3y[j] = 0.0;
      e3z[j] = 1.0;
   }

   /* make vectors from point b of upper breast to shoulder joint */
   xs2ra = 0.0;
   ys2ra = -rb2[4];
   zs2ra = -bu[2];

   xs2la = 0.0;
   ys2la = +rb2[4];
   zs2la = -bu[2];

   /* definite vector from neck-b to head-a */
   xnb2ha = bu[3];
   ynb2ha = 0.0;
   znb2ha = 0.0;

   /* give position of hip joint from lower hip-b */
   xh2rl = 0.0;
   yh2rl = 0.0 -bu[4];
   zh2rl = 0.0 -bu[5];
   xh2ll = 0.0;
   yh2ll = 0.0 +bu[4];
   zh2ll = 0.0 -bu[5];

   /* definite vector from right hip joint to thigh-a */
   xtj2tra = 0.0;
   ytj2tra = 0.0;
   ztj2tra = +bu[5];

   /* definite vector from left hip joint to thigh-a */
   xtj2tla = 0.0;
   ytj2tla = 0.0;
   ztj2tla = +bu[5];

   /* give position of foot joint from right shank-b */
   xsh2rf = 0.0;
   ysh2rf = 0.0;
   zsh2rf = 0.0+bu[6];

   /* give position of foot joint from left shank-b */
   xsh2lf = 0.0;
   ysh2lf = 0.0;
   zsh2lf = 0.0 +bu[6];

   /* definite vector from right foot joint to point-a */
   xfj2ra = 0.0;
   yfj2ra = 0.0;
   zfj2ra = -bu[6];

   /* definite vector from left foot joint to point-a */
   xfj2la = 0.0;
   yfj2la = 0.0;
   zfj2la = -bu[6];

   /* definite vector from right upper arm joint to point-a */
   xuaj2ra = 0.0;
   yuaj2ra = -bu[1];
   zuaj2ra = -bu[2];

   /* definite vector from left upper arm joint to point-a */
   xuaj2la = 0.0;
   yuaj2la = bu[1];
   zuaj2la = -bu[2];

   /* make direction of lower half body downward */
   for (j=8;j<=15;j++) {
      if (j==8)
          theta = M_PI+bu[7];
      else if (j==9) {
          theta = M_PI-bu[7];
          vecrot(xh2rl,yh2rl,zh2rl,0.0,theta,0.0);
          xh2rl = xvec;
          yh2rl = yvec;
          zh2rl = zvec;
          vecrot(xh2ll,yh2ll,zh2ll,0.0,theta,0.0); 
          xh2ll = xvec;
          yh2ll = yvec;
          zh2ll = zvec;
      } else if (j==10) {
          theta = M_PI;
          vecrot(xtj2tra,ytj2tra,ztj2tra,0.0,theta,0.0);
          xtj2tra = xvec;
          ytj2tra = yvec;
          ztj2tra = zvec;
      } else if (j==11) {
          theta = M_PI;
          vecrot(xtj2tla,ytj2tla,ztj2tla,0.0,theta,0.0);
          xtj2tla = xvec;
          ytj2tla = yvec;
          ztj2tla = zvec;
      } else if (j==12) {
          theta = M_PI;
          vecrot(xsh2rf,ysh2rf,zsh2rf,0.0,theta,0.0);
          xsh2rf = xvec;
          ysh2rf = yvec;
          zsh2rf = zvec;
      } else if (j==13) {
          theta = M_PI;
          vecrot(xsh2lf,ysh2lf,zsh2lf,0.0,theta,0.0);
          xsh2lf = xvec;
          ysh2lf = yvec;
          zsh2lf = zvec;
      } else if (j==14) {
          theta = M_PI;
          vecrot(xfj2ra,yfj2ra,zfj2ra,0.0,theta,0.0);
          xfj2ra = xvec;
          yfj2ra = yvec;
          zfj2ra = zvec;
      } else if (j==15) {
          theta = M_PI;
          vecrot(xfj2la,yfj2la,zfj2la,0.0,theta,0.0);
          xfj2la = xvec;
          yfj2la = yvec;
          zfj2la = zvec;
      } else 
          theta = M_PI;

      vecrot(e1x[j],e1y[j],e1z[j],0.0,theta,0.0);
      e1x[j] = xvec;
      e1y[j] = yvec;
      e1z[j] = zvec;
      vecrot(e2x[j],e2y[j],e2z[j],0.0,theta,0.0);
      e2x[j] = xvec;
      e2y[j] = yvec;
      e2z[j] = zvec;
      vecrot(e3x[j],e3y[j],e3z[j],0.0,theta,0.0);
      e3x[j] = xvec;
      e3y[j] = yvec;
      e3z[j] = zvec;
   }

   /* Rotate segments */
   for (i=1;i<=numline;i++) {
      j  = segnum[i];
      ja = axisnum[i];
      theta = axisval[i][time] *M_PI/180.0;

      if (ja==1) {
          vecrot(e1x[j],e1y[j],e1z[j],theta,0.0,0.0);
          e1x[j] = xvec;
          e1y[j] = yvec;
          e1z[j] = zvec;
          vecrot(e2x[j],e2y[j],e2z[j],theta,0.0,0.0);
          e2x[j] = xvec;
          e2y[j] = yvec;
          e2z[j] = zvec;
          vecrot(e3x[j],e3y[j],e3z[j],theta,0.0,0.0);
          e3x[j] = xvec;
          e3y[j] = yvec;
          e3z[j] = zvec;
      } else if (ja==2) {
          vecrot(e1x[j],e1y[j],e1z[j],0.0,theta,0.0);
          e1x[j] = xvec;
          e1y[j] = yvec;
          e1z[j] = zvec;
          vecrot(e2x[j],e2y[j],e2z[j],0.0,theta,0.0);
          e2x[j] = xvec;
          e2y[j] = yvec;
          e2z[j] = zvec;
          vecrot(e3x[j],e3y[j],e3z[j],0.0,theta,0.0);
          e3x[j] = xvec;
          e3y[j] = yvec;
          e3z[j] = zvec;
      } else if (ja==3) {
          vecrot(e1x[j],e1y[j],e1z[j],0.0,0.0,theta);
          e1x[j] = xvec;
          e1y[j] = yvec;
          e1z[j] = zvec;
          vecrot(e2x[j],e2y[j],e2z[j],0.0,0.0,theta);
          e2x[j] = xvec;
          e2y[j] = yvec;
          e2z[j] = zvec;
          vecrot(e3x[j],e3y[j],e3z[j],0.0,0.0,theta);
          e3x[j] = xvec;
          e3y[j] = yvec;
          e3z[j] = zvec;
      }

      if (j==4) {
/*******************
* (4) Upper breast *
*******************/
      if (ja==1) {
          vecrot(xs2ra,ys2ra,zs2ra,theta,0.0,0.0);
          xs2ra = xvec;
          ys2ra = yvec;
          zs2ra = zvec;
          vecrot(xs2la,ys2la,zs2la,theta,0.0,0.0);
          xs2la = xvec;
          ys2la = yvec;
          zs2la = zvec;
      } else if (ja==2) {
          vecrot(xs2ra,ys2ra,zs2ra,0.0,theta,0.0);
          xs2ra = xvec;
          ys2ra = yvec;
          zs2ra = zvec;
          vecrot(xs2la,ys2la,zs2la,0.0,theta,0.0);
          xs2la = xvec;
          ys2la = yvec;
          zs2la = zvec;
      } else if (ja==3) {
          vecrot(xs2ra,ys2ra,zs2ra,0.0,0.0,theta);
          xs2ra = xvec;
          ys2ra = yvec;
          zs2ra = zvec;
          vecrot(xs2la,ys2la,zs2la,0.0,0.0,theta);
          xs2la = xvec;
          ys2la = yvec;
          zs2la = zvec;
      } else if (ja==4) {
          vecrot(xs2ra,ys2ra,zs2ra,theta,0.0,0.0);
          xs2ra = xvec;
          ys2ra = yvec;
          zs2ra = zvec;
      } else if (ja==5) {
          vecrot(xs2ra,ys2ra,zs2ra,0.0,theta,0.0);
          xs2ra = xvec;
          ys2ra = yvec;
          zs2ra = zvec;
      } else if (ja==6) {
          vecrot(xs2ra,ys2ra,zs2ra,0.0,0.0,theta);
          xs2ra = xvec;
          ys2ra = yvec;
          zs2ra = zvec;
      } else if (ja==7) {
          vecrot(xs2la,ys2la,zs2la,theta,0.0,0.0);
          xs2la = xvec;
          ys2la = yvec;
          zs2la = zvec;
      } else if (ja==8) {
          vecrot(xs2la,ys2la,zs2la,0.0,theta,0.0);
          xs2la = xvec;
          ys2la = yvec;
          zs2la = zvec;
      } else if (ja==9) {
          vecrot(xs2la,ys2la,zs2la,0.0,0.0,theta);
          xs2la = xvec;
          ys2la = yvec;
          zs2la = zvec;
      }

      } else if (j==7) {
/***********
* (7) Head *
***********/
      if (ja==1) {
          vecrot(xnb2ha,ynb2ha,znb2ha,theta,0.0,0.0);
      } else if (ja==2) {
          vecrot(xnb2ha,ynb2ha,znb2ha,0.0,theta,0.0);
      } else if (ja==3) {
          vecrot(xnb2ha,ynb2ha,znb2ha,0.0,0.0,theta);
      }
          xnb2ha = xvec;
          ynb2ha = yvec;
          znb2ha = zvec;

      } else if (j==9) {
/****************
* (9) Lower hip *
****************/
      if (ja==1) {
          vecrot(xh2rl,yh2rl,zh2rl,theta,0.0,0.0);
          xh2rl = xvec;
          yh2rl = yvec;
          zh2rl = zvec;
          vecrot(xh2ll,yh2ll,zh2ll,theta,0.0,0.0);
          xh2ll = xvec;
          yh2ll = yvec;
          zh2ll = zvec;
      } else if (ja==2) {
          vecrot(xh2rl,yh2rl,zh2rl,0.0,theta,0.0);
          xh2rl = xvec;
          yh2rl = yvec;
          zh2rl = zvec;
          vecrot(xh2ll,yh2ll,zh2ll,0.0,theta,0.0);
          xh2ll = xvec;
          yh2ll = yvec;
          zh2ll = zvec;
      } else if (ja==3) {
          vecrot(xh2rl,yh2rl,zh2rl,0.0,0.0,theta);
          xh2rl = xvec;
          yh2rl = yvec;
          zh2rl = zvec;
          vecrot(xh2ll,yh2ll,zh2ll,0.0,0.0,theta);
          xh2ll = xvec;
          yh2ll = yvec;
          zh2ll = zvec;
      }

      } else if (j==10) {
/*******************
* (10) Right thigh *
*******************/
      if (ja==1) {
          vecrot(xtj2tra,ytj2tra,ztj2tra,theta,0.0,0.0);
      } else if (ja==2) {
          vecrot(xtj2tra,ytj2tra,ztj2tra,0.0,theta,0.0);
      } else if (ja==3) {
          vecrot(xtj2tra,ytj2tra,ztj2tra,0.0,0.0,theta);
      }
          xtj2tra = xvec;
          ytj2tra = yvec;
          ztj2tra = zvec;

      } else if (j==11) {
/******************
* (11) Left thigh *
******************/
      if (ja==1) {
          vecrot(xtj2tla,ytj2tla,ztj2tla,theta,0.0,0.0);
      } else if (ja==2) {
          vecrot(xtj2tla,ytj2tla,ztj2tla,0.0,theta,0.0);
      } else if (ja==3) {
          vecrot(xtj2tla,ytj2tla,ztj2tla,0.0,0.0,theta);
      }
          xtj2tla = xvec;
          ytj2tla = yvec;
          ztj2tla = zvec;

      } else if (j==12) {
/*******************
* (12) Right shank *
*******************/
      if (ja==1) {
          vecrot(xsh2rf,ysh2rf,zsh2rf,theta,0.0,0.0);
      } else if (ja==2) {
          vecrot(xsh2rf,ysh2rf,zsh2rf,0.0,theta,0.0);
      } else if (ja==3) {
          vecrot(xsh2rf,ysh2rf,zsh2rf,0.0,0.0,theta);
      }
          xsh2rf = xvec;
          ysh2rf = yvec;
          zsh2rf = zvec;

      } else if (j==13) {
/******************
* (13) Left shank *
******************/
      if (ja==1) {
          vecrot(xsh2lf,ysh2lf,zsh2lf,theta,0.0,0.0);
      } else if (ja==2) {
          vecrot(xsh2lf,ysh2lf,zsh2lf,0.0,theta,0.0);
      } else if (ja==3) {
          vecrot(xsh2lf,ysh2lf,zsh2lf,0.0,0.0,theta);
      }
          xsh2lf = xvec;
          ysh2lf = yvec;
          zsh2lf = zvec;

      } else if (j==14) {
/******************
* (14) Right foot *
******************/
      if (ja==1) {
          vecrot(xfj2ra,yfj2ra,zfj2ra,theta,0.0,0.0);
      } else if (ja==2) {
          vecrot(xfj2ra,yfj2ra,zfj2ra,0.0,theta,0.0);
      } else if (ja==3) {
          vecrot(xfj2ra,yfj2ra,zfj2ra,0.0,0.0,theta);
      }
          xfj2ra = xvec;
          yfj2ra = yvec;
          zfj2ra = zvec;

      } else if (j==15) {
/*****************
* (15) Left foot *
*****************/
      if (ja==1) {
          vecrot(xfj2la,yfj2la,zfj2la,theta,0.0,0.0);
      } else if (ja==2) {
          vecrot(xfj2la,yfj2la,zfj2la,0.0,theta,0.0);
      } else if (ja==3) {
          vecrot(xfj2la,yfj2la,zfj2la,0.0,0.0,theta);
      }
          xfj2la = xvec;
          yfj2la = yvec;
          zfj2la = zvec;

      } else if (j==16) {
/***********************
* (16) Right upper arm *
***********************/
      if (ja==1) {
          vecrot(xuaj2ra,yuaj2ra,zuaj2ra,theta,0.0,0.0);
      } else if (ja==2) {
          vecrot(xuaj2ra,yuaj2ra,zuaj2ra,0.0,theta,0.0);
      } else if (ja==3) {
          vecrot(xuaj2ra,yuaj2ra,zuaj2ra,0.0,0.0,theta);
      }
          xuaj2ra = xvec;
          yuaj2ra = yvec;
          zuaj2ra = zvec;

      } else if (j==17) {
/**********************
* (17) Left upper arm *
**********************/
      if (ja==1) {
          vecrot(xuaj2la,yuaj2la,zuaj2la,theta,0.0,0.0);
      } else if (ja==2) {
          vecrot(xuaj2la,yuaj2la,zuaj2la,0.0,theta,0.0);
      } else if (ja==3) {
          vecrot(xuaj2la,yuaj2la,zuaj2la,0.0,0.0,theta);
      }
          xuaj2la = xvec;
          yuaj2la = yvec;
          zuaj2la = zvec;

      }
   }


   /* compute coordinates */
   for (j=1;j<=numb;j++) {

      if (j==1) {
/******************
* (1) Lower waist *
******************/
      xa[j] = 0.0;
      ya[j] = 0.0;
      za[j] = 0.0;
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if ((j==2)||(j==3)||(j==5)||(j==6)) {
/***************************************************************
* (2) Upper waist,(3) Lower breast,(5) Upper shoulder,(6) Neck *
***************************************************************/
      xa[j] = xb[j-1];
      ya[j] = yb[j-1];
      za[j] = zb[j-1];
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==4) {
/*******************
* (4) Upper breast *
*******************/
      xa[j] = xb[j-1];
      ya[j] = yb[j-1];
      za[j] = zb[j-1];
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==7) {
/***********
* (7) Head *
***********/
      xa[j] = xb[j-1] + xnb2ha;
      ya[j] = yb[j-1] + ynb2ha;
      za[j] = zb[j-1] + znb2ha;
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==8) {
/****************
* (8) Upper hip *
****************/
      xa[j] = xa[1];
      ya[j] = ya[1];
      za[j] = za[1];
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==9) {
/****************
* (9) Lower hip *
****************/
      xa[j] = xb[j-1];
      ya[j] = yb[j-1];
      za[j] = zb[j-1];
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==10) {
/*******************
* (10) Right thigh *
*******************/
      xa[j] = xb[9] + xh2rl + xtj2tra;
      ya[j] = yb[9] + yh2rl + ytj2tra;
      za[j] = zb[9] + zh2rl + ztj2tra;
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==11) {
/******************
* (11) Left Thigh *
******************/
      xa[j] = xb[9] + xh2ll + xtj2tla;
      ya[j] = yb[9] + yh2ll + ytj2tla;
      za[j] = zb[9] + zh2ll + ztj2tla;
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==12) {
/*******************
* (12) Right shank *
*******************/
      xa[j] = xb[j-2];
      ya[j] = yb[j-2];
      za[j] = zb[j-2];
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==13) {
/******************
* (13) Left shank *
******************/
      xa[j] = xb[j-2];
      ya[j] = yb[j-2];
      za[j] = zb[j-2];
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==14) {
/******************
* (14) Right foot *
******************/
      xa[j] = xb[j-2]+xfj2ra+xsh2rf;
      ya[j] = yb[j-2]+yfj2ra+ysh2rf;
      za[j] = zb[j-2]+zfj2ra+zsh2rf;
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==15) {
/*****************
* (15) Left foot *
*****************/
      xa[j] = xb[j-2]+xfj2la+xsh2lf;
      ya[j] = yb[j-2]+yfj2la+ysh2lf;
      za[j] = zb[j-2]+zfj2la+zsh2lf;
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==16) {
/***********************
* (16) Right upper arm *
***********************/
      xa[j] = xb[4] + xs2ra + xuaj2ra;
      ya[j] = yb[4] + ys2ra + yuaj2ra;
      za[j] = zb[4] + zs2ra + zuaj2ra;
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if (j==17) {
/**********************
* (17) Left upper arm *
**********************/
      xa[j] = xb[4] + xs2la + xuaj2la;
      ya[j] = yb[4] + ys2la + yuaj2la;
      za[j] = zb[4] + zs2la + zuaj2la;
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];

      } else if ((j==18)||(j==19)||(j==20)||(j==21)) {
/***********************************************************************
* (18) Right fore arm,(19) Left fore arm,(20) Right hand,(21) Left hand*
***********************************************************************/
      xa[j] = xb[j-2];
      ya[j] = yb[j-2];
      za[j] = zb[j-2];
      xb[j] = xa[j] + len[j] * e3x[j];
      yb[j] = ya[j] + len[j] * e3y[j];
      zb[j] = za[j] + len[j] * e3z[j];
      }
   }


   /* Calculate coordinate data */
   for (i=1;i<=numb;i++) {
      for (k=0;k<=divt;k++) {

         /* calculate coordinates */
         x1d[i][k] = xa[i]
                          + ra1[i] * e1x[i] *cos(2.0*M_PI*k/divt)
                          + ra2[i] * e2x[i] *sin(2.0*M_PI*k/divt);
         y1d[i][k] = ya[i]
                          + ra1[i] * e1y[i] *cos(2.0*M_PI*k/divt)
                          + ra2[i] * e2y[i] *sin(2.0*M_PI*k/divt);
         z1d[i][k] = za[i]
                          + ra1[i] * e1z[i] *cos(2.0*M_PI*k/divt)
                          + ra2[i] * e2z[i] *sin(2.0*M_PI*k/divt);
         x2d[i][k] = xb[i]
                          + rb1[i] * e1x[i] *cos(2.0*M_PI*k/divt)
                          + rb2[i] * e2x[i] *sin(2.0*M_PI*k/divt);
         y2d[i][k] = yb[i]
                          + rb1[i] * e1y[i] *cos(2.0*M_PI*k/divt)
                          + rb2[i] * e2y[i] *sin(2.0*M_PI*k/divt);
         z2d[i][k] = zb[i]
                          + rb1[i] * e1z[i] *cos(2.0*M_PI*k/divt)
                          + rb2[i] * e2z[i] *sin(2.0*M_PI*k/divt);

         /* calculate normal vector */
         
         /* first, tangential vector */
         v1x = (ra1[i]+rb1[i]) * e1x[i] *(-sin(2.0*M_PI*k/divt))
             + (ra2[i]+rb2[i]) * e2x[i] *( cos(2.0*M_PI*k/divt));
         v1y = (ra1[i]+rb1[i]) * e1y[i] *(-sin(2.0*M_PI*k/divt))
             + (ra2[i]+rb2[i]) * e2y[i] *( cos(2.0*M_PI*k/divt));
         v1z = (ra1[i]+rb1[i]) * e1z[i] *(-sin(2.0*M_PI*k/divt))
             + (ra2[i]+rb2[i]) * e2z[i] *( cos(2.0*M_PI*k/divt));

         /* second, vector between 1 and 2 */
         v2x = x2d[i][k]- x1d[i][k];
         v2y = y2d[i][k]- y1d[i][k];
         v2z = z2d[i][k]- z1d[i][k];

         /* finally, the normal vector is calculated as outer product */
         v3xd = v1y*v2z - v1z*v2y;
         v3yd = v1z*v2x - v1x*v2z;
         v3zd = v1x*v2y - v1y*v2x;

         /* and normalize it */
         vabs = sqrt(v3xd*v3xd+v3yd*v3yd+v3zd*v3zd);
         v3x[i][k] = v3xd /vabs;
         v3y[i][k] = v3yd /vabs;
         v3z[i][k] = v3zd /vabs;
      }
   }


   /* Draw human body */
   for (i=1;i<=numb;i++) {
      /* Draw side surface */
      glBegin(GL_QUAD_STRIP);
      for (j=0;j<=divt;j++) {
         glNormal3f( -v3x[i][j], -v3z[i][j], 
                                       -v3y[i][j] );
         glVertex3f( x1d[i][j]*scale, z1d[i][j]*scale,
                                            y1d[i][j]*scale );
         glVertex3f( x2d[i][j]*scale, z2d[i][j]*scale,
                                            y2d[i][j]*scale );
      }
      glEnd();

      /* Draw top and bottom surface */
      vtx = e1y[i]*e2z[i] - e1z[i]*e2y[i];
      vty = e1z[i]*e2x[i] - e1x[i]*e2z[i];
      vtz = e1x[i]*e2y[i] - e1y[i]*e2x[i];

      for (j=0;j<=divt-1;j++) {
         glBegin(GL_TRIANGLES);
         glNormal3f( +vtx, +vtz, +vty );
         glVertex3f( xa[i]*scale, za[i]*scale,
                                        ya[i]*scale );
         glVertex3f( x1d[i][j  ]*scale, z1d[i][j  ]*scale,
                                              y1d[i][j  ]*scale );
         glVertex3f( x1d[i][j+1]*scale, z1d[i][j+1]*scale,
                                              y1d[i][j+1]*scale );
         glEnd();

         glBegin(GL_TRIANGLES);
         glNormal3f( -vtx, -vtz, -vty );
         glVertex3f( xb[i]*scale, zb[i]*scale,
                                        yb[i]*scale );
         glVertex3f( x2d[i][j  ]*scale, z2d[i][j  ]*scale,
                                              y2d[i][j  ]*scale );
         glVertex3f( x2d[i][j+1]*scale, z2d[i][j+1]*scale,
                                              y2d[i][j+1]*scale );
         glEnd();
      }
   }


   /* Draw axes */
   glDisable(GL_LIGHTING);
   glDisable(GL_LIGHT0);

   glColor3f(1.0, 0.0, 0.0);  /* red */
   glLineWidth(2.0);
   glBegin(GL_LINES);
   glVertex3f( 1000.0, 0.0, 0.0);
   glVertex3f(-1000.0, 0.0, 0.0);
   glEnd();

   glColor3f(0.0, 1.0, 0.0);  /* green */
   glLineWidth(2.0);
   glBegin(GL_LINES);
   glVertex3f(0.0, 1000.0, 0.0);
   glVertex3f(0.0,-1000.0, 0.0);
   glEnd();

   glColor3f(0.0, 0.0, 1.0);  /* blue */
   glLineWidth(2.0);
   glBegin(GL_LINES);
   glVertex3f(0.0, 0.0, 1000.0);
   glVertex3f(0.0, 0.0,-1000.0);
   glEnd();

   glEnable(GL_LIGHTING);
   glEnable(GL_LIGHT0);

   glPopMatrix();
   glFlush();
   Togl_SwapBuffers(togl);
}



static void zap( struct Togl *togl )
{
   /*struct WHIRLYGIZMO *Wg;	
   Wg = Togl_GetClientData(togl);
   free(Wg);*/
}


static void idle( struct Togl *togl )
{
   Togl_PostRedisplay(togl);
}

/* new window size or exposure */
static void reshape( struct Togl *togl)
{
}


static void init( struct Togl *togl )
{
   int i,j,k;
   GLfloat v1x,v1y,v1z,v2x,v2y,v2z,v3xd,v3yd,v3zd,vabs;
   GLfloat dummy;

   /* Initialization for OepnGL */
   glClearColor (0.4, 0.6, 1.0, 0.0);
   glShadeModel (GL_SMOOTH); 
   glEnable(GL_LIGHTING);
   glEnable(GL_LIGHT0);
   glEnable(GL_COLOR_MATERIAL);
   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

   /* Load body geometry data file */
   gdata = fopen("body_geometry.dat","r");
   for (i=1;i<=numb;i++) {
       fscanf(gdata,"%f %f %f %f %f %f",
          &ra1[i],&ra2[i],&rb1[i],&rb2[i],&len[i],&dummy);
   }
   for (i=1;i<=9;i++) {
       fscanf(gdata,"%f",&bu[i]);
   }
   fclose(gdata);

   Rotx = 0.0;
   Roty = 0.0;
}

int position( struct Togl *togl, int argc, char *argv[] )
{
   char Result[100];
   Tcl_Interp *interp = Togl_Interp(togl);

   sprintf(Result,"%g %g",Roty,Rotx);
   Tcl_SetResult( interp,
                     Result,
                     TCL_VOLATILE );
   return TCL_OK;
}

int positionz( struct Togl *togl, int argc, char *argv[] )
{
   char Result[100];
   Tcl_Interp *interp = Togl_Interp(togl);

   sprintf(Result,"%g",Rotz);
   Tcl_SetResult( interp,
                     Result,
                     TCL_VOLATILE );
   return TCL_OK;
}


int rotate( struct Togl *togl, int argc, char *argv[] )
{
   Tcl_Interp *interp = Togl_Interp(togl);
   Roty = atof( argv[2] );
   Rotx = atof(argv[3] );
   Togl_PostRedisplay(togl);

   strcpy( interp->result, argv[2] );
   return TCL_OK;
}

int rotatez( struct Togl *togl, int argc, char *argv[] )
{
   Tcl_Interp *interp = Togl_Interp(togl);
   Rotz = atof( argv[2] );
   Togl_PostRedisplay(togl);

   strcpy( interp->result, argv[2] );
   return TCL_OK;
}

int fchange( struct Togl *togl, int argc, char *argv[] )
{
   int i,j,k;
   Tcl_Interp *interp = Togl_Interp(togl);
   fscale = atof( argv[2] );
   Togl_PostRedisplay(togl);

   strcpy( interp->result, argv[2] );
   return TCL_OK;
}

int advancetime( struct Togl *togl, int argc, char *argv[] )
{
   time = atof( argv[2] );
   Togl_PostRedisplay(togl);
   return TCL_OK;
}

int passnumline( struct Togl *togl, int argc, char *argv[] )
{
   numline = atof( argv[2] );
   return TCL_OK;
}

int passiline( struct Togl *togl, int argc, char *argv[] )
{
   int i,j,k;
   i = atoi( argv[2] );
   j = atoi( argv[3] );
   segnum[i] = atoi( argv[4] );
   axisnum[i]= atoi( argv[5] );
   for (k=0;k<=j-1;k++) {
      axisval[i][k] = atof( argv[6+k] );
   }
   return TCL_OK;
}

int epsout( struct Togl *togl, int argc, char *argv[] )
{
   Togl_DumpToEpsFile(togl, "swumview_tmp_img.eps", 1, *display );
   return TCL_OK;
}

EXPORT(int,Animate_joint_motion_Init)(Tcl_Interp *interp) 
{
  /*
   * Initialize Tcl, Tk, and the Togl widget module.
   */
#ifdef USE_TCL_STUBS
  if (Tcl_InitStubs(interp, "8.1", 0) == NULL) {return TCL_ERROR;}
#endif
#ifdef USE_TK_STUBS
  if (Tk_InitStubs(interp, "8.1", 0) == NULL) {return TCL_ERROR;}
#endif

   if (Togl_Init(interp) == TCL_ERROR) {
      return TCL_ERROR;
   }

   /*
    * Specify the C callback functions for widget creation, display,
    * and reshape.
    */
   Togl_CreateFunc( init );
   Togl_DestroyFunc( zap );
   Togl_DisplayFunc( display );
   Togl_ReshapeFunc( reshape );
   Togl_TimerFunc( idle );
   Togl_CreateCommand("rotate",rotate);
   Togl_CreateCommand("rotatez",rotatez);
   Togl_CreateCommand("position",position);
   Togl_CreateCommand("positionz",positionz);
   Togl_CreateCommand("advancetime",advancetime);
   Togl_CreateCommand("fchange",fchange);
   Togl_CreateCommand("passnumline",passnumline);
   Togl_CreateCommand("passiline",passiline);
   Togl_CreateCommand("epsout",epsout);
   return TCL_OK;
}
