/* file: dynamics.c
 * DynaTree -Solution to the Equations of Motion of Linear and
 * Tree-like Linkages -- Version 1.0  February 8, 1985.
 *
 * Mathematical solution of the equations of motion and simulation
 * program by:
 *
 * Prof. William W. Armstrong,
 * Department of Computing Science,
 * The University of Alberta,
 * Edmonton, Alberta,
 * Canada,
 * T6G 2H1
 * 
 * Copyright (c) 1985 William W. Armstrong
 *
 * Permission is hereby granted for personal, non-commercial,
 * reproduction and use of this program provided that this notice
 * is included in any material copied from it.
 *
 * No warranty of any kind is provided with this software.       
 * This software is not supported.  Neither the authors, nor the
 * University of Alberta, its officers, agents, servants or employees
 * shall be liable or responsible in any way for any damage to
 * property or direct personal or consequential injury of any nature
 * whatsoever that may be suffered or sustained by any licensee, user
 * or any other party as a consequence of the use or disposition of
 * this software.   
 */


#include <stdio.h>

#include <math.h>

#include "dynavars.h"

#include "hiddenvars.h"

#include "parameters.h"

compute_forces_torques()
/* Not defined for general trees! Caution!*/
{
  int par,link;
  double tempv[3],tempv1[3],tempv2[3],tempv3[3],s,height;

  fixpositions();
  for( link = 1; link <= numlinks; link++){
    /* Zero the external force and torque on the link */
    zerov(FE[link]);
    zerov(pFE[link]);
    zerov(GEH[link]);
  }

  /* Force on hinge */
  for( link = 1; link<=numlinks; link++) {
    eqvec(pH[link],tempv);		/* Get position of Hinge in Inertial Frame*/
    if( pf ){printf("position of Hinge %d in Inertial Frame is",link); prv(tempv);}
    /* If the hinge of the link has hit the
       circular wall of radius 5.0 or the floor
       at height z = 0.0, calculate a confining force.
       We suppose that the links are 1 m long and 0.1 m in radius,
       with spheres of radius 0.1 m at both ends.
       First we compute the distance from the tail to the vertical axis
       in the centre of the arena, and the height of the tail.
    */
    s = tempv[0]*tempv[0]+tempv[1]*tempv[1];
    height = tempv[2];
    if( pf )printf("height of link %d is %le\n",link,height);
    tempv[2] = 0.0;
    if(s > 24.01){  /* 4.9*4.9 = 24.01 */
      if( pf ){printf("Hit wall!!\n");}
      s=sqrt(s);
      sxv(1.0/s,tempv,tempv); /* Unit vector pointing into wall */
      sxv(stiffness*(4.9-s),tempv,FE[link]);
      /* Now compute the velocity of the contact point, and the frictional force */
      sxv(0.1,tempv,tempv); /* vector from hinge to contact point */
      mxv(RI[link],omega[link],tempv3);
      if( pf )printf("Angular velocity of link %d in inertial frame is: ",link); prv(tempv3);
      vxv(tempv3,tempv,tempv1);
      vpv(vH[link],tempv1,tempv1);/*velocity of contact point*/
      sxv(friction,tempv1,tempv1);
      vmv(FE[link],tempv1,FE[link]);
      /* Now get the torque at the hinge */
      vxv(tempv,FE[link],GEH[link]);  
    }
    /* Now add the effect from the floor */
    if (height<0.1){
      if( pf ){printf("Hit floor!!\n");}
      FE[link][2] += stiffness * (0.1-height);
      /* Now compute the velocity of the contact point, and the frictional force */
      setv(0.0,0.0,-0.1,tempv2);	/* vector from hinge to contact*/
      mxv(RI[link],omega[link],tempv3);
      if( pf ){printf("Angular velocity of link %d in inertial frame is:",link); prv(tempv3);}
      vxv(tempv3,tempv2,tempv1);
      if( pf ){printf("Original velocity of link %d in inertial frame is:",link); prv(vH[link]);}
      vpv(vH[link],tempv1,tempv1);
      sxv(friction,tempv1,tempv1);
      if( pf ){printf("New velocity of link %d is:",link); prv(tempv1);}
      vmv(FE[link],tempv1,FE[link]);
      if( pf ){printf("External force on link %d is",link); prv(FE[link]);}
      /* Compute the torque from the floor contact */
      vxv(tempv2,FE[link],tempv1);
      vpv(GEH[link],tempv1,GEH[link]);
      if( pf ){printf("Force on hinge of link %d is:",link); prv(FE[link]);}
    }
  }

  /* Force on tail -- caution, code is almost a repeat of above ***************/
  link = numlinks;
    /* Get the position of the tail-end of the link into tempv */
    setv(0.0,0.0,1.0,tempv1);		/* Tail is at <0,0,1> relative to Hinge */
    mxv(RI[link],tempv1,tempv2);	/* Convert to inertial frame */
    vpv(pH[link],tempv2,tempv);		/* Add to position of Hinge */
    /* If the tail end of the link has hit the
       circular wall of radius 5.0 or the floor
       at height z = 0.0, calculate a confining force.
       We suppose that the links are 1 m long and 0.1 m in radius,
       with spheres of radius 0.1 m at both ends.
       First we compute the distance from the tail to the vertical axis
       in the centre of the arena, and the height of the tail.
    */
    s = tempv[0]*tempv[0]+tempv[1]*tempv[1];
    height = tempv[2];
    tempv[2] = 0.0;
    if(s > 24.01){  /* 4.9*4.9 = 24.01 */
      if( pf ) {printf("Hit wall!!\n");}
      s=sqrt(s);
      sxv(1.0/s,tempv,tempv); /* Unit vector pointing into wall */
      sxv(stiffness*(4.9-s),tempv,tempv1);
      vpv(FE[link],tempv1,FE[link]);
      /* Now compute the velocity of the contact point, and the frictional force */
      sxv(0.1,tempv,tempv);
      vpv(tempv2,tempv,tempv);/* vector from hinge to contact point*/
      mxv(RI[link],omega[link],tempv3);
      vxv(tempv3,tempv,tempv1);
      vpv(vH[link],tempv1,tempv1);/*velocity of contact point*/
      sxv(friction,tempv1,tempv1);
      vmv(FE[link],tempv1,FE[link]);
      /* Now get the torque at the hinge */
      vxv(tempv,FE[link],tempv1);
      vpv(GEH[link],tempv1,GEH[link]);  
    }
    /* Now add the effect from the floor */
    if (height<0.1){
      if( pf ) {printf("Hit floor!!\n");}
      FE[link][2] += stiffness * (0.1-height);
      /* Now compute the velocity of the contact point, and the frictional force */
      setv(0.0,0.0,-0.1,tempv);	
      vpv(tempv2,tempv,tempv2);/* vector from hinge to contact*/
      mxv(RI[link],omega[link],tempv3);
      vxv(tempv3,tempv2,tempv1);
      vpv(vH[link],tempv1,tempv1);
      sxv(friction,tempv1,tempv1);
      if ( pf ) {printf("Velocity of link %d is:",link); prv(tempv1);}
      vmv(FE[link],tempv1,FE[link]);
      if( pf ) {printf("External force on link %d is",link); prv(FE[link]);}
      /* Compute the torque from the floor contact */
      vxv(tempv2,FE[link],tempv1);
      vpv(GEH[link],tempv1,GEH[link]);
      if( pf) {printf("Force on hinge of tail of link %d is:",link); prv(FE[link]);}
    }
      if( pf )printf("\nIn compute_forces_torques():\n");
      if( pf ){for(link=1; link <= numlinks; link++) {
    	printf("Force on hinge of link %d is:",link); prv(FE[link]);}
   	printf("\n");}
      if( pf ){for(link=1; link <= numlinks; link++) {
  	printf("External torque on hinge of link %d is:",link); prv(GEH[link]);}
  	printf("\n");}

/**********************/
  /* Torques at the hinges */
  zerov(gH[1]);
  for( link = 2; link <= numlinks; link++){
    gH[link][0]=( ROT[link][2][1]-ROT[link][1][2])*straightness;
    gH[link][1]=( ROT[link][0][2]-ROT[link][2][0])*straightness;
    gH[link][2]=( ROT[link][1][0]-ROT[link][0][1])*straightness;
    /* Introduce damping */
    mxv(ROTT[link],omega[parent[link]],tempv);
    vmv(omega[link],tempv,tempv);
    sxv(rotdamping,tempv,tempv3);
    vpv(gH[link],tempv3,gH[link]);
  }

    /* Modify the above to get slithering: */
    link=2;
    gH[link][0] += doub1 * sin(elapsed_time * 2.0 * M_PI * doub5 +
    M_PI_2 * (double) int1);
    gH[link][1] += doub2 * sin(elapsed_time * 2.0 * M_PI * doub6 +
    M_PI_2 * (double) int2);
    link=3;
    gH[link][0] += doub3 * sin(elapsed_time * 2.0 * M_PI * doub7 +
    M_PI_2 * (double) int3);
    gH[link][1] += doub4 * sin(elapsed_time * 2.0 * M_PI * doub8 +
    M_PI_2 * (double) int4);
}

