#include <math.h>
#include "../h/cdefs.h"
#include "grf4.h"
#include "grf4Internal.h"

TRSF *identTrsf (tr)
TRSF *tr;
{
	/* Set tr to the identity transform */

	static TRSF ident = { 1.0, 0.0, 0.0, 0.0,
			      1.0, 0.0, 0.0, 0.0,
			      1.0, 0.0, 0.0, 0.0 };
	*tr = ident;
	return (tr);
}

#define SMALL 1.0e-5

TRSF *xyzToTrsf (tr, x, y, z)
register TRSF *tr;
float x, y, z;
{
	/* set tranlation component */

	tr->p.x = x;
	tr->p.y = y;
	tr->p.z = z;
	return(tr);
}

TRSF *multTrsfXyz (tr, x, y, z)
register TRSF *tr;
float x, y, z;
{
	/* multiply by translation components  */

	tr->p.x += tr->n.x*x + tr->o.x*y + tr->a.x*z;
	tr->p.y += tr->n.y*x + tr->o.y*y + tr->a.y*z;
	tr->p.z += tr->n.z*x + tr->o.z*y + tr->a.z*z;

	return (tr);
}

TRSF *axesToTrsf (tr, axes, v1, v2)
TRSF *tr;
register char *axes;
register VECT *v1, *v2;
{
	/* Generalization of vaoToTrsf(). Takes a pair of non-parallel axes
	   and uses them to build the rotation part of a TRSF structure.
	   v1 and v2 should describe two of the three final axes. Which
	   axis each vector describes is given by a code character ('x', 'y',
	   or 'z') in the two-character string 'axes'. 

	   v1 and v2 do not have to be orthogonal. v1 is assumed to be
	   parallel to the axis specified by axes[0]. The projection of
	   v2 onto the plane defined by this axis is then assumed to be
	   parallel to the axis specified by axes[1]. The third axis is
	   taken to be the cross product of the other two axes.

	   The routine returns NULL in the event of a specification error,
	   or if the two defining vectors are very close to parallel.
	 */

	register VECT *r1, *r2;
	register float mag;

	VECT *r3;
	int negate;

	/* Determine which input vector corresponds to which axis */

	if (axes[0] == 'x')
	 { r1 = &tr->n;
	   if (axes[1] == 'y')
	    { r2 = &tr->o;
	      r3 = &tr->a;
	      negate = 0;
	    }
	   else if (axes[1] == 'z')
	    { r2 = &tr->a;
	      r3 = &tr->o;
	      negate = 1;
	    }
	   else
	    { return (NULL);
	    }
	 }
	else if (axes[0] == 'y')
	 { r1 = &tr->o;
	   if (axes[1] == 'x')
	    { r2 = &tr->n;
	      r3 = &tr->a;
	      negate = 1;
	    }
	   else if (axes[1] == 'z')
	    { r2 = &tr->a;
	      r3 = &tr->n;
	      negate = 0;
	    }
	   else
	    { return (NULL);
	    }
	 }
	else if (axes[0] == 'z')
	 { r1 = &tr->a;
	   if (axes[1] == 'x')
	    { r2 = &tr->n;
	      r3 = &tr->o;
	      negate = 0;
	    }
	   else if (axes[1] == 'y')
	    { r2 = &tr->o;
	      r3 = &tr->n;
	      negate = 1;
	    }
	   else
	    { return (NULL);
	    }
	 }
	else
	 { return(NULL);
	 }

	if ((mag = sqrt(v1->x*v1->x + v1->y*v1->y + v1->z*v1->z)) < SMALL)
	 { return (NULL);
	 }
	r1->x = v1->x/mag;
	r1->y = v1->y/mag;
	r1->z = v1->z/mag;
	mag = (v2->x*r1->x + v2->y*r1->y + v2->z*r1->z);
	r2->x = v2->x - r1->x*mag;
	r2->y = v2->y - r1->y*mag;
	r2->z = v2->z - r1->z*mag;
	if ((mag = sqrt(r2->x*r2->x + r2->y*r2->y + r2->z*r2->z)) < SMALL)
	 { return (NULL);
	 }
	r2->x = r2->x/mag;
	r2->y = r2->y/mag;
	r2->z = r2->z/mag;
	if (negate)
	 { r3->x = r1->z*r2->y - r1->y*r2->z;
	   r3->y = r1->x*r2->z - r1->z*r2->x;
	   r3->z = r1->y*r2->x - r1->x*r2->y;
	 }
	else
	 { r3->x = r1->y*r2->z - r1->z*r2->y;
	   r3->y = r1->z*r2->x - r1->x*r2->z;
	   r3->z = r1->x*r2->y - r1->y*r2->x;
	 }
	return(tr);
}

