/* VECT: three-dimensional vector arithmetic & more		*/
/* authors: robert j. mcdermott & paul heckbert			*/
/* 2/28/84: added l3d_vset(), l3d_vray(). jules				*/

/* copy, add, subtract, scalar multiply, linear combination	*/
/* length, unitize						*/
/* dot product, cross product					*/
/* triple scalar product, triple vector product			*/
/* print							*/
/* angle							*/

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

/* vector copy		*/
/* v1 = v0		*/
double *
l3d_vcopy(v0,v1)
	register double v0[],v1[];
	{
	v1[0] = v0[0];
	v1[1] = v0[1];
	v1[2] = v0[2];
	return (v1);
}

/* vector addition	*/
/* v2 = v0 + v1		*/
double *
l3d_vadd(v0,v1,v2)
	register double v0[],v1[],v2[];
	{
	v2[0]=v0[0]+v1[0];
	v2[1]=v0[1]+v1[1];
	v2[2]=v0[2]+v1[2];
	return (v2);
}

/* vector subtraction	*/
/* v2 = v0 - v1		*/
double *
l3d_vsub(v0,v1,v2)
	register double v0[],v1[],v2[];
	{
	v2[0]=v0[0]-v1[0];
	v2[1]=v0[1]-v1[1];
	v2[2]=v0[2]-v1[2];
	return (v2);
}

/* scalar multication	*/
/* v1 = s * v0		*/
double *
l3d_vsmul(s,v0,v1)
	register double s,v0[],v1[];
	{
	v1[0]=s*v0[0];
	v1[1]=s*v0[1];
	v1[2]=s*v0[2];
	return (v1);
}

/* linear combination of two vectors	*/
/* c = sa*a + sb*b			*/
double *
l3d_vcomb(sa,a,sb,b,c)
	double sa,sb;
	register double *a,*b,*c;
	{
	c[0] = sa*a[0]+sb*b[0];
	c[1] = sa*a[1]+sb*b[1];
	c[2] = sa*a[2]+sb*b[2];
	return (c);
}

/* scalar addition of one vector to another */
/* c = a + s*b				    */
double *
l3d_vray(a,s,b,c)
	double s;
	register double *a,*b,*c;
	{
	c[0] = a[0]+s*b[0];
	c[1] = a[1]+s*b[1];
	c[2] = a[2]+s*b[2];
	return (c);
}

/* set coordinates of point	*/
double *
l3d_vset(v,x,y,z)
	double v[],x,y,z;
	{
	v[0]=x; v[1]=y; v[2]=z;
	return (v);
}

/* vector length	*/
/* double |v0|		*/
double l3d_vlen(v0)
	register double v0[];
	{
	return(sqrt(v0[0]*v0[0]+v0[1]*v0[1]+v0[2]*v0[2]));
}

/* unitize a vector, return its former length	*/
/* v = v/|v|					*/
double l3d_vunit(v)
	register double *v;
	{
	double norm;

	norm = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
	if (norm!=0.) {
		v[0] /= norm;
		v[1] /= norm;
		v[2] /= norm;
	}
	return(norm);
}

/* dot product		*/
/* double v0 . v1	*/
double l3d_vdot(v0,v1)
	register double v0[],v1[];
	{
	return(v0[0]*v1[0]+v0[1]*v1[1]+v0[2]*v1[2]);
}

/* cross product	*/
/* v2 = v0 x v1		*/
double *
l3d_vcross(v0,v1,v2)
	register double v0[],v1[],v2[];
	{
	v2[0]=v0[1]*v1[2]-v0[2]*v1[1];
	v2[1]=v0[2]*v1[0]-v0[0]*v1[2];
	v2[2]=v0[0]*v1[1]-v0[1]*v1[0];
	return (v2);
}

/* triple scalar product	*/
/* double v0 . (v1 x v2)	*/
double l3d_vtsp(v0,v1,v2)
	double v0[],v1[],v2[];
	{
	double v3[3];
	l3d_vcross(v1,v2,v3);
	return(l3d_vdot(v0,v3));
}

/* triple vector product	*/
/* v3 = v0 x (v1 x v2)		*/
double *
l3d_vtvp(v0,v1,v2,v3)
	double v0[],v1[],v2[],v3[];
	{
	double v012[3],v021[3];
	l3d_vsmul(l3d_vdot(v0,v2),v1,v021);
	l3d_vsmul(l3d_vdot(v0,v1),v2,v012);
	l3d_vsub(v021,v012,v3);
	return (v3);
}

/* print a vector, first arg is a character string name	*/
l3d_vprint(str,v)
char *str;
double *v;
{
	printf("%s(%g,%g,%g)\n",str,v[0],v[1],v[2]);
}

double
l3d_vangle(v1, v2)			/* acute angle between vectors [0, pi] */
    double *v1, *v2;
{
    double cp[3];
    return (atan2 (l3d_vlen(l3d_vcross(v1, v2, cp)), l3d_vdot(v1, v2)));
}

double
l3d_vangle3(p0, p1, p2)
    double *p0, *p1, *p2;
{
    double v1[3], v2[3];
    return (l3d_vangle(l3d_vsub(p1, p0, v1), l3d_vsub(p2, p0, v2)));
}
