/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1991 by John E. Lloyd.				    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	dspl
 *
 * DESCRIPTION:	These routines operate on, and convert to and from, the
 *      	DSPL data type.
 *
 *      	The DSPL data type describes position and orientation using
 *      	a position vector, a rotation axis, and a rotation angle.
 *
 *      	It is useful in that it allows for easy interpolation of 
 *      	positions, assuming constant velocity.
 * 
 * AUTHOR/DATE:	John Lloyd, Sept 19, 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <math.h>

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

#define SMALL 1.0e-6

#define X_MAX	1
#define Y_MAX	2
#define Z_MAX	3

static VECT zerov = { 0.0, 0.0, 0.0 };

static DSPL *normalizeDspl (rr)
DSPL *rr;
{
	/* make sure the angle of a DSPL is positive */

	if (rr->a < 0.0)
	 { rr->a = -rr->a;
	   rr->u.x = -rr->u.x;
	   rr->u.y = -rr->u.y;
	   rr->u.z = -rr->u.z;
	 }
	return (rr);
}

DSPL *unitDspl (rr, r1)
register DSPL *rr, *r1;
{
	/* Normalize a DSPL object. */

	register float mag;
	float n, r;

	mag = SQRT (r1->u.x*r1->u.x + r1->u.y*r1->u.y + r1->u.z*r1->u.z);
	if (mag < SMALL || r1->a < SMALL)
	 { rr->a = rr->u.x = rr->u.y = rr->u.z = 0.0;
	   return (rr);
	 }

	/* reduce the range of the angle */

	n = r1->a/(2*PI);
	r = n - (int)n;
	if (r <= 0.5)
	 { rr->a = 2*PI*r;
	   rr->u.x = r1->u.x / mag;
	   rr->u.y = r1->u.y / mag;
	   rr->u.z = r1->u.z / mag;
	 }
	else
	 { rr->a = 2*PI*(1.0-r);
	   rr->u.x = -r1->u.x / mag;
	   rr->u.y = -r1->u.y / mag;
	   rr->u.z = -r1->u.z / mag;
	 }
	return (rr);
}

DSPL *scaleDspl (rr, s, r1)
DSPL *rr, *r1;
double s;
{
	/* Scale the magnitude of a DSPL type. Ensure that the returned
	   angle is >= 0.0 (although this is more of a convention than a 
	   necessity) */

	float ang;

	rr->p.x = s * r1->p.x;
	rr->p.y = s * r1->p.y;
	rr->p.z = s * r1->p.z;

	rr->a = s * r1->a;
	rr->u = r1->u;

	return (normalizeDspl(rr));
}

DSPL *trsfToDspl (rr, t1)
register DSPL *rr;
register TRSF *t1;
{
	/* Convert TRSF type to DSPL type.

	   We just hijacked the code from trsfToRot().
	 */

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

	/* do the easy translation stuff first ... */

	rr->p.x = t1->p.x;
	rr->p.y = t1->p.y;
	rr->p.z = t1->p.z;

	/* and now find the equivalent axis of rotation ... */

	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)
	    { rr->a = rr->u.x = rr->u.y = rr->u.z = 0.0;
	      return (rr);
	    }
	   rr->u.x = tmpX/tmpS;	      
	   rr->u.y = tmpY/tmpS;	      
	   rr->u.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)
		  { rr->a = rr->u.x = rr->u.y = rr->u.z = 0.0;
		    return (rr);
		  }
		 if ((tmpS = t1->o.z - t1->a.y) >= 0.0)
		  { rr->u.x = SQRT(tmpX/versine);
		  }
		 else
		  { rr->u.x = -SQRT(tmpX/versine);
		  }
		 rr->u.y = (t1->n.y + t1->o.x)/(2*rr->u.x*versine);
		 rr->u.z = (t1->a.x + t1->n.z)/(2*rr->u.x*versine);
		 sine = tmpS / (2*rr->u.x);
		 break;
	       }
	      case Y_MAX:
	       { if (versine < SMALL || tmpY <= 0.0)
		  { rr->a = rr->u.x = rr->u.y = rr->u.z = 0.0;
		    return (rr);
		  }
		 if ((tmpS = t1->a.x - t1->n.z) >= 0.0)
		  { rr->u.y = SQRT(tmpY/versine);
		  }
		 else
		  { rr->u.y = -SQRT(tmpY/versine);
		  }
		 rr->u.x = (t1->n.y + t1->o.x)/(2*rr->u.y*versine);
		 rr->u.z = (t1->o.z + t1->a.y)/(2*rr->u.y*versine);
		 sine = tmpS / (2*rr->u.y);
		 break;
	       }
	      case Z_MAX:
	       { if (versine < SMALL || tmpZ <= 0.0)
		  { rr->a = rr->u.x = rr->u.y = rr->u.z = 0.0;
		    return (rr);
		  }
		 if ((tmpS = t1->n.y - t1->o.x) >= 0.0)
		  { rr->u.z = SQRT(tmpZ/versine);
		  }
		 else
		  { rr->u.z = -SQRT(tmpZ/versine);
		  }
		 rr->u.x = (t1->a.x + t1->n.z)/(2*rr->u.z*versine);
		 rr->u.y = (t1->o.z + t1->a.y)/(2*rr->u.z*versine);
		 sine = tmpS / (2*rr->u.z);
		 break;
	       }
	    }
	 }
	rr->a = ATAN2 (sine, cosine);
	return (normalizeDspl(rr));
}