TRSF *multTrsfAxes (tr, axes, v1, v2)
TRSF *tr;
char *axes;
VECT *v1, *v2;
{
	/* multiply by the rotation specified by the vectors v1 and v2 */
	TRSF temp;

	temp.p.x = temp.p.y = temp.p.z = 0.;
	if (!axesToTrsf (&temp, axes, v1, v2))
	 { return (NULL);
	 }
	return (multTrsf (tr, tr, &temp));
}

TRSF *rpyToTrsf(tr, roll, pitch, yaw)
register TRSF *tr;
float roll, pitch, yaw;
{
	/* Set the rotational component of 'tr' as specified by
	   the input roll-pitch-yaw angles.
	 */
	   
	float sroll, spitch, syaw, croll, cpitch, cyaw;

	sroll = sin(DTOR * roll);
	croll = cos(DTOR * roll);
	spitch = sin(DTOR * pitch);
	cpitch = cos(DTOR * pitch);
	syaw = sin(DTOR * yaw);
	cyaw = cos(DTOR * yaw);

	tr->n.x = croll * cpitch;
	tr->n.y = sroll * cpitch;
	tr->n.z = - spitch;
	tr->o.x = croll * spitch * syaw - sroll * cyaw;
	tr->o.y = sroll * spitch * syaw + croll * cyaw;
	tr->o.z = cpitch * syaw;
	tr->a.x = croll * spitch * cyaw + sroll * syaw;
	tr->a.y = sroll * spitch * cyaw - croll * syaw;
	tr->a.z = cpitch * cyaw;
	return(tr);
}

trsfToRpy (roll, pitch, yaw, tr)
float *roll, *pitch, *yaw;
register TRSF *tr;
{
	/* Return the roll-pitch-yaw angles associated with the rotational
	   component of tr.
	 */
	float sroll, croll, nx, ny, p;

	nx = tr->n.x;
	ny = tr->n.y;
	if (FABS(nx) < SMALL && FABS(ny) < SMALL) {
		*roll = 0.;
		*pitch = RTOD * atan2(- tr->n.z, nx);
		*yaw = RTOD * atan2(- tr->a.y, tr->o.y);
	} 
	else {
		*roll = RTOD * (p = atan2(ny, nx));
		sroll = sin(p);
		croll = cos(p);
		*pitch = RTOD * 
			 atan2(- tr->n.z, croll * nx + sroll * ny);
		*yaw =   RTOD * 
			 atan2(sroll * tr->a.x - croll * tr->a.y,
			       croll * tr->o.y - sroll * tr->o.x);
	}
}

TRSF *multTrsfRpy(tr, roll, pitch, yaw)
register TRSF *tr;
float roll, pitch, yaw;
{
	/* Multiply tr by a rotation equivalent to the specified
	   roll-pitch-yaw angles.
	 */

	TRSF temp;

	temp.p.x = temp.p.y = temp.p.z = 0.;
	return(multTrsf (tr, tr, rpyToTrsf(&temp, roll, pitch, yaw)));
}

TRSF *multTrsf(r, t1, t2) /*::*/
register TRSF *r;		/* 4(ap) -> r11 */
register TRSF *t1;		/* 8(ap) -> r10 */
register TRSF *t2;		/* 12(ap) -> r9 */
{
	float x, y, z;
	TRSF temp;
	register TRSF *rp;

	if (t1 == r)
	 { temp = *t1;
	   t1 = &temp;
	 }

	x = t2->n.x; y = t2->n.y; z = t2->n.z;	
	r->n.x = x * t1->n.x + y * t1->o.x + z * t1->a.x;
	r->n.y = x * t1->n.y + y * t1->o.y + z * t1->a.y;
	r->n.z = x * t1->n.z + y * t1->o.z + z * t1->a.z;

	x = t2->o.x; y = t2->o.y; z = t2->o.z;	
	r->o.x = x * t1->n.x + y * t1->o.x + z * t1->a.x;
	r->o.y = x * t1->n.y + y * t1->o.y + z * t1->a.y;
	r->o.z = x * t1->n.z + y * t1->o.z + z * t1->a.z;

	x = t2->a.x; y = t2->a.y; z = t2->a.z;	
	r->a.x = x * t1->n.x + y * t1->o.x + z * t1->a.x;
	r->a.y = x * t1->n.y + y * t1->o.y + z * t1->a.y;
	r->a.z = x * t1->n.z + y * t1->o.z + z * t1->a.z;

	x = t2->p.x; y = t2->p.y; z = t2->p.z;	
	r->p.x = x * t1->n.x + y * t1->o.x + z * t1->a.x;
	r->p.y = x * t1->n.y + y * t1->o.y + z * t1->a.y;
	r->p.z = x * t1->n.z + y * t1->o.z + z * t1->a.z;

	r->p.x += t1->p.x;
	r->p.y += t1->p.y;
	r->p.z += t1->p.z;
	return(r);
}

