/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : trans.c
 *      Remarks : All the vector and transform utility functions.
 *      Usage   : part of the library
 */

/*LINTLIBRARY*/


#include "../h/rccl.h"
#include "../h/umac.h"

/*
 * make a copy of a vector
 */

VECT_PTR assignvect(r, v) /*::*/
register VECT_PTR r, v;
{
	r->x = v->x;
	r->y = v->y;
	r->z = v->z;
	return(r);
}


/*
 * computes dot product of two vectors
 */

real dot(u, v) /*::*/
register VECT_PTR u, v;
{
	return(u->x * v->x + u->y * v->y + u->z * v->z);
}


/*
 * computes the cross product of two vectors
 */

VECT_PTR cross(r, u, v) /*::*/
register VECT_PTR r, u, v;
{
	r->x = u->y * v->z - u->z * v->y;
	r->y = u->z * v->x - u->x * v->z;
	r->z = u->x * v->y - u->y * v->x;
	return(r);
}


/*
 * brings the magnitude of a vector to unity
 */

VECT_PTR unit(v, u) /*::*/
register VECT_PTR v, u;
{
	real m;

	m = sqrt(dot(u, u));
	if (m < SMALL) {
		giveup("cannot unit vector", YES);
	}
	v->x = u->x / m;
	v->y = u->y / m;
	v->z = u->z / m;
	return(v);
}


/*
 * assigns t (rhs) transform to r (lhs)  transform
 */

TRSF_PTR assigntr(r, t) /*::*/
register TRSF_PTR r, t;
{
	r->n.x = t->n.x;
	r->o.x = t->o.x;
	r->a.x = t->a.x;
	r->p.x = t->p.x;
	r->n.y = t->n.y;
	r->o.y = t->o.y;
	r->a.y = t->a.y;
	r->p.y = t->p.y;
	r->n.z = t->n.z;
	r->o.z = t->o.z;
	r->a.z = t->a.z;
	r->p.z = t->p.z;
	return(r);
}


/*
 * assigns translation part of t (rhs) to  r (lhs)
 */

TRSF_PTR taketrsl(r, t) /*::*/
register TRSF_PTR r, t;
{
	r->p.x = t->p.x;
	r->p.y = t->p.y;
	r->p.z = t->p.z;
	return(r);
}

/*
 * assigns rotational part of t (rhs) to  r (lhs)
 */

TRSF_PTR takerot(r, t) /*::*/
register TRSF_PTR r, t;
{
	r->n.x = t->n.x;
	r->o.x = t->o.x;
	r->a.x = t->a.x;
	r->n.y = t->n.y;
	r->o.y = t->o.y;
	r->a.y = t->a.y;
	r->n.z = t->n.z;
	r->o.z = t->o.z;
	r->a.z = t->a.z;
	return(r);
}



/*
 * returns in r matrix the product of t by u
 * r t u should be different matrices
 */

TRSF_PTR trmult(r, t, u) /*::*/
register TRSF_PTR r, t, u;
{
	r->n.x = t->n.x * u->n.x + t->o.x * u->n.y + t->a.x * u->n.z;
	r->n.y = t->n.y * u->n.x + t->o.y * u->n.y + t->a.y * u->n.z;
	r->n.z = t->n.z * u->n.x + t->o.z * u->n.y + t->a.z * u->n.z;
	r->o.x = t->n.x * u->o.x + t->o.x * u->o.y + t->a.x * u->o.z;
	r->o.y = t->n.y * u->o.x + t->o.y * u->o.y + t->a.y * u->o.z;
	r->o.z = t->n.z * u->o.x + t->o.z * u->o.y + t->a.z * u->o.z;
	r->a.x = t->n.x * u->a.x + t->o.x * u->a.y + t->a.x * u->a.z;
	r->a.y = t->n.y * u->a.x + t->o.y * u->a.y + t->a.y * u->a.z;
	r->a.z = t->n.z * u->a.x + t->o.z * u->a.y + t->a.z * u->a.z;
	r->p.x = t->n.x * u->p.x + t->o.x * u->p.y + t->a.x * u->p.z + t->p.x;
	r->p.y = t->n.y * u->p.x + t->o.y * u->p.y + t->a.y * u->p.z + t->p.y;
	r->p.z = t->n.z * u->p.x + t->o.z * u->p.y + t->a.z * u->p.z + t->p.z;
	return(r);
}


