qf
/
quakeforge
mirror of https://git.code.sf.net/p/quake/quakeforge
0
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
quakeforge/libs/util/mathlib.c

1495 lines
33 KiB
C
Raw Blame History

/*
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 "qfalloca.h"
#include <math.h>
#define IMPLEMENT_R_Cull
#define IMPLEMENT_VectorNormalize
#include "QF/mathlib.h"
#include "QF/qtypes.h"
#include "QF/set.h"
#include "QF/sys.h"
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);
VectorScale (tv, 2, tv);
VectorMultAdd (tv, s * s - dqq, v, out);
}
VISIBLE void
QuatRotation(const vec3_t a, const vec3_t b, quat_t out)
{
vec_t ma, mb;
vec_t den, mba_mab;
vec3_t t;
ma = VectorLength(a);
mb = VectorLength(b);
den = 2 * ma * mb;
VectorScale (a, mb, t);
VectorMultAdd(t, ma, b, t);
mba_mab = VectorLength(t);
CrossProduct (a, b, t);
VectorScale(t, 1 / mba_mab, out);
out[3] = mba_mab / den;
}
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;
vec_t *_m[4] = {
m + (homogenous ? 0 : 0),
m + (homogenous ? 4 : 3),
m + (homogenous ? 8 : 6),
m + (homogenous ? 12 : 9),
};
xx = 2 * q[0] * q[0];
xy = 2 * q[0] * q[1];
xz = 2 * q[0] * q[2];
xw = 2 * q[0] * q[3];
yy = 2 * q[1] * q[1];
yz = 2 * q[1] * q[2];
yw = 2 * q[1] * q[3];
zz = 2 * q[2] * q[2];
zw = 2 * q[2] * q[3];
if (vertical) {
VectorSet (1.0f - yy - zz, xy + zw, xz - yw, _m[0]);
VectorSet (xy - zw, 1.0f - xx - zz, yz + xw, _m[1]);
VectorSet (xz + yw, yz - xw, 1.0f - xx - yy, _m[2]);
} else {
VectorSet (1.0f - yy - zz, xy - zw, xz + yw, _m[0]);
VectorSet (xy + zw, 1.0f - xx - zz, yz - xw, _m[1]);
VectorSet (xz - yw, yz + xw, 1.0f - xx - yy, _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, const 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. However, the
rotations themselves are right-handed in that they follow the right-hand
rule for the world axes: pitch around +y, yaw around +z, and roll around
+x.
This results in the entity frame having forward pointed along the world +x
axis, right along the world -y axis, and up along the world +z axis.
Whether this means the entity frame is left-handed depends on whether
forward is local X and right is local Y (left handed), or forward is local
Y and right is local X (right handed).
NOTE: these matrices have forward, left and up vectors horizontal rather
than vertical and are thus the inverse of the matrices to produce the
actual rotation.
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 the trig produces +Y but right is -Y
right[0] = -(sr * sp * cy + cr * -sy);
right[1] = -(sr * sp * sy + cr * cy);
right[2] = -(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);
CrossProduct (a, x, n);
lambda[2] = DotProduct (n, ab);
lambda[0] = div - lambda[1] - lambda[2];
VectorScale (lambda, 1 / div, lambda);
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);
VectorAdd (points[0], points[1], d);
VectorScale (d, 0.5, d);
VectorSubtract (center, d, d);
CrossProduct (v, d, n);
nn = DotProduct (n, n);
vv = DotProduct (v, v);
in_affine = nn < 1e-5 * vv * vv;
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-5 * 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)
{
set_t was_support = SET_STATIC_INIT (num_points, alloca);
sphere_t sphere;
const vec_t *best;
const vec_t *support[4];
int num_support;
vec_t dist, best_dist;
int i;
int best_i = 0;
int iters = 0;
if (num_points < 1) {
VectorZero (sphere.center);
sphere.radius = 0;
return sphere;
}
for (i = 0; i < 4; i++)
support[i] = 0;
set_empty (&was_support);
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_i = i;
best = points[i];
}
}
num_support = 1;
support[0] = best;
sphere.radius = best_dist; // note: radius squared until the end
set_add (&was_support, best_i);
while (!test_support_points (support, &num_support, sphere.center)) {
vec3_t affine, v, p, r;
vec_t x, best_x = 0, rr, pv, pp;
int i;
if (iters++ > 2 * num_points)
Sys_Error ("stuck SEB");
closest_affine_point (support, num_support, sphere.center, affine);
VectorSubtract (support[0], affine, r);
rr = DotProduct (r, r);
VectorSubtract (sphere.center, affine, v);
best = 0;
for (i = 0; i < num_points; i++) {
if (SET_TEST_MEMBER (&was_support, i)) {
continue;
}
VectorSubtract (points[i], affine, p);
pp = DotProduct (p, p);
pv = DotProduct (p, v);
if (pp <= rr || pv <= 0 || pv * pv < 1e-6 * rr) {
continue;
}
x = (pp - rr) / (2 * pv);
if (x > best_x) {
best = points[i];
best_i = i;
best_x = x;
}
}
VectorMultAdd (affine, best_x, v, sphere.center);
sphere.radius = VectorDistance_fast (sphere.center, support[0]);
if (best) {
support[num_support++] = best;
set_add (&was_support, best_i);
}
}
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;
}
Reference in New Issue View Git Blame Copy Permalink