
/* file: matrix.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.
 * The author assumes no responsibility for damages resulting from
 * the use of this software however caused.
 */


/* This package of functions performs matrix and vector operations. */

/* WARNING: in general one cannot use the last argument (output)
 * as an input argument to a function.  e. g. for matrix multiplication,
 * mxm(tempm,tempm1,tempm) is incorrect, since altered values of tempm
 * are used in the later parts of the computation as inputs.  Exceptions
 * to this are where the alteration is done componentwise:
 * sxv, sxm, vpv, vmv, mpm, mmm,...
 */

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

sxv(a,b,c)
/* c is vector b multiplied by scalar a */

double	a,b[3],c[3];
{
	c[0]  =  b[0] * a;
	c[1]  =  b[1] * a;
	c[2]  =  b[2] * a;
}


sxm(a,b,c)
/* c is the product of scalar a and 3x3 matrix b */

double a,b[9],c[9];

{
	register short int ii;
	for(ii = 0; ii < 9 ;ii++){
		c[ii] = a * b[ii];
	}
}

mxm(a,b,c)
/* c is the product of 3x3 matrices a,b */
double a[9],b[9],c[9];
{

c[8]= a[8] * b[8]+ a[7] * b[5]+ a[6] * b[2];

c[7]= a[8] * b[7]+ a[7] * b[4]+ a[6] * b[1];

c[6]= a[8] * b[6]+ a[7] * b[3]+ a[6] * b[0];

c[5]= a[5] * b[8]+ a[4] * b[5]+ a[3] * b[2];

c[4]= a[5] * b[7]+ a[4] * b[4]+ a[3] * b[1];

c[3]= a[5] * b[6]+ a[4] * b[3]+ a[3] * b[0];

c[2]= a[2] * b[8]+ a[1] * b[5]+ a[0] * b[2];

c[1]= a[2] * b[7]+ a[1] * b[4]+ a[0] * b[1];

c[0]= a[2] * b[6]+ a[1] * b[3]+ a[0] * b[0];

}

mtm(a,b,c)
/* c is the product of 3x3 matrices: transpose of a times b */
double a[3][3],b[3][3],c[3][3];
{
	register short int ii,jj,kk;
	for(ii=0;ii<3;ii++){
		for(jj=0;jj<3;jj++){
			c[ii][jj] = 0;
			for(kk=0;kk<3;kk++){
				c[ii][jj] += a[kk][ii] * b[kk][jj];
			}
		}
	}
}

mxv(a,b,c)
/* c is the product of 3x3 matrix a and vector b */
double a[9],b[3],c[3];
{

c[2]= a[8] * b[2]+ a[7] * b[1]+ a[6] * b[0];

c[1]= a[5] * b[2]+ a[4] * b[1]+ a[3] * b[0];

c[0]= a[2] * b[2]+ a[1] * b[1]+ a[0] * b[0];

}

mt(a,b)
/* b becomes the transpose of 3x3 matrix a */


double a[3][3],b[3][3];
{
	register short int ii,jj;
	for(ii=0;ii<3;ii++) {
		for(jj=0;jj<3;jj++) {
			b[ii][jj] = a[jj][ii];
		}
	}
}

eqm(a,b)
/* assign the value of 3x3 matrix a to b */

double a[3][3],b[3][3];
{
	register short int ii,jj;
	for(ii=0;ii<3;ii++) {
		for(jj=0;jj<3;jj++) {
			b[ii][jj] = a[ii][jj];
		}
	}
}

eqvec(a,b)
/* assign the 3-vector a to b */

double a[3],b[3];
{
	b[0] = a[0];
	b[1] = a[1];
	b[2] = a[2];
}

vpv(a,b,c)
/* c becomes the vector sum of 3-vectors a and b */
double a[3],b[3],c[3];
{
	c[0] = a[0] + b[0];
	c[1] = a[1] + b[1];
	c[2] = a[2] + b[2];
}


vmv(a,b,c)
/* c becomes the vector difference of 3-vectors: a minus b */
double a[3],b[3],c[3];
{
	c[0] = a[0] - b[0];
	c[1] = a[1] - b[1];
	c[2] = a[2] - b[2];
}

