blankart/src/m_fixed.c

// BLANKART
//-----------------------------------------------------------------------------
// Copyright (C) 1993-1996 by id Software, Inc.
// Copyright (C) 1998-2000 by DooM Legacy Team.
// Copyright (C) 1999-2020 by Sonic Team Junior.
//
// This program is free software distributed under the
// terms of the GNU General Public License, version 2.
// See the 'LICENSE' file for more details.
//-----------------------------------------------------------------------------
/// \file  m_fixed.c
/// \brief Fixed point implementation

#include "doomdef.h"
#include "m_fixed.h"
#include "tables.h" // ANGLETOFINESHIFT

fixed_t FixedSqrt(fixed_t x)
{
	// The neglected art of Fixed Point arithmetic
	// Jetro Lauha
	// Seminar Presentation
	// Assembly 2006, 3rd- 6th August 2006
	// (Revised: September 13, 2006)
	// URL: http://jet.ro/files/The_neglected_art_of_Fixed_Point_arithmetic_20060913.pdf
	register UINT32 root, remHi, remLo, testDiv, count;
	root = 0;         /* Clear root */
	remHi = 0;        /* Clear high part of partial remainder */
	remLo = x;        /* Get argument into low part of partial remainder */
	count = (15 + (FRACBITS >> 1));    /* Load loop counter */
	do
	{
		remHi = (remHi << 2) | (remLo >> 30); remLo <<= 2;  /* get 2 bits of arg */
		root <<= 1;   /* Get ready for the next bit in the root */
		testDiv = (root << 1) + 1;    /* Test radical */
		if (remHi >= testDiv)
		{
			remHi -= testDiv;
			root += 1;
		}
	} while (count-- != 0);
	return root;
}

// Shitty 64-bit duplicate so certain distancing edgecases in k_odds die
INT64 FixedSqrt64(INT64 x)
{
	// The neglected art of Fixed Point arithmetic
	// Jetro Lauha
	// Seminar Presentation
	// Assembly 2006, 3rd- 6th August 2006
	// (Revised: September 13, 2006)
	// URL: http://jet.ro/files/The_neglected_art_of_Fixed_Point_arithmetic_20060913.pdf
	register UINT64 root, remHi, remLo, testDiv, count;
	root = 0;         /* Clear root */
	remHi = 0;        /* Clear high part of partial remainder */
	remLo = x;        /* Get argument into low part of partial remainder */
	count = (31 + (FRACBITS >> 1));    /* Load loop counter */
	do
	{
		remHi = (remHi << 2) | (remLo >> 62); remLo <<= 2;  /* get 2 bits of arg */
		root <<= 1;   /* Get ready for the next bit in the root */
		testDiv = (root << 1) + 1;    /* Test radical */
		if (remHi >= testDiv)
		{
			remHi -= testDiv;
			root += 1;
		}
	} while (count-- != 0);
	return root;
}

fixed_t FixedHypot(fixed_t x, fixed_t y)
{
	// Moved the code from R_PointToDist2 to here,
	// since R_PointToDist2 did the same thing,
	// except less prone to overflowing.

	angle_t angle;
	fixed_t dist;

	x = abs(x);
	y = abs(y);

	if (y > x)
	{
		fixed_t temp;

		temp = x;
		x = y;
		y = temp;
	}

	if (!y)
		return x;

	angle = (tantoangle[FixedDiv(y, x)>>DBITS] + ANGLE_90) >> ANGLETOFINESHIFT;

	// use as cosine
	dist = FixedDiv(x, FINESINE(angle));

	return dist;
}

vector2_t *FV2_Load(vector2_t *vec, fixed_t x, fixed_t y)
{
	vec->x = x;
	vec->y = y;
	return vec;
}

vector2_t *FV2_UnLoad(vector2_t *vec, fixed_t *x, fixed_t *y)
{
	*x = vec->x;
	*y = vec->y;
	return vec;
}

vector2_t *FV2_Copy(vector2_t *a_o, const vector2_t *a_i)
{
	return memcpy(a_o, a_i, sizeof(vector2_t));
}

vector2_t *FV2_AddEx(const vector2_t *a_i, const vector2_t *a_c, vector2_t *a_o)
{
	a_o->x = a_i->x + a_c->x;
	a_o->y = a_i->y + a_c->y;
	return a_o;
}

vector2_t *FV2_Add(vector2_t *a_i, const vector2_t *a_c)
{
	return FV2_AddEx(a_i, a_c, a_i);
}

vector2_t *FV2_SubEx(const vector2_t *a_i, const vector2_t *a_c, vector2_t *a_o)
{
	a_o->x = a_i->x - a_c->x;
	a_o->y = a_i->y - a_c->y;
	return a_o;
}

vector2_t *FV2_Sub(vector2_t *a_i, const vector2_t *a_c)
{
	return FV2_SubEx(a_i, a_c, a_i);
}

vector2_t *FV2_MulEx(const vector2_t *a_i, fixed_t a_c, vector2_t *a_o)
{
	a_o->x = FixedMul(a_i->x, a_c);
	a_o->y = FixedMul(a_i->y, a_c);
	return a_o;
}

vector2_t *FV2_Mul(vector2_t *a_i, fixed_t a_c)
{
	return FV2_MulEx(a_i, a_c, a_i);
}

