/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*---------------------------------------------------------------------------
 *
 * MODULE NAME:	trsf
 *
 * DESCRIPTION: Utilities for defining the fields of a transform given 
 *		a translation and various rotation definitions.
 *
 * AUTHOR:	Vincent Hayward, Purdue University, 1983.
 *		CVaRL installation and modification by John Lloyd.
 *
 *--------------------------------------------------------------------------*/

#include <math.h>

#include "../h/fastmath.h"
#include "../h/cdefs.h"
#include "../h/robotTypes.h"
#include "../h/robotMath.h"
#include "../h/vector.h"

#define SMALL 1.0e-5

TRSF *xyzToTrsf (tr, x, y, z)
register TRSF *tr;
double 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;
double 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 *vaoToTrsf (tr, ax, ay, az, ox, oy, oz)
register TRSF *tr;
double ax, ay, az, ox, oy, oz;
{
	/* Set the rotational component of tr as specified by the
	   input vectors 'a' and 'o'
	 */

	tr->a.x = ax;
	tr->a.y = ay;
	tr->a.z = az;
	tr->o.x = ox;
	tr->o.y = oy;
	tr->o.z = oz;

	/* 'o' and 'a' need not be orthogonal.			

	   Define the rotational as follows: Take the user's 'a' vector	
	   and normalize it to the final 'a' vector. Then take the 'o'  
	   vector and produce a perpendicular nonunit 'n' vector; 	
	   Produce a new 'o' as the normalized cross of 'a' and 'n',    
	   and get the final 'n' as the cross of 'o' and 'a'.
	*/

 (void) unitVect(&tr->a, &tr->a);
 (void) crossVect(&tr->n, &tr->o, &tr->a);
 (void) crossVect(&tr->o, &tr->a, &tr->n);
 (void) unitVect(&tr->o, &tr->o);
 (void) crossVect(&tr->n, &tr->o, &tr->a);

	return(tr);
}

TRSF *multTrsfVao (tr, ax, ay, az, ox, oy, oz)
register TRSF *tr;
double ax, ay, az, ox, oy, oz;
{
	/* multiply by the rotation specified with the input
	   'a' and 'o' angles.
	 */
	TRSF temp;

	temp.p.x = temp.p.y = temp.p.z = 0.;
	return (multTrsf (tr, tr, vaoToTrsf (&temp, ax, ay, az, ox, oy, oz)));
}

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 *rotToTrsf(tr, v, h)
register TRSF *tr;
VECT *v;
double h;
{
	/* set the rotational component of the transform to be equal
	   to a rotation 'h' about the vector 'v'.
	*/
	VECT kl;
	float s, c, ver, x, y, z, xv, yv, zv, xs, ys, zs;

       	(void) unitVect(&kl, v);
	s = sin(DEGTORAD * h);
	ver = 1. - (c = cos(DEGTORAD * 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;

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

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

	tr->a.x = z * xv + ys;
	tr->a.y = z * yv - xs;
	tr->a.z = z * zv + c;

	return(tr);
}

TRSF *multTrsfRot(tr, v, h)
register TRSF *tr;
register VECT *v;
double h;
{
	/* multiply tr by a rotation equivalent to a rotation 'h' about 
	   the vector 'v'.
	*/
	TRSF temp; 

	temp.p.x = temp.p.y = temp.p.z = 0.;
	return(multTrsf (tr, tr, rotToTrsf(&temp, v, h)));
}

TRSF *eulToTrsf(tr, phi, the, psi)
register TRSF *tr;
double phi, the, psi;
{
	/* Set the rotational component of 'tr' as specified by
	   the input Euler angles.
	 */
	   
	float sphi, sthe, spsi, cphi, cthe, cpsi;

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

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

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

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

TRSF *multTrsfEul(tr, phi, the, psi)
register TRSF *tr;
double phi, the, psi;
{
	/* Multiply tr by a rotation equivalent to the specified
	   Euler angles.
	 */

	TRSF temp;

	temp.p.x = temp.p.y = temp.p.z = 0.;
	return (multTrsf (tr, tr, eulToTrsf(&temp, phi, the, psi)));
}

TRSF *rpyToTrsf(tr, roll, pitch, yaw)
register TRSF *tr;
double 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(DEGTORAD * roll);
	croll = cos(DEGTORAD * roll);
	spitch = sin(DEGTORAD * pitch);
	cpitch = cos(DEGTORAD * pitch);
	syaw = sin(DEGTORAD * yaw);
	cyaw = cos(DEGTORAD * 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);
}

TRSF *multTrsfRpy(tr, roll, pitch, yaw)
register TRSF *tr;
double 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 *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);
}

TRSF *unitTrsf (tr)
TRSF *tr;
{
	/* normalize a transform according to
		
		a = unit(a)
		n = unit(oXa)
		o = aXn
	*/

	unitVect (&tr->a, &tr->a);
	crossVect (&tr->n, &tr->o, &tr->a);
	unitVect (&tr->n, &tr->n);
	crossVect (&tr->o, &tr->a, &tr->n);

	return (tr);
}
	
