/*-=-=-=-=-=-=-=-=-=-=-=- 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:	vect
 *
 * DESCRIPTION:	Routines for manipulating the type VECT, a spatial
 *		three vector.
 *
 * AUTHOR/DATE:	Vincent Hayward, Purdue University, 1983.
 *		CVaRL installation and modification by John Lloyd.
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <math.h>

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

#define SMALL 1.0e-6

VECT *copyVect (vr, v1)
register VECT *vr, *v1;
{
	/* Don't really need this anymore ... */

	*vr = *v1;
	return (vr);
}

float dotVect(v1, v2)
register VECT *v1, *v2;
{
	/* compute v1 . v2 */

	return(v1->x * v2->x + v1->y * v2->y + v1->z * v2->z);
}

VECT *crossVect(vr, v1, v2)
register VECT *vr;
VECT *v1, *v2;
{
	/* Compute vr = v1 X v2 */

	register float v1x, v1y, v2x, v2y;

	v1x = v1->x;
	v2x = v2->x;
	v1y = v1->y;
	v2y = v2->y;

	vr->x = v1y * v2->z - v1->z * v2y;
	vr->y = v1->z * v2x - v1x * v2->z;
	vr->z = v1x * v2y - v1y * v2x;
	return(vr);
}

VECT *unitVect(vr, v1)
register VECT *vr, *v1;
{
	/* Compute vr = v1/|v1| */

	float m;

	m = sqrt(dotVect(v1, v1));
	if (m < SMALL) {
	   return (NULL);
	}
	vr->x = v1->x / m;
	vr->y = v1->y / m;
	vr->z = v1->z / m;
	return(vr);
}

VECT *multTrsfVect(vr, T, v1)
VECT *vr;
register TRSF *T;
VECT *v1;
{
	/* Compute vr = T v1 */

	register float x, y, z;

	x = v1->x;
	y = v1->y;
	z = v1->z;

	vr->x = T->n.x*x + T->o.x*y + T->a.x*z + T->p.x;
	vr->y = T->n.y*x + T->o.y*y + T->a.y*z + T->p.y;
	vr->z = T->n.z*x + T->o.z*y + T->a.z*z + T->p.z;
	return (vr);
}

VECT *multLiTrsfVect(vr, T, v1)
VECT *vr;
register TRSF *T;
VECT *v1;
{
	/*		 -1	*/
	/* Compute vr = T   v1  */

	register float x, y, z;

	x = v1->x - T->p.x;
	y = v1->y - T->p.y;
	z = v1->z - T->p.z;

	vr->x = T->n.x*x + T->n.y*y + T->n.z*z;
	vr->y = T->o.x*x + T->o.y*y + T->o.z*z;
	vr->z = T->a.x*x + T->a.y*y + T->a.z*z;

	return (vr);
}

VECT *addVect(vr, v1, v2)
VECT *vr, *v1, *v2;
{
	vr->x = v1->x + v2->x;
	vr->y = v1->y + v2->y;
	vr->z = v1->z + v2->z;
	return (vr);
}

VECT* subtractVect(vr, v1, v2)
VECT *vr, *v1, *v2;
{
	vr->x = v1->x - v2->x;
	vr->y = v1->y - v2->y;
	vr->z = v1->z - v2->z;
	return (vr);
}

VECT* scaleVect(vr, a, v1)
VECT *vr, *v1;
double a;
{
	vr->x = a * v1->x;
	vr->y = a * v1->y;
	vr->z = a * v1->z;
	return (vr);
}

VECT *combineVect (vr, a, v1, b, v2)
VECT *vr, *v1, *v2;
double a, b;
{
	vr->x = a * v1->x + b * v2->x;
	vr->y = a * v1->y + b * v2->y;
	vr->z = a * v1->z + b * v2->z;
	return (vr);
}

float normVect(v1)
register VECT *v1;
{
	return (sqrt(v1->x*v1->x + v1->y*v1->y + v1->z*v1->z));
}

VECT *zeroVect(vr)
VECT *vr;
{
	vr->x = 0.0;
	vr->y = 0.0;
	vr->z = 0.0;
	return (vr);
}