TRSF *invertTrsf(i, t)
register TRSF *i;		/* 4(ap) -> r11 */
register TRSF *t;		/* 8(ap) -> r10 */
{
	TRSF temp;

	if (i == t)
	 { temp = *t;
	   t = &temp;
	 }

	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);
}

TRSF *multLiTrsf (r, t1, t2)
register TRSF *r;
register TRSF *t1;
register TRSF *t2;
{
	register float x, y, z;
	TRSF temp;

	if (t1 == r)
	 { temp = *t1;
	   t1 = &temp;
	 }

	x = t2->n.x; y = t2->n.y; z = t2->n.z;
	r->n.x = x * t1->n.x + y * t1->n.y + z * t1->n.z;
	r->n.y = x * t1->o.x + y * t1->o.y + z * t1->o.z;
	r->n.z = x * t1->a.x + y * t1->a.y + z * t1->a.z;

	x = t2->o.x; y = t2->o.y; z = t2->o.z;
	r->o.x = x * t1->n.x + y * t1->n.y + z * t1->n.z;
	r->o.y = x * t1->o.x + y * t1->o.y + z * t1->o.z;
	r->o.z = x * t1->a.x + y * t1->a.y + z * t1->a.z;

	x = t2->a.x; y = t2->a.y; z = t2->a.z;
	r->a.x = x * t1->n.x + y * t1->n.y + z * t1->n.z;
	r->a.y = x * t1->o.x + y * t1->o.y + z * t1->o.z;
	r->a.z = x * t1->a.x + y * t1->a.y + z * t1->a.z;

	x = t2->p.x - t1->p.x; y = t2->p.y - t1->p.y; z = t2->p.z - t1->p.z;
	r->p.x = x * t1->n.x + y * t1->n.y + z * t1->n.z;
	r->p.y = x * t1->o.x + y * t1->o.y + z * t1->o.z;
	r->p.z = x * t1->a.x + y * t1->a.y + z * t1->a.z;

	return (r);
}

TRSF *multRiTrsf (r, t1, t2)
register TRSF *r;
register TRSF *t1;
register TRSF *t2;
{
	register float n, o, a, px, py, pz;
	TRSF temp;

	if (t2 == r)
	 { temp = *t2;
	   t2 = &temp;
	 }
	
	n = t1->n.x; o = t1->o.x; a = t1->a.x;
	r->n.x = n * t2->n.x + o * t2->o.x + a * t2->a.x;
	r->o.x = n * t2->n.y + o * t2->o.y + a * t2->a.y;
	r->a.x = n * t2->n.z + o * t2->o.z + a * t2->a.z;

	n = t1->n.y; o = t1->o.y; a = t1->a.y;
	r->n.y = n * t2->n.x + o * t2->o.x + a * t2->a.x;
	r->o.y = n * t2->n.y + o * t2->o.y + a * t2->a.y;
	r->a.y = n * t2->n.z + o * t2->o.z + a * t2->a.z;

	n = t1->n.z; o = t1->o.z; a = t1->a.z;
	r->n.z = n * t2->n.x + o * t2->o.x + a * t2->a.x;
	r->o.z = n * t2->n.y + o * t2->o.y + a * t2->a.y;
	r->a.z = n * t2->n.z + o * t2->o.z + a * t2->a.z;

	px = -t2->p.x; py = -t2->p.y; pz = -t2->p.z;
	r->p.x = t1->p.x + px * r->n.x + py * r->o.x + pz * r->a.x;
	r->p.y = t1->p.y + px * r->n.y + py * r->o.y + pz * r->a.y;
	r->p.z = t1->p.z + px * r->n.z + py * r->o.z + pz * r->a.z;

	return (r);
}

printTrsf (fmt, T)
char *fmt;
TRSF *T;
{
	printf ("%6.3f %6.3f %6.3f  %9.2f\n", T->n.x, T->o.x, T->a.x, T->p.x);
	printf ("%6.3f %6.3f %6.3f  %9.2f\n", T->n.y, T->o.y, T->a.y, T->p.y);
	printf ("%6.3f %6.3f %6.3f  %9.2f\n", T->n.z, T->o.z, T->a.z, T->p.z);
}

VECT *zeroVect(vr)
VECT *vr;
{
	vr->x = 0.0;
	vr->y = 0.0;
	vr->z = 0.0;
	return (vr);
}