slowband_in()
{

	int link,outlink;
	double tempv[3],tempv1[3],tempv2[3];
	double tempm[3][3],tempm1[3][3],tempm2[3][3],tempm3[3][3];

	for(link = numlinks;link > 0; link--){

		/* compute aC, Q, W and derived quantities for all outlinks of link */
		zerom(W[link]);
		zerom(tempm3); /* accumulates for W ltilde */
		for(outlink = numlinks;outlink > 1;outlink--){
			if(parent[outlink] == link){
				vxv(omega[link],l[outlink],tempv);
				vxv(omega[link],tempv,aC[outlink]);
				mxm(ROT[outlink],M[outlink],tempm);
				mxm(tempm,ROTT[outlink],Q[outlink]);
				mxm(ltilde[outlink],Q[outlink],W[outlink]);
				mpm(W[link],W[outlink],W[link]);
				mxm(W[outlink],ltilde[outlink],tempm1);
				mpm(tempm3,tempm1,tempm3);
			}

		      }

		/* compute T, K, M for the link */
		mpm(J[link],tempm3,tempm);
		inv(tempm,T[link]);
		mmm(W[link],mctilde[link],tempm3);
		mxm(T[link],tempm3,K[link]);
		zerom(tempm2); /* accumulates for sum of Q(1-ltilde K) */

		for(outlink=numlinks;outlink>1;outlink--){
			if(parent[outlink] == link){
				mxm(ltilde[outlink],K[link],tempm);
				mxm(Q[outlink],tempm,tempm1);
				mmm(Q[outlink],tempm1,tempm1);
				mpm(tempm2,tempm1,tempm2);
			}
		      }

		mmm(tempm2,munitm[link],tempm2);
		mxm(mctilde[link],K[link],tempm);
		mpm(tempm2,tempm,M[link]);

		if( pf ) printf("Matrix M[%d] is",link);
		if( pf ) prm(M[link]);

		/* compute g1sigma[link] */
		mxv(J[link],omega[link],tempv);
		vxv(omega[link],tempv,tempv1);
		mxv(RIT[link],GEH[link],tempv);
		vmv(tempv,tempv1,tempv);
		mxv(RIT[link],aG,tempv1);
		vxv(mc[link],tempv1,tempv2);
/*		vpv(tempv,tempv2,tempv1); */
/* ***		mxv(RIT[link],FE[link],tempv2);/* transform external force */
/* ***		vxv(pFE[link],tempv2,tempv2);  /* torque at the hinge */
/*printf("'torque at the hinge': "); prv(tempv2); */
/* ***		vpv(tempv1,tempv2,g1sigma[link]); */
		vpv(tempv,tempv2,g1sigma[link]);

		/* compute fsigma[link] */
		vxv(omega[link],mc[link],tempv);
		vxv(omega[link],tempv,tempv1);
		vpv(maG[link],FE[link],tempv);
		mxv(RIT[link],tempv,tempv2);
		vmv(tempv2,tempv1,fsigma[link]);
		/* assumes that FE[link] varies slowly */
	}

} /*end of slowband_in */

