
/* file init.c */


/* Three-link snake initialization
 * Started August 21, 1989
 *
 * Program by:
 *
 * Prof. William W. Armstrong,
 * Department of Computing Science,
 * The University of Alberta,
 * Edmonton, Alberta,
 * Canada,
 * T6G 2H1
 * 
 * Copyright (c) 1989 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.
 * The author assumes no responsibility for damages resulting from
 * the use of this software however caused.
 */

#include <stdio.h>
#include <math.h>
#include "dynavars.h"
#include "hiddenvars.h"
#include "parameters.h"

extern	char	*nameptr ;


initialize()
/* This function initializes the tree configuration and the
 * quantities required in the dynamics computations
 */

{
FILE	*filestrm ;
int	ii,link;
	int Flink;
	double Fx,Fy,Fz,cc,aGzval,tempv[3];

/* We define the number of links and give the parent of each link */
	numlinks = 3;
	parent[0] = 0; /* should never be used! */
	parent[1] = 0;  /* 1 = head */
	parent[2] = 1;  /* 2 = centre */
	parent[3] = 2;  /* 3 = tail */
/* Set most quantities to zero or the unit matrix */
/* Start at 0 -- Aug 21 89, go to (MAX-1) --Aug 28 89  */
/*	for(link=0;link < MAX;link++){                 */
/* Start at 1, go to numlinks --Oct 01 90  */
	for(link=1;link <= numlinks;link++){
		zerov(pH[link]);
		zerov(vH[link]);
		zerov(omega[link]);
		zerov(omegadot[link]);
		unitm(J[link]);
		zerov(fH[link]);
		zerov(aH[link]);
		zerov(gH[link]);
		unitm(ROT[link]);
		unitm(ROTT[link]);
		unitm(RI[link]);
		unitm(RIT[link]);
		unitm(RJ[link]);
		zerov(GEH[link]);
		zerov(FE[link]);
		zerov(pFE[link]);
		zerom(Q[link]);
		zerom(W[link]);
		unitm(T[link]);
		zerom(K[link]);
		zerov(d[link]);
		zerom(M[link]);
		zerov(aC[link]);
		zerov(fprime[link]);
		zerov(fdblprime[link]);
		zerov(gsigma[link]);
		zerov(g1sigma[link]);
		zerov(fsigma[link]);
		zerov(deltarot[link]);
	}

/* set the vector from the parent hinge to the hinge of link ii,
 * this is in coordinates of the parent
 */
	zerov(l[1]);  /* link 1 has no parent */
	setv(0.0,0.0,1.0,l[2]);
	setv(0.0,0.0,1.0,l[3]);


/* set the centres of mass of the links */
	setv(0.0,0.0,0.5,c[1]);
	setv(0.0,0.0,0.5,c[2]);
	setv(0.0,0.0,0.5,c[3]);

/* set the masses of the links */
	m[1] = 10.0;
	m[2] = 10.0;
	m[3] = 10.0;

/* set the inertia matrix diagonal entries */

	J[1][0][0] = 3.3833;
	J[1][1][1] = 3.3833;
	J[1][2][2] = 0.1;

	J[2][0][0] = 3.3833;
	J[2][1][1] = 3.3833;
	J[2][2][2] = 0.1;

	J[3][0][0] = 3.3833;
	J[3][1][1] = 3.3833;
	J[3][2][2] = 0.1;

	filestrm = fopen( nameptr, "r" );

/* enter the simulation time limit, starting from 0.0 */
	fscanf( filestrm, "%lf",&time_limit);
	fscanf( filestrm, "%lf",&deltat);
	fscanf( filestrm, "%d",&slowfreq);
	fscanf( filestrm, "%lf",&aGzval);

/* acceleration of gravity (down = +z axis) */
		setv(0.0,0.0,-aGzval,aG);

/* set the constants */
	for(link = 1; link <= numlinks; link++){
		sxv(m[link],c[link],mc[link]);
		tilde(c[link],mctilde[link]);
		sxm(m[link],mctilde[link],mctilde[link]);
		unitm(munitm[link]);
		sxm(m[link],munitm[link],munitm[link]);
		sxv(m[link],aG,maG[link]);
		if (link != 1) tilde(l[link],ltilde[link]);
	}

/* set the torques */
	while(1){
		fscanf( filestrm, "%d",&Flink);
		if( Flink < 1 )break;
		fscanf( filestrm, "%lf",&Fx);
		fscanf( filestrm, "%lf",&Fy);
		fscanf( filestrm, "%lf",&Fz);
		setv(Fx,Fy,Fz,gH[Flink]);
	}
	fscanf( filestrm, "%lf",&stiffness);
	deadband = 0.05;
	fscanf( filestrm, "%lf",&friction);
	fscanf( filestrm, "%lf",&straightness);
	fscanf( filestrm, "%lf",&rotdamping);

/* position of hinge of link 1 (three reals) */
	fscanf( filestrm, "%lf %lf %lf", &pH[1][0], &pH[1][1], &pH[1][2]);
	fixpositions();
	while(1){
/* link to be oriented ( link=0 to end */
		fscanf( filestrm, " %d", &link);
		if(link < 1 )break;
/* roll, pitch, yaw, in degrees */
		fscanf( filestrm, " %lf %lf %lf",&tempv[0], &tempv[1], &tempv[2]);
		cc = M_PI/180.0;
		sxv(cc,tempv,tempv);
		vrotm(tempv,RI[link]);
		for(ii=1;ii<=numlinks;ii++){
			mt(RI[link],RIT[link]);
			if(ii > 1 ){
				mxm(RIT[parent[link]],RI[link],ROT[link]);
				mt(ROT[link],ROTT[link]);
			}
		}
		fixpositions();
	}

	fscanf( filestrm, "%lf", &doub1 );
	fscanf( filestrm, "%lf", &doub2 );
	fscanf( filestrm, "%lf", &doub3 );
	fscanf( filestrm, "%lf", &doub4 );
	fscanf( filestrm, "%lf", &doub5 );
	fscanf( filestrm, "%lf", &doub6 );
	fscanf( filestrm, "%lf", &doub7 );
	fscanf( filestrm, "%lf", &doub8 );
	fscanf( filestrm, "%lf", &doub9 );
	fscanf( filestrm, "%lf", &doub10 );
	fscanf( filestrm, "%d", &int1 );
	fscanf( filestrm, "%d", &int2 );
	fscanf( filestrm, "%d", &int3 );
	fscanf( filestrm, "%d", &int4 );
	fscanf( filestrm, "%d", &int5 );
	fscanf( filestrm, "%d", &int6 );
	fscanf( filestrm, "%d", &int7 );
	fscanf( filestrm, "%d", &int8 );
	fscanf( filestrm, "%d", &int9 );
	fscanf( filestrm, "%d", &int10 );

}
