// copyright Min Zhong, cs838 proj2, April, 2000


#include "vector.h"
#include "proj2.h"

void v3_assign(double vTo[3], double vFrom[3])
{
	vTo[X] = vFrom[X];
	vTo[Y] = vFrom[Y];
	vTo[Z] = vFrom[Z];
}


void v3_crossp(double v1[3], double v2[3], double vres[3])
{

	vres[X] = v1[Y] * v2[Z] - v1[Z] * v2[Y];
	vres[Y] = -v1[X] * v2[Z] + v1[Z] * v2[X];
	vres[Z] = v1[X] * v2[Y] - v1[Y] * v2[X];

}

void v3_dotp(double v1[3], double v2[3], double &a)
{
	a = acos( v1[X] * v2[X] + v1[Y] * v2[Y] + v1[Z] * v2[Z] );
}


/* vect vprod[3] = v[3] * s */
void v3_scale(double v[3], double s, double vprod[3])
{
	vprod[X] = v[X] * s;
	vprod[Y] = v[Y] * s;
	vprod[Z] = v[Z] * s;
}


void v3_add(double v1[3], double v2[3], double vres[3])
{

	vres[X] = v1[X] + v2[X];
	vres[Y] = v1[Y] + v2[Y];
	vres[Z] = v1[Z] + v2[Z];
}


void v3_sub(double v1[3], double v2[3], double vres[3])
{

	vres[X] = v1[X] - v2[X];
	vres[Y] = v1[Y] - v2[Y];
	vres[Z] = v1[Z] - v2[Z];
}


/* in case of tiny mag, make the vect 0 */
void v3_normalize(double v[3], double v_unit[3])
{
	double mag = sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
	double mag_inv;

	if ( (mag > (-NEAR_ZERO)) && (mag < NEAR_ZERO) ) {
		v_unit[X] = 0;
		v_unit[Y] = 0;
		v_unit[Z] = 0;
	}
	else {
		mag_inv = 1 / mag;
		v_unit[X] = v[X] * mag_inv;
		v_unit[Y] = v[Y] * mag_inv;
		v_unit[Z] = v[Z] * mag_inv;
	}
}


/* vect vprod[3] = v[3] * s */
void v4_scale(double v[4], double s, double vres[4])
{
  vres[0] = v[0] * s;
  vres[1] = v[1] * s;
  vres[2] = v[2] * s;
  vres[3] = v[3] * s;
}


void v4_add(double v1[4], double v2[4], double vres[4])
{

  vres[0] = v1[0] + v2[0];
  vres[1] = v1[1] + v2[1];
  vres[2] = v1[2] + v2[2];
  vres[3] = v1[3] + v2[3];

}


void v4_sub(double v1[4], double v2[4], double vres[4])
{

  vres[0] = v1[0] - v2[0];
  vres[1] = v1[1] - v2[1];
  vres[2] = v1[2] - v2[2];
  vres[3] = v1[3] - v2[3];

}

void v4_assign(double vTo[4], double vFrom[4])
{
	vTo[0] = vFrom[0];
	vTo[1] = vFrom[1];
	vTo[2] = vFrom[2];
	vTo[3] = vFrom[3];
}


// set vect to a const
void v4_set(double vTo[4], double c)
{
	vTo[0] = c;
	vTo[1] = c;
	vTo[2] = c;
	vTo[3] = c;
}


/* in case of tiny mag, make the vect 0 */
void v4_normalize(double v[4], double v_unit[4])
{
	double mag = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]);
	double mag_inv;

	if ( (mag > (-NEAR_ZERO)) && (mag < NEAR_ZERO) ) {
		v_unit[0] = 0;
		v_unit[1] = 0;
		v_unit[2] = 0;
		v_unit[3] = 0;
	}
	else {
		mag_inv = 1 / mag;
		v_unit[0] = v[0] * mag_inv;
		v_unit[1] = v[1] * mag_inv;
		v_unit[2] = v[2] * mag_inv;
		v_unit[3] = v[3] * mag_inv;

	}
}