TRSF *dsplToTrsf (tr, r1)
register TRSF *tr;
register DSPL *r1;
{
	/* Convert DSPL type to TRSF type. 

	   We just hijacked the code from 'rotToTrsf()' 
	 */

	register float ver, xv, yv, zv, xs, ys, zs;
	float sine, cosine;

	tr->p.x = r1->p.x;
	tr->p.y = r1->p.y;
	tr->p.z = r1->p.z;

	if (r1->a == 0.0)
	 { tr->n.x = tr->o.y = tr->a.z = 1.0;
	   tr->n.y = tr->o.z = tr->a.x = 0.0;
	   tr->n.z = tr->o.x = tr->a.y = 0.0;
	 }
	else
	 { 
	   SINCOS (r1->a, &sine, &cosine);
	   ver = 1. - cosine;
	   xv = ver * r1->u.x;
	   xs = r1->u.x * sine;
	   yv = ver * r1->u.y;
	   ys = r1->u.y * sine;
	   zv = ver * r1->u.z;
	   zs = r1->u.z * sine;
   
	   tr->n.x = r1->u.x * xv + cosine;
	   tr->n.y = r1->u.x * yv + zs;
	   tr->n.z = r1->u.x * zv - ys;
   
	   tr->o.x = r1->u.y * xv - zs;
	   tr->o.y = r1->u.y * yv + cosine;
	   tr->o.z = r1->u.y * zv + xs;
   
	   tr->a.x = r1->u.z * xv + ys;
	   tr->a.y = r1->u.z * yv - xs;
	   tr->a.z = r1->u.z * zv + cosine;
	 }

	return (tr);
}