vector2_t *FV2_DivideEx(const vector2_t *a_i, fixed_t a_c, vector2_t *a_o)
{
	a_o->x = FixedDiv(a_i->x, a_c);
	a_o->y = FixedDiv(a_i->y, a_c);
	return a_o;
}

vector2_t *FV2_Divide(vector2_t *a_i, fixed_t a_c)
{
	return FV2_DivideEx(a_i, a_c, a_i);
}

// Vector Complex Math
vector2_t *FV2_Midpoint(const vector2_t *a_1, const vector2_t *a_2, vector2_t *a_o)
{
	a_o->x = FixedDiv(a_2->x - a_1->x, 2 * FRACUNIT);
	a_o->y = FixedDiv(a_2->y - a_1->y, 2 * FRACUNIT);
	a_o->x = a_1->x + a_o->x;
	a_o->y = a_1->y + a_o->y;
	return a_o;
}

fixed_t FV2_Distance(const vector2_t *p1, const vector2_t *p2)
{
	fixed_t xs = FixedMul(p2->x - p1->x, p2->x - p1->x);
	fixed_t ys = FixedMul(p2->y - p1->y, p2->y - p1->y);
	return FixedSqrt(xs + ys);
}

fixed_t FV2_Magnitude(const vector2_t *a_normal)
{
	fixed_t xs = FixedMul(a_normal->x, a_normal->x);
	fixed_t ys = FixedMul(a_normal->y, a_normal->y);
	return FixedSqrt(xs + ys);
}

// Also returns the magnitude
fixed_t FV2_NormalizeEx(const vector2_t *a_normal, vector2_t *a_o)
{
	fixed_t magnitude = FV2_Magnitude(a_normal);
	a_o->x = FixedDiv(a_normal->x, magnitude);
	a_o->y = FixedDiv(a_normal->y, magnitude);
	return magnitude;
}

fixed_t FV2_Normalize(vector2_t *a_normal)
{
	return FV2_NormalizeEx(a_normal, a_normal);
}

vector2_t *FV2_NegateEx(const vector2_t *a_1, vector2_t *a_o)
{
	a_o->x = -a_1->x;
	a_o->y = -a_1->y;
	return a_o;
}

vector2_t *FV2_Negate(vector2_t *a_1)
{
	return FV2_NegateEx(a_1, a_1);
}

vector2_t *FV2_RotateEx(const vector2_t *a_1, vector2_t *a_o, fixed_t ang_fixed)
{
	fixed_t sine, cosine;
	fixed_t x1, y1, x2, y2;

	angle_t ang = FixedAngle(ang_fixed);

	// Precalculate sine and cosine
	sine = FSIN(ang);
	cosine = FCOS(ang);

	// X vector: x1 (cos), y1 (sin)
	x1 = FixedMul(a_1->x, cosine);
	y1 = FixedMul(a_1->x, sine);

	// Y vector: x2 (-sin), y2 (cos)
	x2 = FixedMul(a_1->y, -sine);
	y2 = FixedMul(a_1->y, cosine);

	a_o->x = x1 + x2;
	a_o->y = y1 + y2;
	return a_o;
}

vector2_t *FV2_Rotate(vector2_t *a_1, fixed_t ang_fixed)
{
	return FV2_RotateEx(a_1, a_1, ang_fixed);
}

boolean FV2_Equal(const vector2_t *a_1, const vector2_t *a_2)
{
	fixed_t Epsilon = FRACUNIT / FRACUNIT;

	if ((abs(a_2->x - a_1->x) > Epsilon) ||
		(abs(a_2->y - a_1->y) > Epsilon))
	{
		return true;
	}

	return false;
}

fixed_t FV2_Dot(const vector2_t *a_1, const vector2_t *a_2)
{
	return (FixedMul(a_1->x, a_2->x) + FixedMul(a_1->y, a_2->y));
}

//
// Point2Vec
//
// Given two points, create a vector between them.
//
vector2_t *FV2_Point2Vec(const vector2_t *point1, const vector2_t *point2, vector2_t *a_o)
{
	a_o->x = point1->x - point2->x;
	a_o->y = point1->y - point2->y;
	return a_o;
}

vector3_t *FV3_Load(vector3_t *vec, fixed_t x, fixed_t y, fixed_t z)
{
	vec->x = x;
	vec->y = y;
	vec->z = z;
	return vec;
}

vector4_t *FV4_Load(vector4_t *vec, fixed_t x, fixed_t y, fixed_t z, fixed_t a)
{
	vec->x = x;
	vec->y = y;
	vec->z = z;
	vec->a = a;
	return vec;
}

vector3_t *FV3_UnLoad(vector3_t *vec, fixed_t *x, fixed_t *y, fixed_t *z)
{
	*x = vec->x;
	*y = vec->y;
	*z = vec->z;
	return vec;
}

vector4_t *FV4_UnLoad(vector4_t *vec, fixed_t *x, fixed_t *y, fixed_t *z, fixed_t *a)
{
	*x = vec->x;
	*y = vec->y;
	*z = vec->z;
	*a = vec->a;
	return vec;
}

vector3_t *FV3_Copy(vector3_t *a_o, const vector3_t *a_i)
{
	return memcpy(a_o, a_i, sizeof(vector3_t));
}

