001e4e11ca
The FixedMul() C implementation would produce off by one results, causing constant desyncs on 64 bit builds and builds without an ASM implementation of the function. This is fixed by shifting instead of dividing, possibly avoiding rounding errors.
1020 lines
24 KiB
C
1020 lines
24 KiB
C
// SONIC ROBO BLAST 2
|
||
//-----------------------------------------------------------------------------
|
||
// Copyright (C) 1993-1996 by id Software, Inc.
|
||
// Copyright (C) 1998-2000 by DooM Legacy Team.
|
||
// Copyright (C) 1999-2016 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
|
||
|
||
#if 0 //#ifndef NO_M
|
||
#include <math.h>
|
||
#define HAVE_SQRT
|
||
#if 0 //#ifndef _WIN32 // MSVCRT does not have *f() functions
|
||
#define HAVE_SQRTF
|
||
#endif
|
||
#endif
|
||
#include "doomdef.h"
|
||
#include "m_fixed.h"
|
||
|
||
#ifdef __USE_C_FIXEDMUL__
|
||
|
||
/** \brief The FixedMul function
|
||
|
||
\param a fixed_t number
|
||
\param b fixed_t number
|
||
|
||
\return a*b>>FRACBITS
|
||
|
||
*/
|
||
fixed_t FixedMul(fixed_t a, fixed_t b)
|
||
{
|
||
// Need to cast to unsigned before shifting to avoid undefined behaviour
|
||
// for negative integers
|
||
return (fixed_t)(((UINT64)((INT64)a * b)) >> FRACBITS);
|
||
}
|
||
|
||
#endif //__USE_C_FIXEDMUL__
|
||
|
||
#ifdef __USE_C_FIXEDDIV__
|
||
/** \brief The FixedDiv2 function
|
||
|
||
\param a fixed_t number
|
||
\param b fixed_t number
|
||
|
||
\return a/b * FRACUNIT
|
||
|
||
*/
|
||
fixed_t FixedDiv2(fixed_t a, fixed_t b)
|
||
{
|
||
INT64 ret;
|
||
|
||
if (b == 0)
|
||
I_Error("FixedDiv: divide by zero");
|
||
|
||
ret = (((INT64)a * FRACUNIT) ) / b;
|
||
|
||
if ((ret > INT32_MAX) || (ret < INT32_MIN))
|
||
I_Error("FixedDiv: divide by zero");
|
||
return (fixed_t)ret;
|
||
}
|
||
|
||
#endif // __USE_C_FIXEDDIV__
|
||
|
||
fixed_t FixedSqrt(fixed_t x)
|
||
{
|
||
#ifdef HAVE_SQRT
|
||
const float fx = FIXED_TO_FLOAT(x);
|
||
float fr;
|
||
#ifdef HAVE_SQRTF
|
||
fr = sqrtf(fx);
|
||
#else
|
||
fr = (float)sqrt(fx);
|
||
#endif
|
||
return FLOAT_TO_FIXED(fr);
|
||
#else
|
||
// 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;
|
||
#endif
|
||
}
|
||
|
||
fixed_t FixedHypot(fixed_t x, fixed_t y)
|
||
{
|
||
fixed_t ax, yx, yx2, yx1;
|
||
if (abs(y) > abs(x)) // |y|>|x|
|
||
{
|
||
ax = abs(y); // |y| => ax
|
||
yx = FixedDiv(x, y); // (x/y)
|
||
}
|
||
else // |x|>|y|
|
||
{
|
||
ax = abs(x); // |x| => ax
|
||
yx = FixedDiv(y, x); // (x/y)
|
||
}
|
||
yx2 = FixedMul(yx, yx); // (x/y)^2
|
||
yx1 = FixedSqrt(1*FRACUNIT + yx2); // (1 + (x/y)^2)^1/2
|
||
return FixedMul(ax, yx1); // |x|*((1 + (x/y)^2)^1/2)
|
||
}
|
||
|
||
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 M_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);
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
vector3_t *FV3_Copy(vector3_t *a_o, const vector3_t *a_i)
|
||
{
|
||
return M_Memcpy(a_o, a_i, sizeof(vector3_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;
|
||
}
|
||
|
||
vector3_t *FV3_Add(vector3_t *a_i, const vector3_t *a_c)
|
||
{
|
||
return FV3_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;
|
||
}
|
||
|
||
vector3_t *FV3_Sub(vector3_t *a_i, const vector3_t *a_c)
|
||
{
|
||
return FV3_SubEx(a_i, a_c, a_i);
|
||
}
|
||
|
||
vector3_t *FV3_MulEx(const vector3_t *a_i, fixed_t a_c, vector3_t *a_o)
|
||
{
|
||
a_o->x = FixedMul(a_i->x, a_c);
|
||
a_o->y = FixedMul(a_i->y, a_c);
|
||
a_o->z = FixedMul(a_i->z, a_c);
|
||
return a_o;
|
||
}
|
||
|
||
vector3_t *FV3_Mul(vector3_t *a_i, fixed_t a_c)
|
||
{
|
||
return FV3_MulEx(a_i, a_c, a_i);
|
||
}
|
||
|
||
vector3_t *FV3_DivideEx(const vector3_t *a_i, fixed_t a_c, vector3_t *a_o)
|
||
{
|
||
a_o->x = FixedDiv(a_i->x, a_c);
|
||
a_o->y = FixedDiv(a_i->y, a_c);
|
||
a_o->z = FixedDiv(a_i->z, a_c);
|
||
return a_o;
|
||
}
|
||
|
||
vector3_t *FV3_Divide(vector3_t *a_i, fixed_t a_c)
|
||
{
|
||
return FV3_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;
|
||
}
|
||
|
||
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 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);
|
||
}
|
||
|
||
// 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 FV3_Normalize(vector3_t *a_normal)
|
||
{
|
||
return FV3_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;
|
||
}
|
||
|
||
vector3_t *FV3_Negate(vector3_t *a_1)
|
||
{
|
||
return FV3_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;
|
||
}
|
||
|
||
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));
|
||
}
|
||
|
||
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 <20>Line[0]<5D> and <20>Line[1]<5D>
|
||
FV3_Mul(&V, t);
|
||
|
||
return FV3_AddEx(&Line[0], &V, out);
|
||
}
|
||
|
||
//
|
||
// 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.
|
||
//
|
||
void 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);
|
||
|
||
FV3_NormalizeEx(a_normal, a_normal);
|
||
}
|
||
|
||
//
|
||
// 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
|
||
//
|
||
void 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
|
||
}
|
||
|
||
//
|
||
// MultMatrix
|
||
//
|
||
// Multiples one matrix into another
|
||
//
|
||
void FM_MultMatrix(matrix_t *dest, const matrix_t *multme)
|
||
{
|
||
matrix_t result;
|
||
UINT8 i, j;
|
||
#define M(row,col) multme->m[col * 4 + row]
|
||
#define D(row,col) dest->m[col * 4 + row]
|
||
#define R(row,col) result.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));
|
||
}
|
||
|
||
M_Memcpy(dest, &result, sizeof(matrix_t));
|
||
|
||
#undef R
|
||
#undef D
|
||
#undef M
|
||
}
|
||
|
||
//
|
||
// 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
|
||
}
|
||
|
||
#ifdef M_TESTCASE
|
||
//#define MULDIV_TEST
|
||
#define SQRT_TEST
|
||
|
||
static inline void M_print(INT64 a)
|
||
{
|
||
const fixed_t w = (a>>FRACBITS);
|
||
fixed_t f = a%FRACUNIT;
|
||
fixed_t d = FRACUNIT;
|
||
|
||
if (f == 0)
|
||
{
|
||
printf("%d", (fixed_t)w);
|
||
return;
|
||
}
|
||
else while (f != 1 && f/2 == f>>1)
|
||
{
|
||
d /= 2;
|
||
f /= 2;
|
||
}
|
||
|
||
if (w == 0)
|
||
printf("%d/%d", (fixed_t)f, d);
|
||
else
|
||
printf("%d+(%d/%d)", (fixed_t)w, (fixed_t)f, d);
|
||
}
|
||
|
||
FUNCMATH FUNCINLINE static inline fixed_t FixedMulC(fixed_t a, fixed_t b)
|
||
{
|
||
return (fixed_t)((((INT64)a * b) ) / FRACUNIT);
|
||
}
|
||
|
||
FUNCMATH FUNCINLINE static inline fixed_t FixedDivC2(fixed_t a, fixed_t b)
|
||
{
|
||
INT64 ret;
|
||
|
||
if (b == 0)
|
||
I_Error("FixedDiv: divide by zero");
|
||
|
||
ret = (((INT64)a * FRACUNIT) ) / b;
|
||
|
||
if ((ret > INT32_MAX) || (ret < INT32_MIN))
|
||
I_Error("FixedDiv: divide by zero");
|
||
return (fixed_t)ret;
|
||
}
|
||
|
||
FUNCMATH FUNCINLINE static inline fixed_t FixedDivC(fixed_t a, fixed_t b)
|
||
{
|
||
if ((abs(a) >> (FRACBITS-2)) >= abs(b))
|
||
return (a^b) < 0 ? INT32_MIN : INT32_MAX;
|
||
|
||
return FixedDivC2(a, b);
|
||
}
|
||
|
||
FUNCMATH FUNCINLINE static inline fixed_t FixedSqrtC(fixed_t x)
|
||
{
|
||
const float fx = FIXED_TO_FLOAT(x);
|
||
float fr;
|
||
#ifdef HAVE_SQRTF
|
||
fr = sqrtf(fx);
|
||
#else
|
||
fr = (float)sqrt(fx);
|
||
#endif
|
||
return FLOAT_TO_FIXED(fr);
|
||
}
|
||
int main(int argc, char** argv)
|
||
{
|
||
int n = 10;
|
||
INT64 a, b;
|
||
fixed_t c, d;
|
||
(void)argc;
|
||
(void)argv;
|
||
|
||
#ifdef MULDIV_TEST
|
||
for (a = 1; a <= INT32_MAX; a += FRACUNIT)
|
||
for (b = 0; b <= INT32_MAX; b += FRACUNIT)
|
||
{
|
||
c = FixedMul(a, b);
|
||
d = FixedMulC(a, b);
|
||
if (c != d)
|
||
{
|
||
printf("(");
|
||
M_print(a);
|
||
printf(") * (");
|
||
M_print(b);
|
||
printf(") = (");
|
||
M_print(c);
|
||
printf(") != (");
|
||
M_print(d);
|
||
printf(") \n");
|
||
n--;
|
||
printf("%d != %d\n", c, d);
|
||
}
|
||
c = FixedDiv(a, b);
|
||
d = FixedDivC(a, b);
|
||
if (c != d)
|
||
{
|
||
printf("(");
|
||
M_print(a);
|
||
printf(") / (");
|
||
M_print(b);
|
||
printf(") = (");
|
||
M_print(c);
|
||
printf(") != (");
|
||
M_print(d);
|
||
printf(")\n");
|
||
n--;
|
||
printf("%d != %d\n", c, d);
|
||
}
|
||
if (n <= 0)
|
||
exit(-1);
|
||
}
|
||
#endif
|
||
|
||
#ifdef SQRT_TEST
|
||
for (a = 0; a <= INT32_MAX; a += 1)
|
||
{
|
||
c = FixedSqrt(a);
|
||
d = FixedSqrtC(a);
|
||
b = abs(c-d);
|
||
if (b > 1)
|
||
{
|
||
printf("sqrt(");
|
||
M_print(a);
|
||
printf(") = {(");
|
||
M_print(c);
|
||
printf(") != (");
|
||
M_print(d);
|
||
printf(")} \n");
|
||
//n--;
|
||
printf("%d != %d {", c, d);
|
||
M_print(b);
|
||
printf("}\n");
|
||
}
|
||
if (n <= 0)
|
||
exit(-1);
|
||
}
|
||
#endif
|
||
exit(0);
|
||
}
|
||
|
||
static void *cpu_cpy(void *dest, const void *src, size_t n)
|
||
{
|
||
return memcpy(dest, src, n);
|
||
}
|
||
|
||
void *(*M_Memcpy)(void* dest, const void* src, size_t n) = cpu_cpy;
|
||
|
||
void I_Error(const char *error, ...)
|
||
{
|
||
(void)error;
|
||
exit(-1);
|
||
}
|
||
#endif
|