/*-=-=-=-=-=-=-=-=-=-=-=- 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:	trsfToVel
 *
 * DESCRIPTION:	routines to convert a pair of transforms into a velocity, 
 *		and to extrapolate a transform given a velocity and a 
 *		scale factor.
 *
 * CAVEATS:	These routines have become sort of obsolete since the 
 *		RPV stuff was added (check rpv.c).
 *
 * AUTHOR/DATE:	John Lloyd, Spring 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <math.h>
#include "../h/fastmath.h"
#include "../h/robotTypes.h"

trsfToVel (vr, t1, t2)
DIFF *vr;
TRSF *t1, *t2;
{
	/* this computes a velocity from two frames, assumably taken
	   at time 0 and time 1 (the velocity should be scaled if the
	   difference is greater).

	   The velocity is computed in WORLD coordinates, according
	   to the equation:
					-1
			DEL	= dT (T2)
						-1
				= (T2 - T1) (T2)

	   The velocity is extracted from the diagonal and translational
	   components of DEL.
	 */

	float dnx, dox, dax;
	float dny, doy, day;

	dnx = t2->n.x - t1->n.x;
	dox = t2->o.x - t1->o.x;
	dax = t2->a.x - t1->a.x;
	dny = t2->n.y - t1->n.y;
	doy = t2->o.y - t1->o.y;
	day = t2->a.y - t1->a.y;

	vr->r.x = -( dny * t2->n.z + doy * t2->o.z + day * t2->a.z);
	vr->r.y =  ( dnx * t2->n.z + dox * t2->o.z + dax * t2->a.z);
	vr->r.z = -( dnx * t2->n.y + dox * t2->o.y + dax * t2->a.y);

	vr->t.x = t2->p.x - t1->p.x;
	vr->t.y = t2->p.y - t1->p.y;
	vr->t.z = t2->p.z - t1->p.z;
}

#define EPSILON	1.0e-7

velToTrsf (tr, t1, scale, v1)
TRSF *tr, *t1;
float scale;
DIFF *v1;
{
	/* given a velocity in WORLD coordinates, this routine extraplolates
	   that velocity over some time interval given by 'scale' and then
	   applies the differential to t1.

			TR = DEL(scale) T1
	 */
	register float x, y, z;
	VECT kl;
	TRSF trans;

	float omega;
	float ver, s, c, xv, yv, zv, xs, ys, zs;

	omega = sqrt(v1->r.x*v1->r.x + v1->r.y*v1->r.y + v1->r.z*v1->r.z);
	if (omega > EPSILON)
	 { 
	   kl.x = v1->r.x / omega;
	   kl.y = v1->r.y / omega;
	   kl.z = v1->r.z / omega;
   
	   SINCOS(scale * omega, &s, &c);
   
	   ver = 1. - c;
	   xv = ver * (x = kl.x);
	   xs = x * s;
	   yv = ver * (y = kl.y);
	   ys = y * s;
	   zv = ver * (z = kl.z);
	   zs = z * s;
   
	   trans.n.x = x * xv + c;
	   trans.n.y = x * yv + zs;
	   trans.n.z = x * zv - ys;
   
	   trans.o.x = y * xv - zs;
	   trans.o.y = y * yv + c;
	   trans.o.z = y * zv + xs;
   
	   trans.a.x = z * xv + ys;
	   trans.a.y = z * yv - xs;
	   trans.a.z = z * zv + c;
	 }
	else
	 { trans.n.x = trans.o.y = trans.a.z = 1.0;
	   trans.n.y = trans.o.z = trans.a.x = 0.0;
	   trans.n.z = trans.o.x = trans.a.y = 0.0;
	 }

	trans.p.x = v1->t.x * scale;
	trans.p.y = v1->t.y * scale;
	trans.p.z = v1->t.z * scale;

	multTrsf (tr, &trans, t1);	
}
	
#if TEST

#include "../h/robotMath.h"

main()
{
	TRSF t1, t2, t3, tcheck;
	DIFF vel;
	DIFF velout;
	float roll, pitch, yaw;
	float x, y, z;
	float scale = 10.0;
	
	VECT check;
	QTRN q;
	TRSF td;
	float sine, newsine;
	float theta;

	while (1)
	 { printf ("Enter x, y, z, roll, pitch, and yaw:\n");
	   printf ("T1> ");
	   if (scanf ("%f%f%f%f%f%f", &x, &y, &z, &roll, &pitch, &yaw) != 6)
	    { fatalMsg (-1, "error reading values\n");
	    }
	   xyzToTrsf (&t1, x, y, z);
	   rpyToTrsf (&t1, roll, pitch, yaw);

	   printf ("T2> ");
	   if (scanf ("%f%f%f%f%f%f", &x, &y, &z, &roll, &pitch, &yaw) != 6)
	    { fatalMsg (-1, "error reading values\n");
	    }
	   xyzToTrsf (&t2, x, y, z);
	   rpyToTrsf (&t2, roll, pitch, yaw);

	   trsfToVel (&vel, &t1, &t2);
	   velout = vel;
	   velout.r.x *= RADTODEG;
	   velout.r.y *= RADTODEG;
	   velout.r.z *= RADTODEG;
	   printVf ("vel:        %8.3f\n", &velout, 6);

	   multRiTrsf (&td, &t2, &t1);	      
	   trsfToQtrn (&q, &td);

	   sine = sqrt (q.v.x*q.v.x + q.v.y*q.v.y + q.v.z*q.v.z);
	   theta = 2*atan2(sine, q.c);

	   if (sine < EPSILON)
	    { check.x = 0.0;
	      check.y = 0.0;
	      check.z = 0.0;
	    }
	   else
	    { 
	      check.x = RADTODEG * q.v.x * theta / sine;
	      check.y = RADTODEG * q.v.y * theta / sine;
	      check.z = RADTODEG * q.v.z * theta / sine;
	    }

	   printVf ("rvel check:                            %8.3f\n", 
		     &check, 3);

	   velToTrsf (&t3, &t1, scale, &vel);
	   theta *= scale;
	   q.c = cos(theta/2.0);
	   newsine = sin(theta/2.0);

	   if (sine < EPSILON)
	    {
	      q.c = 1.0;
	      q.v.x = 0.0;
	      q.v.y = 0.0;
	      q.v.z = 0.0;
	    }
	   else
	    { 
	      q.v.x *= newsine / sine;
	      q.v.y *= newsine / sine;
	      q.v.z *= newsine / sine;
	    }

	   qtrnToTrsf (&tcheck, &q);
	   multTrsf (&tcheck, &tcheck, &t1);

	   printf ("Scaling by 10:\n");
	   printTrsf ("output:     %8.3p %8.3r\n", &t3);
	   printTrsf ("rotcheck:                              %8.3r\n", 
		       &tcheck);
	 }
}
#endif