trsfToEul (phi, the, psi, tr)
float *phi, *the, *psi; /* pointers to angles */
register TRSF *tr;
{
	/* Return the Euler angles associated with the rotational
	   component of tr.
	 */
	float sphi, cphi, p;

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

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 = RADTODEG * atan2(- tr->n.z, nx);
		*yaw = RADTODEG * atan2(- tr->a.y, tr->o.y);
	} 
	else {
		*roll = RADTODEG * (p = atan2(ny, nx));
		sroll = sin(p);
		croll = cos(p);
		*pitch = RADTODEG * 
			 atan2(- tr->n.z, croll * nx + sroll * ny);
		*yaw =   RADTODEG * 
			 atan2(sroll * tr->a.x - croll * tr->a.y,
			       croll * tr->o.y - sroll * tr->o.x);
	}
}

#define X_MAX	1
#define Y_MAX	2
#define Z_MAX	3
#undef SMALL
#define SMALL 1.0e-7

trsfToRot (v, h, t1)
VECT *v;
float *h;
register TRSF *t1;
{
	/* Return the axis and angle of rotation associated with the 
	   rotational component of tr.
	 */

	register cosine, tmpX, tmpY, tmpZ, tmpS;
	float versine, sine;
	int max;

	cosine = 0.5*(t1->n.x + t1->o.y + t1->a.z - 1.0);

	if (cosine > 0.0)
	 { /* then we can compute things the easy way. */

	   tmpX = t1->o.z - t1->a.y;
	   tmpY = t1->a.x - t1->n.z;
	   tmpZ = t1->n.y - t1->o.x;

	   tmpS = sqrt (tmpX*tmpX + tmpY*tmpY + tmpZ*tmpZ);

	   if (tmpS < SMALL)
	    { *h = v->y = v->z = 0.0;
	      v->x = 1.0;
	      return;
	    }
	   v->x = tmpX/tmpS;	      
	   v->y = tmpY/tmpS;	      
	   v->z = tmpZ/tmpS;
	   sine = tmpS / 2.0;
	 }
	else
	 { versine = 1.0 - cosine;

	   tmpX = FABS (t1->n.x - cosine);
	   tmpY = FABS (t1->o.y - cosine);
	   tmpZ = FABS (t1->a.z - cosine);
   
	   if (tmpX >= tmpY)
	    { max = (tmpX >= tmpZ ? X_MAX : Z_MAX);
	    }
	   else
	    { max = (tmpY >= tmpZ ? Y_MAX : Z_MAX);
	    }
	   switch (max)
	    { case X_MAX:
	       { if (versine < SMALL || tmpX <= 0.0)
		  { *h = v->y = v->z = 0.0;
		    v->x = 1.0;
		    return;
		  }
		 if ((tmpS = t1->o.z - t1->a.y) >= 0.0)
		  { v->x = sqrt(tmpX/versine);
		  }
		 else
		  { v->x = -sqrt(tmpX/versine);
		  }
		 v->y = (t1->n.y + t1->o.x)/(2*v->x*versine);
		 v->z = (t1->a.x + t1->n.z)/(2*v->x*versine);
		 sine = tmpS / (2*v->x);
		 break;
	       }
	      case Y_MAX:
	       { if (versine < SMALL || tmpY <= 0.0)
		  { *h = v->y = v->z = 0.0;
		    v->x = 1.0;
		    return;
		  }
		 if ((tmpS = t1->a.x - t1->n.z) >= 0.0)
		  { v->y = sqrt(tmpY/versine);
		  }
		 else
		  { v->y = -sqrt(tmpY/versine);
		  }
		 v->x = (t1->n.y + t1->o.x)/(2*v->y*versine);
		 v->z = (t1->o.z + t1->a.y)/(2*v->y*versine);
		 sine = tmpS / (2*v->y);
		 break;
	       }
	      case Z_MAX:
	       { if (versine < SMALL || tmpZ <= 0.0)
		  { *h = v->y = v->z = 0.0;
		    v->x = 1.0;
		    return;
		  }
		 if ((tmpS = t1->n.y - t1->o.x) >= 0.0)
		  { v->z = sqrt(tmpZ/versine);
		  }
		 else
		  { v->z = -sqrt(tmpZ/versine);
		  }
		 v->x = (t1->a.x + t1->n.z)/(2*v->z*versine);
		 v->y = (t1->o.z + t1->a.y)/(2*v->z*versine);
		 sine = tmpS / (2*v->z);
		 break;
	       }
	    }
	 }
	*h = RADTODEG * ATAN2 (sine, cosine);
}

float cmpTrsf (t1, t2)
TRSF *t1, *t2;
{
	/* This function is mostly for debugging.
	   It returns an estimate of the % error between t1 and t2. It is most
	   accurate when the transforms are close together */

	VECT delta;
	double size;
	double error;

	error = normVect (subtractVect (&delta, &t1->n, &t2->n)) + 
		normVect (subtractVect (&delta, &t1->o, &t2->o)) + 
		normVect (subtractVect (&delta, &t1->a, &t2->a));

	size = (normVect(&t1->p) + normVect(&t2->p)) / 2.0;
	if (size > SMALL)
	 { error += 3*normVect(subtractVect (&delta, &t1->p, &t2->p))/size;
	 }

	return (error/6);	
}

#if TEST

/* This test program only tests axesToTrsf() */

main()
{
	TRSF trsf;
	char axes[32];
	VECT v1, v2;

	identTrsf(&trsf);

	while(1)
	 { printf ("axes, v1, v2> ");
	   scanf ("%s", axes);
	   scanVf ((float*)&v1, 3);
	   scanVf ((float*)&v2, 3);
	   if (!axesToTrsf (&trsf, axes, &v1, &v2))
	    { printf("Error\n");
	    }
	   else
	    { printTrsf ("%m\n", &trsf);
	    }
	 }
}

#endif
