/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * MODULE NAME:	transLib
 *
 * DESCRIPTION:	Support functions for the robot specific operations. Most
 *		of the contents of this file have been derived directly
 *		from RCCL source code.
 * 
 * AUTHOR/DATE:	John Lloyd, Summer 1987
 *		Vincent Hayward, Purdue University, 1983
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include "cdefs.h"
#include "types.h"
#include "error.h"

#include <math.h>
#include "parameters.h"

#define SMALL	(1000*DOUBLEPREC)

rotToTrsf (t, v, h) /*::*/
double *t, *v;
double h;
{
	double s, c, ver, x, y, z, xv, yv, zv, xs, ys, zs;

	s = sin(DEGTORAD * h);
	ver = 1. - (c = cos(DEGTORAD * h));
	xv = ver * (x = v[0]);
	xs = x * s;
	yv = ver * (y = v[1]);
	ys = y * s;
	zv = ver * (z = v[2]);
	zs = z * s;

	t[0] = x * xv + c;
	t[4] = x * yv + zs;
	t[8] = x * zv - ys;

	t[1] = y * xv - zs;
	t[5] = y * yv + c;
	t[9] = y * zv + xs;

	t[2] = z * xv + ys;
	t[6] = z * yv - xs;
	t[10] = z * zv + c;

	return(1);
}

unitVector (r, v, n)
double *r, *v;
int n;
{
	int j;
	double mag = 0.0;
	
	for (j=0; j<n; j++)
	 { mag += v[j]*v[j];
	 }
	if (fabs(mag) < SMALL)
 	 { return (-1);
	 }
	for (j=0; j<n; j++)
	 { r[j] = v[j]/ sqrt(mag);
	 }
	return (1);
}

eulToTrsf (t, phi, the, psi) /*::*/
double *t;
double phi, the, psi;
{
	double 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);

	t[0]  =   cphi * cthe * cpsi - sphi * spsi;
	t[4]  =   sphi * cthe * cpsi + cphi * spsi;
	t[8]  = - sthe * cpsi;

	t[1]  = - cphi * cthe * spsi - sphi * cpsi;
	t[5]  = - sphi * cthe * spsi + cphi * cpsi;
	t[9]  =   sthe * spsi;

	t[2]  =   cphi * sthe;
	t[6]  =   sphi * sthe;
	t[10] =   cthe;

	return(1);
}

rpyToTrsf (t, roll, pitch, yaw)
double *t, roll, pitch, yaw;
{
	double 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);

	t[0]  = croll * cpitch;
	t[4]  = sroll * cpitch;
	t[8]  = - spitch;

	t[1]  = croll * spitch * syaw - sroll * cyaw;
	t[5]  = sroll * spitch * syaw + croll * cyaw;
	t[9]  = cpitch * syaw;

	t[2]  = croll * spitch * cyaw + sroll * syaw;
	t[6]  = sroll * spitch * cyaw - croll * syaw;
	t[10] = cpitch * cyaw;

	return(1);
}

crossVector (vr, v1, v2)
double *vr, *v1, *v2;
{
	vr[0] = v1[1] * v2[2] - v1[2] * v2[1];
	vr[1] = v1[2] * v2[0] - v1[0] * v2[2];
	vr[2] = v1[0] * v2[1] - v1[1] * v2[0];
}

ftrans (ft, t1)
matrix *ft, *t1;
{
	int i, j;
	double d, v[3], p[3], r[3];

	p[0] = t1->buf[3];
	p[1] = t1->buf[7];
	p[2] = t1->buf[11];

	for (i=0; i<3; i++)
	 { for (j=0; j<3; j++)
	    { d = ELEMENT (t1, j, i);

	      ELEMENT (ft, i, j) = d;
	      ELEMENT (ft, i+3, j+3) = d;
	      ELEMENT (ft, i, j+3) = 0.0;
	      v[j] = d;
	    }
	   crossVector (r, p, v);
	   for (j=0; j<3; j++)
	    { ELEMENT (ft, i+3, j) = r[j];
	    }
	 }
}

