/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 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:	quaternion
 *
 * DESCRIPTION:	Routines for manipulating the QTRN data type, which
 *		describes a quaternion. Quaternions are a useful way
 *		of representing rotations.
 *
 * AUTHOR/DATE:	John Lloyd, October, 1986
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <math.h>

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

#define QSMALL 1.0e-7

#ifdef vax
	asm ("#define c   0");
	asm ("#define vx  4");
	asm ("#define vy  8");
	asm ("#define vz 12");
	asm ("#define s  16");

	asm ("#define c1 r2");
	asm ("#define x1 r3");
	asm ("#define y1 r4");
	asm ("#define z1 r5");
	asm ("#define c2 r6");
	asm ("#define x2 r7");
	asm ("#define y2 r8");
	asm ("#define z2 r9");
#endif

trsfToQtrn (q, t)
register QTRN *q;
register TRSF *t;
{
	/* Convert the rotational part of the transform t into
	   the quaternion q.

	   Based on the follwoing algorithm:

	   Let the elements of the matrix be given by Q_ij. For i,j,k
	   such that epsilon (i,j,k) = 1, use the maximum of 

	   q_i = 1/2 sqrt (1 + Q_ii - Q_jj - Q_kk)
	   q   = 1/2 sqrt (1 + Q_ii + Q_jj + Q_kk)

	   and the relations

	   q_j q_k = 1/4 (Q_kj + Q_jk)
	   q q_i   = 1/4 (Q_kj - Q_jk)

	   We use this algorithm because it is numerically well behaved.

	   Execution time for C versions of these functions are about
	   310 microseconds for trsfToQtrn, and 280
	   microseconds for qtrnToTrsf, on a MicroVAX II.
	*/

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

	int maxCX, maxYZ, max;
	float sumC, sumX, sumY, sumZ;
	float s;

	sumC = t->n.x + t->o.y + t->a.z;
	sumX = t->n.x - t->o.y - t->a.z;
	sumY = t->o.y - t->a.z - t->n.x;
	sumZ = t->a.z - t->n.x - t->o.y;

	if (sumC > sumX)
	 { if (sumY > sumZ)
	    { max = (sumC > sumY) ? C_MAX : Y_MAX;
	    }
	   else
	    { max = (sumC > sumZ) ? C_MAX : Z_MAX;
	    }
	 }
	else
	 { if (sumY > sumZ)
	    { max = (sumX > sumY) ? X_MAX : Y_MAX;
	    }
	   else
	    { max = (sumX > sumZ) ? X_MAX : Z_MAX;
	    }
	 }

	switch (max)
	 { case C_MAX:
/*		printf ("C_MAX\n");	*/

		s = sqrt (1.0 + sumC) * 0.5;
		q->c = s;
	        s *= 4.0;
		q->v.x = (t->o.z - t->a.y) / s;
		q->v.y = (t->a.x - t->n.z) / s;
		q->v.z = (t->n.y - t->o.x) / s;
	        break;
	   case X_MAX:
/*		printf ("X_MAX\n");	*/

		s = sqrt (1.0 + sumX) * 0.5;
		q->v.x = s;
		s *= 4.0;
		q->c =   (t->o.z - t->a.y) / s;
	        q->v.y = (t->o.x + t->n.y) / s;
		q->v.z = (t->a.x + t->n.z) / s;
		break;
	   case Y_MAX:
/*		printf ("Y_MAX\n");	*/

		s = sqrt (1.0 + sumY) * 0.5;
		q->v.y = s;
		s *= 4.0;
		q->c =   (t->a.x - t->n.z) / s;
	        q->v.x = (t->o.x + t->n.y) / s;
		q->v.z = (t->o.z + t->a.y) / s;
		break;
	   case Z_MAX:
/*		printf ("Z_MAX\n");	*/

		s = sqrt (1.0 + sumZ) * 0.5;
		q->v.z = s;
		s *= 4.0;
		q->c =   (t->n.y - t->o.x) / s;
	        q->v.y = (t->o.z + t->a.y) / s;
		q->v.x = (t->a.x + t->n.z) / s;
		break;
	 }
	if (q->c < 0.0)
	 { q->c   = -q->c;
	   q->v.x = -q->v.x;
	   q->v.y = -q->v.y;
	   q->v.z = -q->v.z;
	 }
}