vector4_t *FV4_Copy(vector4_t *a_o, const vector4_t *a_i)
{
	return memcpy(a_o, a_i, sizeof(vector4_t));
}

vector3_t *FV3_AddEx(const vector3_t *a_i, const vector3_t *a_c, vector3_t *a_o)
{
	a_o->x = a_i->x + a_c->x;
	a_o->y = a_i->y + a_c->y;
	a_o->z = a_i->z + a_c->z;
	return a_o;
}

vector4_t *FV4_AddEx(const vector4_t *a_i, const vector4_t *a_c, vector4_t *a_o)
{
	a_o->x = a_i->x + a_c->x;
	a_o->y = a_i->y + a_c->y;
	a_o->z = a_i->z + a_c->z;
	a_o->a = a_i->a + a_c->a;
	return a_o;
}

vector3_t *FV3_Add(vector3_t *a_i, const vector3_t *a_c)
{
	return FV3_AddEx(a_i, a_c, a_i);
}

vector4_t *FV4_Add(vector4_t *a_i, const vector4_t *a_c)
{
	return FV4_AddEx(a_i, a_c, a_i);
}

vector3_t *FV3_SubEx(const vector3_t *a_i, const vector3_t *a_c, vector3_t *a_o)
{
	a_o->x = a_i->x - a_c->x;
	a_o->y = a_i->y - a_c->y;
	a_o->z = a_i->z - a_c->z;
	return a_o;
}

vector4_t *FV4_SubEx(const vector4_t *a_i, const vector4_t *a_c, vector4_t *a_o)
{
	a_o->x = a_i->x - a_c->x;
	a_o->y = a_i->y - a_c->y;
	a_o->z = a_i->z - a_c->z;
	a_o->a = a_i->a - a_c->a;
	return a_o;
}

vector3_t *FV3_Sub(vector3_t *a_i, const vector3_t *a_c)
{
	return FV3_SubEx(a_i, a_c, a_i);
}

vector4_t *FV4_Sub(vector4_t *a_i, const vector4_t *a_c)
{
	return FV4_SubEx(a_i, a_c, a_i);
}

vector3_t *FV3_MulEx(const vector3_t *a_i, const vector3_t *a_c, vector3_t *a_o)
{
	a_o->x = FixedMul(a_i->x, a_c->x);
	a_o->y = FixedMul(a_i->y, a_c->y);
	a_o->z = FixedMul(a_i->z, a_c->z);
	return a_o;
}

vector4_t *FV4_MulEx(const vector4_t *a_i, const vector4_t *a_c, vector4_t *a_o)
{
	a_o->x = FixedMul(a_i->x, a_c->x);
	a_o->y = FixedMul(a_i->y, a_c->y);
	a_o->z = FixedMul(a_i->z, a_c->z);
	a_o->a = FixedMul(a_i->a, a_c->a);
	return a_o;
}

vector3_t *FV3_Mul(vector3_t *a_i, const vector3_t *a_c)
{
	return FV3_MulEx(a_i, a_c, a_i);
}

vector4_t *FV4_Mul(vector4_t *a_i, const vector4_t *a_c)
{
	return FV4_MulEx(a_i, a_c, a_i);
}

vector3_t *FV3_DivideEx(const vector3_t *a_i, const vector3_t *a_c, vector3_t *a_o)
{
	a_o->x = FixedDiv(a_i->x, a_c->x);
	a_o->y = FixedDiv(a_i->y, a_c->y);
	a_o->z = FixedDiv(a_i->z, a_c->z);
	return a_o;
}

vector4_t *FV4_DivideEx(const vector4_t *a_i, const vector4_t *a_c, vector4_t *a_o)
{
	a_o->x = FixedDiv(a_i->x, a_c->x);
	a_o->y = FixedDiv(a_i->y, a_c->y);
	a_o->z = FixedDiv(a_i->z, a_c->z);
	a_o->a = FixedDiv(a_i->a, a_c->a);
	return a_o;
}

vector3_t *FV3_Divide(vector3_t *a_i, const vector3_t *a_c)
{
	return FV3_DivideEx(a_i, a_c, a_i);
}

vector4_t *FV4_Divide(vector4_t *a_i, const vector4_t *a_c)
{
	return FV4_DivideEx(a_i, a_c, a_i);
}

// Vector Complex Math
vector3_t *FV3_Midpoint(const vector3_t *a_1, const vector3_t *a_2, vector3_t *a_o)
{
	a_o->x = FixedDiv(a_2->x - a_1->x, 2 * FRACUNIT);
	a_o->y = FixedDiv(a_2->y - a_1->y, 2 * FRACUNIT);
	a_o->z = FixedDiv(a_2->z - a_1->z, 2 * FRACUNIT);
	a_o->x = a_1->x + a_o->x;
	a_o->y = a_1->y + a_o->y;
	a_o->z = a_1->z + a_o->z;
	return a_o;
}

vector4_t *FV4_Midpoint(const vector4_t *a_1, const vector4_t *a_2, vector4_t *a_o)
{
	a_o->x = FixedDiv(a_2->x - a_1->x, 2 * FRACUNIT);
	a_o->y = FixedDiv(a_2->y - a_1->y, 2 * FRACUNIT);
	a_o->z = FixedDiv(a_2->z - a_1->z, 2 * FRACUNIT);
	a_o->a = FixedDiv(a_2->a - a_1->a, 2 * FRACUNIT);
	a_o->x = a_1->x + a_o->x;
	a_o->y = a_1->y + a_o->y;
	a_o->z = a_1->z + a_o->z;
	a_o->a = a_1->z + a_o->a;
	return a_o;
}

