/*-=-=-=-=-=-=-=-=-=-=-=- 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:	cartBlend
 *
 * DESCRIPTION:	The functions here set up, compute, and apply the a
 *		correction function that blends two Cartesian paths S1 and
 *      	S2 over some transition interval. The paths are not known,
 *      	in general, only their initial positions and velocities
 *      	are. These are 	specified using the functions
 *      	setCartesianBlend1() and setCartesianBlend2(), which take
 *      	sample points of S1 and S2 at two different times, and
 *      	compute the blend parameters based on that.
 *
 *      	The function apply_cartesian_blend() applies the
 *      	correction as a function of a time argument that is
 *      	assumed to vary from 0.0 to 1.0.
 *
 * AUTHOR/DATE:	John Lloyd, October, 1986
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <math.h>

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

#include "../rccl/cartBlend.h"

VECT  ZEROV = {0.0, 0.0, 0.0};

static TRSF R00;

setCartesianBlend1 (cbp, S1_0, S2_0)
register CARTESIAN_BLEND_PARAMS *cbp;
TRSF *S1_0, *S2_0;
{
	/*
	   Computes first part of blending coefficients.

	   cbp --	parameter block
	   S1_0 --	first position along path S1.
	   S2_0 --	first position along path S2.

	   Fairly straight forward function. First, we compute the rotation
	   from S2_0 to S1_0 by

			 -1
		R0 = R2_0   R1_0

	   Then we start computing the polynominal coefficients used in
	   the translational blending */

	TRSF R0;

	cbp->R1_0 = *S1_0;
	cbp->R2_0 = *S2_0;

	multLiTrsf (&R0, &cbp->R2_0, &cbp->R1_0);

		R00 = R0;

	trsfToQtrn (&cbp->q0, &R0);
	trsfToQtrn (&cbp->q1_0, &cbp->R1_0);
	trsfToQtrn (&cbp->q2_0, &cbp->R2_0);

	/* Compute first part of Hermite coefficients for translation blend */

	cbp->P0.x = cbp->R1_0.p.x - cbp->R2_0.p.x;
	cbp->P0.y = cbp->R1_0.p.y - cbp->R2_0.p.y;
	cbp->P0.z = cbp->R1_0.p.z - cbp->R2_0.p.z;

	cbp->P3.x = -10.0 * cbp->P0.x;
	cbp->P3.y = -10.0 * cbp->P0.y;
	cbp->P3.z = -10.0 * cbp->P0.z;

	cbp->P4.x =  15.0 * cbp->P0.x;
	cbp->P4.y =  15.0 * cbp->P0.y;
	cbp->P4.z =  15.0 * cbp->P0.z;

	cbp->P5.x =  -6.0 * cbp->P0.x;
	cbp->P5.y =  -6.0 * cbp->P0.y;
	cbp->P5.z =  -6.0 * cbp->P0.z;
}

#define DOT(a,b)	 ((a).x * (b).x + (a).y * (b).y + (a).z * (b).z)
#define CROSS(r,a,b)	{ (r).x = (a).y * (b).z - (a).z * (b).y; \
			  (r).y = (a).z * (b).x - (a).x * (b).z; \
			  (r).z = (a).x * (b).y - (a).y * (b).x; \
			}

#define RSMALL 1.0e-10