qtrnToTrsf (t, q)
register TRSF *t;
register QTRN *q;
{
	/* This is based on a algorithm in Ken Shoemake's paper

		``Animating Rotation with Quaternion Curves''
		SIGGRAPH 1985.

	   It sets the rotational component of t to be equal to the
	   rotation described by the quaternion q.
	*/

	double twoX, twoY, twoZ;
	double twoXW, twoXX, twoXY;
	double twoYW, twoYY, twoYZ;
	double twoZW, twoZZ, twoZX;

	twoX = q->v.x + q->v.x;
	twoY = q->v.y + q->v.y;
	twoZ = q->v.z + q->v.z;

	twoXW = twoX * q->c;
	twoYW = twoY * q->c;
	twoZW = twoZ * q->c;
	twoXX = twoX * q->v.x;
	twoYY = twoY * q->v.y;
	twoZZ = twoZ * q->v.z;
	twoXY = twoX * q->v.y;
	twoYZ = twoY * q->v.z;
	twoZX = twoZ * q->v.x;

	t->n.x = 1.0 - twoYY - twoZZ;
	t->n.y = twoXY + twoZW;
	t->n.z = twoZX - twoYW;
	t->o.x = twoXY - twoZW;
	t->o.y = 1.0 - twoXX - twoZZ;
	t->o.z = twoYZ + twoXW;
	t->a.x = twoZX + twoYW;
	t->a.y = twoYZ - twoXW;
	t->a.z = 1.0 - twoXX - twoYY;
}

multQtrn (r, q1, q2)
register QTRN *r;
register QTRN *q1, *q2;
{
	/*		       
	   computes r = q1 . q2
	 */

#ifdef vax
	asm ("pushr	$0x1c0");

	asm ("movq	0(r10), r2");
	asm ("movq	8(r10), r4");
	asm ("movq	0(r9), r6");
	asm ("movq	8(r9), r8");

	asm ("jsb	qmult");
	asm ("popr	$0x1c0");
#else
	float c, x, y, z;

	c =   q1->c * q2->c 
	       - (q1->v.x * q2->v.x + q1->v.y * q2->v.y + q1->v.z * q2->v.z);

	x = q1->c * q2->v.x + q1->v.x * q2->c +
	         q1->v.y * q2->v.z - q1->v.z * q2->v.y;
	y = q1->c * q2->v.y + q1->v.y * q2->c +
	         q1->v.z * q2->v.x - q1->v.x * q2->v.z;
	z = q1->c * q2->v.z + q1->v.z * q2->c +
	         q1->v.x * q2->v.y - q1->v.y * q2->v.x;

	r->c = c;
	r->v.x = x;
	r->v.y = y;
	r->v.z = z;
#endif
}

multRiQtrn (r, q1, q2)
register QTRN *r;
register QTRN *q1, *q2;
{
	/*		       -1
	   computes r = q1 . q2
	 */

#ifdef vax
	asm ("pushr	$0x1c0");

	asm ("movq	0(r10), r2");
	asm ("movq	8(r10), r4");

	asm ("movf	0(r9), r6");	/* negate the vector part of q2 */
	asm ("mnegf	4(r9), r7");
	asm ("mnegf	8(r9), r8");
	asm ("mnegf    12(r9), r9");

	asm ("jsb	qmult");
	asm ("popr	$0x1c0");
#else
	float c, x, y, z;

	c =   q1->c * q2->c 
	       + (q1->v.x * q2->v.x + q1->v.y * q2->v.y + q1->v.z * q2->v.z);

	/* Use the negative of the q2 vector */

	x = -q1->c * q2->v.x + q1->v.x * q2->c +
	         q1->v.z * q2->v.y - q1->v.y * q2->v.z;
	y = -q1->c * q2->v.y + q1->v.y * q2->c +
	         q1->v.x * q2->v.z - q1->v.z * q2->v.x;
	z = -q1->c * q2->v.z + q1->v.z * q2->c +
	         q1->v.y * q2->v.x - q1->v.x * q2->v.y;

	r->c = c;
	r->v.x = x;
	r->v.y = y;
	r->v.z = z;
#endif
}

multLiQtrn (r, q1, q2)
register QTRN *r;
register QTRN *q1, *q2;
{
	/*		  -1
	   computes r = q1  . q2
	 */


#ifdef vax
	/* Turbo charge the routine! */
	asm ("pushr	$0x1c0");

	asm ("movf	0(r10), r2");	/* Negate the vector part of q1 */
	asm ("mnegf	4(r10), r3");
	asm ("mnegf	8(r10), r4");
	asm ("mnegf    12(r10), r5");

	asm ("movq	0(r9), r6");
	asm ("movq	8(r9), r8");

	asm ("jsb	qmult");
	asm ("popr	$0x1c0");
#else
	float c, x, y, z;

	c =   q1->c * q2->c 
	       + (q1->v.x * q2->v.x + q1->v.y * q2->v.y + q1->v.z * q2->v.z);

	/* use the negative of the q1 vector */

	x = q1->c * q2->v.x - q1->v.x * q2->c +
		 q1->v.z * q2->v.y - q1->v.y * q2->v.z;
	y = q1->c * q2->v.y - q1->v.y * q2->c +
	         q1->v.x * q2->v.z - q1->v.z * q2->v.x;
	z = q1->c * q2->v.z - q1->v.z * q2->c +
	         q1->v.y * q2->v.x - q1->v.x * q2->v.y;

	r->c = c;
	r->v.x = x;
	r->v.y = y;
	r->v.z = z;
#endif
}