fixed_t FV3_Distance(const vector3_t *p1, const vector3_t *p2)
{
	fixed_t xs = FixedMul(p2->x - p1->x, p2->x - p1->x);
	fixed_t ys = FixedMul(p2->y - p1->y, p2->y - p1->y);
	fixed_t zs = FixedMul(p2->z - p1->z, p2->z - p1->z);
	return FixedSqrt(xs + ys + zs);
}

fixed_t FV4_Distance(const vector4_t *p1, const vector4_t *p2)
{
	fixed_t xs = FixedMul(p2->x - p1->x, p2->x - p1->x);
	fixed_t ys = FixedMul(p2->y - p1->y, p2->y - p1->y);
	fixed_t zs = FixedMul(p2->z - p1->z, p2->z - p1->z);
	fixed_t za = FixedMul(p2->a - p1->a, p2->a - p1->a);
	return FixedSqrt(xs + ys + zs + za);
}

fixed_t FV3_Magnitude(const vector3_t *a_normal)
{
	fixed_t xs = FixedMul(a_normal->x, a_normal->x);
	fixed_t ys = FixedMul(a_normal->y, a_normal->y);
	fixed_t zs = FixedMul(a_normal->z, a_normal->z);
	return FixedSqrt(xs + ys + zs);
}

fixed_t FV4_Magnitude(const vector4_t *a_normal)
{
	fixed_t xs = FixedMul(a_normal->x, a_normal->x);
	fixed_t ys = FixedMul(a_normal->y, a_normal->y);
	fixed_t zs = FixedMul(a_normal->z, a_normal->z);
	fixed_t as = FixedMul(a_normal->a, a_normal->a);
	return FixedSqrt(xs + ys + zs + as);
}

// Also returns the magnitude
fixed_t FV3_NormalizeEx(const vector3_t *a_normal, vector3_t *a_o)
{
	fixed_t magnitude = FV3_Magnitude(a_normal);
	a_o->x = FixedDiv(a_normal->x, magnitude);
	a_o->y = FixedDiv(a_normal->y, magnitude);
	a_o->z = FixedDiv(a_normal->z, magnitude);
	return magnitude;
}

fixed_t FV4_NormalizeEx(const vector4_t *a_normal, vector4_t *a_o)
{
	fixed_t magnitude = FV4_Magnitude(a_normal);
	a_o->x = FixedDiv(a_normal->x, magnitude);
	a_o->y = FixedDiv(a_normal->y, magnitude);
	a_o->z = FixedDiv(a_normal->z, magnitude);
	a_o->a = FixedDiv(a_normal->a, magnitude);
	return magnitude;
}

fixed_t FV3_Normalize(vector3_t *a_normal)
{
	return FV3_NormalizeEx(a_normal, a_normal);
}

fixed_t FV4_Normalize(vector4_t *a_normal)
{
	return FV4_NormalizeEx(a_normal, a_normal);
}

vector3_t *FV3_NegateEx(const vector3_t *a_1, vector3_t *a_o)
{
	a_o->x = -a_1->x;
	a_o->y = -a_1->y;
	a_o->z = -a_1->z;
	return a_o;
}

vector4_t *FV4_NegateEx(const vector4_t *a_1, vector4_t *a_o)
{
	a_o->x = -a_1->x;
	a_o->y = -a_1->y;
	a_o->z = -a_1->z;
	a_o->a = -a_1->a;
	return a_o;
}

vector3_t *FV3_Negate(vector3_t *a_1)
{
	return FV3_NegateEx(a_1, a_1);
}

vector4_t *FV4_Negate(vector4_t *a_1)
{
	return FV4_NegateEx(a_1, a_1);
}

boolean FV3_Equal(const vector3_t *a_1, const vector3_t *a_2)
{
	fixed_t Epsilon = FRACUNIT / FRACUNIT;

	if ((abs(a_2->x - a_1->x) > Epsilon) ||
		(abs(a_2->y - a_1->y) > Epsilon) ||
		(abs(a_2->z - a_1->z) > Epsilon))
	{
		return true;
	}

	return false;
}

boolean FV4_Equal(const vector4_t *a_1, const vector4_t *a_2)
{
	fixed_t Epsilon = FRACUNIT / FRACUNIT;

	if ((abs(a_2->x - a_1->x) > Epsilon) ||
		(abs(a_2->y - a_1->y) > Epsilon) ||
		(abs(a_2->z - a_1->z) > Epsilon) ||
		(abs(a_2->a - a_1->a) > Epsilon))
	{
		return true;
	}

	return false;
}

fixed_t FV3_Dot(const vector3_t *a_1, const vector3_t *a_2)
{
	return (FixedMul(a_1->x, a_2->x) + FixedMul(a_1->y, a_2->y) + FixedMul(a_1->z, a_2->z));
}

fixed_t FV4_Dot(const vector4_t *a_1, const vector4_t *a_2)
{
	return (FixedMul(a_1->x, a_2->x) + FixedMul(a_1->y, a_2->y) + FixedMul(a_1->z, a_2->z) + FixedMul(a_1->a, a_2->a));
}