setCartesianBlend2 (cbp, S1_1, S2_1, time)
register CARTESIAN_BLEND_PARAMS *cbp;
TRSF *S1_1, *S2_1;
register float time;
{
	/* Called at the second time interval to finish computing the
	   blend coefficients between two Cartesian paths S1 and S2.
	
	   cbp --	parameter block
	   S1_1 --	second point along path S1
	   S2_1 --	second point along path S2
	   time --	time between first and second points (i.e., time
			between calls to setCartesianBlend1() and
		 	setCartesianBlend2()), expressed as a fraction
			of the transition interval (i.e., 1 / transInterval )

	  This function is a bit more involved.

	  The first thing it does is compute the initial rotational
	  velocities of path S1 and S2 as seen in S1_0 and S2_0,
	  respectively. This can be done by computing the differential
	  transform between both frames. Based on the relation

		  T     -1
		   V = T   dT

	  where V is a differential transform matrix and dT is an
	  incremental change in T, we get:

		S1_0          T
		   DT_1 = R1_0  (R1_1 - R1_0)

		S2_0          T
		   DT_2 = R2_0  (R2_1 - R2_0)

	  NOTE that we do not need to compute the DT completely, since
	  we want only 3 off-diagnonal rotational elements from them.
	  These can be computed explicitly by computing the dot product
	  of the appropriate column from Ri_0 and the appropriate column
	  from the (Ri_1 - Ri_0) term. This gives us velocitites
	  w_1 and w_2. We can map w_2 into the frame S1_0 using the
	  (already computed) transform R0_0, which maps R2_0 to R1_0:

		S1_0         T  S2_0
		   w_2 = R0_0      w_2

	  Now we can find the net relative velocity as seen in S1_0:

		   w = w_1 - w_2

	  We use this to compute the axis and angle of the velocity
	  quaternion qv, which is used by the blending
	  function to slowly ramp the velocity to zero. The velocity
	  quaternion is also used to compute the axis and angle
	  for the quaternion qc, which is ramped up to bring the
	  original displacements, and the integral of the velocity
	  displacement, to zero.

	  Lastly, we finish computing the polynominal coeffcients for the
	  translational blending */
	

	VECT w1, w2, w;
	VECT dA, dO;
	float cosval, sinval;
	QTRN qvFinal, qtemp;

	float theta;
	TRSF vTrans;

	identTrsf (&vTrans);

	/* compute w1 in S1_0 */

	dO.x = S1_1->o.x - cbp->R1_0.o.x;
	dO.y = S1_1->o.y - cbp->R1_0.o.y;
	dO.z = S1_1->o.z - cbp->R1_0.o.z;
	dA.x = S1_1->a.x - cbp->R1_0.a.x;
	dA.y = S1_1->a.y - cbp->R1_0.a.y;
	dA.z = S1_1->a.z - cbp->R1_0.a.z;

	w1.x = -DOT (cbp->R1_0.o, dA);
	w1.y =  DOT (cbp->R1_0.n, dA);
	w1.z = -DOT (cbp->R1_0.n, dO);

	/* compute w2 in S2_0 */

	dO.x = S2_1->o.x - cbp->R2_0.o.x;
	dO.y = S2_1->o.y - cbp->R2_0.o.y;
	dO.z = S2_1->o.z - cbp->R2_0.o.z;
	dA.x = S2_1->a.x - cbp->R2_0.a.x;
	dA.y = S2_1->a.y - cbp->R2_0.a.y;
	dA.z = S2_1->a.z - cbp->R2_0.a.z;

	w.x = -DOT (cbp->R2_0.o, dA);
	w.y =  DOT (cbp->R2_0.n, dA);
	w.z = -DOT (cbp->R2_0.n, dO);

	w2.x =  DOT (R00.n, w);
	w2.y =  DOT (R00.o, w);
	w2.z =  DOT (R00.a, w);
/*
	rciPrintVf (" W: %g\n", &w, 3);
	rciPrintVf ("W2: %g\n", &w2, 3);
*/
	w.x = w1.x - w2.x;
	w.y = w1.y - w2.y;
	w.z = w1.z - w2.z;

	/* compute the velocity quaternion */

	theta = SQRT (DOT(w,w));
	if (theta < RSMALL)
	 { theta = 0.0;
	    cbp->vAxis.x = 0.0;
	    cbp->vAxis.y = 0.0;
	    cbp->vAxis.z = 0.0;
	 }
	else
  	 { cbp->vAxis.x = w.x / theta;
	   cbp->vAxis.y = w.y / theta;
	   cbp->vAxis.z = w.z / theta;
	 }

	/* Compute qvFinal, which is the INVERSE of where the rotational
	   velocity w would take us to at the transition midpoint */

	cbp->vTheta = theta * 0.25 / time;
	SINCOS (cbp->vTheta, &sinval, &cosval);
	qvFinal.c = cosval;		
	qvFinal.v.x = cbp->vAxis.x * sinval;
	qvFinal.v.y = cbp->vAxis.y * sinval;
	qvFinal.v.z = cbp->vAxis.z * sinval;

	multQtrn (&qtemp, &qvFinal, &cbp->q0);
	invertQtrn (&qtemp, &qtemp);

	cbp->cAxis = qtemp.v;
	sinval = SQRT (DOT(cbp->cAxis, cbp->cAxis));
	cbp->cTheta = ATAN2 (sinval, qtemp.c);

	/* Normalize cTheta to take the shortest route */

	if (FABS(sinval) < RSMALL)
	 { cbp->cAxis.x = cbp->cAxis.y = cbp->cAxis.z = 0.0;
	 }
	else
	 { if (cbp->cTheta <= PIB2)
	    { cbp->cAxis.x /= sinval;
	      cbp->cAxis.y /= sinval;
	      cbp->cAxis.z /= sinval;
	    }
	   else
	    { cbp->cTheta = PI - cbp->cTheta;
	      cbp->cAxis.x /= sinval;
	      cbp->cAxis.y /= sinval;
	      cbp->cAxis.z /= sinval;
	    }
	 }

	cbp->vTheta *= 2.0;

	/* Compute Hermite coefficients for translational blending */

	cbp->P1.x = (S1_1->p.x - S2_1->p.x - cbp->P0.x) / time;
	cbp->P1.y = (S1_1->p.y - S2_1->p.y - cbp->P0.y) / time;
	cbp->P1.z = (S1_1->p.z - S2_1->p.z - cbp->P0.z) / time;

	cbp->P3.x += -6.0 * cbp->P1.x;
	cbp->P3.y += -6.0 * cbp->P1.y;
	cbp->P3.z += -6.0 * cbp->P1.z;

	cbp->P4.x +=  8.0 * cbp->P1.x;
	cbp->P4.y +=  8.0 * cbp->P1.y;
	cbp->P4.z +=  8.0 * cbp->P1.z;

	cbp->P5.x += -3.0 * cbp->P1.x;
	cbp->P5.y += -3.0 * cbp->P1.y;
	cbp->P5.z += -3.0 * cbp->P1.z;

}

