vector.c

著名物理引擎Hawk的源代码

/* vector.c, HAWK game engine
 *
 * Copyright 1997-1998 by Phil Frisbie, Jr.
 * for Hawk Software
 *
 */

#include "vector.h"
#include "internal.h"

double VectorLength(vec3_t v)
{
	int		i;
	double	length;
	
	length = 0;
	for (i=0 ; i< 3 ; i++)
		length += v[i]*v[i];
	length = sqrt (length);

	return length;
}

BOOL VectorCompare (vec3_t v1, vec3_t v2)
{
	int		i;
	
	for (i=0 ; i<3 ; i++)
		if (fabs(v1[i]-v2[i]) > EQUAL_EPSILON)
			return FALSE;
			
	return TRUE;
}

vec_t Q_rint (vec_t in)
{
	return floor (in + 0.5);
}

void VectorMA (vec3_t va, double scale, vec3_t vb, vec3_t vc)
{
	vc[X] = va[X] + scale*vb[X];
	vc[Y] = va[Y] + scale*vb[Y];
	vc[Z] = va[Z] + scale*vb[Z];
}

void CrossProduct (vec3_t v1, vec3_t v2, vec3_t cross)
{
	cross[X] = v1[Y]*v2[Z] - v1[Z]*v2[Y];
	cross[Y] = v1[Z]*v2[X] - v1[X]*v2[Z];
	cross[Z] = v1[X]*v2[Y] - v1[Y]*v2[X];
}

vec_t vect4DotProductGen(vec4_t v1, vec4_t v2)
{
	return v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z] + v1[W]*v2[W];
}

void vect4SubtractGen(vec4_t va, vec4_t vb, vec4_t out)
{
	out[X] = va[X]-vb[X];
	out[Y] = va[Y]-vb[Y];
	out[Z] = va[Z]-vb[Z];
	out[W] = va[W]-vb[W];
}

void vect4AddGen(vec4_t va, vec4_t vb, vec4_t out)
{
	out[X] = va[X]+vb[X];
	out[Y] = va[Y]+vb[Y];
	out[Z] = va[Z]+vb[Z];
	out[W] = va[W]+vb[W];
}

void vect4CopyGen(vec4_t in, vec4_t out)
{
	out[X] = in[X];
	out[Y] = in[Y];
	out[Z] = in[Z];
	out[W] = in[W];
}

void vect4ScaleGen(vec4_t v, vec_t scale, vec4_t out)
{
	out[X] = v[X] * scale;
	out[Y] = v[Y] * scale;
	out[Z] = v[Z] * scale;
	out[W] = v[W] * scale;
}

vec_t vect4DotProduct3DNow(vec4_t v1, vec4_t v2)
{
	return v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z] + v1[W]*v2[W];
}

void vect4Subtract3DNow(vec4_t va, vec4_t vb, vec4_t out)
{
	out[X] = va[X]-vb[X];
	out[Y] = va[Y]-vb[Y];
	out[Z] = va[Z]-vb[Z];
	out[W] = va[W]-vb[W];
}

void vect4Add3DNow(vec4_t va, vec4_t vb, vec4_t out)
{
	out[X] = va[X]+vb[X];
	out[Y] = va[Y]+vb[Y];
	out[Z] = va[Z]+vb[Z];
	out[W] = va[W]+vb[W];
}

void vect4Copy3DNow(vec4_t in, vec4_t out)
{
	out[X] = in[X];
	out[Y] = in[Y];
	out[Z] = in[Z];
	out[W] = in[W];
}

void vect4Scale3DNow(vec4_t v, vec_t scale, vec4_t out)
{
	out[X] = v[X] * scale;
	out[Y] = v[Y] * scale;
	out[Z] = v[Z] * scale;
	out[W] = v[W] * scale;
}

vec_t VectorNormalize (vec3_t in, vec3_t out)
{
	vec_t	length, ilength;

	length = sqrt (in[0]*in[0] + in[1]*in[1] + in[2]*in[2]);
	if (length == 0)
	{
		VectorClear (out);
		return 0;
	}

	ilength = 1.0/length;
	out[X] = in[X]*ilength;
	out[Y] = in[Y]*ilength;
	out[Z] = in[Z]*ilength;

	return length;
}

vec_t ColorNormalize (vec3_t in, vec3_t out)
{
	float	max, scale;

	max = in[0];
	if (in[1] > max)
		max = in[1];
	if (in[2] > max)
		max = in[2];

	if (max == 0)
		return 0;

	scale = 1.0 / max;

	VectorScale (in, scale, out);

	return max;
}

void VectorInverse (vec3_t v)
{
	v[X] = -v[X];
	v[Y] = -v[Y];
	v[Z] = -v[Z];
}

void ClearBounds (vec3_t mins, vec3_t maxs)
{
	mins[0] = mins[1] = mins[2] = 99999;
	maxs[0] = maxs[1] = maxs[2] = -99999;
}

void AddPointToBounds (vec3_t v, vec3_t mins, vec3_t maxs)
{
	int		i;
	vec_t	val;

	for (i=0 ; i<3 ; i++)
	{
		val = v[i];
		if (val < mins[i])
			mins[i] = val;
		if (val > maxs[i])
			maxs[i] = val;
	}
}

void vectorInit(void)
{
	if(Cpu.has3DNow)
	{
		vect4DotProduct = vect4DotProduct3DNow;
		vect4Subtract = vect4Subtract3DNow;
		vect4Add = vect4Add3DNow;
		vect4Copy = vect4Copy3DNow;
		vect4Scale = vect4Scale3DNow;
	}
	else
	{
		vect4DotProduct = vect4DotProductGen;
		vect4Subtract = vect4SubtractGen;
		vect4Add = vect4AddGen;
		vect4Copy = vect4CopyGen;
		vect4Scale = vect4ScaleGen;
	}
}