#include "precompiled.h"

/*
* Globals initialization
*/
#ifndef HOOK_GAMEDLL

vec3_t vec3_origin = {0, 0, 0};
int nanmask = 255<<23;

#endif // HOOK_GAMEDLL

/* <2ce436> ../cstrike/pm_shared/pm_math.c:35 */
float anglemod(float a)
{
	a = (360.0 / 65536) * ((int)(a  *(65536 / 360.0)) & 65535);
	return a;
}

/* <2ce461> ../cstrike/pm_shared/pm_math.c:41 */
void AngleVectors(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
{
	float sr, sp, sy, cr, cp;

	float_precision cy;
	float_precision angle;

	angle = (float_precision)(angles[YAW] * (M_PI * 2 / 360));
	sy = sin(angle);
	cy = cos(angle);

	angle = (float_precision)(angles[PITCH] * (M_PI * 2 / 360));
	sp = sin(angle);
	cp = cos(angle);

	angle = (float_precision)(angles[ROLL] * (M_PI * 2 / 360));
	sr = sin(angle);
	cr = cos(angle);

	if (forward)
	{
		forward[0] = cp * cy;
		forward[1] = cp * sy;
		forward[2] = -sp;
	}
	if (right)
	{
		right[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
		right[1] = (-1 * sr * sp * sy + -1 * cr * cy);
		right[2] = -1 * sr * cp;
	}
	if (up)
	{
		up[0] = (cr * sp * cy + -sr * -sy);
		up[1] = (cr * sp * sy + -sr * cy);
		up[2] = cr * cp;
	}
}

/* <2ce521> ../cstrike/pm_shared/pm_math.c:76 */
void AngleVectorsTranspose(const vec_t *angles, vec_t *forward, vec_t *right, vec_t *up)
{
	float angle;
	float sr, sp, sy, cr, cp, cy;

	angle = angles[YAW] * (M_PI * 2 / 360);
	sy = sin(angle);
	cy = cos(angle);
	angle = angles[PITCH] * (M_PI * 2 / 360);
	sp = sin(angle);
	cp = cos(angle);
	angle = angles[ROLL] * (M_PI * 2 / 360);
	sr = sin(angle);
	cr = cos(angle);

	if (forward)
	{
		forward[0] = cp * cy;
		forward[1] = (sr * sp * cy + cr * -sy);
		forward[2] = (cr * sp * cy + -sr * -sy);
	}
	if (right)
	{
		right[0] = cp * sy;
		right[1] = (sr * sp * sy + cr * cy);
		right[2] = (cr * sp * sy + -sr * cy);
	}
	if (up)
	{
		up[0] = -sp;
		up[1] = sr * cp;
		up[2] = cr * cp;
	}
}

/* <2ce5d0> ../cstrike/pm_shared/pm_math.c:112 */
void AngleMatrix(const vec_t *angles, float (*matrix)[4])
{
	float_precision angle;
	float_precision  sr, sp, sy, cr, cp, cy;

	angle = (float_precision)(angles[ROLL] * (M_PI * 2 / 360));
	sy = sin(angle);
	cy = cos(angle);

	angle = (float_precision)(angles[YAW] * (M_PI * 2 / 360));
	sp = sin(angle);
	cp = cos(angle);

	angle = (float_precision)(angles[PITCH] * (M_PI * 2 / 360));
	sr = sin(angle);
	cr = cos(angle);

	matrix[0][0] = cr * cp;
	matrix[1][0] = cr * sp;
	matrix[2][0] = -sr;

	matrix[0][1] = (sy * sr) * cp - cy * sp;
	matrix[1][1] = (sy * sr) * sp + cy * cp;
	matrix[2][1] = sy * cr;

	matrix[0][2] = (cy * sr) * cp + sy * sp;
	matrix[1][2] = (cy * sr) * sp - sy * cp;
	matrix[2][2] = cy * cr;

	matrix[0][3] = 0.0f;
	matrix[1][3] = 0.0f;
	matrix[2][3] = 0.0f;
}

/* <2ce67b> ../cstrike/pm_shared/pm_math.c:142 */
void AngleIMatrix(const vec_t *angles, float (*matrix)[4])
{
	float angle;
	float sr, sp, sy, cr, cp, cy;

	angle = angles[YAW] * (M_PI * 2 / 360);
	sy = sin(angle);
	cy = cos(angle);
	angle = angles[PITCH] * (M_PI * 2 / 360);
	sp = sin(angle);
	cp = cos(angle);
	angle = angles[ROLL] * (M_PI * 2 / 360);
	sr = sin(angle);
	cr = cos(angle);

	// matrix = (YAW * PITCH) * ROLL
	matrix[0][0] = cp * cy;
	matrix[0][1] = cp * sy;
	matrix[0][2] = -sp;
	matrix[1][0] = sr * sp * cy + cr * -sy;
	matrix[1][1] = sr * sp * sy + cr * cy;
	matrix[1][2] = sr * cp;
	matrix[2][0] = (cr * sp * cy + -sr * -sy);
	matrix[2][1] = (cr * sp * sy + -sr * cy);
	matrix[2][2] = cr * cp;
	matrix[0][3] = 0.0;
	matrix[1][3] = 0.0;
	matrix[2][3] = 0.0;
}

/* <2ce710> ../cstrike/pm_shared/pm_math.c:172 */
void NormalizeAngles(float *angles)
{
	int i;
	// Normalize angles
	for (i = 0; i < 3; ++i)
	{
		if (angles[i] > 180.0)
		{
			angles[i] -= 360.0;
		}
		else if (angles[i] < -180.0)
		{
			angles[i] += 360.0;
		}
	}
}

// Interpolate Euler angles.
// FIXME:  Use Quaternions to avoid discontinuities
// Frac is 0.0 to 1.0 (i.e., should probably be clamped, but doesn't have to be)

/* <2ce75f> ../cstrike/pm_shared/pm_math.c:198 */
void InterpolateAngles(float *start, float *end, float *output, float frac)
{
	int i;
	float ang1, ang2;
	float d;

	NormalizeAngles(start);
	NormalizeAngles(end);

	for (i = 0; i < 3; ++i)
	{
		ang1 = start[i];
		ang2 = end[i];

		d = ang2 - ang1;
		if (d > 180)
		{
			d -= 360;
		}
		else if (d < -180)
		{
			d += 360;
		}

		output[i] = ang1 + d * frac;
	}

	NormalizeAngles(output);
}

/* <2ce893> ../cstrike/pm_shared/pm_math.c:235 */
float AngleBetweenVectors(const vec_t *v1, const vec_t *v2)
{
	float angle;
	float l1 = Length(v1);
	float l2 = Length(v2);

	if (!l1 || !l2)
		return 0.0f;

	angle = acos(DotProduct(v1, v2)) / (l1 * l2);
	angle = (angle * 180.0f) / M_PI;

	return angle;
}

/* <2ce953> ../cstrike/pm_shared/pm_math.c:251 */
void VectorTransform(const vec_t *in1, float (*in2)[4], vec_t *out)
{
	out[0] = _DotProduct(in1, in2[0]) + in2[0][3];
	out[1] = _DotProduct(in1, in2[1]) + in2[1][3];
	out[2] = _DotProduct(in1, in2[2]) + in2[2][3];
}

/* <2ce996> ../cstrike/pm_shared/pm_math.c:259 */
int VectorCompare(const vec_t *v1, const vec_t *v2)
{
	int i;
	for (i = 0; i < 3; ++i)
	{
		if (v1[i] != v2[i])
			return 0;
	}

	return 1;
}

/* <2ce9de> ../cstrike/pm_shared/pm_math.c:270 */
void VectorMA(const vec_t *veca, float scale, const vec_t *vecb, vec_t *vecc)
{
	vecc[0] = veca[0] + scale * vecb[0];
	vecc[1] = veca[1] + scale * vecb[1];
	vecc[2] = veca[2] + scale * vecb[2];
}

/* <2cea34> ../cstrike/pm_shared/pm_math.c:278 */
float_precision _DotProduct(const vec_t *v1, const vec_t *v2)
{
	return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}

/* <2cea6e> ../cstrike/pm_shared/pm_math.c:283 */
void _VectorSubtract(vec_t *veca, vec_t *vecb, vec_t *out)
{
	out[0] = veca[0] - vecb[0];
	out[1] = veca[1] - vecb[1];
	out[2] = veca[2] - vecb[2];
}

/* <2ceab5> ../cstrike/pm_shared/pm_math.c:290 */
void _VectorAdd(vec_t *veca, vec_t *vecb, vec_t *out)
{
	out[0] = veca[0] + vecb[0];
	out[1] = veca[1] + vecb[1];
	out[2] = veca[2] + vecb[2];
}

/* <2ceafc> ../cstrike/pm_shared/pm_math.c:297 */
void _VectorCopy(vec_t *in, vec_t *out)
{
	out[0] = in[0];
	out[1] = in[1];
	out[2] = in[2];
}

/* <2ceb8d> ../cstrike/pm_shared/pm_math.c:307 */
void _CrossProduct(const vec_t *v1, const vec_t *v2, vec_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];
}

/* <2ce85f> ../cstrike/pm_shared/pm_math.c:313 */
float_precision Length(const vec_t *v)
{
	int i;
	float_precision length = 0.0f;

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

	return sqrt(length);
}

/* <2cebc2> ../cstrike/pm_shared/pm_math.c:325 */
float Distance(const vec_t *v1, const vec_t *v2)
{
	vec_t d[3];
	VectorSubtract(v2, v1, d);
	return Length(d);
}

/* <2cec3c> ../cstrike/pm_shared/pm_math.c:332 */
float_precision VectorNormalize(vec_t *v)
{
	float_precision length;
	float_precision ilength;

	length = sqrt((float_precision)(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));

	if (length)
	{
		ilength = 1.0 / length;

		v[0] *= ilength;
		v[1] *= ilength;
		v[2] *= ilength;
	}

	return length;
}

/* <2ceca2> ../cstrike/pm_shared/pm_math.c:351 */
void VectorInverse(vec_t *v)
{
	v[0] = -v[0];
	v[1] = -v[1];
	v[2] = -v[2];
}

/* <2cecc9> ../cstrike/pm_shared/pm_math.c:358 */
void VectorScale(const vec_t *in, vec_t scale, vec_t *out)
{
	out[0] = scale * in[0];
	out[1] = scale * in[1];
	out[2] = scale * in[2];
}

/* <2ced0f> ../cstrike/pm_shared/pm_math.c:366 */
int Q_log2(int val)
{
	int answer = 0;
	while (val >>= 1)
		++answer;

	return answer;
}

/* <2ced4d> ../cstrike/pm_shared/pm_math.c:374 */
void VectorMatrix(vec_t *forward, vec_t *right, vec_t *up)
{
	vec_t tmp[3];

	if (forward[0] == 0 && forward[1] == 0)
	{
		right[0] = 1;
		right[1] = 0;
		right[2] = 0;

		up[0] = -forward[2];
		up[1] = 0;
		up[2] = 0;
		return;
	}

	tmp[0] = 0;
	tmp[1] = 0;
	tmp[2] = 1.0f;

	_CrossProduct(forward, tmp, right);
	VectorNormalize(right);
	_CrossProduct(right, forward, up);
	VectorNormalize(up);
}

/* <2cee68> ../cstrike/pm_shared/pm_math.c:398 */
void VectorAngles(const vec_t *forward, vec_t *angles)
{
	float tmp, yaw, pitch;

	if (forward[1] == 0 && forward[0] == 0)
	{
		yaw = 0;
		if (forward[2] > 0)
			pitch = 90;
		else
			pitch = 270;
	}
	else
	{
		yaw = (atan2(forward[1], forward[0]) * 180 / M_PI);
		if (yaw < 0)
			yaw += 360;

		tmp = sqrt (forward[0] * forward[0] + forward[1] * forward[1]);
		pitch = (atan2(forward[2], tmp) * 180 / M_PI);
		if (pitch < 0)
			pitch += 360;
	}

	angles[0] = pitch;
	angles[1] = yaw;
	angles[2] = 0;
}