fastband_in()
{

	int link,outlink;
	double tempv[3],tempv1[3],tempv2[3],tempv3[3],tempv4[3],tempv5[3],tempv6[3];

	for(link = numlinks;link > 0; link--){
		vmv(g1sigma[link],gH[link],tempv);
		/* tempv accumulates for gsigma[link] */
		zerov(tempv2);
		/* tempv2 accumulates for d[link] */
		for(outlink=numlinks;outlink>1;outlink--){
			if(parent[outlink] == link){
				mxv(ROT[outlink],gH[outlink],tempv1);
				vpv(tempv,tempv1,tempv);
				mxv(Q[outlink],aC[outlink],tempv3);
				vpv(tempv3,fdblprime[outlink],tempv3);
				vxv(l[outlink],tempv3,tempv4);
				vpv(tempv4,tempv2,tempv2);
			}
		      }
		eqvec(tempv,gsigma[link]);
		vpv(tempv,tempv2,tempv);
		mxv(T[link],tempv,d[link]);
		zerov(tempv6); /* tempv6 accumulates for fprime[link] */
		for(outlink=numlinks;outlink>1;outlink--){ /* changed to >1 */
			if(parent[outlink] == link){
			  vxv(l[outlink],d[link],tempv4);
				vmv(aC[outlink],tempv4,tempv4);
				mxv(Q[outlink],tempv4,tempv5);
				vpv(fdblprime[outlink],tempv5,tempv5);
				vpv(tempv6,tempv5,tempv6);
			}
		      }
		vxv(mc[link],d[link],tempv);
		vpv(tempv6,tempv,tempv6);
		vpv(tempv6,fsigma[link],fprime[link]);
		if(link != 1)mxv(ROT[link],fprime[link],fdblprime[link]);
	      }
}