invertQtrn (r, q)
QTRN *r, *q;
{
	/* Computes the inverse of q */

	r->c = q->c;
	r->v.x = -q->v.x;
	r->v.y = -q->v.y;
	r->v.z = -q->v.z;
}

unitQtrn (r, q)
QTRN *r, *q;
{
	/* normalizes a quaternion vector */

	double m;

	m = sqrt (q->c*q->c + q->v.x*q->v.x + q->v.y*q->v.y + q->v.z*q->v.z);
	if (m < QSMALL)
	 { r->c = 1.0;
	   r->v.x = 0.0;
	   r->v.y = 0.0;
	   r->v.z = 0.0;
	 }
	else
  	 { r->c = q->c / m;
	   r->v.x = q->v.x / m;
	   r->v.y = q->v.y / m;
	   r->v.z = q->v.z / m;
	 }
}

float normQtrn (q)
QTRN *q;
{
	/* computes the magnitude of q */

	return (sqrt (q->c*q->c + 
		      q->v.x*q->v.x + q->v.y*q->v.y + q->v.z*q->v.z));
}

qtrnToVect (v, q)
VECT *v;
QTRN *q;
{
	/* Convert a quaternion to a unit vector whose magnitude
	   is equal to the rotationa angle associated with the
	   quaternion.
	*/

	float theta, sinval, cosval;

	cosval = q->c;
	sinval = sqrt (q->v.x*q->v.x + q->v.y*q->v.y + q->v.z*q->v.z);
	theta = ATAN2 (sinval, cosval);
	
	if (cosval < 0.0)
	 { theta -= PI;
	 }
	if (sinval < QSMALL)
	 { v->x = v->y = v->z = 0.0;
	 }
	else
	 { v->x = q->v.x * 2 * theta / sinval;
	   v->y = q->v.y * 2 * theta / sinval;
	   v->z = q->v.z * 2 * theta / sinval;
	 }
}

#ifdef vax
	/* Assembly code for doing quaternion multiplication. Assumes that
	   q1 is in r2-r5, q2 is in r6-r9, a pointer to the result is in r11,
	   and r0 and r1 are expendible */

	asm (".text");
	asm ("qmult:");

	/* Compute c = - v1 . v2 + c1 c2 */

	asm ("mulf3	x1, x2, r0");
	asm ("mulf3	y1, y2, r1");
	asm ("addf2	r1, r0");
	asm ("mulf3	z1, z2, r1");
	asm ("addf2	r1, r0");
	asm ("mulf3	c1, c2, r1");
	asm ("subf3	r0, r1, c(r11)");

	/* Now compute the vector part */

	asm ("mulf3	c1, x2, r0");		/* X component */
	asm ("mulf3	x1, c2, r1");
	asm ("addf2	r1, r0");
	asm ("mulf3	y1, z2, r1");
	asm ("addf2 	r1, r0");
	asm ("mulf3	z1, y2, r1");
	asm ("subf3	r1, r0, vx(r11)");

	asm ("mulf3	c1, y2, r0");		/* Y component */
	asm ("mulf3	y1, c2, r1");
	asm ("addf2	r1, r0");
	asm ("mulf3	z1, x2, r1");
	asm ("addf2 	r1, r0");
	asm ("mulf3	x1, z2, r1");
	asm ("subf3	r1, r0, vy(r11)");

	asm ("mulf3	c1, z2, r0");		/* Z component */
	asm ("mulf3	z1, c2, r1");
	asm ("addf2	r1, r0");
	asm ("mulf3	x1, y2, r1");
	asm ("addf2 	r1, r0");
	asm ("mulf3	y1, x2, r1");
	asm ("subf3	r1, r0, vz(r11)");

	asm ("rsb");
#endif