/*
 * matrix mult in place
 */

TRSF_PTR trmultinp(r, m) /*::*/
register TRSF_PTR r, m;
{
	TRSF temp;

	return(trmult(r, assigntr(&temp, r), m));
}


/*
 * matrix mult by inverse
 */

TRSF_PTR trmultinv(r, m) /*::*/
register TRSF_PTR r, m;
{
	TRSF temp1, temp2;

	return(trmult(r, assigntr(&temp2, r), invert(&temp1, m)));
}


/*
 * computes the inverse of a transform
 */

TRSF_PTR invert(i, t)  /*::*/
register TRSF_PTR i, t;
{
	i->n.x = t->n.x;
	i->n.y = t->o.x;
	i->n.z = t->a.x;
	i->o.x = t->n.y;
	i->o.y = t->o.y;
	i->o.z = t->a.y;
	i->a.x = t->n.z;
	i->a.y = t->o.z;
	i->a.z = t->a.z;

	i->p.x = - (t->p.x * t->n.x + t->p.y * t->n.y + t->p.z * t->n.z);
	i->p.y = - (t->p.x * t->o.x + t->p.y * t->o.y + t->p.z * t->o.z);
	i->p.z = - (t->p.x * t->a.x + t->p.y * t->a.y + t->p.z * t->a.z);
	return(i);
}


/*
 * invert in place
 */

TRSF_PTR invertinp(t) /*::*/
TRSF_PTR t;
{
	TRSF temp;

	return(invert(t, assigntr(&temp, t)));
}


/*
 * set a pure translation transform
 */

TRSF_PTR trsl(t, x, y, z) /*::*/
register TRSF_PTR t;
real x, y, z;
{
	t->p.x = x;
	t->p.y = y;
	t->p.z = z;
	return(t);
}


/*
 * multiply a transform by a pure translation transform
 */

TRSF_PTR trslm(t, x, y, z) /*::*/
register TRSF_PTR t;
real x, y, z;
{
	static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};

	temp.n.x = temp.o.y = temp.a.z = 1.;
	temp.n.y = temp.n.z = temp.o.x = temp.o.z = temp.a.x = temp.a.y = 0.;
	return(trmultinp(t, trsl(&temp, x, y, z)));
}


/*
 * set a transform given the a o vectors
 */

TRSF_PTR vao(t, ax, ay, az, ox, oy, oz) /*::*/
register TRSF_PTR t;
real ax, ay, az, ox, oy, oz;
{
	t->a.x = ax;
	t->a.y = ay;
	t->a.z = az;
	t->o.x = ox;
	t->o.y = oy;
	t->o.z = oz;

 (void) unit(&t->a, &t->a);
 (void) cross(&t->n, &t->o, &t->a);
 (void) cross(&t->o, &t->a, &t->n);
 (void) unit(&t->o, &t->o);
 (void) cross(&t->n, &t->o, &t->a);
	return(t);
}


/*
 * multiply a transform by a rotation expressed with a o vectors
 */

TRSF_PTR vaom(t, ax, ay, az, ox, oy, oz) /*::*/
register TRSF_PTR t;
real ax, ay, az, ox, oy, oz;
{
	static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};

	temp.p.z = temp.p.y = temp.p.z = 0.;
	return(trmultinp(t, vao(&temp, ax, ay, az, ox, oy, oz)));
}


/*
 * sets the terms of a transform given a rotation theta around a vector k
 */