fastband_out()
{

	int link,inlink;
	double temps;
	double tempv[3],tempm[3][3],tempm1[3][3];

	/* treat body 1, the root, in a special way: */

	if( pf ) printf("M[1] is:\n");
	if( pf ) prm(M[1]);

	inv(M[1],tempm);

	if( pf ) printf(" Its inverse is:\n");
	if( pf ) prm(tempm);
	if( pf ) printf(" And the product ( unit?) matrix to check :\n");

	mxm(M[1],tempm,tempm1 );

	if ( pf ) prm(tempm1);

	mxv(tempm,fprime[1],aH[1]);
	sxv(-1.0,aH[1],aH[1]);

	if( pf ) printf(" K[1], aH[1], d[1] are:\n");
	if( pf ) prm(K[1]);
	if( pf ) prv(aH[1]);
	if( pf ) prv(d[1]);

	mxv(K[1],aH[1],tempv);
	vpv(tempv,d[1],omegadot[1]);

	for(link = 2; link <= numlinks; link++){
		/*compute aH[link] */
		inlink = parent[link];
		vxv(l[link],omegadot[inlink],tempv);
		vmv(aC[link],tempv,tempv);
		vpv(aH[inlink],tempv,tempv);
		mxv(ROTT[link],tempv,aH[link]); /* mtm(ROT -> mxv(ROTT Oct 3 90 */
		/* compute omegadot[link] */
		mxv(K[link],aH[link],tempv);
		vpv(tempv,d[link],omegadot[link]);
		/* compute the constraint force at the hinge ( optional) */
		mxv(M[link], aH[link], tempv);
		vpv(tempv,fprime[link],fH[link]);
	      }

	if( pf )printf("\nIn fastband_out():\n");
	if( pf )for(link = 1; link <= numlinks; link++){
		printf("K for link %d is:\n",link); prm(K[link]);}
	if( pf )for(link = 1; link <= numlinks; link++){
		printf("d for link %d is: ",link); prv(d[link]);}
	if( pf )for(link = 1; link <= numlinks; link++){
		printf("omegadot for link %d is: ",link); prv(omegadot[link]);}
	if( pf )for(link = 1; link <= numlinks; link++){
		printf("aC for link %d is: ",link); prv(aC[link]);}
	if( pf )for(link = 1; link <= numlinks; link++){
		printf("aH for link %d is: ",link); prv(aH[link]);}
	if( pf )printf("In Inertial Frame omegadot, aC, and aH are:\n");
	if( pf )for(link = 1; link <= numlinks; link++){
    	    mxv(RI[link],omegadot[link],tempv);
		printf("omegadot for link %d is: ",link); prv(tempv);}
	if( pf )for(link = 2; link <= numlinks; link++){
     	   mxv(RI[parent[link]],aC[link],tempv);
		printf("aC for link %d is: ",link); prv(tempv);}
	if( pf )for(link = 1; link <= numlinks; link++){
   		mxv(RI[link],aH[link],tempv);
		printf("aH for link %d is: ",link); prv(tempv);}
}

