/*
	name:	algebra.c

	Linear algebra
	--------------

	These functions simplify vector algebra etc.


    This source-code is part of the RayLab 1.1 package, and it is provided
    for your compiling pleasure.  You may use it, change it, re-compile it
    etc., as long as nobody else but you receive the changes/compilations
    that you have made!

    You may not use any part(s) of this source-code for your own productions
    without the permission from the author of RayLab. Please read the legal
    information found in the users documentation for RayLab for more details.

*/

#include  "defs.h"


void CreateVector(VECTOR *v, double vx, double vy, double vz)
{
	v->x=vx;
	v->y=vy;
	v->z=vz;
}



/* v2 = v1 */

void CopyVector(VECTOR *v2, VECTOR *v1)
{
	v2->x=v1->x;
	v2->y=v1->y;
	v2->z=v1->z;
}



/* v3 = v1 + v2 */

void AddVector(VECTOR *v3, VECTOR *v1, VECTOR *v2)
{
	v3->x=(v1->x)+(v2->x);
	v3->y=(v1->y)+(v2->y);
	v3->z=(v1->z)+(v2->z);
}



/* v3 = v1 - v2 */

void SubVector(VECTOR *v3, VECTOR *v1, VECTOR *v2)
{
	v3->x=(v1->x)-(v2->x);
	v3->y=(v1->y)-(v2->y);
	v3->z=(v1->z)-(v2->z);
}



/* v2 = -v1 */

void NegVector(VECTOR *v2, VECTOR *v1)
{
	v2->x=-(v1->x);
	v2->y=-(v1->y);
	v2->z=-(v1->z);
}



/* v3 = v1 x v2 */

void CrossProduct(VECTOR *v3, VECTOR *v1, VECTOR *v2)
{
	v3->x=(v1->y)*(v2->z)-(v1->z)*(v2->y);
	v3->y=(v1->z)*(v2->x)-(v1->x)*(v2->z);
	v3->z=(v1->x)*(v2->y)-(v1->y)*(v2->x);
}



/* Returns  v1 · v2 */

double DotProduct(VECTOR *v1, VECTOR *v2)
{
	return((v1->x)*(v2->x)+(v1->y)*(v2->y)+(v1->z)*(v2->z));
}



/* v2 = t * v1 */

void ScaleVector(VECTOR *v2, double t, VECTOR *v1)
{
	v2->x=t*(v1->x);
	v2->y=t*(v1->y);
	v2->z=t*(v1->z);
}


/* v2 = v1/|v1|  =>  |v2| = 1 */

void NormalizeVector(VECTOR *v2, VECTOR *v1)
{
	register double	l;

	l=1.0/sqrt(v1->x*v1->x+v1->y*v1->y+v1->z*v1->z);	/* l = 1/|v1| */
	v2->x=l*v1->x; v2->y=l*v1->y; v2->z=l*v1->z;
}


/* Returns |v| */

double VectorLength(VECTOR *v)
{
	return(sqrt((v->x)*(v->x)+(v->y)*(v->y)+(v->z)*(v->z)));
}



/* Returns the (smallest) angle between v1 and v2 */

double VectorsAngle(VECTOR *v1, VECTOR *v2)
{
	return(acos(DotProduct(v1,v2)/(VectorLength(v1)*VectorLength(v2))));
}



void CreatePoint(POINT *p, double px, double py, double pz)
{
	p->x=px;
	p->y=py;
	p->z=pz;
}



/* p2 = p1 */

void CopyPoint(POINT *p2, POINT *p1)
{
	p2->x=p1->x;
	p2->y=p1->y;
	p2->z=p1->z;
}



/* This routine creates a line from two given points */

void MakeLine(LINE *l, POINT *p1, POINT *p2)
{
	CopyPoint(&(l->Origin),p1);
	CreateVector(&(l->Direction),(p2->x)-(p1->x),(p2->y)-(p1->y),(p2->z)-(p1->z));
}



/* This routine rotates a 2D point in a 2D system */
/* I.e. (x,y) rotates (ar) radians counter clockwise around (0,0) */

void Rotate2D(double *x, double *y, double ar)
{
	double	x1,y1;

	x1=(*x)*cos(ar)-(*y)*sin(ar);
	y1=(*x)*sin(ar)+(*y)*cos(ar);
	*x=x1; *y=y1;
}


/* This routine rotates a 3D point in 3D space. The point (x,y,z) */
/* is rotated around the x, the y and the z axis (in that order). */
/* The vector RotV gives the degrees to rotate around each axis   */

void RotatePoint(POINT *p, VECTOR *RotV)
{
	double	x1,y1,z1;

	x1=p->x; y1=p->y; z1=p->z;
	if(RotV->x!=0.0) Rotate2D(&y1,&z1,RotV->x);
	if(RotV->y!=0.0) Rotate2D(&z1,&x1,RotV->y);
	if(RotV->z!=0.0) Rotate2D(&x1,&y1,RotV->z);
	p->x=x1; p->y=y1; p->z=z1;
}


/* This is the same thing, but with a vector instead of a point. */

void RotateVector(VECTOR *v, VECTOR *RotV)
{
	double	x1,y1,z1;

	x1=v->x; y1=v->y; z1=v->z;
	if(RotV->x!=0.0) Rotate2D(&y1,&z1,RotV->x);
	if(RotV->y!=0.0) Rotate2D(&z1,&x1,RotV->y);
	if(RotV->z!=0.0) Rotate2D(&x1,&y1,RotV->z);
	v->x=x1; v->y=y1; v->z=z1;
}


/* These routines will rotate in the reversed order (z,y,x). */

void RevRotatePoint(POINT *p, VECTOR *RotV)
{
	double	x1,y1,z1;

	x1=p->x; y1=p->y; z1=p->z;
	if(RotV->z!=0.0) Rotate2D(&x1,&y1,RotV->z);
	if(RotV->y!=0.0) Rotate2D(&z1,&x1,RotV->y);
	if(RotV->x!=0.0) Rotate2D(&y1,&z1,RotV->x);
	p->x=x1; p->y=y1; p->z=z1;
}

void RevRotateVector(VECTOR *v, VECTOR *RotV)
{
	double	x1,y1,z1;

	x1=v->x; y1=v->y; z1=v->z;
	if(RotV->z!=0.0) Rotate2D(&x1,&y1,RotV->z);
	if(RotV->y!=0.0) Rotate2D(&z1,&x1,RotV->y);
	if(RotV->x!=0.0) Rotate2D(&y1,&z1,RotV->x);
	v->x=x1; v->y=y1; v->z=z1;
}