vtrans (ft, t1)
matrix *ft, *t1;
{
	int i, j;
	double d, v[3], p[3], r[3];

	p[0] = t1->buf[3];
	p[1] = t1->buf[7];
	p[2] = t1->buf[11];

	for (i=0; i<3; i++)
	 { for (j=0; j<3; j++)
	    { d = ELEMENT (t1, j, i);

	      ELEMENT (ft, i, j) = d;
	      ELEMENT (ft, i+3, j+3) = d;
	      ELEMENT (ft, i+3, j) = 0.0;
	      v[j] = d;
	    }
	   crossVector (r, p, v);
	   for (j=0; j<3; j++)
	    { ELEMENT (ft, i, j+3) = r[j];
	    }
	 }
}

noatorot(r, t) /*::*/
double *r, *t;
{
	double sin, cos;
	double q[4];	

	if (trsfToQtrn (q, t) == -1)
	 { return (-1);
	 }
	cos = q[0];
	sin = q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
	if (sin < 0.0)
	 { sin = 0.0;
	 }
 	else
  	 { sin = sqrt (sin);
	 }
	if (sin < SMALL)
	 { r[0] = r[1] = r[2] = r[3] = 0.0;
	 }
 	else
  	 { r[0] = 2 * RADTODEG * atan2 (sin, cos);
	   r[1] = q[1] / sin;
	   r[2] = q[2] / sin;
	   r[3] = q[3] / sin;
	 }
	return (1);
}

trsfToRpy(r, t) /*::*/
double *r, *t;
{
	double sroll, croll, nx, ny, p;

	nx = t[0];
	ny = t[4];
	if (FABS(nx) < SMALL && FABS(ny) < SMALL) {
		r[0] = 0.;
		r[1] = RADTODEG * atan2(- t[8], nx);
		r[2] = RADTODEG * atan2(- t[6], t[5]);
	} 
	else {
		r[0] = RADTODEG * (p = atan2(ny, nx));
		sroll = sin(p);
		croll = cos(p);
		r[1] = RADTODEG * 
			 atan2(- t[8], croll * nx + sroll * ny);
		r[2] =   RADTODEG * 
			 atan2(sroll * t[2] - croll * t[6],
			       croll * t[5] - sroll * t[1]);
	}
	return (1);
}

trsfToEul(r, t) /*::*/
double *r, *t;
{
	double sphi, cphi, p;

	if (FABS(t[6]) > SMALL || FABS(t[2]) > SMALL) {
		r[0] = RADTODEG * (p = atan2(t[6], t[2]));
		sphi = sin(p);
		cphi = cos(p);
		r[1] = RADTODEG * 
		       atan2(cphi * t[2] + sphi * t[6],
			     t[10]);
		r[2] = RADTODEG * 
		       atan2(- sphi * t[0] + cphi * t[4],
			     - sphi * t[1] + cphi * t[5]);
	} 
	else {
		r[0] = 0.;
		r[1] = RADTODEG * atan2(t[2], t[10]);
		r[2] = RADTODEG * atan2(t[4], t[5]);
	}
	return (1);
}

trsfToQtrn (q, t)
register double *q;
register double *t;
{
	/* 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.
	*/

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

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

	sumC = t[0] + t[5] + t[10];
	sumX = t[0] - t[5] - t[10];
	sumY = t[5] - t[10] - t[0];
	sumZ = t[10] - t[0] - t[5];

	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:
		s = sqrt (1.0 + sumC) * 0.5;
		q[0] = s;
	        s *= 4.0;

		if (FABS(s) < SMALL) goto dividebyzero;

		q[1] = (t[9] - t[6]) / s;
		q[2] = (t[2] - t[8]) / s;
		q[3] = (t[4] - t[1]) / s;
	        break;
	   case X_MAX:
		s = sqrt (1.0 + sumX) * 0.5;
		q[1] = s;
		s *= 4.0;

		if (FABS(s) < SMALL) goto dividebyzero;

		q[0] =   (t[9] - t[6]) / s;
	        q[2] = (t[1] + t[4]) / s;
		q[3] = (t[2] + t[8]) / s;
		break;
	   case Y_MAX:
		s = sqrt (1.0 + sumY) * 0.5;
		q[2] = s;
		s *= 4.0;

		if (FABS(s) < SMALL) goto dividebyzero;

		q[0] =   (t[2] - t[8]) / s;
	        q[1] = (t[1] + t[4]) / s;
		q[3] = (t[9] + t[6]) / s;
		break;
	   case Z_MAX:
		s = sqrt (1.0 + sumZ) * 0.5;
		q[3] = s;
		s *= 4.0;

		if (FABS(s) < SMALL) goto dividebyzero;

		q[0] =   (t[4] - t[1]) / s;
	        q[2] = (t[9] + t[6]) / s;
		q[1] = (t[2] + t[8]) / s;
		break;
	 }
	if (q[0] < 0.0)
	 { q[0]   = -q[0];
	   q[1] = -q[1];
	   q[2] = -q[2];
	   q[3] = -q[3];
	 }
	return (1);