check()
{
	int link,outlink;
	double tempv[3],tempv1[3],tempv2[3],tempm[3][3];
/*
 *	if( pf ) printf(" numlinks = %d\n ",numlinks);
 */
	for(link = 1;link <= numlinks;link++){
		/* first check the values of omegadot of the links */
		zerov(tempv); /* tempv accumulates for omegadot equation */
		for(outlink = numlinks;outlink>1;outlink--){
			if( parent[outlink] == link){
				mxv(ROT[outlink],fH[outlink],tempv1);
				vxv(l[outlink],tempv1,tempv2);
				vpv(tempv,tempv2,tempv);
				mxv(ROT[outlink],gH[outlink],tempv1);
				vpv(tempv,tempv1,tempv);
			}
		}
	        mxv(RIT[link],GEH[link],tempv1); /* mtm(RI -> mxv(RIT Oct 3 90 */
		vpv(tempv,tempv1,tempv);
		mxv(RIT[link],FE[link],tempv1); /* mtm(RI -> mxv(RIT Oct 3 90 */
		vxv(pFE[link],tempv1,tempv2);
		vpv(tempv,tempv2,tempv);
		sxv(m[link],c[link],tempv1);
		vxv(tempv1,aH[link],tempv2);
		vmv(tempv,tempv2,tempv);
		mxv(J[link],omega[link],tempv2);
		vxv(omega[link],tempv2,tempv1);
		vmv(tempv,tempv1,tempv);
		vmv(tempv,gH[link],tempv);
	 	mxv(RIT[link],aG,tempv1); /* mtm(RI -> mxv(RIT Oct 3 90 */
		vxv(mc[link],tempv1,tempv2);
		vpv(tempv,tempv2,tempv);
		inv(J[link],tempm);
		mxv(tempm,tempv,tempv1);

		if( pf ) printf(" The two values of omegadot for link %d are:\n",link);
		if ( pf ) prv(tempv1);
		if ( pf ) prv(omegadot[link]);

		/* now compute the constraint force fH */
		vxv(omega[link],c[link],tempv1);
		vxv(tempv1,omega[link],tempv);
		vxv(c[link],omegadot[link],tempv1);
		/* The next line was changed back to the original, Sept 17, 89 */
		vpv(tempv,tempv1,tempv);
		vmv(tempv,aH[link],tempv);
		sxv(m[link],tempv,tempv);
		for(outlink = numlinks; outlink > 1;outlink--){
			if( parent[outlink] == link ){
				mxv(ROT[outlink],fH[outlink],tempv1);
				vpv(tempv,tempv1,tempv);
			}
		}
	/* ***	sxv(m[link],aG,tempv1); 	Changed to the following line Oct 6 90 */
	/* ***	vpv(FE[link],tempv1,tempv2);                                           */
		vpv(FE[link],maG[link],tempv2);
		mxv(RIT[link],tempv2,tempv1); /* mtm(RI -> mxv(RIT Oct 3 90 */
		vpv(tempv,tempv1,tempv);

		if( pf ) printf(" The two values of fH for link %d are:\n",link);
		if( pf ) prv(tempv);
		if( pf ) prv(fH[link]);

		if( pf ) printf(" The matrix ROT for link %d is:\n",link);
		if( pf ) prm(ROT[link]);

		if( pf ) printf(" The matrix RI for link %d is:\n",link);
		if( pf ) prm(RI[link]);
	} /* go to next link */
}/* end of check */