vector3_t *FV3_Cross(const vector3_t *a_1, const vector3_t *a_2, vector3_t *a_o)
{
	a_o->x = FixedMul(a_1->y, a_2->z) - FixedMul(a_1->z, a_2->y);
	a_o->y = FixedMul(a_1->z, a_2->x) - FixedMul(a_1->x, a_2->z);
	a_o->z = FixedMul(a_1->x, a_2->y) - FixedMul(a_1->y, a_2->x);
	return a_o;
}

//
// ClosestPointOnLine
//
// Finds the point on a line closest
// to the specified point.
//
vector3_t *FV3_ClosestPointOnLine(const vector3_t *Line, const vector3_t *p, vector3_t *out)
{
	// Determine t (the length of the vector from <20>Line[0]<5D> to <20>p<EFBFBD>)
	vector3_t c, V;
	fixed_t t, d = 0;
	FV3_SubEx(p, &Line[0], &c);
	FV3_SubEx(&Line[1], &Line[0], &V);
	FV3_NormalizeEx(&V, &V);

	d = FV3_Distance(&Line[0], &Line[1]);
	t = FV3_Dot(&V, &c);

	// Check to see if <20>t<EFBFBD> is beyond the extents of the line segment
	if (t < 0)
	{
		return FV3_Copy(out, &Line[0]);
	}
	if (t > d)
	{
		return FV3_Copy(out, &Line[1]);
	}

	// Return the point between "Line[0]" and "Line[1]"
	vector3_t T = { t, t, t };
	FV3_Mul(&V, &T);

	return FV3_AddEx(&Line[0], &V, out);
}

//
// ClosestPointOnVector
//
// Similar to ClosestPointOnLine, but uses a vector instead of two points.
//
void FV3_ClosestPointOnVector(const vector3_t *dir, const vector3_t *p, vector3_t *out)
{
	fixed_t t = FV3_Dot(dir, p);

	// Return the point on the line closest
	vector3_t T = { t, t, t };
	FV3_MulEx(dir, &T, out);
	return;
}

//
// ClosestPointOnTriangle
//
// Given a triangle and a point,
// the closest point on the edge of
// the triangle is returned.
//
void FV3_ClosestPointOnTriangle(const vector3_t *tri, const vector3_t *point, vector3_t *result)
{
	UINT8 i;
	fixed_t dist, closestdist;
	vector3_t EdgePoints[3];
	vector3_t Line[2];

	FV3_Copy(&Line[0], &tri[0]);
	FV3_Copy(&Line[1], &tri[1]);
	FV3_ClosestPointOnLine(Line, point, &EdgePoints[0]);

	FV3_Copy(&Line[0], &tri[1]);
	FV3_Copy(&Line[1], &tri[2]);
	FV3_ClosestPointOnLine(Line, point, &EdgePoints[1]);

	FV3_Copy(&Line[0], &tri[2]);
	FV3_Copy(&Line[1], &tri[0]);
	FV3_ClosestPointOnLine(Line, point, &EdgePoints[2]);

	// Find the closest one of the three
	FV3_Copy(result, &EdgePoints[0]);
	closestdist = FV3_Distance(point, &EdgePoints[0]);
	for (i = 1; i < 3; i++)
	{
		dist = FV3_Distance(point, &EdgePoints[i]);

		if (dist < closestdist)
		{
			closestdist = dist;
			FV3_Copy(result, &EdgePoints[i]);
		}
	}

	// We now have the closest point! Whee!
}

//
// Point2Vec
//
// Given two points, create a vector between them.
//
vector3_t *FV3_Point2Vec(const vector3_t *point1, const vector3_t *point2, vector3_t *a_o)
{
	a_o->x = point1->x - point2->x;
	a_o->y = point1->y - point2->y;
	a_o->z = point1->z - point2->z;
	return a_o;
}

//
// Normal
//
// Calculates the normal of a polygon.
//
fixed_t FV3_Normal(const vector3_t *a_triangle, vector3_t *a_normal)
{
	vector3_t a_1;
	vector3_t a_2;

	FV3_Point2Vec(&a_triangle[2], &a_triangle[0], &a_1);
	FV3_Point2Vec(&a_triangle[1], &a_triangle[0], &a_2);

	FV3_Cross(&a_1, &a_2, a_normal);

	return FV3_NormalizeEx(a_normal, a_normal);
}

//
// Strength
//
// Measures the 'strength' of a vector in a particular direction.
//
fixed_t FV3_Strength(const vector3_t *a_1, const vector3_t *dir)
{
	vector3_t normal;
	FV3_NormalizeEx(a_1, &normal);
	fixed_t dot = FV3_Dot(&normal, dir);

	FV3_ClosestPointOnVector(dir, a_1, &normal);

	fixed_t dist = FV3_Magnitude(&normal);

	if (dot < 0) // Not facing same direction, so negate result.
		dist = -dist;

	return dist;
}

//
// PlaneDistance
//
// Calculates distance between a plane and the origin.
//
fixed_t FV3_PlaneDistance(const vector3_t *a_normal, const vector3_t *a_point)
{
	return -(FixedMul(a_normal->x, a_point->x) + FixedMul(a_normal->y, a_point->y) + FixedMul(a_normal->z, a_point->z));
}

