/*-=-=-=-=-=-=-=-=-=-=-=- 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:	diff.c
 *
 * DESCRIPTION:	This module contains routines to handle data structures
 *		of type DIFF
 * 
 * AUTHOR/DATE: Vincent Hayward, Purdue University, 1983.
 *		CVaRL installation by John Lloyd
 * 
 *-------------------------------------------------------------------------*/

#include "../h/robotTypes.h"

TRSF *diffToTrsf(tr, d1) /*::*/
register TRSF *tr;
register DIFF *d1;
{
	/* Convert the differential motion described by 'd1' into
	   a transform.
	 */

	tr->p.x = d1->t.x;
	tr->p.y = d1->t.y;
	tr->p.z = d1->t.z;
	tr->n.x = tr->o.y = tr->a.z = 1.;
	tr->a.y = -(tr->o.z = d1->r.x);
	tr->n.z = -(tr->a.x = d1->r.y);
	tr->o.x = -(tr->n.y = d1->r.z);

	/* Renormalize the matrix to protect against cumulative error	*/

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

	return (tr);
}

DIFF *trsfToDiff(dr, t1) /*::*/
register DIFF *dr;
register TRSF *t1;
{
	/* Convert a differential motion described by a transform
	    into a type DIFF.
	 */

	dr->t.x = t1->p.x;
	dr->t.y = t1->p.y;
	dr->t.z = t1->p.z;
	dr->r.x = t1->o.z;
	dr->r.y = t1->a.x;
	dr->r.z = t1->n.y;

	return(dr);
}

FORCE *transForce(fr, f1, tr) /*::*/
register FORCE *fr, *f1;
register TRSF *tr;
{
	/* Transform the forces f1 into a frame obtained by applying
	   tr to the current frame.
	 */

	VECT k;

	/* k = f X p + m */

	k.x = f1->f.y * tr->p.z - f1->f.z * tr->p.y + f1->m.x;
	k.y = f1->f.z * tr->p.x - f1->f.x * tr->p.z + f1->m.y;
	k.z = f1->f.x * tr->p.y - f1->f.y * tr->p.x + f1->m.z;

	fr->m.x = tr->n.x * k.x + tr->n.y * k.y + tr->n.z * k.z;
	fr->m.y = tr->o.x * k.x + tr->o.y * k.y + tr->o.z * k.z;
	fr->m.z = tr->a.x * k.x + tr->a.y * k.y + tr->a.z * k.z;

	fr->f.x = tr->n.x * f1->f.x + tr->n.y * f1->f.y + 
		  tr->n.z * f1->f.z;
	fr->f.y = tr->o.x * f1->f.x + tr->o.y * f1->f.y +
		  tr->o.z * f1->f.z;
	fr->f.z = tr->a.x * f1->f.x + tr->a.y * f1->f.y +
		  tr->a.z * f1->f.z;

	return (fr);
}

DIFF *transDiff(dr, d1, tr) /*::*/
register DIFF *dr, *d1;
register TRSF *tr;
{
	/* Transform the DIFF vector d1 a frame obtained by applying
	   tr to the current frame.
	 */

	VECT k;

	/* k = r X p + d */

	k.x = d1->r.y * tr->p.z - d1->r.z * tr->p.y + d1->t.x;
	k.y = d1->r.z * tr->p.x - d1->r.x * tr->p.z + d1->t.y;
	k.z = d1->r.x * tr->p.y - d1->r.y * tr->p.x + d1->t.z;

	dr->t.x = tr->n.x * k.x + tr->n.y * k.y + tr->n.z * k.z;
	dr->t.y = tr->o.x * k.x + tr->o.y * k.y + tr->o.z * k.z;
	dr->t.z = tr->a.x * k.x + tr->a.y * k.y + tr->a.z * k.z;

	dr->r.x = tr->n.x * d1->r.x + tr->n.y * d1->r.y + 
		  tr->n.z * d1->r.z;
	dr->r.y = tr->o.x * d1->r.x + tr->o.y * d1->r.y +
		  tr->o.z * d1->r.z;
	dr->r.z = tr->a.x * d1->r.x + tr->a.y * d1->r.y +
		  tr->a.z * d1->r.z;
	return(dr);
}

DIFF *copyDiff(dr, d1) /*::*/
register DIFF *dr, *d1;
{
	/* Sort of obsolete by now ... */

	*dr = *d1;
	return(dr);
}

FORCE *copyForce(fr, f1) /*::*/
register FORCE *fr, *f1;
{
	/* and so is this routine */

	*fr = *f1;
	return(fr);
}

#ifdef TEST

#include "../h/cputime.h"

#define	COMMAND(s)	(strcmp (key_buf, s) == 0)
#define	COMMANDS(s)	(strncmp (key_buf, s, strlen(s)) == 0)
char key_buf [256];

DIFF d1 = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };
DIFF d2;
TRSF t1;

main()
{
	char *keytree, *tree_match_parse(), *tree_match_err_at();

	int numTimes;

	keytree = tree_match_parse("					\
(	exit								\
	time								\
	 ([transDiff]							\
	   (%d", &numTimes,",0,,	\"number of iterations\"))	\
)",0);

	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }

	rpyToTrsf (&t1, 10., 20., 30., 10., 20., 30.);

	do
 	 { tree_match (keytree, "DIFF TEST> ", key_buf);

	   if (COMMANDS("time"))
	    { double start, stop;
	      int i;

	      start = cputime (User);

	      if (COMMAND ("time transDiff"))
	       { for (i=numTimes; i>0; i--)
		  { transDiff (&d2, &d1, &t1);
		  }
	       }
	      stop = cputime (User);
	      printf ("Execution time = %lg seconds\n", stop-start);
	    }
	 }
	while (!(COMMAND("exit")));
}


#endif