TRSF *extrapTrsfByDspl (tr, s, r1)
TRSF *tr;
double s;
register DSPL *r1;
{
	/* Compute the equivalent of

		TR = dsplToTrsf (scale R1)

	   This is done by extrapolating the DSPL data (which is
	   trivial ... we just scale the angle and translation components)
	   and then map this into TR.

	   If subroutine calls were cheap, we could simply call 
	   dsplToTrsf(), but this code has to run on a VAX, so we unroll
	   the dsplToTrsf() call inside this routine.
	*/

	register float ver, xv, yv, zv, xs, ys, zs;
	float sine, cosine;

	/* Extrapolate the axis of rotation, and map it into tr. */

	if (r1->a == 0.0)
	 { tr->n.x = tr->o.y = tr->a.z = 1.0;
	   tr->n.y = tr->o.z = tr->a.x = 0.0;
	   tr->n.z = tr->o.x = tr->a.y = 0.0;
	 }
	else
	 { SINCOS (s*r1->a, &sine, &cosine);
	   ver = 1. - cosine;
	   xv = ver * r1->u.x;
	   xs = r1->u.x * sine;
	   yv = ver * r1->u.y;
	   ys = r1->u.y * sine;
	   zv = ver * r1->u.z;
	   zs = r1->u.z * sine;
   
	   tr->n.x = r1->u.x * xv + cosine;
	   tr->n.y = r1->u.x * yv + zs;
	   tr->n.z = r1->u.x * zv - ys;
   
	   tr->o.x = r1->u.y * xv - zs;
	   tr->o.y = r1->u.y * yv + cosine;
	   tr->o.z = r1->u.y * zv + xs;
   
	   tr->a.x = r1->u.z * xv + ys;
	   tr->a.y = r1->u.z * yv - xs;
	   tr->a.z = r1->u.z * zv + cosine;
	 }

	/* Again, the translation part is easy ... */

	tr->p.x = r1->p.x * s;
	tr->p.y = r1->p.y * s;
	tr->p.z = r1->p.z * s;

	return (tr);
}

#if TEST
#include <stdio.h>
#include "../h/ctree.h"
#include "../h/vector.h"
#include "../h/cputime.h"

#define SCANVF(v,s) 	  scanVf((float*)(v),(s))
#define PRINTVF(f,v,s) 	  printVf((f),(float*)(v),(s))
#define NORMVF(v,s)	  normVf((float*)(v),(s))
#define ADDVF(r,a,b,s)	  addVf((float*)(r),(float*)(a),(float*)(b),(s))
#define SUBVF(r,a,b,s)    subtractVf((float*)(r),(float*)(a),(float*)(b),(s))
#define SCALEVF(r,a,b,s)  scaleVf((float*)(r),(a),(float*)(b),(s))

char key_buf [256];

TRSF tr, t1;
DSPL  rr, r1;

VECT zerovect = { 0.0, 0.0, 0.0 };