TRSF_PTR rot(t, k, h) /*::*/
register TRSF_PTR t;
register VECT_PTR k;
real h;
{
	VECT kl;
	real s, c, ver, x, y, z, xv, yv, zv, xs, ys, zs;

 (void) unit(&kl, k);
	s = sin(dgtord_m * h);
	ver = 1. - (c = cos(dgtord_m * h));
	xv = ver * (x = kl.x);
	xs = x * s;
	yv = ver * (y = kl.y);
	ys = y * s;
	zv = ver * (z = kl.z);
	zs = z * s;

	t->n.x = x * xv + c;
	t->n.y = x * yv + zs;
	t->n.z = x * zv - ys;

	t->o.x = y * xv - zs;
	t->o.y = y * yv + c;
	t->o.z = y * zv + xs;

	t->a.x = z * xv + ys;
	t->a.y = z * yv - xs;
	t->a.z = z * zv + c;
	return(t);
}


/*
 * multiply a transform by a rotation about a vector
 */

TRSF_PTR rotm(t, k, h) /*::*/
register TRSF_PTR t;
register VECT_PTR k;
real h;
{
	static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};

	temp.p.z = temp.p.y = temp.p.z = 0.;
	return(trmultinp(t, rot(&temp, k, h)));
}


/*
 * sets the terms of a transform given the euler angles
 * expressed in degrees
 */

TRSF_PTR eul(t, phi, the, psi) /*::*/
register TRSF_PTR t;
real phi, the, psi;
{
	real sphi, sthe, spsi, cphi, cthe, cpsi;

	sphi = sin(dgtord_m * phi);
	cphi = cos(dgtord_m * phi);
	sthe = sin(dgtord_m * the);
	cthe = cos(dgtord_m * the);
	spsi = sin(dgtord_m * psi);
	cpsi = cos(dgtord_m * psi);

	t->n.x =   cphi * cthe * cpsi - sphi * spsi;
	t->n.y =   sphi * cthe * cpsi + cphi * spsi;
	t->n.z = - sthe * cpsi;

	t->o.x = - cphi * cthe * spsi - sphi * cpsi;
	t->o.y = - sphi * cthe * spsi + cphi * cpsi;
	t->o.z =   sthe * spsi;

	t->a.x = cphi * sthe;
	t->a.y = sphi * sthe;
	t->a.z = cthe;
	return(t);
}


/*
 * multiply a transform by a rotation expressed with euler angles
 */

TRSF_PTR eulm(t, phi, the, psi) /*::*/
register TRSF_PTR t;
real phi, the, psi;
{
	static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};

	temp.p.z = temp.p.y = temp.p.z = 0.;
	return(t, trmultinp(t, eul(&temp, phi, the, psi)));
}


/*
 * returns the euler angles of the rotation part of a transform with
 * euler angles given in degrees
 */

noatoeul(phi, the, psi, t) /*::*/
real *phi, *the, *psi; /* pointers to angles */
register TRSF_PTR t;
{
	real sphi, cphi, p;

	if (FABS(t->a.y) > SMALL || FABS(t->a.x) > SMALL) {
		*phi = rdtodg_m * (p = atan2(t->a.y, t->a.x));
		sphi = sin(p);
		cphi = cos(p);
		*the = rdtodg_m * atan2(cphi * t->a.x + sphi * t->a.y, t->a.z);
		*psi = rdtodg_m * atan2(- sphi * t->n.x + cphi * t->n.y ,
					- sphi * t->o.x + cphi * t->o.y);
	}
	else {
		*phi = 0.;
		*the = rdtodg_m * atan2(t->a.x, t->a.z);
		*psi = rdtodg_m * atan2(t->n.y, t->o.y);
	}
}


/*
 * sets the term of a transform with a rotation expressed with rpy
 * angles in degrees
 */