#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];

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

	float roll, pitch, yaw;
	float w, x, y, z;
	static TRSF t = {"", 0, 1.0, 0.0, 0.0, 0.0,
				1.0, 0.0, 0.0, 0.0,
				1.0, 0.0, 0.0, 0.0 };
	TRSF t1, t2;
	QTRN q, q1, q2;
	int i, numIterations;

	t1 = t2 = t;

	keytree = tree_match_parse("					\
(	exit								\
	quaternion-to-transform						\
	multiply							\
	time								\
	 ([conversions multiply]					\
	   (%d", &numIterations, ",,,	\"Number of iterations\"))	\
	transform-to-quaternion						\
)",0);

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

	   if (COMMAND("transform-to-quaternion") ||
	       COMMAND("time conversions") )
	    { printf ("Enter yaw, pitch, and roll of the transform > ");
	      if (scanf ("%f%f%f", &yaw, &pitch, &roll) != 3)
	       { printf ("ERROR reading values\n");
		 continue;
	       }
	      while (getchar() != '\n')
		 ;
	      rpyToTrsf (&t, roll, pitch, yaw);
	    }
	   if (COMMAND ("transform-to-quaternion"))
	    {
	      printf ("Input Transform:\n");
	      printr (&t, stdout);
	      trsfToQtrn (&q, &t);
           printf ("Quaternion:  [%10.5f  %10.5f %10.5f %10.5f] (%10.8lf) \n",
		       q.c, q.v.x, q.v.y, q.v.z, normQtrn (&q));
	      qtrnToTrsf (&t, &q);
	      printf ("Output Transform:\n");
	      printr (&t, stdout);
	    }
	   else if (COMMAND ("multiply"))
	    { 
	      printf ("Enter yaw, pitch and roll for 1st rotation > ");
	      if (scanf ("%f%f%f", &yaw, &pitch, &roll) != 3)
	       { printf ("ERROR reading input\n");
	         continue;
	       }
	      rpyToTrsf (&t1, roll, pitch, yaw);
	      printf ("Enter yaw, pitch, and roll for 2nd rotation > ");
	      if (scanf ("%f%f%f", &yaw, &pitch, &roll) != 3)
	       { printf ("ERROR reading input\n");
	         continue;
	       }
	      rpyToTrsf (&t2, roll, pitch, yaw);

	      while (getchar() != '\n')
		 ;

	      multTrsf (&t, &t1, &t2);
	      trsfToQtrn (&q, &t);
	      printf ("Check:       %10.5f %10.5f    %10.5f %10.5f %10.5f\n",
		       q.c, q.s, q.v.x, q.v.y, q.v.z);
	      printr (&t, stdout);
	      trsfToQtrn (&q1, &t1);
	      trsfToQtrn (&q2, &t2);
	      printf ("Q1:          %10.5f %10.5f    %10.5f %10.5f %10.5f\n",
		       q1.c, q1.s, q1.v.x, q1.v.y, q1.v.z);
	      printf ("Q2:          %10.5f %10.5f    %10.5f %10.5f %10.5f\n",
		       q2.c, q2.s, q2.v.x, q2.v.y, q2.v.z);
	      multQtrn (&q, &q1, &q2);
	      printf ("Quaternion:  %10.5f %10.5f    %10.5f %10.5f %10.5f\n",
		       q.c, q.s, q.v.x, q.v.y, q.v.z);
	      invertQtrn (&q1, &q1);
	      multLiQtrn (&q, &q1, &q2);
	      printf ("multLiTrsf: %10.5f %10.5f    %10.5f %10.5f %10.5f\n",
		       q.c, q.s, q.v.x, q.v.y, q.v.z);
	      invertQtrn (&q1, &q1);
	      invertQtrn (&q2, &q2);
	      multRiQtrn (&q, &q1, &q2);
	      printf ("invMultRght: %10.5f %10.5f    %10.5f %10.5f %10.5f\n",
		       q.c, q.s, q.v.x, q.v.y, q.v.z);
	      qtrnToTrsf (&t, &q);
	      printr (&t, stdout);
	    }
	   else if (COMMAND ("time conversions"))
	    { double start, stop;

	      start = cputime (User);
	      for (i = numIterations; i; i--)
	       { trsfToQtrn (&q, &t);
	       }
	      stop = cputime (User);
	      printf ("trsfToQtrn time = %g microseconds\n", 
		      (stop-start)/numIterations *1000000.);
	      start = cputime (User);
	      for (i = numIterations; i; i--)
	       { qtrnToTrsf (&t, &q);
	       }
	      stop = cputime (User);
	      printf ("qtrnToTrsf time = %g microseconds\n", 
		      (stop-start)/numIterations *1000000.);
	    }
	   else if (COMMAND ("time multiply"))
	    { double start, stop;
	      
	      q1.c = q2.c = .6777;
	      q1.v.x = q2.v.z = .55432;
	      q1.v.y = q2.v.y = .61123;
	      q1.v.z = q2.v.x = .71234;
	      	      
	      start = cputime (User);
	      for (i=numIterations; i; i--)
	       { multQtrn (&q, &q1, &q2);
	       }
	      stop = cputime (User);
	      printf ("multiply time = %g microseconds\n", 
		      (stop-start)/numIterations *1000000.);
	    }
	 }
	while (!(COMMAND("exit")));
}

#endif	

	   
	
