// mathlib.c -- math primitives

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

vec3_t vec3_origin = {0,0,0};

boolean VectorCompare (vec3_t v1, vec3_t v2)
{
	int		i;
	
	for (i=0 ; i<3 ; i++)
		if (v1[i] != v2[i])
			return false;
			
	return true;
}


void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
{
	cross[0] = v1[1]*v2[2] - v1[2]*v2[1];
	cross[1] = v1[2]*v2[0] - v1[0]*v2[2];
	cross[2] = v1[0]*v2[1] - v1[1]*v2[0];
}

vec_t DotProduct (vec3_t v1, vec3_t v2)
{
	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
}

void VectorSubtract (vec3_t va, vec3_t vb, vec3_t out)
{
	out[0] = va[0]-vb[0];
	out[1] = va[1]-vb[1];
	out[2] = va[2]-vb[2];
}

void VectorAdd (vec3_t va, vec3_t vb, vec3_t out)
{
	out[0] = va[0]+vb[0];
	out[1] = va[1]+vb[1];
	out[2] = va[2]+vb[2];
}

void VectorCopy (vec3_t in, vec3_t out)
{
	out[0] = in[0];
	out[1] = in[1];
	out[2] = in[2];
}

void VectorNormalize (vec3_t v)
{
	int		i;
	float	length;
	
	length = 0;
	for (i=0 ; i< 3 ; i++)
		length += v[i]*v[i];
	length = sqrt (length);

	for (i=0 ; i< 3 ; i++)
		v[i] /= length;	
}

void VectorInverse (vec3_t v)
{
	v[0] = -v[0];
	v[1] = -v[1];
	v[2] = -v[2];
}

void VectorScale (vec3_t v, vec_t scale)
{
	v[0] *= scale;
	v[1] *= scale;
	v[2] *= scale;
}

/*
===================
PointOnPlaneSide

Returns 1 if the point is on the same side as the normal is pointing,
-1 if on the back side, or 0 if coplanar
====================
*/

int	PointOnPlaneSide (vec3_t pt, plane_t *plane)
{
	vec3_t		temp;
	vec_t		dot;
	
	VectorSubtract (pt,plane->origin,temp);
	dot = DotProduct (temp,plane->normal);

	if (dot > 0)
		return SIDE_FRONT;
	if (dot < 0)
		return SIDE_BACK;
		
	return SIDE_ON;		// coplanar
}

int	PointOnPlaneSideFudge (vec3_t pt, plane_t *plane)
{
	vec3_t		temp;
	vec_t		dot;
	
	VectorSubtract (pt,plane->origin,temp);
	dot = DotProduct (temp,plane->normal);

	if (dot > -1 && dot < 1)
		return SIDE_ON;	// coplanar
		
	if (dot > 0)
		return SIDE_FRONT;
	return SIDE_BACK;
}


/*
===================
LineHitPlane

Calculates the intersection point of the line and plane
====================
*/

void LineHitPlane (line_t *line, plane_t *plane, vec3_t point)
{
	vec3_t		temp;
	vec_t		partial,total;
	
	VectorSubtract (plane->origin,line->origin,temp);
	partial = DotProduct (temp,plane->normal);
	total = DotProduct (line->vector,plane->normal);

	point[0] = line->origin[0] + line->vector[0]*partial/total;
	point[1] = line->origin[1] + line->vector[1]*partial/total;
	point[2] = line->origin[2] + line->vector[2]*partial/total;
}