boolean FV3_IntersectedPlane(const vector3_t *a_triangle, const vector3_t *a_line, vector3_t *a_normal, fixed_t *originDistance)
{
	fixed_t distance1 = 0, distance2 = 0;

	FV3_Normal(a_triangle, a_normal);

	*originDistance = FV3_PlaneDistance(a_normal, &a_triangle[0]);

	distance1 = (FixedMul(a_normal->x, a_line[0].x) + FixedMul(a_normal->y, a_line[0].y)
		+ FixedMul(a_normal->z, a_line[0].z)) + *originDistance;

	distance2 = (FixedMul(a_normal->x, a_line[1].x) + FixedMul(a_normal->y, a_line[1].y)
		+ FixedMul(a_normal->z, a_line[1].z)) + *originDistance;

	// Positive or zero number means no intersection
	if (FixedMul(distance1, distance2) >= 0)
		return false;

	return true;
}

//
// PlaneIntersection
//
// Returns the distance from
// rOrigin to where the ray
// intersects the plane. Assumes
// you already know it intersects
// the plane.
//
fixed_t FV3_PlaneIntersection(const vector3_t *pOrigin, const vector3_t *pNormal, const vector3_t *rOrigin, const vector3_t *rVector)
{
	fixed_t d = -(FV3_Dot(pNormal, pOrigin));
	fixed_t number = FV3_Dot(pNormal, rOrigin) + d;
	fixed_t denom = FV3_Dot(pNormal, rVector);
	return -FixedDiv(number, denom);
}

//
// IntersectRaySphere
// Input : rO - origin of ray in world space
//         rV - vector describing direction of ray in world space
//         sO - Origin of sphere
//         sR - radius of sphere
// Notes : Normalized directional vectors expected
// Return: distance to sphere in world units, -1 if no intersection.
//
fixed_t FV3_IntersectRaySphere(const vector3_t *rO, const vector3_t *rV, const vector3_t *sO, fixed_t sR)
{
	vector3_t Q;
	fixed_t c, v, d;
	FV3_SubEx(sO, rO, &Q);

	c = FV3_Magnitude(&Q);
	v = FV3_Dot(&Q, rV);
	d = FixedMul(sR, sR) - (FixedMul(c, c) - FixedMul(v, v));

	// If there was no intersection, return -1
	if (d < 0 * FRACUNIT)
		return (-1 * FRACUNIT);

	// Return the distance to the [first] intersecting point
	return (v - FixedSqrt(d));
}

//
// IntersectionPoint
//
// This returns the intersection point of the line that intersects the plane
//
vector3_t *FV3_IntersectionPoint(const vector3_t *vNormal, const vector3_t *vLine, fixed_t distance, vector3_t *ReturnVec)
{
	vector3_t vLineDir; // Variables to hold the point and the line's direction
	fixed_t Numerator = 0, Denominator = 0, dist = 0;

	// Here comes the confusing part.  We need to find the 3D point that is actually
	// on the plane.  Here are some steps to do that:

	// 1)  First we need to get the vector of our line, Then normalize it so it's a length of 1
	FV3_Point2Vec(&vLine[1], &vLine[0], &vLineDir);		// Get the Vector of the line
	FV3_NormalizeEx(&vLineDir, &vLineDir);				// Normalize the lines vector


	// 2) Use the plane equation (distance = Ax + By + Cz + D) to find the distance from one of our points to the plane.
	//    Here I just chose a arbitrary point as the point to find that distance.  You notice we negate that
	//    distance.  We negate the distance because we want to eventually go BACKWARDS from our point to the plane.
	//    By doing this is will basically bring us back to the plane to find our intersection point.
	Numerator = -(FixedMul(vNormal->x, vLine[0].x) +		// Use the plane equation with the normal and the line
		FixedMul(vNormal->y, vLine[0].y) +
		FixedMul(vNormal->z, vLine[0].z) + distance);

	// 3) If we take the dot product between our line vector and the normal of the polygon,
	//    this will give us the cosine of the angle between the 2 (since they are both normalized - length 1).
	//    We will then divide our Numerator by this value to find the offset towards the plane from our arbitrary point.
	Denominator = FV3_Dot(vNormal, &vLineDir);		// Get the dot product of the line's vector and the normal of the plane

	// Since we are using division, we need to make sure we don't get a divide by zero error
	// If we do get a 0, that means that there are INFINITE points because the the line is
	// on the plane (the normal is perpendicular to the line - (Normal.Vector = 0)).
	// In this case, we should just return any point on the line.

	if (Denominator == 0 * FRACUNIT) // Check so we don't divide by zero
	{
		ReturnVec->x = vLine[0].x;
		ReturnVec->y = vLine[0].y;
		ReturnVec->z = vLine[0].z;
		return ReturnVec;	// Return an arbitrary point on the line
	}

	// We divide the (distance from the point to the plane) by (the dot product)
	// to get the distance (dist) that we need to move from our arbitrary point.  We need
	// to then times this distance (dist) by our line's vector (direction).  When you times
	// a scalar (single number) by a vector you move along that vector.  That is what we are
	// doing.  We are moving from our arbitrary point we chose from the line BACK to the plane
	// along the lines vector.  It seems logical to just get the numerator, which is the distance
	// from the point to the line, and then just move back that much along the line's vector.
	// Well, the distance from the plane means the SHORTEST distance.  What about in the case that
	// the line is almost parallel with the polygon, but doesn't actually intersect it until half
	// way down the line's length.  The distance from the plane is short, but the distance from
	// the actual intersection point is pretty long.  If we divide the distance by the dot product
	// of our line vector and the normal of the plane, we get the correct length.  Cool huh?

	dist = FixedDiv(Numerator, Denominator); // Divide to get the multiplying (percentage) factor

	// Now, like we said above, we times the dist by the vector, then add our arbitrary point.
	// This essentially moves the point along the vector to a certain distance.  This now gives
	// us the intersection point.  Yay!

	// Return the intersection point
	ReturnVec->x = vLine[0].x + FixedMul(vLineDir.x, dist);
	ReturnVec->y = vLine[0].y + FixedMul(vLineDir.y, dist);
	ReturnVec->z = vLine[0].z + FixedMul(vLineDir.z, dist);
	return ReturnVec;
}