main(argc, argv)
int argc;
char **argv;
{
	char **argvec;
	char *keytree;
	char tName[256];
	char rName[256];
	char oName[256];

	float tx, ty, tz;
	float vx, vy, vz;
	float roll, pitch, yaw;
	float angle;
	float scale;

	int interactive = NO;

	argvec = argv;
	argc--; argv++;
	while (argc)
	 { if (ARGUMENT ("-i"))
	    { interactive = YES;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { fprintf (stderr, "Usage: %s [-i]\n", argvec[0]);
	   exit (-1);
	 }

	if (interactive == NO)
	 { doAutoTest();
	   return;
	 }

	keytree = tree_match_parse("					\
(	quit								\
	set 								\
	 (trsf								\
	   (%f", &tx, 	",,, \"x translation value (mm.)\"		\
	   (%f", &ty, 	",,, \"y translation value (mm.)\"		\
	   (%f", &tz, 	",,, \"z translation value (mm.)\"		\
	   (%f", &yaw, 	",,, \"yaw angle (degrees)\"			\
	   (%f", &pitch, ",,, \"pitch angle (degrees)\"			\
	   (%f", &roll,	",,, \"roll angle (degrees)\"))))))		\
	  dspl								\
	   (%f", &tx, 	",,, \"x translation value (mm.)\"		\
	   (%f", &ty, 	",,, \"y translation value (mm.)\"		\
	   (%f", &tz, 	",,, \"z translation value (mm.)\"		\
	   (%f", &vx, 	",,, \"rotation axis x\"			\
	   (%f", &vy,   ",,, \"rotation axis y\"			\
	   (%f", &vz,   ",,, \"rotation axis z\"			\
	   (%f", &angle,",,, \"rotation angle (degrees)\")))))))	\
	 )								\
	trsfToDspl							\
	dsplToTrsf							\
	extrapTrsfByDspl						\
	 (%f", &scale, ",,, \"scale factor\")				\
	show								\
)");
	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	do
	 { tree_match (keytree, "DSPL> ", key_buf);

	   if (COMMAND("set trsf"))
	    { xyzToTrsf (&t1, tx, ty, tz);
	      rpyToTrsf (&t1, roll, pitch, yaw);
	    }
	   else if (COMMAND("set dspl"))
	    { VECT uv;

	      r1.p.x = tx;
	      r1.p.y = ty;
	      r1.p.z = tz;
	      uv.x = vx;
	      uv.y = vy;
	      uv.z = vz;
	      unitVect (&uv, &uv);
	      r1.u.x = uv.x;
	      r1.u.y = uv.y;
	      r1.u.z = uv.z;
	      r1.a = DEGTORAD * angle;
	    }
	   else if (COMMAND("show"))
	    { printDspl ("r1", &r1);
	      printDspl ("rr", &rr);
	      printf ("%s:\n", "t1");
	      printTrsf ("%13.8.5m\n", &t1);
	      printf ("%s:\n", "tr");
	      printTrsf ("%13.8.5m\n", &tr);
	    }
	   else if (COMMAND("trsfToDspl"))
	    { trsfToDspl (&rr, &t1);
	      printDspl ("rr", &rr);
	    }
	   else if (COMMAND("dsplToTrsf"))
	    { dsplToTrsf (&tr, &r1);
	      printf ("tr: \n");
	      printTrsf ("%13.8.5m\n", &tr);
	    }
	   else if (COMMAND("extrapTrsfByDspl"))
	    { tr = t1;
	      extrapTrsfByDspl (&tr, scale, &r1);
	      printf ("tr:\n");
	      printTrsf ("%13.8.5m\n\n", &tr);
	    }
	 }
	while (!(COMMAND("quit")));
}

printDspl (name, r1)
char *name;
DSPL *r1;
{
	printf ("%s:\n", name);
	PRINTVF ("%13.5f\n", &r1->p, 3);
	PRINTVF ("%13.5f ", &r1->u, 3);
	printf ("%13.5f\n", RADTODEG*r1->a);
}

double cmpDspl (dspl1, dspl2)
DSPL *dspl1, *dspl2;
{
	/* Returns an estimate of the % error between dspl1 and dspl2. */

	VECT delta;
	double size;
	double error;

	error = 0;

	if (dspl1->a != 0.0 && dspl2->a != 0.0)
	 { float errN, errP;

	   errN = NORMVF (SUBVF (&delta, &dspl1->u, &dspl2->u, 3), 3);
	   errP = NORMVF (ADDVF (&delta, &dspl1->u, &dspl2->u, 3), 3);

	   error += 3*MIN(errN, errP);
	 }

	size = (NORMVF(&dspl1->p, 3) + NORMVF(&dspl2->p, 3)) / 2.0;
	if (size > SMALL)
	 { error += 3*NORMVF(SUBVF(&delta, &dspl1->p, &dspl2->p,3),3)/size;
	 }

	size = FABS(dspl1->a + dspl2->a)/2.0;
	if (size > SMALL)
	 { error += FABS(dspl1->a - dspl2->a)/size;
	 }
	return (error/7);
}

double rangeAngle (a)
double a;
{
	/* return a in the range -PI < a <= PI */

	double n, r;

	n = a/(2*PI);
	r = n - (int)n;
	if (r >= 0.0)
	 { if (r <= 0.5)
	    { return (2*PI*r);
	    }
	   else
	    { return (2*PI*(r-1.0));
	    }
	 }
	else
	 { if (r > -0.5)
	    { return (2*PI*r);
	    }
	   else
	    { return (2*PI*(r+1.0));
	    }
	 }
}

doAutoTest()
{
	/* Automatic test procedure.
	*/

	static struct {
	    VECT p;
	    float a;
	} testcases[] =
	{   0.0,   0.0,   0.0,    0.0,     
	    1.0,   2.0,   3.0,    0.1,
	    1.0,   2.0,   3.0,    0.01,
	    1.0,   2.0,   3.0,    0.001,
	    1.0,   2.0,   3.0,    0.0001,
	    1.0,   2.0,   3.0,    0.00001,
	   -1.0,  -2.0,  -3.0,   -0.001,
	    1.0,   2.0,   3.0,    1.0,
	    4.0,   5.0,   6.0,   -1.0,
	    4.0,   5.0,   6.0,   90.0,
	   -4.0,  -5.0,  -6.0,  -90.0,
	    4.0,   5.0,   6.0,  179.99999,
	    4.0,   5.0,   6.0,  179.9999,
	    4.0,   5.0,   6.0,  179.999,
	    4.0,   5.0,   6.0,  179.99,
	    4.0,   5.0,   6.0,  179.9,
	    4.0,   5.0,   6.0, -179.9999,
	    4.0,   5.0,   6.0,  180.0,
	    4.0,   5.0,   6.0, -180.0,
	    4.0,   5.0,   6.0,  180.0001,
	   -4.0,  -5.0,  -6.0, -180.0001,
	   -1.0,  -2.0,  -3.0,  270.0,
	    1.0,   2.0,   3.0, -270.0,
	};

	static VECT testvects[] =
	{ 0.0,  0.0,  1.0,
	  0.0,  0.0, -1.0,
	  0.0,  1.0,  0.0,
	  0.0, -1.0,  0.0,
	  0.0,  1.0,  1.0,
	  0.0,  1.0, -1.0,
	  0.0, -1.0,  1.0,
	  0.0, -1.0, -1.0,
	  1.0,  0.0,  1.0,
	  1.0,  0.0, -1.0,
	  1.0,  1.0,  0.0,
	  1.0, -1.0,  0.0,
	  1.0,  1.0,  1.0,
	  1.0,  1.0, -1.0,
	  1.0, -1.0,  1.0,
	  1.0, -1.0, -1.0,
	 -1.0,  0.0,  1.0,
	 -1.0,  0.0, -1.0,
	 -1.0,  1.0,  0.0,
	 -1.0, -1.0,  0.0,
	 -1.0,  1.0,  1.0,
	 -1.0,  1.0, -1.0,
	 -1.0, -1.0,  1.0,
	 -1.0, -1.0, -1.0,
	};

	static float testscales[] =
	{ -3.0, -0.3, 0.0, 0.3, 1.0, 3.0,
	};

	float error, maxerror;

	int ic, maxIc;
	int iv, maxIv;
	int is, maxIs;

	TRSF trsfIn, trsfOut;
	DSPL dsplIn, dsplOut;

	float time1, time2, looptime;
	int k;
	
	/* Try each of the above test cases with each of the test vectors.

	   In each case, create the appropriate transform using
	   quaternions. Then convert to DSPL and check the result.
	   Convert DSPL back to a TRSF and check the result again.
	   Then test the extrap function with each of the test scales.
	*/

	maxerror = 0.0;
	for (ic=0; ic<ARRAY_SIZE(testcases); ic++)
	 { for (iv=0; iv<ARRAY_SIZE(testvects); iv++)
	    { VECT uv;
	      QTRN qtrn;
	      double ang;
	      double sinAngBy2;
	      double cosAngBy2;

	      ang = rangeAngle (testcases[ic].a * DEGTORAD);
	      uv = testvects[iv];
	      unitVect (&uv, &uv);

	      /* built the input TRSF using a quaternion */
	      sinAngBy2 = sin(ang/2.0);
	      cosAngBy2 = cos(ang/2.0);
	      if (cosAngBy2 < 0.0)
	       { cosAngBy2 = -cosAngBy2;
		 sinAngBy2 = -sinAngBy2;
	       }
	      qtrn.c =   cosAngBy2;
	      qtrn.v.x = sinAngBy2 * uv.x;
	      qtrn.v.y = sinAngBy2 * uv.y;
	      qtrn.v.z = sinAngBy2 * uv.z;
	      qtrnToTrsf (&trsfIn, &qtrn);
	      trsfIn.p = testcases[ic].p;

	      /* build a DSPL to test the output against */

	      dsplIn.p = testcases[ic].p;
	      if (ang > 0.0)
	       { dsplIn.a = ang;
		 dsplIn.u.x = uv.x;
		 dsplIn.u.y = uv.y;
		 dsplIn.u.z = uv.z;
	       }
	      else if (ang < 0.0)
	       { dsplIn.a = -ang;
		 dsplIn.u.x = -uv.x;
		 dsplIn.u.y = -uv.y;
		 dsplIn.u.z = -uv.z;
	       }
	      else if (ang == 0.0)
	       { dsplIn.a = 0.0;
		 dsplIn.p = zerovect;
	       }

	      /* transform is now built. Roll it through trsfToDspl()
		  and see what we get.
	       */

	      is = -1;

	      trsfToDspl (&dsplOut, &trsfIn);
	      if ((error = cmpDspl (&dsplOut, &dsplIn)) > maxerror)
	       { maxerror = error;
		 maxIs = is; maxIc = ic; maxIv = iv;
	       }
	      dsplToTrsf (&trsfOut, &dsplIn);
	      if ((error = cmpTrsf (&trsfOut, &trsfIn)) > maxerror)
	       { maxerror = error;
		 maxIs = is; maxIc = ic; maxIv = iv;
	       }

	      for (is=0; is<ARRAY_SIZE(testscales); is++)
	       { float angScale;
		 DSPL dsplInScale;

	      if (ic==9 && iv==4 && is==0)
	       { breakpoint();
	       }


		 SCALEVF (&dsplInScale.p, testscales[is], &dsplIn.p, 3);
		 if ((angScale = rangeAngle(dsplIn.a*testscales[is])) >= 0.0)
		  { dsplInScale.a = angScale;
		    dsplInScale.u = dsplIn.u;
		  }
		 else
		  { dsplInScale.a = -angScale;
		    dsplInScale.u.x = -dsplIn.u.x;
		    dsplInScale.u.y = -dsplIn.u.y;
		    dsplInScale.u.z = -dsplIn.u.z;
		  }

		 extrapTrsfByDspl (&trsfOut, testscales[is], &dsplIn);
		 trsfToDspl (&dsplOut, &trsfOut);
		 if ((error = cmpDspl (&dsplOut, &dsplInScale)) > maxerror)
		  { maxerror = error;
		    maxIs = is; maxIc = ic; maxIv = iv;
		  }
	       }
	    }
	 }

#define TOLERANCE	1.0e-5
	printf ("\n%s\n\n", maxerror <= TOLERANCE ?
			   "ROUTINES OK" : "POSSIBLE ERROR CONDITION" );
	printf ("max error = %g at test indicies ic=%d iv=%d is=%d\n",
		 maxerror, maxIc, maxIv, maxIs);

	dsplIn.a = .75*PI;
	dsplIn.u.x = 1.0;
	dsplIn.u.y = 0.0;
	dsplIn.u.z = 0.0;
	
#define TESTCOUNT 20000

	time1 = cputime (User);
	for (k=0; k<TESTCOUNT; k++)
	   ;
	time2 = cputime (User);
	looptime = time2 - time1;

	time1 = cputime (User);
	for (k=0; k<TESTCOUNT; k++)
	 { dsplToTrsf (&trsfOut, &dsplIn);
	 }
	time2 = cputime (User);
	printf ("dsplToTrsf() takes %g usec.\n", 
		 1000000*(time2-time1)/TESTCOUNT);

	time1 = cputime (User);
	for (k=0; k<TESTCOUNT; k++)
	 { trsfToDspl (&dsplOut, &trsfOut);
	 }
	time2 = cputime (User);
	printf ("trsfToDspl() takes %g usec. (for theta > PI/2)\n", 
		 1000000*(time2-time1)/TESTCOUNT);

	dsplIn.a = .25*PI;
	dsplIn.u.x = 1.0;
	dsplIn.u.y = 0.0;
	dsplIn.u.z = 0.0;

	dsplToTrsf (&trsfOut, &dsplIn);
	time1 = cputime (User);
	for (k=0; k<TESTCOUNT; k++)
	 { trsfToDspl (&dsplOut, &trsfOut);
	 }
	time2 = cputime (User);
	printf ("trsfToDspl() takes %g usec. (for theta <= PI/2)\n", 
		 1000000*(time2-time1)/TESTCOUNT);
}

breakpoint()
{
}

#endif