fast_integ()
{
	double tempv[3],tempv1[3];
	int link;
	for( link = 1; link <= numlinks; link++){
		sxv(deltat,omegadot[link],tempv);
		/* Insert a better integration -- Aug 21 89 */
		sxv(0.5,tempv,tempv1);
		vpv(omega[link],tempv1,tempv1);
		vpv(omega[link],tempv,omega[link]);
		sxv(deltat,tempv1,tempv);

		if ( pf ) {printf("Rotation added to link %d is:",link); prv(tempv);}

		vpv(tempv,deltarot[link],deltarot[link]);
	}
	for(link=1;link<=numlinks;link++){
		mxv(RI[link],aH[link],tempv1);
		sxv(deltat,tempv1,tempv);
		/* Insert a better integration -- Aug 21 89 */
		sxv(0.5,tempv,tempv1);
		vpv(vH[link],tempv1,tempv1);
		if ( pf ) {printf("Velocity added to link %d is:",link); prv(tempv1);}
		vpv(vH[link],tempv,vH[link]);
		if (link == 1){ /* !=0 had no effect */
			sxv(deltat,tempv1,tempv);
			if ( pf ) {printf("Displacement added to link %d is:",link); prv(tempv);}
			vpv(pH[link],tempv,pH[link]);
		}
		/* The above is done for links >1 to deal with friction at the floor */
		fixpositions(); /* Added Oct 6 90 */
	}
}

slow_integ()
{
	double tempv[3],tempv1[3],tempm[3][3],tempm1[3][3];
	int link,par;
	for( link = 1 ; link <= numlinks; link++){
		/* Changed Sept 17, 89. An orthon has been inserted
		   to stop errors accumulating.
		*/
		vrotm(deltarot[link],tempm);

		if( pf ) printf(" The incremental rotation matrix for link %d is:\n",link);
		if( pf ) prm(tempm);

		zerov(deltarot[link]);
		mxm(RI[link],tempm,tempm1);
		orthon(tempm1);
		eqm(tempm1,RI[link]);
		mt(RI[link],RIT[link]);
		if( link != 1 ){
			mxm(RIT[parent[link]],tempm1,ROT[link]);
			mt(ROT[link],ROTT[link]);
		}
      }


	fixpositions();
}

fixpositions()	/* fix positions of links in inertial frame. */
		/* *must* be called between writes to RI and */
		/* reads of pH                               */{
	int par,link;
	double tempv[3];
	for( link = 2; link <= numlinks; link++ ){
		par = parent[link];
		mxv(RI[par],l[link],tempv);
		vpv(pH[par],tempv,pH[link]);
	}
}