TRSF_PTR rpy(t, phi, the, psi) /*::*/
register TRSF_PTR t;
real phi, the, psi;
{
	real sphi, sthe, spsi, cphi, cthe, cpsi;

	sphi = sin(dgtord_m * phi);
	cphi = cos(dgtord_m * phi);
	sthe = sin(dgtord_m * the);
	cthe = cos(dgtord_m * the);
	spsi = sin(dgtord_m * psi);
	cpsi = cos(dgtord_m * psi);

	t->n.x = cphi * cthe;
	t->n.y = sphi * cthe;
	t->n.z = - sthe;
	t->o.x = cphi * sthe * spsi - sphi * cpsi;
	t->o.y = sphi * sthe * spsi + cphi * cpsi;
	t->o.z = cthe * spsi;
	t->a.x = cphi * sthe * cpsi + sphi * spsi;
	t->a.y = sphi * sthe * cpsi - cphi * spsi;
	t->a.z = cthe * cpsi;
	return(t);
}


/*
 * multilpy a transform with a rotation expressed with rpy anlges
 */

TRSF_PTR rpym(t, phi, the, psi) /*::*/
register TRSF_PTR t;
real phi, the, psi;
{
	static TRSF temp = {"", const,1.,0.,0.,0.,1.,0.,0.,0.,1.,0.,0.,0.};

	temp.p.z = temp.p.y = temp.p.z = 0.;
	return(t, trmultinp(t, rpy(&temp, phi, the, psi)));
}


/*
 * computes rpy angles from the n o a vectors of a transform
 */

noatorpy(phi, the, psi, t) /*::*/
real *phi, *the, *psi;
register TRSF_PTR t;
{
	real sphi, cphi, nx, ny, p;

	nx = t->n.x;
	ny = t->n.y;
	if (FABS(nx) < SMALL && FABS(ny) < SMALL) {
		*phi = 0.;
		*the = rdtodg_m * atan2(- t->n.z, nx);
		*psi = rdtodg_m * atan2(- t->a.y, t->o.y);
	}
	else {
		*phi = rdtodg_m * (p = atan2(ny, nx));
		sphi = sin(p);
		cphi = cos(p);
		*the = rdtodg_m * atan2(- t->n.z, cphi * nx + sphi * ny);
		*psi = rdtodg_m * atan2(sphi * t->a.x - cphi * t->a.y ,
					cphi * t->o.y - sphi * t->o.x);
	}
}



/*
 * prints out the numerical information
 */

printr(t, fp)  /*::*/
TRSF_PTR t;
FILE *fp;
{
	fprintf(fp, "%8.3f %8.3f %8.3f %8.3f\n",
		t->n.x, t->o.x, t->a.x, t->p.x);
	fprintf(fp, "%8.3f %8.3f %8.3f %8.3f\n",
		t->n.y, t->o.y, t->a.y, t->p.y);
	fprintf(fp, "%8.3f %8.3f %8.3f %8.3f\n",
		t->n.z, t->o.z, t->a.z, t->p.z);
}


/*
 * prints name value and rpy and euler angles of a transform
 */

printrn(t, fp) /*::*/
TRSF_PTR t;
FILE *fp;
{
	fprintf(fp, "%s :\n", t->name);
	printr(t, fp);
	printe(t, fp);
	printy(t, fp);
}


/*
 * prints out the euler representation of a transform
 */

printe(e, fp) /*::*/
TRSF_PTR e;
FILE *fp;
{
	real p, t, s;

	noatoeul(&p, &t, &s, e);
	fprintf(fp,
	"EUL x:%-7.3f y:%-7.3f z:%-7.3f phi:%-7.3f the:%-7.3f psi:%-7.3f\n",
	e->p.x, e->p.y, e->p.z, p, t, s);
}


/*
 * prints out the rpy representation of a transform
 */

printy(e, fp) /*::*/
TRSF_PTR e;
FILE *fp;
{
	real p, t, s;

	noatorpy(&p, &t, &s, e);
	fprintf(fp,
	"RPY x:%-7.3f y:%-7.3f z:%-7.3f phi:%-7.3f the:%-7.3f psi:%-7.3f\n",
	e->p.x, e->p.y, e->p.z, p, t, s);
}