dividebyzero:
	return (-1);
}

qtrnToTrsf (t, q)
double *t;
double *q;
{
	/* This is also based on matrial in Shoemake's artical (see
	   trsfToQtrn), and assumes the quaternion is a unit 
	   quaternion. */

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

	twoX = q[1] + q[1];
	twoY = q[2] + q[2];
	twoZ = q[3] + q[3];

	twoXW = twoX * q[0];
	twoYW = twoY * q[0];
	twoZW = twoZ * q[0];
	twoXX = twoX * q[1];
	twoYY = twoY * q[2];
	twoZZ = twoZ * q[3];
	twoXY = twoX * q[2];
	twoYZ = twoY * q[3];
	twoZX = twoZ * q[1];

	t[0] = 1.0 - twoYY - twoZZ;
	t[4] = twoXY + twoZW;
	t[8] = twoZX - twoYW;
	t[1] = twoXY - twoZW;
	t[5] = 1.0 - twoXX - twoZZ;
	t[9] = twoYZ + twoXW;
	t[2] = twoZX + twoYW;
	t[6] = twoYZ - twoXW;
	t[10] = 1.0 - twoXX - twoYY;
	
	return (1);
}


vaoToTrsf(t, a, o)
double *t, *a, *o;
{
	double n[3];

	/* a = a / |a|
	   n = o X a		
	   n = n / |n|		
	   o = a X n		
	*/

	if (unitVector (a, a, 3) == -1)
	 { return (-1);
	 }
	n[0] = o[1]*a[2] - a[1]*o[2];
	n[1] = o[2]*a[0] - a[2]*o[0];
	n[2] = o[0]*a[1] - a[0]*o[1];
	if (unitVector (n, n, 3) == -1)
	 { return (-1);
	 }
	o[0] = a[1]*n[2] - n[1]*a[2];
	o[1] = a[2]*n[0] - n[2]*a[0];
	o[2] = a[0]*n[1] - n[0]*a[1];

	t[0]  = n[0];
	t[4]  = n[1];
	t[8]  = n[2];

	t[1]  = o[0];
	t[5]  = o[1];
	t[9]  = o[2];

	t[2]  = a[0];
	t[6]  = a[1];
	t[10] = a[2];
}

#define NX	0
#define NY	4
#define NZ	8

#define OX	1
#define OY	5
#define OZ	9

#define AX	2
#define AY	6
#define AZ     10

#define PX	3
#define PY      7
#define PZ     11

#define DX	0
#define DY	1
#define DZ	2
#define DTHE	3
#define DPHI	4
#define DPSI	5

setDriveparams (d, t)
double *d, *t;
{
	double n1, n2;		/* local variables for common subexpr */
	double x, y;
	double lthe, lpsi;
	double spsi, cpsi, vthe;

	d[DX] = t[PX];
	d[DY] = t[PY];
	d[DZ] = t[PZ];

	if (FABS(t[AX]) < SMALL && FABS(t[AY]) < SMALL)
	 { lpsi = d[DPSI] = 0.;
	 }
	else 
	 { lpsi = atan2 (t[AY], t[AX]);
	   d[DPSI] = lpsi;
	 }

	/* compute theta */

	y = sqrt(t[AX] * t[AX] + t[AY] * t[AY]);

	if (y < SMALL && FABS(t[AZ]) < SMALL)
	 { lthe = d[DTHE] = 0.;
	 }
	else
	 { lthe = atan2 (y, t[AZ]);
	   d[DTHE] = lthe;
	 }

	/* compute phi */

	spsi = sin(lpsi);
	cpsi = cos(lpsi);
	vthe = 1. - t[AZ];

	n1 = - spsi * cpsi * vthe;
	n2 =   cpsi * cpsi * vthe + t[AZ];

	y = n1 * t[NX] + n2 * t[NY] - t[AY] * t[NZ];
	x = n1 * t[OX] + n2 * t[OY] - t[AY] * t[OZ];

	if (FABS(y) < SMALL && FABS(x) < SMALL)
	 { d[DPHI] = 0.;
	 }
	else
	 { d[DPHI] = atan2 (y, x);
	 }

	return (1);
}


