mirror of
https://git.code.sf.net/p/quake/quakeforge
synced 2024-11-10 23:32:09 +00:00
7ae047654b
When I ported SEB to python, I discovered that I apparently didn't really understand the paper's description of the end condition and the usage of the affine and convex sets for center testing. This cleans up the test and makes SEB more correct for the cases that have less than 4 supporting points (especially when there are less than 4 points total).
1464 lines
33 KiB
C
1464 lines
33 KiB
C
/*
|
|
mathlib.c
|
|
|
|
math primitives
|
|
|
|
Copyright (C) 1996-1997 Id Software, Inc.
|
|
|
|
This program is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU General Public License
|
|
as published by the Free Software Foundation; either version 2
|
|
of the License, or (at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
|
|
See the GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program; if not, write to:
|
|
|
|
Free Software Foundation, Inc.
|
|
59 Temple Place - Suite 330
|
|
Boston, MA 02111-1307, USA
|
|
|
|
*/
|
|
#ifdef HAVE_CONFIG_H
|
|
# include "config.h"
|
|
#endif
|
|
|
|
#ifdef HAVE_STRING_H
|
|
# include <string.h>
|
|
#endif
|
|
#ifdef HAVE_STRINGS_H
|
|
# include <strings.h>
|
|
#endif
|
|
|
|
#include <math.h>
|
|
|
|
#define IMPLEMENT_R_Cull
|
|
#define IMPLEMENT_VectorNormalize
|
|
|
|
#include "QF/mathlib.h"
|
|
#include "QF/qtypes.h"
|
|
#include "QF/sys.h"
|
|
|
|
VISIBLE int nanmask = 255 << 23;
|
|
static plane_t _frustum[4];
|
|
VISIBLE plane_t *const frustum = _frustum;
|
|
static vec3_t _vec3_origin = { 0, 0, 0 };
|
|
VISIBLE const vec_t * const vec3_origin = _vec3_origin;
|
|
static quat_t _quat_origin = { 0, 0, 0, 0 };
|
|
VISIBLE const vec_t * const quat_origin = _quat_origin;
|
|
|
|
#define DEG2RAD(a) (a * (M_PI / 180.0))
|
|
|
|
#define FMANTBITS 23
|
|
#define FMANTMASK ((1 << FMANTBITS) - 1)
|
|
#define FEXPBITS 8
|
|
#define FEXPMASK ((1 << FEXPBITS) - 1)
|
|
#define FBIAS (1 << (FEXPBITS - 1))
|
|
#define FEXPMAX ((1 << FEXPBITS) - 1)
|
|
|
|
#define HMANTBITS 10
|
|
#define HMANTMASK ((1 << HMANTBITS) - 1)
|
|
#define HEXPBITS 5
|
|
#define HEXPMASK ((1 << HEXPBITS) - 1)
|
|
#define HBIAS (1 << (HEXPBITS - 1))
|
|
#define HEXPMAX ((1 << HEXPBITS) - 1)
|
|
|
|
int16_t
|
|
FloatToHalf (float x)
|
|
{
|
|
union {
|
|
float f;
|
|
uint32_t u;
|
|
} uf;
|
|
unsigned sign;
|
|
int exp;
|
|
unsigned mant;
|
|
int16_t half;
|
|
|
|
uf.f = x;
|
|
sign = (uf.u >> (FEXPBITS + FMANTBITS)) & 1;
|
|
exp = ((uf.u >> FMANTBITS) & FEXPMASK) - FBIAS + HBIAS;
|
|
mant = (uf.u & FMANTMASK) >> (FMANTBITS - HMANTBITS);
|
|
if (exp <= 0) {
|
|
mant |= 1 << HMANTBITS;
|
|
mant >>= min (1 - exp, HMANTBITS + 1);
|
|
exp = 0;
|
|
} else if (exp >= HEXPMAX) {
|
|
mant = 0;
|
|
exp = HEXPMAX;
|
|
}
|
|
half = (sign << (HEXPBITS + HMANTBITS)) | (exp << HMANTBITS) | mant;
|
|
return half;
|
|
}
|
|
|
|
float
|
|
HalfToFloat (int16_t x)
|
|
{
|
|
union {
|
|
float f;
|
|
uint32_t u;
|
|
} uf;
|
|
unsigned sign;
|
|
int exp;
|
|
unsigned mant;
|
|
|
|
sign = (x >> (HEXPBITS + HMANTBITS)) & 1;
|
|
exp = ((x >> HMANTBITS) & HEXPMASK);
|
|
mant = (x & HMANTMASK) << (FMANTBITS - HMANTBITS);
|
|
|
|
if (exp == 0) {
|
|
if (mant) {
|
|
while (mant < (1 << FMANTBITS)) {
|
|
mant <<= 1;
|
|
exp--;
|
|
}
|
|
mant &= (1 << FMANTBITS) - 1;
|
|
exp += FBIAS - HBIAS + 1;
|
|
}
|
|
} else if (exp == HEXPMAX) {
|
|
exp = FEXPMAX;
|
|
} else {
|
|
exp += FBIAS - HBIAS;
|
|
}
|
|
uf.u = (sign << (FEXPBITS + FMANTBITS)) | (exp << FMANTBITS) | mant;
|
|
return uf.f;
|
|
}
|
|
|
|
static void
|
|
ProjectPointOnPlane (vec3_t dst, const vec3_t p, const vec3_t normal)
|
|
{
|
|
float inv_denom, d;
|
|
vec3_t n;
|
|
|
|
inv_denom = 1.0F / DotProduct (normal, normal);
|
|
|
|
d = DotProduct (normal, p) * inv_denom;
|
|
|
|
VectorScale (normal, inv_denom * d, n);
|
|
|
|
VectorSubtract (p, n, dst);
|
|
}
|
|
|
|
// assumes "src" is normalized
|
|
static void
|
|
PerpendicularVector (vec3_t dst, const vec3_t src)
|
|
{
|
|
int pos, i;
|
|
float minelem = 1.0F;
|
|
vec3_t tempvec;
|
|
|
|
/* find the smallest magnitude axially aligned vector */
|
|
for (pos = 0, i = 0; i < 3; i++) {
|
|
if (fabs (src[i]) < minelem) {
|
|
pos = i;
|
|
minelem = fabs (src[i]);
|
|
}
|
|
}
|
|
VectorZero (tempvec);
|
|
tempvec[pos] = 1.0F;
|
|
|
|
/* project the point onto the plane defined by src */
|
|
ProjectPointOnPlane (dst, tempvec, src);
|
|
|
|
/* normalize the result */
|
|
VectorNormalize (dst);
|
|
}
|
|
|
|
#if defined(_WIN32) && !defined(__GNUC__)
|
|
# pragma optimize( "", off )
|
|
#endif
|
|
|
|
VISIBLE void
|
|
VectorVectors(const vec3_t forward, vec3_t right, vec3_t up)
|
|
{
|
|
float d;
|
|
|
|
right[0] = forward[2];
|
|
right[1] = -forward[0];
|
|
right[2] = forward[1];
|
|
|
|
d = DotProduct(forward, right);
|
|
VectorMultSub (right, d, forward, right);
|
|
VectorNormalize (right);
|
|
CrossProduct(right, forward, up);
|
|
}
|
|
|
|
VISIBLE void
|
|
RotatePointAroundVector (vec3_t dst, const vec3_t axis, const vec3_t point,
|
|
float degrees)
|
|
{
|
|
float m[3][3];
|
|
float im[3][3];
|
|
float zrot[3][3];
|
|
float tmpmat[3][3];
|
|
float rot[3][3];
|
|
int i;
|
|
vec3_t vr, vup, vf;
|
|
|
|
VectorCopy (axis, vf);
|
|
|
|
PerpendicularVector (vr, axis);
|
|
CrossProduct (vr, vf, vup);
|
|
|
|
m[0][0] = vr[0];
|
|
m[1][0] = vr[1];
|
|
m[2][0] = vr[2];
|
|
|
|
m[0][1] = vup[0];
|
|
m[1][1] = vup[1];
|
|
m[2][1] = vup[2];
|
|
|
|
m[0][2] = vf[0];
|
|
m[1][2] = vf[1];
|
|
m[2][2] = vf[2];
|
|
|
|
memcpy (im, m, sizeof (im));
|
|
|
|
im[0][1] = m[1][0];
|
|
im[0][2] = m[2][0];
|
|
im[1][0] = m[0][1];
|
|
im[1][2] = m[2][1];
|
|
im[2][0] = m[0][2];
|
|
im[2][1] = m[1][2];
|
|
|
|
memset (zrot, 0, sizeof (zrot));
|
|
zrot[0][0] = zrot[1][1] = zrot[2][2] = 1.0F;
|
|
|
|
zrot[0][0] = cos (DEG2RAD (degrees));
|
|
zrot[0][1] = sin (DEG2RAD (degrees));
|
|
zrot[1][0] = -sin (DEG2RAD (degrees));
|
|
zrot[1][1] = cos (DEG2RAD (degrees));
|
|
|
|
R_ConcatRotations (m, zrot, tmpmat);
|
|
R_ConcatRotations (tmpmat, im, rot);
|
|
|
|
for (i = 0; i < 3; i++) {
|
|
dst[i] = DotProduct (rot[i], point);
|
|
}
|
|
}
|
|
|
|
VISIBLE void
|
|
QuatMult (const quat_t q1, const quat_t q2, quat_t out)
|
|
{
|
|
vec_t s;
|
|
vec3_t v;
|
|
|
|
s = q1[3] * q2[3] - DotProduct (q1, q2);
|
|
CrossProduct (q1, q2, v);
|
|
VectorMultAdd (v, q1[3], q2, v);
|
|
VectorMultAdd (v, q2[3], q1, out);
|
|
out[3] = s;
|
|
}
|
|
|
|
VISIBLE void
|
|
QuatMultVec (const quat_t q, const vec3_t v, vec3_t out)
|
|
{
|
|
vec3_t tv;
|
|
vec_t dqv, dqq;
|
|
vec_t s;
|
|
|
|
s = q[3];
|
|
CrossProduct (q, v, tv);
|
|
dqv = DotProduct (q, v);
|
|
dqq = DotProduct (q, q);
|
|
VectorScale (tv, s, tv);
|
|
VectorMultAdd (tv, dqv, q, tv);
|
|
VectorAdd (tv, tv, tv);
|
|
VectorMultAdd (tv, s * s - dqq, v, out);
|
|
}
|
|
|
|
VISIBLE void
|
|
QuatInverse (const quat_t in, quat_t out)
|
|
{
|
|
quat_t q;
|
|
vec_t m;
|
|
|
|
m = QDotProduct (in, in); // in * in*
|
|
QuatConj (in, q);
|
|
QuatScale (q, 1 / m, out);
|
|
}
|
|
|
|
VISIBLE void
|
|
QuatExp (const quat_t a, quat_t b)
|
|
{
|
|
vec3_t n;
|
|
vec_t th;
|
|
vec_t r;
|
|
vec_t c, s;
|
|
|
|
VectorCopy (a, n);
|
|
th = VectorNormalize (n);
|
|
r = expf (a[3]);
|
|
c = cosf (th);
|
|
s = sinf (th);
|
|
VectorScale (n, r * s, b);
|
|
b[3] = r * c;
|
|
}
|
|
|
|
VISIBLE void
|
|
QuatToMatrix (const quat_t q, vec_t *m, int homogenous, int vertical)
|
|
{
|
|
vec_t xx, xy, xz, xw, yy, yz, yw, zz, zw, ww;
|
|
vec_t *_m[4] = {
|
|
m + (homogenous ? 0 : 0),
|
|
m + (homogenous ? 4 : 3),
|
|
m + (homogenous ? 8 : 6),
|
|
m + (homogenous ? 12 : 9),
|
|
};
|
|
|
|
xx = q[0] * q[0];
|
|
xy = q[0] * q[1];
|
|
xz = q[0] * q[2];
|
|
xw = q[0] * q[3];
|
|
|
|
yy = q[1] * q[1];
|
|
yz = q[1] * q[2];
|
|
yw = q[1] * q[3];
|
|
|
|
zz = q[2] * q[2];
|
|
zw = q[2] * q[3];
|
|
|
|
ww = q[3] * q[3];
|
|
|
|
if (vertical) {
|
|
VectorSet (ww + xx - yy - zz, 2 * (xy + zw), 2 * (xz - yw), _m[0]);
|
|
VectorSet (2 * (xy - zw), ww - xx + yy - zz, 2 * (yz + xw), _m[1]);
|
|
VectorSet (2 * (xz + yw), 2 * (yz - xw), ww - xx - yy + zz, _m[2]);
|
|
} else {
|
|
VectorSet (ww + xx - yy - zz, 2 * (xy - zw), 2 * (xz + yw), _m[0]);
|
|
VectorSet (2 * (xy + zw), ww - xx + yy - zz, 2 * (yz - xw), _m[1]);
|
|
VectorSet (2 * (xz - yw), 2 * (yz + xw), ww - xx - yy + zz, _m[2]);
|
|
}
|
|
if (homogenous) {
|
|
_m[0][3] = 0;
|
|
_m[1][3] = 0;
|
|
_m[2][3] = 0;
|
|
VectorZero (_m[3]);
|
|
_m[3][3] = 1;
|
|
}
|
|
}
|
|
|
|
#if defined(_WIN32) && !defined(__GNUC__)
|
|
# pragma optimize( "", on )
|
|
#endif
|
|
|
|
VISIBLE float
|
|
anglemod (float a)
|
|
{
|
|
a = (360.0 / 65536) * ((int) (a * (65536 / 360.0)) & 65535);
|
|
return a;
|
|
}
|
|
|
|
/*
|
|
BOPS_Error
|
|
|
|
Split out like this for ASM to call.
|
|
*/
|
|
void __attribute__ ((noreturn)) BOPS_Error (void);
|
|
VISIBLE void __attribute__ ((noreturn))
|
|
BOPS_Error (void)
|
|
{
|
|
Sys_Error ("BoxOnPlaneSide: Bad signbits");
|
|
}
|
|
|
|
#ifndef USE_INTEL_ASM
|
|
|
|
/*
|
|
BoxOnPlaneSide
|
|
|
|
Returns 1, 2, or 1 + 2
|
|
*/
|
|
VISIBLE int
|
|
BoxOnPlaneSide (const vec3_t emins, const vec3_t emaxs, plane_t *p)
|
|
{
|
|
float dist1, dist2;
|
|
int sides;
|
|
|
|
#if 0
|
|
// this is done by the BOX_ON_PLANE_SIDE macro before
|
|
// calling this function
|
|
|
|
// fast axial cases
|
|
if (p->type < 3) {
|
|
if (p->dist <= emins[p->type])
|
|
return 1;
|
|
if (p->dist >= emaxs[p->type])
|
|
return 2;
|
|
return 3;
|
|
}
|
|
#endif
|
|
|
|
// general case
|
|
switch (p->signbits) {
|
|
case 0:
|
|
dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] +
|
|
p->normal[2] * emaxs[2];
|
|
dist2 = p->normal[0] * emins[0] + p->normal[1] * emins[1] +
|
|
p->normal[2] * emins[2];
|
|
break;
|
|
case 1:
|
|
dist1 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] +
|
|
p->normal[2] * emaxs[2];
|
|
dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] +
|
|
p->normal[2] * emins[2];
|
|
break;
|
|
case 2:
|
|
dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] +
|
|
p->normal[2] * emaxs[2];
|
|
dist2 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] +
|
|
p->normal[2] * emins[2];
|
|
break;
|
|
case 3:
|
|
dist1 = p->normal[0] * emins[0] + p->normal[1] * emins[1] +
|
|
p->normal[2] * emaxs[2];
|
|
dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] +
|
|
p->normal[2] * emins[2];
|
|
break;
|
|
case 4:
|
|
dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] +
|
|
p->normal[2] * emins[2];
|
|
dist2 = p->normal[0] * emins[0] + p->normal[1] * emins[1] +
|
|
p->normal[2] * emaxs[2];
|
|
break;
|
|
case 5:
|
|
dist1 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] +
|
|
p->normal[2] * emins[2];
|
|
dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] +
|
|
p->normal[2] * emaxs[2];
|
|
break;
|
|
case 6:
|
|
dist1 = p->normal[0] * emaxs[0] + p->normal[1] * emins[1] +
|
|
p->normal[2] * emins[2];
|
|
dist2 = p->normal[0] * emins[0] + p->normal[1] * emaxs[1] +
|
|
p->normal[2] * emaxs[2];
|
|
break;
|
|
case 7:
|
|
dist1 = p->normal[0] * emins[0] + p->normal[1] * emins[1] +
|
|
p->normal[2] * emins[2];
|
|
dist2 = p->normal[0] * emaxs[0] + p->normal[1] * emaxs[1] +
|
|
p->normal[2] * emaxs[2];
|
|
break;
|
|
default:
|
|
BOPS_Error ();
|
|
}
|
|
|
|
#if 0
|
|
int i;
|
|
vec3_t corners[2];
|
|
|
|
for (i = 0; i < 3; i++) {
|
|
if (plane->normal[i] < 0) {
|
|
corners[0][i] = emins[i];
|
|
corners[1][i] = emaxs[i];
|
|
} else {
|
|
corners[1][i] = emins[i];
|
|
corners[0][i] = emaxs[i];
|
|
}
|
|
}
|
|
dist = DotProduct (plane->normal, corners[0]) - plane->dist;
|
|
dist2 = DotProduct (plane->normal, corners[1]) - plane->dist;
|
|
sides = 0;
|
|
if (dist1 >= 0)
|
|
sides = 1;
|
|
if (dist2 < 0)
|
|
sides |= 2;
|
|
#endif
|
|
|
|
sides = 0;
|
|
if (dist1 >= p->dist)
|
|
sides = 1;
|
|
if (dist2 < p->dist)
|
|
sides |= 2;
|
|
|
|
#ifdef PARANOID
|
|
if (sides == 0)
|
|
Sys_Error ("BoxOnPlaneSide: sides==0");
|
|
#endif
|
|
|
|
return sides;
|
|
}
|
|
#endif
|
|
|
|
/*
|
|
angles is a left(?) handed system: 'pitch yaw roll' with x (pitch) axis to
|
|
the right, y (yaw) axis up and z (roll) axis forward.
|
|
|
|
the math in AngleVectors has the entity frame as left handed with x
|
|
(forward) axis forward, y (right) axis to the right and z (up) up. However,
|
|
the world is a right handed system with x to the right, y forward and
|
|
z up.
|
|
|
|
pitch =
|
|
cp 0 -sp
|
|
0 1 0
|
|
sp 0 cp
|
|
|
|
yaw =
|
|
cy sy 0
|
|
-sy cy 0
|
|
0 0 1
|
|
|
|
roll =
|
|
1 0 0
|
|
0 cr sr
|
|
0 -sr cr
|
|
|
|
final = roll * (pitch * yaw)
|
|
final =
|
|
[forward]
|
|
[-right] -ve due to left handed to right handed conversion
|
|
[up]
|
|
*/
|
|
VISIBLE void
|
|
AngleVectors (const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
|
|
{
|
|
float angle, 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);
|
|
|
|
forward[0] = cp * cy;
|
|
forward[1] = cp * sy;
|
|
forward[2] = -sp;
|
|
// need to flip right because it's a left handed system in a right handed
|
|
// world
|
|
right[0] = -1 * (sr * sp * cy + cr * -sy);
|
|
right[1] = -1 * (sr * sp * sy + cr * cy);
|
|
right[2] = -1 * (sr * cp);
|
|
up[0] = (cr * sp * cy + -sr * -sy);
|
|
up[1] = (cr * sp * sy + -sr * cy);
|
|
up[2] = cr * cp;
|
|
}
|
|
|
|
VISIBLE void
|
|
AngleQuat (const vec3_t angles, quat_t q)
|
|
{
|
|
float alpha, sr, sp, sy, cr, cp, cy;
|
|
|
|
// alpha is half the angle
|
|
alpha = angles[YAW] * (M_PI / 360);
|
|
sy = sin (alpha);
|
|
cy = cos (alpha);
|
|
alpha = angles[PITCH] * (M_PI / 360);
|
|
sp = sin (alpha);
|
|
cp = cos (alpha);
|
|
alpha = angles[ROLL] * (M_PI / 360);
|
|
sr = sin (alpha);
|
|
cr = cos (alpha);
|
|
|
|
QuatSet (cy * cp * sr - sy * sp * cr, // x
|
|
cy * sp * cr + sy * cp * sr, // y
|
|
sy * cp * cr - cy * sp * sr, // z
|
|
cy * cp * cr + sy * sp * sr, // w
|
|
q);
|
|
}
|
|
|
|
VISIBLE int
|
|
_VectorCompare (const vec3_t v1, const vec3_t v2)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < 3; i++)
|
|
if (fabs (v1[i] - v2[i]) > EQUAL_EPSILON)
|
|
return 0;
|
|
|
|
return 1;
|
|
}
|
|
|
|
VISIBLE void
|
|
_VectorMA (const vec3_t veca, float scale, const vec3_t vecb, vec3_t vecc)
|
|
{
|
|
vecc[0] = veca[0] + scale * vecb[0];
|
|
vecc[1] = veca[1] + scale * vecb[1];
|
|
vecc[2] = veca[2] + scale * vecb[2];
|
|
}
|
|
|
|
VISIBLE vec_t
|
|
_DotProduct (const vec3_t v1, const vec3_t v2)
|
|
{
|
|
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
|
|
}
|
|
|
|
VISIBLE void
|
|
_VectorSubtract (const vec3_t veca, const vec3_t vecb, vec3_t out)
|
|
{
|
|
out[0] = veca[0] - vecb[0];
|
|
out[1] = veca[1] - vecb[1];
|
|
out[2] = veca[2] - vecb[2];
|
|
}
|
|
|
|
VISIBLE void
|
|
_VectorAdd (const vec3_t veca, const vec3_t vecb, vec3_t out)
|
|
{
|
|
out[0] = veca[0] + vecb[0];
|
|
out[1] = veca[1] + vecb[1];
|
|
out[2] = veca[2] + vecb[2];
|
|
}
|
|
|
|
VISIBLE void
|
|
_VectorCopy (const vec3_t in, vec3_t out)
|
|
{
|
|
out[0] = in[0];
|
|
out[1] = in[1];
|
|
out[2] = in[2];
|
|
}
|
|
|
|
VISIBLE void
|
|
CrossProduct (const vec3_t v1, const vec3_t v2, vec3_t cross)
|
|
{
|
|
float v10 = v1[0];
|
|
float v11 = v1[1];
|
|
float v12 = v1[2];
|
|
float v20 = v2[0];
|
|
float v21 = v2[1];
|
|
float v22 = v2[2];
|
|
|
|
cross[0] = v11 * v22 - v12 * v21;
|
|
cross[1] = v12 * v20 - v10 * v22;
|
|
cross[2] = v10 * v21 - v11 * v20;
|
|
}
|
|
|
|
VISIBLE vec_t
|
|
_VectorLength (const vec3_t v)
|
|
{
|
|
float length;
|
|
|
|
length = sqrt (DotProduct (v, v));
|
|
return length;
|
|
}
|
|
|
|
VISIBLE vec_t
|
|
_VectorNormalize (vec3_t v)
|
|
{
|
|
int i;
|
|
double length;
|
|
|
|
length = 0;
|
|
for (i = 0; i < 3; i++)
|
|
length += v[i] * v[i];
|
|
length = sqrt (length);
|
|
if (length == 0)
|
|
return 0;
|
|
|
|
for (i = 0; i < 3; i++)
|
|
v[i] /= length;
|
|
|
|
return length;
|
|
}
|
|
|
|
VISIBLE void
|
|
_VectorScale (const vec3_t in, vec_t scale, vec3_t out)
|
|
{
|
|
out[0] = in[0] * scale;
|
|
out[1] = in[1] * scale;
|
|
out[2] = in[2] * scale;
|
|
}
|
|
|
|
VISIBLE int
|
|
Q_log2 (int val)
|
|
{
|
|
int answer = 0;
|
|
|
|
while ((val >>= 1) != 0)
|
|
answer++;
|
|
return answer;
|
|
}
|
|
|
|
VISIBLE void
|
|
R_ConcatRotations (float in1[3][3], float in2[3][3], float out[3][3])
|
|
{
|
|
out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
|
|
in1[0][2] * in2[2][0];
|
|
out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
|
|
in1[0][2] * in2[2][1];
|
|
out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
|
|
in1[0][2] * in2[2][2];
|
|
out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
|
|
in1[1][2] * in2[2][0];
|
|
out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
|
|
in1[1][2] * in2[2][1];
|
|
out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
|
|
in1[1][2] * in2[2][2];
|
|
out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
|
|
in1[2][2] * in2[2][0];
|
|
out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
|
|
in1[2][2] * in2[2][1];
|
|
out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
|
|
in1[2][2] * in2[2][2];
|
|
}
|
|
|
|
VISIBLE void
|
|
R_ConcatTransforms (float in1[3][4], float in2[3][4], float out[3][4])
|
|
{
|
|
out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
|
|
in1[0][2] * in2[2][0];
|
|
out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
|
|
in1[0][2] * in2[2][1];
|
|
out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
|
|
in1[0][2] * in2[2][2];
|
|
out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
|
|
in1[0][2] * in2[2][3] + in1[0][3];
|
|
out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
|
|
in1[1][2] * in2[2][0];
|
|
out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
|
|
in1[1][2] * in2[2][1];
|
|
out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
|
|
in1[1][2] * in2[2][2];
|
|
out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
|
|
in1[1][2] * in2[2][3] + in1[1][3];
|
|
out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
|
|
in1[2][2] * in2[2][0];
|
|
out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
|
|
in1[2][2] * in2[2][1];
|
|
out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
|
|
in1[2][2] * in2[2][2];
|
|
out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
|
|
in1[2][2] * in2[2][3] + in1[2][3];
|
|
}
|
|
|
|
/*
|
|
FloorDivMod
|
|
|
|
Returns mathematically correct (floor-based) quotient and remainder for
|
|
numer and denom, both of which should contain no fractional part. The
|
|
quotient must fit in 32 bits.
|
|
*/
|
|
VISIBLE void
|
|
FloorDivMod (double numer, double denom, int *quotient, int *rem)
|
|
{
|
|
double x;
|
|
int q, r;
|
|
|
|
#ifndef PARANOID
|
|
if (denom <= 0.0)
|
|
Sys_Error ("FloorDivMod: bad denominator %f", denom);
|
|
|
|
// if ((floor(numer) != numer) || (floor(denom) != denom))
|
|
// Sys_Error ("FloorDivMod: non-integer numer or denom %f %f",
|
|
// numer, denom);
|
|
#endif
|
|
|
|
if (numer >= 0.0) {
|
|
x = floor (numer / denom);
|
|
q = (int) x;
|
|
r = (int) floor (numer - (x * denom));
|
|
} else {
|
|
// perform operations with positive values, and fix mod to make
|
|
// floor-based
|
|
x = floor (-numer / denom);
|
|
q = -(int) x;
|
|
r = (int) floor (-numer - (x * denom));
|
|
if (r != 0) {
|
|
q--;
|
|
r = (int) denom - r;
|
|
}
|
|
}
|
|
|
|
*quotient = q;
|
|
*rem = r;
|
|
}
|
|
|
|
VISIBLE int
|
|
GreatestCommonDivisor (int i1, int i2)
|
|
{
|
|
if (i1 > i2) {
|
|
if (i2 == 0)
|
|
return (i1);
|
|
return GreatestCommonDivisor (i2, i1 % i2);
|
|
} else {
|
|
if (i1 == 0)
|
|
return (i2);
|
|
return GreatestCommonDivisor (i1, i2 % i1);
|
|
}
|
|
}
|
|
|
|
#ifndef USE_INTEL_ASM
|
|
/*
|
|
Invert24To16
|
|
|
|
Inverts an 8.24 value to a 16.16 value
|
|
*/
|
|
VISIBLE fixed16_t
|
|
Invert24To16 (fixed16_t val)
|
|
{
|
|
if (val < 256)
|
|
return (0xFFFFFFFF);
|
|
|
|
return (fixed16_t)
|
|
(((double) 0x10000 * (double) 0x1000000 / (double) val) + 0.5);
|
|
}
|
|
#endif
|
|
|
|
void
|
|
Mat3Init (const quat_t rot, const vec3_t scale, mat3_t mat)
|
|
{
|
|
QuatToMatrix (rot, mat, 0, 1);
|
|
VectorScale (mat + 0, scale[0], mat + 0);
|
|
VectorScale (mat + 3, scale[1], mat + 3);
|
|
VectorScale (mat + 6, scale[2], mat + 6);
|
|
}
|
|
|
|
void
|
|
Mat3Transpose (const mat3_t a, mat3_t b)
|
|
{
|
|
vec_t t;
|
|
int i, j;
|
|
|
|
for (i = 0; i < 2; i++) {
|
|
b[i * 3 + i] = a[i * 3 + i]; // in case b != a
|
|
for (j = i + 1; j < 3; j++) {
|
|
t = a[i * 3 + j]; // in case b == a
|
|
b[i * 3 + j] = a[j * 3 + i];
|
|
b[j * 3 + i] = t;
|
|
}
|
|
}
|
|
b[i * 3 + i] = a[i * 3 + i]; // in case b != a
|
|
}
|
|
|
|
vec_t
|
|
Mat3Determinant (const mat3_t m)
|
|
{
|
|
vec3_t t;
|
|
CrossProduct (m + 3, m + 6, t);
|
|
return DotProduct (m + 0, t);
|
|
}
|
|
|
|
typedef vec_t mat2_t[2 * 2];
|
|
|
|
static void
|
|
Mat3Sub2 (const mat3_t m3, mat2_t m2, int i, int j)
|
|
{
|
|
int si, sj, di, dj;
|
|
|
|
for (di = 0; di < 2; di++) {
|
|
for (dj = 0; dj < 2; dj++) {
|
|
si = di + ((di >= i) ? 1 : 0);
|
|
sj = dj + ((dj >= j) ? 1 : 0);
|
|
m2[di * 2 + dj] = m3[si * 3 + sj];
|
|
}
|
|
}
|
|
}
|
|
|
|
static vec_t
|
|
Mat2Det (const mat2_t m)
|
|
{
|
|
return m[0] * m[3] - m[1] * m[2];
|
|
}
|
|
|
|
int
|
|
Mat3Inverse (const mat3_t a, mat3_t b)
|
|
{
|
|
mat3_t tb;
|
|
mat2_t m2;
|
|
vec_t *m = b;
|
|
int i, j;
|
|
vec_t det;
|
|
vec_t sign[2] = { 1, -1};
|
|
|
|
det = Mat3Determinant (a);
|
|
if (det * det < 1e-6) {
|
|
Mat3Identity (b);
|
|
return 0;
|
|
}
|
|
if (b == a)
|
|
m = tb;
|
|
for (i = 0; i < 3; i++) {
|
|
for (j = 0; j < 3; j++) {
|
|
Mat3Sub2 (a, m2, i, j);
|
|
m[j * 3 + i] = sign[(i + j) & 1] * Mat2Det (m2) / det;
|
|
}
|
|
}
|
|
if (m != b)
|
|
Mat3Copy (m, b);
|
|
return 1;
|
|
}
|
|
|
|
void Mat3Mult (const mat3_t a, const mat3_t b, mat3_t c)
|
|
{
|
|
mat3_t ta, tb; // in case c == b or c == a
|
|
int i, j, k;
|
|
|
|
Mat3Transpose (a, ta); // transpose so we can use dot
|
|
Mat3Copy (b, tb);
|
|
|
|
k = 0;
|
|
for (i = 0; i < 3; i++) {
|
|
for (j = 0; j < 3; j++) {
|
|
c[k++] = DotProduct (ta + 3 * j, tb + 3 * i);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Mat3MultVec (const mat3_t a, const vec3_t b, vec3_t c)
|
|
{
|
|
int i;
|
|
vec3_t tb;
|
|
|
|
VectorCopy (b, tb);
|
|
for (i = 0; i < 3; i++)
|
|
c[i] = a[i + 0] * tb[0] + a[i + 3] * b[1] + a[i + 6] * b[2];
|
|
}
|
|
|
|
#define sqr(x) ((x) * (x))
|
|
void Mat3SymEigen (const mat3_t m, vec3_t e)
|
|
{
|
|
vec_t p, q, r;
|
|
vec_t phi;
|
|
mat3_t B;
|
|
|
|
p = sqr (m[1]) + sqr (m[2]) + sqr (m[5]);
|
|
if (p < 1e-6) {
|
|
e[0] = m[0];
|
|
e[1] = m[4];
|
|
e[2] = m[8];
|
|
return;
|
|
}
|
|
q = Mat3Trace (m) / 3;
|
|
p = sqr (m[0] - q) + sqr (m[4] - q) + sqr (m[8] - q) + 2 * p;
|
|
p = sqrt (p);
|
|
Mat3Zero (B);
|
|
B[0] = B[4] = B[8] = q;
|
|
Mat3Subtract (m, B, B);
|
|
Mat3Scale (B, 1.0 / p, B);
|
|
r = Mat3Determinant (B) / 2;
|
|
if (r >= 1)
|
|
phi = 0;
|
|
else if (r <= -1)
|
|
phi = M_PI / 3;
|
|
else
|
|
phi = acos (r) / 3;
|
|
|
|
e[0] = q + 2 * p * cos (phi);
|
|
e[2] = q + 2 * p * cos (phi + M_PI * 2 / 3);
|
|
e[1] = 3 * q - e[0] - e[2];
|
|
}
|
|
|
|
void
|
|
Mat4Init (const quat_t rot, const vec3_t scale, const vec3_t trans, mat4_t mat)
|
|
{
|
|
QuatToMatrix (rot, mat, 1, 1);
|
|
VectorScale (mat + 0, scale[0], mat + 0);
|
|
VectorScale (mat + 4, scale[1], mat + 4);
|
|
VectorScale (mat + 8, scale[2], mat + 8);
|
|
VectorCopy (trans, mat + 12);
|
|
}
|
|
|
|
void
|
|
Mat4Transpose (const mat4_t a, mat4_t b)
|
|
{
|
|
vec_t t;
|
|
int i, j;
|
|
|
|
for (i = 0; i < 3; i++) {
|
|
b[i * 4 + i] = a[i * 4 + i]; // in case b != a
|
|
for (j = i + 1; j < 4; j++) {
|
|
t = a[i * 4 + j]; // in case b == a
|
|
b[i * 4 + j] = a[j * 4 + i];
|
|
b[j * 4 + i] = t;
|
|
}
|
|
}
|
|
b[i * 4 + i] = a[i * 4 + i]; // in case b != a
|
|
}
|
|
|
|
static void
|
|
Mat4Sub3 (const mat4_t m4, mat3_t m3, int i, int j)
|
|
{
|
|
int si, sj, di, dj;
|
|
|
|
for (di = 0; di < 3; di++) {
|
|
for (dj = 0; dj < 3; dj++) {
|
|
si = di + ((di >= i) ? 1 : 0);
|
|
sj = dj + ((dj >= j) ? 1 : 0);
|
|
m3[di * 3 + dj] = m4[si * 4 + sj];
|
|
}
|
|
}
|
|
}
|
|
|
|
static vec_t
|
|
Mat4Det (const mat4_t m)
|
|
{
|
|
mat3_t t;
|
|
int i;
|
|
vec_t res = 0, det, s = 1;
|
|
|
|
for (i = 0; i < 4; i++, s = -s) {
|
|
Mat4Sub3 (m, t, 0, i);
|
|
det = Mat3Determinant (t);
|
|
res += m[i] * det * s;
|
|
}
|
|
return res;
|
|
}
|
|
|
|
int
|
|
Mat4Inverse (const mat4_t a, mat4_t b)
|
|
{
|
|
mat4_t tb;
|
|
mat3_t m3;
|
|
vec_t *m = b;
|
|
int i, j;
|
|
vec_t det;
|
|
vec_t sign[2] = { 1, -1};
|
|
|
|
det = Mat4Det (a);
|
|
if (det * det < 1e-6) {
|
|
Mat4Identity (b);
|
|
return 0;
|
|
}
|
|
if (b == a)
|
|
m = tb;
|
|
for (i = 0; i < 4; i++) {
|
|
for (j = 0; j < 4; j++) {
|
|
Mat4Sub3 (a, m3, i, j);
|
|
m[j * 4 + i] = sign[(i + j) & 1] * Mat3Determinant (m3) / det;
|
|
}
|
|
}
|
|
if (m != b)
|
|
Mat4Copy (m, b);
|
|
return 1;
|
|
}
|
|
|
|
void
|
|
Mat4Mult (const mat4_t a, const mat4_t b, mat4_t c)
|
|
{
|
|
mat4_t ta, tb; // in case c == b or c == a
|
|
int i, j, k;
|
|
|
|
Mat4Transpose (a, ta); // transpose so we can use dot
|
|
Mat4Copy (b, tb);
|
|
|
|
k = 0;
|
|
for (i = 0; i < 4; i++) {
|
|
for (j = 0; j < 4; j++) {
|
|
c[k++] = QDotProduct (ta + 4 * j, tb + 4 * i);
|
|
}
|
|
}
|
|
}
|
|
|
|
void
|
|
Mat4MultVec (const mat4_t a, const vec3_t b, vec3_t c)
|
|
{
|
|
int i;
|
|
vec3_t tb;
|
|
|
|
VectorCopy (b, tb);
|
|
for (i = 0; i < 3; i++)
|
|
c[i] = a[i + 0] * tb[0] + a[i + 4] * b[1] + a[i + 8] * b[2] + a[i +12];
|
|
}
|
|
|
|
void
|
|
Mat4as3MultVec (const mat4_t a, const vec3_t b, vec3_t c)
|
|
{
|
|
int i;
|
|
vec3_t tb;
|
|
|
|
VectorCopy (b, tb);
|
|
for (i = 0; i < 3; i++)
|
|
c[i] = a[i + 0] * tb[0] + a[i + 4] * b[1] + a[i + 8] * b[2];
|
|
}
|
|
|
|
int
|
|
Mat3Decompose (const mat3_t mat, quat_t rot, vec3_t shear, vec3_t scale)
|
|
{
|
|
vec3_t row[3], shr, scl;
|
|
vec_t l, t;
|
|
int i, j;
|
|
|
|
for (i = 0; i < 3; i++)
|
|
for (j = 0; j < 3; j++)
|
|
row[j][i] = mat[i * 3 + j];
|
|
|
|
l = DotProduct (row[0], row[0]);
|
|
if (l < 1e-5)
|
|
return 0;
|
|
scl[0] = sqrt (l);
|
|
VectorScale (row[0], 1/scl[0], row[0]);
|
|
shr[0] = DotProduct (row[0], row[1]);
|
|
|
|
VectorMultSub (row[1], shr[0], row[0], row[1]);
|
|
l = DotProduct (row[1], row[1]);
|
|
if (l < 1e-5)
|
|
return 0;
|
|
scl[1] = sqrt (l);
|
|
shr[0] /= scl[1];
|
|
VectorScale (row[1], 1/scl[1], row[1]);
|
|
shr[1] = DotProduct (row[0], row[2]);
|
|
|
|
VectorMultSub (row[2], shr[1], row[0], row[2]);
|
|
shr[2] = DotProduct (row[1], row[2]);
|
|
VectorMultSub (row[2], shr[2], row[1], row[2]);
|
|
l = DotProduct (row[2], row[2]);
|
|
if (l < 1e-5)
|
|
return 0;
|
|
scl[2] = sqrt (l);
|
|
shr[1] /= scl[2];
|
|
shr[2] /= scl[2];
|
|
VectorScale (row[2], 1/scl[2], row[2]);
|
|
if (scale)
|
|
VectorCopy (scl, scale);
|
|
if (shear)
|
|
VectorCopy (shr, shear);
|
|
if (!rot)
|
|
return 1;
|
|
|
|
t = 1 + row[0][0] + row[1][1] + row[2][2];
|
|
if (t >= 1e-5) {
|
|
vec_t s = sqrt (t) * 2;
|
|
rot[0] = (row[2][1] - row[1][2]) / s;
|
|
rot[1] = (row[0][2] - row[2][0]) / s;
|
|
rot[2] = (row[1][0] - row[0][1]) / s;
|
|
rot[3] = s / 4;
|
|
} else {
|
|
if (row[0][0] > row[1][1] && row[0][0] > row[2][2]) {
|
|
vec_t s = sqrt (1 + row[0][0] - row[1][1] - row[2][2]) * 2;
|
|
rot[0] = s / 4;
|
|
rot[1] = (row[1][0] + row[0][1]) / s;
|
|
rot[2] = (row[0][2] + row[2][0]) / s;
|
|
rot[3] = (row[2][1] - row[1][2]) / s;
|
|
} else if (row[1][1] > row[2][2]) {
|
|
vec_t s = sqrt (1 + row[1][1] - row[0][0] - row[2][2]) * 2;
|
|
rot[0] = (row[1][0] + row[0][1]) / s;
|
|
rot[1] = s / 4;
|
|
rot[2] = (row[2][1] + row[1][2]) / s;
|
|
rot[3] = (row[0][2] - row[2][0]) / s;
|
|
} else {
|
|
vec_t s = sqrt (1 + row[2][2] - row[0][0] - row[1][1]) * 2;
|
|
rot[0] = (row[0][2] + row[2][0]) / s;
|
|
rot[1] = (row[2][1] + row[1][2]) / s;
|
|
rot[2] = s / 4;
|
|
rot[3] = (row[1][0] - row[0][1]) / s;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
int
|
|
Mat4Decompose (const mat4_t mat, quat_t rot, vec3_t shear, vec3_t scale,
|
|
vec3_t trans)
|
|
{
|
|
mat3_t m3;
|
|
|
|
if (trans)
|
|
VectorCopy (mat + 12, trans);
|
|
Mat4toMat3 (mat, m3);
|
|
return Mat3Decompose (m3, rot, shear, scale);
|
|
}
|
|
|
|
void
|
|
BarycentricCoords (const vec_t **points, int num_points, const vec3_t p,
|
|
vec_t *lambda)
|
|
{
|
|
vec3_t a, b, c, x, ab, bc, ca, n;
|
|
vec_t div;
|
|
if (num_points > 4)
|
|
Sys_Error ("Don't know how to compute the barycentric coordinates "
|
|
"for %d points", num_points);
|
|
switch (num_points) {
|
|
case 1:
|
|
lambda[0] = 1;
|
|
return;
|
|
case 2:
|
|
VectorSubtract (p, points[0], x);
|
|
VectorSubtract (points[1], points[0], a);
|
|
lambda[1] = DotProduct (x, a) / DotProduct (a, a);
|
|
lambda[0] = 1 - lambda[1];
|
|
return;
|
|
case 3:
|
|
VectorSubtract (p, points[0], x);
|
|
VectorSubtract (points[1], points[0], a);
|
|
VectorSubtract (points[2], points[0], b);
|
|
CrossProduct (a, b, ab);
|
|
div = DotProduct (ab, ab);
|
|
CrossProduct (x, b, n);
|
|
lambda[1] = DotProduct (n, ab) / div;
|
|
CrossProduct (a, x, n);
|
|
lambda[2] = DotProduct (n, ab) / div;
|
|
lambda[0] = 1 - lambda[1] - lambda[2];
|
|
return;
|
|
case 4:
|
|
VectorSubtract (p, points[0], x);
|
|
VectorSubtract (points[1], points[0], a);
|
|
VectorSubtract (points[2], points[0], b);
|
|
VectorSubtract (points[3], points[0], c);
|
|
CrossProduct (a, b, ab);
|
|
CrossProduct (b, c, bc);
|
|
CrossProduct (c, a, ca);
|
|
div = DotProduct (a, bc);
|
|
lambda[1] = DotProduct (x, bc) / div;
|
|
lambda[2] = DotProduct (x, ca) / div;
|
|
lambda[3] = DotProduct (x, ab) / div;
|
|
lambda[0] = 1 - lambda[1] - lambda[2] - lambda[3];
|
|
return;
|
|
}
|
|
Sys_Error ("Not enough points to project or enclose the point");
|
|
}
|
|
|
|
static int
|
|
circum_circle (const vec_t **points, int num_points, sphere_t *sphere)
|
|
{
|
|
vec3_t a, c, b;
|
|
vec3_t bc, ca, ab;
|
|
vec_t aa, bb, cc;
|
|
vec_t div;
|
|
vec_t alpha, beta, gamma;
|
|
|
|
switch (num_points) {
|
|
case 1:
|
|
VectorCopy (points[0], sphere->center);
|
|
return 1;
|
|
case 2:
|
|
VectorBlend (points[0], points[1], 0.5, sphere->center);
|
|
return 1;
|
|
case 3:
|
|
VectorSubtract (points[0], points[1], a);
|
|
VectorSubtract (points[0], points[2], b);
|
|
VectorSubtract (points[1], points[2], c);
|
|
aa = DotProduct (a, a);
|
|
bb = DotProduct (b, b);
|
|
cc = DotProduct (c, c);
|
|
div = DotProduct (a, c);
|
|
div = 2 * (aa * cc - div * div);
|
|
if (fabs (div) < EQUAL_EPSILON) {
|
|
// degenerate
|
|
return 0;
|
|
}
|
|
alpha = cc * DotProduct (a, b) / div;
|
|
beta = -bb * DotProduct (a, c) / div;
|
|
gamma = aa * DotProduct (b, c) / div;
|
|
VectorScale (points[0], alpha, sphere->center);
|
|
VectorMultAdd (sphere->center, beta, points[1], sphere->center);
|
|
VectorMultAdd (sphere->center, gamma, points[2], sphere->center);
|
|
return 1;
|
|
case 4:
|
|
VectorSubtract (points[1], points[0], a);
|
|
VectorSubtract (points[2], points[0], b);
|
|
VectorSubtract (points[3], points[0], c);
|
|
CrossProduct (b, c, bc);
|
|
CrossProduct (c, a, ca);
|
|
CrossProduct (a, b, ab);
|
|
div = 2 * DotProduct (a, bc);
|
|
if (fabs (div) < EQUAL_EPSILON) {
|
|
// degenerate
|
|
return 0;
|
|
}
|
|
aa = DotProduct (a, a) / div;
|
|
bb = DotProduct (b, b) / div;
|
|
cc = DotProduct (c, c) / div;
|
|
VectorScale (bc, aa, sphere->center);
|
|
VectorMultAdd (sphere->center, bb, ca, sphere->center);
|
|
VectorMultAdd (sphere->center, cc, ab, sphere->center);
|
|
VectorAdd (sphere->center, points[0], sphere->center);
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
CircumSphere (const vec3_t points[], int num_points, sphere_t *sphere)
|
|
{
|
|
const vec_t *p[] = {points[0], points[1], points[2], points[3]};
|
|
|
|
if (num_points > 4)
|
|
return 0;
|
|
sphere->radius = 0;
|
|
if (num_points) {
|
|
if (circum_circle (p, num_points, sphere)) {
|
|
if (num_points > 1)
|
|
sphere->radius = VectorDistance (sphere->center, points[0]);
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
VectorZero (sphere->center);
|
|
return 1;
|
|
}
|
|
|
|
static void
|
|
closest_affine_point (const vec_t **points, int num_points, const vec3_t x,
|
|
vec3_t closest)
|
|
{
|
|
vec3_t a, b, n, d;
|
|
vec_t l;
|
|
|
|
switch (num_points) {
|
|
default:
|
|
case 1:
|
|
VectorCopy (points[0], closest);
|
|
break;
|
|
case 2:
|
|
VectorSubtract (points[1], points[0], n);
|
|
VectorSubtract (x, points[0], d);
|
|
l = DotProduct (d, n) / DotProduct (n, n);
|
|
VectorMultAdd (points[0], l, n, closest);
|
|
break;
|
|
case 3:
|
|
VectorSubtract (points[1], points[0], a);
|
|
VectorSubtract (points[2], points[0], b);
|
|
CrossProduct (a, b, n);
|
|
VectorSubtract (points[0], x, d);
|
|
l = DotProduct (d, n) / DotProduct (n, n);
|
|
VectorMultAdd (x, l, n, closest);
|
|
break;
|
|
}
|
|
}
|
|
|
|
static int
|
|
test_support_points(const vec_t **points, int *num_points, const vec3_t center)
|
|
{
|
|
int in_affine = 0;
|
|
int in_convex = 0;
|
|
vec3_t v, d, n, a, b;
|
|
vec_t nn, dd, vv, dn;
|
|
|
|
switch (*num_points) {
|
|
case 1:
|
|
in_affine = VectorCompare (points[0], center);
|
|
// the convex hull and affine hull for a single point are the same
|
|
in_convex = in_affine;
|
|
break;
|
|
case 2:
|
|
VectorSubtract (points[1], points[0], v);
|
|
VectorSubtract (center, points[0], d);
|
|
CrossProduct (v, d, n);
|
|
nn = DotProduct (n, n);
|
|
dd = DotProduct (d, d);
|
|
vv = DotProduct (v, v);
|
|
in_affine = nn < 1e-8 * vv * dd;
|
|
break;
|
|
case 3:
|
|
VectorSubtract (points[1], points[0], a);
|
|
VectorSubtract (points[2], points[0], b);
|
|
VectorSubtract (center, points[0], d);
|
|
CrossProduct (a, b, n);
|
|
dn = DotProduct (d, n);
|
|
dd = DotProduct (d, d);
|
|
nn = DotProduct (n, n);
|
|
in_affine = dn * dn < 1e-8 * dd * nn;
|
|
break;
|
|
case 4:
|
|
in_affine = 1;
|
|
break;
|
|
default:
|
|
Sys_Error ("Invalid number of points (%d) in test_support_points",
|
|
*num_points);
|
|
}
|
|
|
|
// if in_convex is not true while in_affine is, then need to test as
|
|
// there is more than one dimension for the affine hull (a single support
|
|
// point is never dropped as it cannot be redundant)
|
|
if (in_affine && !in_convex) {
|
|
vec_t lambda[4];
|
|
int dropped = 0;
|
|
int count = *num_points;
|
|
|
|
BarycentricCoords (points, count, center, lambda);
|
|
|
|
for (int i = 0; i < count; i++) {
|
|
points[i - dropped] = points[i];
|
|
if (lambda[i] < -1e-4) {
|
|
dropped++;
|
|
(*num_points)--;
|
|
}
|
|
}
|
|
in_convex = !dropped;
|
|
if (dropped) {
|
|
for (int i = count - dropped; i < count; i++) {
|
|
points[i] = 0;
|
|
}
|
|
}
|
|
}
|
|
return in_convex;
|
|
}
|
|
|
|
sphere_t
|
|
SmallestEnclosingBall (const vec3_t points[], int num_points)
|
|
{
|
|
sphere_t sphere;
|
|
const vec_t *best;
|
|
const vec_t *support[4];
|
|
int num_support;
|
|
vec_t dist, best_dist;
|
|
int i;
|
|
int iters = 0;
|
|
|
|
if (num_points < 1) {
|
|
VectorZero (sphere.center);
|
|
sphere.radius = 0;
|
|
return sphere;
|
|
}
|
|
|
|
for (i = 0; i < 4; i++)
|
|
support[i] = 0;
|
|
|
|
VectorCopy (points[0], sphere.center);
|
|
best_dist = dist = 0;
|
|
best = points[0];
|
|
for (i = 1; i < num_points; i++) {
|
|
dist = VectorDistance_fast (points[i], sphere.center);
|
|
if (dist > best_dist) {
|
|
best_dist = dist;
|
|
best = points[i];
|
|
}
|
|
}
|
|
num_support = 1;
|
|
support[0] = best;
|
|
sphere.radius = best_dist; // note: radius squared until the end
|
|
|
|
while (!test_support_points (support, &num_support, sphere.center)) {
|
|
vec3_t affine;
|
|
vec3_t center_to_affine, center_to_point;
|
|
vec_t affine_dist, point_proj, point_dist, bound;
|
|
vec_t scale = 1;
|
|
int i;
|
|
|
|
if (iters++ > 10)
|
|
Sys_Error ("stuck SEB");
|
|
best = 0;
|
|
|
|
closest_affine_point (support, num_support, sphere.center, affine);
|
|
VectorSubtract (affine, sphere.center, center_to_affine);
|
|
affine_dist = DotProduct (center_to_affine, center_to_affine);
|
|
for (i = 0; i < num_points; i++) {
|
|
if (points[i] == support[0] || points[i] == support[1]
|
|
|| points[i] == support[2])
|
|
continue;
|
|
VectorSubtract (points[i], sphere.center, center_to_point);
|
|
point_proj = DotProduct (center_to_affine, center_to_point);
|
|
if (affine_dist - point_proj <= 0
|
|
|| ((affine_dist - point_proj) * (affine_dist - point_proj)
|
|
< 1e-8 * sphere.radius * affine_dist))
|
|
continue;
|
|
point_dist = DotProduct (center_to_point, center_to_point);
|
|
bound = sphere.radius - point_dist;
|
|
bound /= 2 * (affine_dist - point_proj);
|
|
if (bound < scale) {
|
|
best = points[i];
|
|
scale = bound;
|
|
}
|
|
}
|
|
VectorMultAdd (sphere.center, scale, center_to_affine, sphere.center);
|
|
sphere.radius = VectorDistance_fast (sphere.center, support[0]);
|
|
if (best) {
|
|
support[num_support++] = best;
|
|
}
|
|
}
|
|
best_dist = 0;
|
|
for (i = 0; i < num_points; i++) {
|
|
dist = VectorDistance_fast (sphere.center, points[i]);
|
|
if (dist > best_dist)
|
|
best_dist = dist;
|
|
}
|
|
sphere.radius = sqrt (best_dist);
|
|
return sphere;
|
|
}
|