applyCartesianBlend (result, cbp, t2, t)
register CARTESIAN_BLEND_PARAMS *cbp;
TRSF *result, *t2;
register float t;	
{
	/* All we do in this function is computed the run time correction
	   terms and apply them to the transform t2.

	   't' is the transition time and is assummed to vary from 
	   0.0 to 1.0 over the whole transition.

	   The rotational correction is simply the quaternion product

					  -1
		qc = q1_0 qv(t) qc(t) q2_0 

	   where qv(t) compensates for the initial velocities and
	   qc(t) compensates for the displacements. 

	   The translational correction is computed simply using polynominal
	   blending coeffcients */
	
	float tCubed, hv, hc;
	float vSin, vCos, cSin, cCos;
	QTRN q;
	TRSF RC;
	VECT P;
	QTRN qv, qc;

	tCubed = t * t * t;
	hv = (0.5 * t - 1.0) * tCubed + t;
	hc = tCubed * ((-6.0 * t + 15.0) * t - 10.0);
	hc = -hc;

	SINCOS (hv * cbp->vTheta, &vSin, &vCos);
	SINCOS (hc * cbp->cTheta, &cSin, &cCos);

	qv.c = vCos;
	qv.v = cbp->vAxis;
	qv.v.x *= vSin;
	qv.v.y *= vSin;
	qv.v.z *= vSin;

	qc.c = cCos;
	qc.v = cbp->cAxis;
	qc.v.x *= cSin;
	qc.v.y *= cSin;
	qc.v.z *= cSin;

	multQtrn (&q, &qc, &qv);	

	multQtrn (&q, &cbp->q0, &q);
	P = t2->p;
	qtrnToTrsf (&RC, &q);
	RC.p = ZEROV;
	multTrsf (result, t2, &RC);

	result->p.x = P.x + cbp->P0.x + t*(cbp->P1.x + t*(t*(cbp->P3.x 
				      + t*(cbp->P4.x + t*cbp->P5.x))));
	result->p.y = P.y + cbp->P0.y + t*(cbp->P1.y + t*(t*(cbp->P3.y 
				      + t*(cbp->P4.y + t*cbp->P5.y))));
	result->p.z = P.z + cbp->P0.z + t*(cbp->P1.z + t*(t*(cbp->P3.z 
				      + t*(cbp->P4.z + t*cbp->P5.z))));
}