driveFunction (t, d)  /*::*/
double *t, *d;
{
	double sphi, cphi, spsi, cpsi, sthe, cthe, vthe, loc;

	t[PX] = d[DX];
	t[PY] = d[DY];
	t[PZ] = d[DZ];

	sphi = sin (d[DPHI]);
	cphi = cos (d[DPHI]);
	spsi = sin (d[DPSI]);
	cpsi = cos (d[DPSI]);
	sthe = sin (d[DTHE]);
	cthe = cos (d[DTHE]);
	vthe = 1. - cthe;

	t[AX] = cpsi * sthe;
	t[AY] = spsi * sthe;
	t[AZ] = cthe;

	t[OX] = - sphi * (spsi * spsi * vthe + cthe)
		 + cphi * (loc = - spsi * cpsi * vthe);
	t[OY] = - sphi * (loc) + cphi * (cpsi * cpsi * vthe + cthe);
	t[OZ] = sphi * t[AX] - cphi * t[AY];

	t[NX] = t[OY] * t[AZ] - t[OZ] * t[AY];
	t[NY] = t[OZ] * t[AX] - t[OX] * t[AZ];
	t[NZ] = t[OX] * t[AY] - t[OY] * t[AX];

	return (1);
}

noatorot2 (r, t)
double *r, *t;
{
	double cosine, versine, sine;
	double tmpX, tmpY, tmpZ, tmpS;
	int max;

	cosine = 0.5*(t[0] + t[5] + t[10] - 1.0);
	versine = 1.0 - cosine;

	tmpX = FABS (t[NX] - cosine);
	tmpY = FABS (t[OY] - cosine);
	tmpZ = FABS (t[AZ] - cosine);

	if (tmpX >= tmpY)
	 { max = (tmpX >= tmpZ ? X_MAX : Z_MAX);
	 }
	else
	 { max = (tmpY >= tmpZ ? Y_MAX : Z_MAX);
	 }

	printf ("case %d\n", max);
	switch (max)
	 { case X_MAX:
	    { if (versine < SMALL || tmpX <= 0.0)
	       { r[0] = r[1] = r[2] = r[3] = 0.0;
		 return (1);
	       }
	      if ((tmpS = t[OZ] - t[AY]) >= 0.0)
	       { r[1] = sqrt(tmpX/versine);
	       }
	      else
	       { r[1] = -sqrt(tmpX/versine);
	       }
	      r[2] = (t[NY] + t[OX])/(2*r[1]*versine);
	      r[3] = (t[AX] + t[NZ])/(2*r[1]*versine);
	      sine = tmpS / (2*r[1]);
	      break;
	    }
	   case Y_MAX:
	    { if (versine < SMALL || tmpY <= 0.0)
	       { r[0] = r[1] = r[2] = r[3] = 0.0;
		 return (1);
	       }
	      if ((tmpS = t[AX] - t[NZ]) >= 0.0)
	       { r[2] = sqrt(tmpY/versine);
	       }
	      else
	       { r[2] = -sqrt(tmpY/versine);
	       }
	      r[1] = (t[NY] + t[OX])/(2*r[2]*versine);
	      r[3] = (t[OZ] + t[AY])/(2*r[2]*versine);
	      sine = tmpS / (2*r[2]);
	      break;
	    }
	   case Z_MAX:
	    { if (versine < SMALL || tmpZ <= 0.0)
	       { r[0] = r[1] = r[2] = r[3] = 0.0;
		 return (1);
	       }
	      if ((tmpS = t[NY] - t[OX]) >= 0.0)
	       { r[3] = sqrt(tmpZ/versine);
	       }
	      else
	       { r[3] = -sqrt(tmpZ/versine);
	       }
	      r[1] = (t[AX] + t[NZ])/(2*r[3]*versine);
	      r[2] = (t[OZ] + t[AY])/(2*r[3]*versine);
	      sine = tmpS / (2*r[3]);
	      break;
	    }
	 }
	r[0] = RADTODEG * atan2 (sine, cosine);
	return (1);
}