vdv(a,b,pc)
/* c becomes the scalar or dot product of 3-vectors: a . b */
double a[3],b[3],*pc;
{
	*pc = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

vxv(a,b,c)
/* c becomes the vector product axb */
double a[3],b[3],c[3];
{
	c[0] = a[1] * b[2] - a[2] * b[1];
	c[1] = a[2] * b[0] - a[0] * b[2];
	c[2] = a[0] * b[1] - a[1] * b[0];
}

zerov(z)
/* sets z to the zero vector */
double z[3];
{
	z[0] = 0.0;
	z[1] = 0.0;
	z[2] = 0.0;
}

unitm(m)
/* sets m to the unit 3x3 matrix */
double m[3][3];
{
	m[0][0] = 1.0;
	m[1][0] = 0.0;
	m[2][0] = 0.0;
	m[0][1] = 0.0;
	m[1][1] = 1.0;
	m[2][1] = 0.0;
	m[0][2] = 0.0;
	m[1][2] = 0.0;
	m[2][2] = 1.0;
}

zerom(m)
/* sets m to the zero 3x3 matrix */
double m[3][3];
{
	m[0][0] = 0.0;
	m[1][0] = 0.0;
	m[2][0] = 0.0;
	m[0][1] = 0.0;
	m[1][1] = 0.0;
	m[2][1] = 0.0;
	m[0][2] = 0.0;
	m[1][2] = 0.0;
	m[2][2] = 0.0;
}

dyad(a,b,c)
/* c is the dyad formed from a and b */
double a[3],b[3],c[3][3];
{
	register short int ii,jj;
	for(ii = 0; ii < 3; ii++){
		for(jj = 0; jj < 3; jj++){
			c[ii][jj] = a[ii] * b[jj];
		}
	}
}

mpm(a,b,c)
/* c becomes the sum of 3x3 matrices a and b */
double a[3][3],b[3][3],c[3][3];
{
	c[0][0] = a[0][0] + b[0][0];
	c[0][1] = a[0][1] + b[0][1];
	c[0][2] = a[0][2] + b[0][2];
	c[1][0] = a[1][0] + b[1][0];
	c[1][1] = a[1][1] + b[1][1];
	c[1][2] = a[1][2] + b[1][2];
	c[2][0] = a[2][0] + b[2][0];
	c[2][1] = a[2][1] + b[2][1];
	c[2][2] = a[2][2] + b[2][2];
}

mmm(a,b,c)
/* c becomes the difference of 3x3 matrices: a minus b */
double a[3][3],b[3][3],c[3][3];
{
	c[0][0] = a[0][0] - b[0][0];
	c[0][1] = a[0][1] - b[0][1];
	c[0][2] = a[0][2] - b[0][2];
	c[1][0] = a[1][0] - b[1][0];
	c[1][1] = a[1][1] - b[1][1];
	c[1][2] = a[1][2] - b[1][2];
	c[2][0] = a[2][0] - b[2][0];
	c[2][1] = a[2][1] - b[2][1];
	c[2][2] = a[2][2] - b[2][2];
}

det(a,b)
/* b is the determinant of 3x3 matrix a */

double a[3][3],b[];
{
	b[0] = a[0][0] * ( a[1][1] * a[2][2] - a[1][2] * a[2][1] )
		+      a[0][1] * ( a[1][2] * a[2][0] - a[1][0] * a[2][2] )
			+      a[0][2] * ( a[1][0] * a[2][1] - a[1][1] * a[2][0] );
}

inv(a,b)
/* b becomes the inverse of 3x3 matrix a */
double a[3][3],b[3][3];
{
	double d;
	det(a,&d);
	b[0][0] = ( a[1][1] * a[2][2] - a[1][2] * a[2][1] )/d;
	b[1][0] = ( a[1][2] * a[2][0] - a[1][0] * a[2][2] )/d;
	b[2][0] = ( a[1][0] * a[2][1] - a[1][1] * a[2][0] )/d;
	b[0][1] = ( a[2][1] * a[0][2] - a[2][2] * a[0][1] )/d;
	b[1][1] = ( a[2][2] * a[0][0] - a[2][0] * a[0][2] )/d;
	b[2][1] = ( a[2][0] * a[0][1] - a[2][1] * a[0][0] )/d;
	b[0][2] = ( a[0][1] * a[1][2] - a[0][2] * a[1][1] )/d;
	b[1][2] = ( a[0][2] * a[1][0] - a[0][0] * a[1][2] )/d;
	b[2][2] = ( a[0][0] * a[1][1] - a[0][1] * a[1][0] )/d;
}

mtv(a,b,c)
/* c becomes the product of the transpose of 3x3 matrix a and 3-vector b */
double a[3][3],b[3],c[3];

{
	register short int ii,jj;
	for(ii = 0; ii < 3; ii++){
		c[ii] = 0;
		for(jj = 0; jj < 3; jj++){
			c[ii] += a[jj][ii] * b[jj];
		}
	}
}

tilde(a,b)
/* b becomes a matrix which gives the same effect when
 * it is used as a left multiplier as the cross product with a;
 * i. e.
 * tilde(a,b);
 * mxv(b,c,d);
 * is equivalent to
 * vxv(a,c,d)
 */

double a[3],b[3][3];
{
	b[0][0] = 0.0;
	b[1][1] = 0.0;
	b[2][2] = 0.0;
	b[1][0] = a[2];
	b[2][0] = -a[1];
	b[0][1] = -a[2];
	b[2][1] = a[0];
	b[0][2] = a[1];
	b[1][2] = -a[0];
}
vrotm(a,b)
/* Converts a vector a to a rotation matrix b.
 * a has components of roll, pitch, and yaw in that order
 * (expressed in radians); b is the resulting rotation matrix
 * when yaw is applied first, then pitch, then roll; the result is then
 * the product of roll, pitch and yaw matrices from left to right.
 * b transforms coordinates in the moved frame to coordinates in the original
 * frame.
 * Corrected Sept. 7, 1989:  "moved" and "original" interchanged.
 */

double a[3],b[3][3];
{
	double phi,theta,psi;
	phi=a[0];
	theta=a[1];
	psi=a[2];
	b[0][0] = cos(theta) * cos(psi);
	b[0][1] = sin(phi) * cos(psi) * sin(theta) - cos(phi) * sin(psi);
	b[0][2] = cos(phi) * cos(psi) *sin(theta) + sin(phi) * sin(psi);
	b[1][0] = cos(theta) * sin(psi);
	b[1][1] = sin(phi) * sin(psi) * sin(theta) + cos(phi) * cos(psi);
	b[1][2] = cos(phi) * sin(psi) * sin(theta) - sin(phi) * cos(psi);
	b[2][0] = -sin(theta);
	b[2][1] = sin(phi) * cos(theta);
	b[2][2] = cos(phi) * cos(theta);
}

setv(a,b,c,d)
/* sets vector d to have components a, b, c */
double a,b,c;
double d[3];
{
	d[0] = a;
	d[1] = b;
	d[2] = c;
}


prs(a)
/* prints a scalar a */
double a;
{
	printf(" Scalar is %e \n",a);
}

prv(b)
/* prints  vector */
double b[3];
{
	printf(" Vector is %e %e %e\n",b[0],b[1],b[2]);
}

prm(c)
/* prints a matrix */
double c[3][3];
{
	printf(" Matrix is \n %e %e %e \n %e %e %e \n %e %e %e\n",
	c[0][0],c[0][1],c[0][2],
	c[1][0],c[1][1],c[1][2],
	c[2][0],c[2][1],c[2][2]);
}
orthon(r)
/* operates on a matrix which is approximately
 * orthogonal to orthogonalize it.*/
double r[3][3];
{
	double s;

/* normalize the third column of r */
	s = 1.0/sqrt (r[0][2]*r[0][2]+r[1][2]*r[1][2]+r[2][2]*r[2][2]);

/* s is the reciprocal of
 *the length of the third column
 */
	r[0][2] = r[0][2] * s;
	r[1][2] = r[1][2] * s;
	r[2][2] = r[2][2] * s;

/* orthogonalize the second column w. r. t. the third.*/
	s = r[0][1]*r[0][2]+r[1][1]*r[1][2]+r[2][1]*r[2][2];
	r[0][1] = r[0][1] - r[0][2] * s;
	r[1][1] = r[1][1] - r[1][2] * s;
	r[2][1] = r[2][1] - r[2][2] * s;

/* normalize the second column */
	s = 1.0/sqrt (r[0][1]*r[0][1]+r[1][1]*r[1][1]+r[2][1]*r[2][1]);
	r[0][1] = r[0][1] * s;
	r[1][1] = r[1][1] * s;
	r[2][1] = r[2][1] * s;

/* form the vector product of the second and third columns
 * to get the first column
 */
	r[0][0] = r[1][1]*r[2][2] - r[2][1]*r[1][2];
	r[1][0] = r[2][1]*r[0][2] - r[0][1]*r[2][2];
	r[2][0] = r[0][1]*r[1][2] - r[1][1]*r[0][2];
}

randv(vect)
double vect[3];

#define MID_RAND 1073741823
{
	vect[0] = (double)rand()/MID_RAND;
	vect[1] = (double)rand()/MID_RAND;
	vect[2] = (double)rand()/MID_RAND;
}