//
// PointOnLineSide
//
// If on the front side of the line, returns 1.
// If on the back side of the line, returns 0.
// 2D only.
//
UINT8 FV3_PointOnLineSide(const vector3_t *point, const vector3_t *line)
{
	fixed_t s1 = FixedMul((point->y - line[0].y), (line[1].x - line[0].x));
	fixed_t s2 = FixedMul((point->x - line[0].x), (line[1].y - line[0].y));
	return (UINT8)(s1 - s2 < 0);
}

//
// PointInsideBox
//
// Given four points of a box,
// determines if the supplied point is
// inside the box or not.
//
boolean FV3_PointInsideBox(const vector3_t *point, const vector3_t *box)
{
	vector3_t lastLine[2];

	FV3_Load(&lastLine[0], box[3].x, box[3].y, box[3].z);
	FV3_Load(&lastLine[1], box[0].x, box[0].y, box[0].z);

	if (FV3_PointOnLineSide(point, &box[0])
		|| FV3_PointOnLineSide(point, &box[1])
		|| FV3_PointOnLineSide(point, &box[2])
		|| FV3_PointOnLineSide(point, lastLine))
		return false;

	return true;
}
//
// LoadIdentity
//
// Loads the identity matrix into a matrix
//
void FM_LoadIdentity(matrix_t* matrix)
{
#define M(row,col)  matrix->m[col * 4 + row]
	memset(matrix, 0x00, sizeof(matrix_t));

	M(0, 0) = FRACUNIT;
	M(1, 1) = FRACUNIT;
	M(2, 2) = FRACUNIT;
	M(3, 3) = FRACUNIT;
#undef M
}

//
// CreateObjectMatrix
//
// Creates a matrix that can be used for
// adjusting the position of an object
//
void FM_CreateObjectMatrix(matrix_t *matrix, fixed_t x, fixed_t y, fixed_t z, fixed_t anglex, fixed_t angley, fixed_t anglez, fixed_t upx, fixed_t upy, fixed_t upz, fixed_t radius)
{
	vector3_t upcross;
	vector3_t upvec;
	vector3_t basevec;

	FV3_Load(&upvec, upx, upy, upz);
	FV3_Load(&basevec, anglex, angley, anglez);
	FV3_Cross(&upvec, &basevec, &upcross);
	FV3_Normalize(&upcross);

	FM_LoadIdentity(matrix);

	matrix->m[0] = upcross.x;
	matrix->m[1] = upcross.y;
	matrix->m[2] = upcross.z;
	matrix->m[3] = 0 * FRACUNIT;

	matrix->m[4] = upx;
	matrix->m[5] = upy;
	matrix->m[6] = upz;
	matrix->m[7] = 0;

	matrix->m[8] = anglex;
	matrix->m[9] = angley;
	matrix->m[10] = anglez;
	matrix->m[11] = 0;

	matrix->m[12] = x - FixedMul(upx, radius);
	matrix->m[13] = y - FixedMul(upy, radius);
	matrix->m[14] = z - FixedMul(upz, radius);
	matrix->m[15] = FRACUNIT;
}

//
// MultMatrixVec
//
// Multiplies a vector by the specified matrix
//
const vector3_t *FM_MultMatrixVec3(const matrix_t *matrix, const vector3_t *vec, vector3_t *out)
{
#define M(row,col)  matrix->m[col * 4 + row]
	out->x = FixedMul(vec->x, M(0, 0))
		+ FixedMul(vec->y, M(0, 1))
		+ FixedMul(vec->z, M(0, 2))
		+ M(0, 3);

	out->y = FixedMul(vec->x, M(1, 0))
		+ FixedMul(vec->y, M(1, 1))
		+ FixedMul(vec->z, M(1, 2))
		+ M(1, 3);

	out->z = FixedMul(vec->x, M(2, 0))
		+ FixedMul(vec->y, M(2, 1))
		+ FixedMul(vec->z, M(2, 2))
		+ M(2, 3);
#undef M
	return out;
}

