
/* file: dynavars.h
 * Three-link snake
 * This header defines the required dynamic variables,
 * where the first index is the link number for link-related
 * quantities.  Most vector quantities indexed by link number ii
 * are represented in frame ii, which is rigidly attached
 * to link ii with origin at its hinge except (1) positions
 * and velocities of hinges are represented in the inertial
 * frame ("world coordinates") for convenience in
 * preparing graphics displays; (2) l is represented
 * in the parent frame where it is constant; (3) the external
 * forces and torques are represented in the inertial frame
 * for convenience in testing reactions to them.
 */

#define MAX 5  /* > maximum number of links PLUS ONE SINCE WE COUNT FROM 1 */

/* number of links and the tree structure */

int numlinks, parent[MAX];

/* simulation parameters */

double elapsed_time, time_limit, deltat;

int slowfreq, count;


int pf; /* set pf = 1 to print values, 0 otherwise */


double aG[3]; /* acceleration of gravity (in INERTIAL frame) */
double pH[MAX][3]; /* position of the hinge of link ii (in INERTIAL frame) */
double vH[MAX][3]; /* velocity of the hinge of link ii (in INERTIAL frame) */
double aH[MAX][3]; /* acceleration of the hinge of link ii ( in frame ii)  */
double omega[MAX][3]; /* angular velocity of link ii ( in frame ii) */
double omegadot[MAX][3]; /* angular acceleration of link ii (in frame ii) */
double J[MAX][3][3]; /* inertia matrix of link ii ( constant in frame ii) */
double l[MAX][3]; /* vector from hinge jj=parent[ii] to ii ( in frame jj) */
double m[MAX]; /* mass  of link ii (scalar) */
double c[MAX][3]; /* centre of mass  of link ii (in frame ii) */

/* constraint and external forces and torques at hinges */

double fH[MAX][3]; /* force on parent at the hinge of link ii (in frame ii) */
double gH[MAX][3]; /* torque on parent at hinge  of link ii (in frame ii) */
double FE[MAX][3]; /* external force on link ii at pFE ( INERTIAL frame) */
double pFE[MAX][3]; /* position of application of FE ( in frame ii) */
double GEH[MAX][3]; /* external torque on link ii at hinge( INERTIAL frame) */

/* orientation-specifying 3 x 3 orthogonal matrices */

double ROT[MAX][3][3];/*rotation matrix converting from frame ii to parent */
double ROTT[MAX][3][3]; /* its transpose = its inverse */
double RI[MAX][3][3]; /* transforms from frame ii to the inertial frame */
double RIT[MAX][3][3]; /* its transpose = its inverse */

/* desired joint angle matrices */

double RJ[MAX][3][3];

/* converts an original orientation of hinge ii
 * to a desired orientation in terms of the parent
 * frame of link ii.  The unit matrix indicates
 * the original orientation w. r. t. the parent.
 * If the link ii is in the desired orientation,
 * then ROTT[ii] RJ[ii] is the unit matrix.  Otherwise,
 * its off-diagonal terms generate restorative torques
 * in the compute_torques function. */
			

/* stiffness and friction at the joints */

double stiffness,friction,deadband,straightness,rotdamping;
/* for determining floor and wall properties and torques gH at the joints */

