/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : drive.c
 *      Remarks : Straight line trajectory drive function.
 *                Exact implementation of Paul's book.
 *      Usage   : part of the lib
 */

#include "../h/rccl.h"
#include "../h/manip.h"
#include "../h/umac.h"

/*
 * set drive parameters of a motion defined by the p1 p2 transforms
 */

setpar_n(dpt, p1, p2) /*::*/
register DRVP_PTR dpt;
register TRSF_PTR p1, p2;
{
	real dot();
	VECT diff;             /* local vector for storing p2p - p1p */
	real n1, n2, n3;   /* local variables for common subexpr */
	real x, y;
	real lthe, lpsi;
	real spsi, cpsi, vthe, cthe, sthe;

	/* x y z */

	diff.x = p2->p.x - p1->p.x;
	diff.y = p2->p.y - p1->p.y;
	diff.z = p2->p.z - p1->p.z;

	dpt->dx = dot(&p1->n, &diff);
	dpt->dy = dot(&p1->o, &diff);
	dpt->dz = dot(&p1->a, &diff);

	/* psi */

	n1 = dot(&p1->o, &p2->a);
	n2 = dot(&p1->n, &p2->a);

	if (FABS(n1) < SMALL && FABS(n2) < SMALL ) {
		lpsi = dpt->dpsi = 0.;
	}
	else {
		lpsi = dpt->dpsi = atan2(n1, n2);
	}

	/* theta */

	y = sqrt(n2 * n2 + n1 * n1);
	x = dot(&p1->a, &p2->a);

	if (y < SMALL ) {
		lthe = dpt->dthe = 0.;
	}
	else {
		lthe = dpt->dthe = atan2(y, x);
	}

	/* phi */

	SINCOS(spsi, cpsi, lpsi);
	SINCOS(sthe, cthe, lthe);
	vthe = 1. - cthe;

	n1 = - spsi * cpsi * vthe;
	n2 =   cpsi * cpsi * vthe + cthe;
	n3 = - spsi * sthe;

	y = n1 * dot(&p1->n, &p2->n) +
	    n2 * dot(&p1->o, &p2->n) +
	    n3 * dot(&p1->a, &p2->n);

	x = n1 * dot(&p1->n, &p2->o) +
	    n2 * dot(&p1->o, &p2->o) +
	    n3 * dot(&p1->a, &p2->o);

	if (FABS(y) < SMALL ) {
		dpt->dphi = 0.;
	}
	else {
		dpt->dphi = atan2(y, x);
	}
}


/*
 * drive function : computes the drive transform given the drive parameters
 */

drivefn_n(t, d)  /*::*/
register TRSF_PTR t;
register DRVP_PTR d;
{
	real sphi, cphi, spsi, cpsi, sthe, cthe, vthe, loc;

	t->p.x = d->dx;
	t->p.y = d->dy;
	t->p.z = d->dz;

	SINCOS(sphi, cphi, d->dphi);
	SINCOS(spsi, cpsi, d->dpsi);
	SINCOS(sthe, cthe, d->dthe);
	vthe = 1. - cthe;

	t->a.x = cpsi * sthe;
	t->a.y = spsi * sthe;
	t->a.z = cthe;

	t->o.x = - sphi * (spsi * spsi * vthe + cthe)
		 + cphi * (loc = - spsi * cpsi * vthe);
	t->o.y = - sphi * (loc) + cphi * (cpsi * cpsi * vthe + cthe);
	t->o.z = sphi * cpsi * sthe - cphi * spsi * sthe;

	Cross(&t->n, &t->o, &t->a);
}