const vector4_t *FM_MultMatrixVec4(const matrix_t *matrix, const vector4_t *vec, vector4_t *out)
{
#define M(row,col)  matrix->m[col * 4 + row]
	out->x = FixedMul(vec->x, M(0, 0))
		+ FixedMul(vec->y, M(0, 1))
		+ FixedMul(vec->z, M(0, 2))
		+ FixedMul(vec->a, M(0, 3));

	out->y = FixedMul(vec->x, M(1, 0))
		+ FixedMul(vec->y, M(1, 1))
		+ FixedMul(vec->z, M(1, 2))
		+ FixedMul(vec->a, M(1, 3));

	out->z = FixedMul(vec->x, M(2, 0))
		+ FixedMul(vec->y, M(2, 1))
		+ FixedMul(vec->z, M(2, 2))
		+ FixedMul(vec->a, M(2, 3));


	out->a = FixedMul(vec->x, M(3, 0))
		+ FixedMul(vec->y, M(3, 1))
		+ FixedMul(vec->z, M(3, 2))
		+ FixedMul(vec->a, M(3, 3));
#undef M
	return out;
}

//
// MultMatrix
//
// Multiples one matrix into another
//
void FM_MultMatrixEx(const matrix_t *m1, const matrix_t *m2, matrix_t *dest)
{
	UINT8 i, j;
#define M(row,col)  m1->m[col * 4 + row]
#define D(row,col)  m2->m[col * 4 + row]
#define R(row,col)  dest->m[col * 4 + row]

	for (i = 0; i < 4; i++)
	{
		for (j = 0; j < 4; j++)
			R(i, j) = FixedMul(D(i, 0), M(0, j)) + FixedMul(D(i, 1), M(1, j)) + FixedMul(D(i, 2), M(2, j)) + FixedMul(D(i, 3), M(3, j));
	}

#undef R
#undef D
#undef M
}

void FM_MultMatrix(matrix_t *dest, const matrix_t *multme)
{
	FM_MultMatrixEx(dest, multme, dest);
}

//
// Translate
//
// Translates a matrix
//
void FM_Translate(matrix_t *dest, fixed_t x, fixed_t y, fixed_t z)
{
	matrix_t trans;
#define M(row,col)  trans.m[col * 4 + row]

	memset(&trans, 0x00, sizeof(matrix_t));

	M(0, 0) = M(1, 1) = M(2, 2) = M(3, 3) = FRACUNIT;
	M(0, 3) = x;
	M(1, 3) = y;
	M(2, 3) = z;

	FM_MultMatrix(dest, &trans);
#undef M
}

//
// Scale
//
// Scales a matrix
//
void FM_Scale(matrix_t *dest, fixed_t x, fixed_t y, fixed_t z)
{
	matrix_t scale;
#define M(row,col)  scale.m[col * 4 + row]

	memset(&scale, 0x00, sizeof(matrix_t));

	M(3, 3) = FRACUNIT;
	M(0, 0) = x;
	M(1, 1) = y;
	M(2, 2) = z;

	FM_MultMatrix(dest, &scale);
#undef M
}

// "Approach" functions taken from or based on the (decompiled) SM64 implementation
// https://github.com/n64decomp/sm64/blob/master/src/engine/math_util.c

/** \brief Returns the INT32 value 'current' after it tries to approach target, going up at most
 'inc' and going down at most 'dec'.
 *
 * If target is close to the max or min of INT32, then it's possible to overflow past it without
 stopping.

	\param current	(INT32) current value

	\param target	(INT32) target value

	\param inc	(INT32) value to increment by

	\param dec	(INT32) value to decrement by

	\return		(INT32) current value after incrementing/decrementing
*/
INT32 ApproachINT32(INT32 current, INT32 target, INT32 inc, INT32 dec)
{
	if (current < target)
	{
		current += inc;
		if (current > target)
		{
			current = target;
		}
	}
	else
	{
		current -= dec;
		if (current < target)
		{
			current = target;
		}
	}
	return current;
}

/** \brief Returns the unsigned INT32 value ``current`` after it tries to approach target, going
 up at most ``inc`` and going down at most ``dec``.
 *
 * If target is close to the max or min of UINT32, then it's possible to overflow past it without
 stopping.

	\param current	(UINT32) current value

	\param target	(UINT32) target value

	\param inc	(UINT32) value to increment by

	\param dec	(UINT32) value to decrement by

	\return		(UINT32) current value after incrementing/decrementing
*/
UINT32 ApproachUINT32(UINT32 current, UINT32 target, UINT32 inc, UINT32 dec)
{
	if (current < target)
	{
		current += inc;
		if (current > target)
		{
			current = target;
		}
	}
	else
	{
		current -= dec;
		if (current < target)
		{
			current = target;
		}
	}
	return current;
}

/** \brief Returns the fixed-point value 'current' after it tries to approach target, going up at
 most 'inc' and going down at most 'dec'.
 *
 * If target is close to the max or min of fixed_t, then it's possible to overflow past it without
 stopping.

	\param current	(fixed_t) current value

	\param target	(fixed_t) target value

	\param inc	(fixed_t) value to increment by

	\param dec	(fixed_t) value to decrement by

	\return		(fixed_t) current value after incrementing/decrementing
*/
fixed_t ApproachFixed(fixed_t current, fixed_t target, fixed_t inc, fixed_t dec)
{
	if (current < target)
	{
		current += inc;
		if (current > target)
		{
			current = target;
		}
	}
	else
	{
		current -= dec;
		if (current < target)
		{
			current = target;
		}
	}
	return current;
}