Add a function to find the circumsphere of up to 4 points.

It seems to be a little sloppy (an error of a bit less than 1e-4). This
might be why I'm having trouble with my SEB code.

include/QF/mathlib.h

@@ -213,6 +213,7 @@ R_CullSphere (const vec3_t origin, const float radius)
 	return false;
 }
 
+int CircumSphere (const vec3_t points[], int num_points, sphere_t *sphere);
 void BarycentricCoords (const vec_t **points, int num_points, const vec3_t p,
 		                vec_t *lambda);
 

include/QF/qtypes.h

@@ -110,4 +110,9 @@ typedef struct plane_s {
 	byte    pad[2];
 } plane_t;
 
+typedef struct sphere_s {
+	vec3_t      center;
+	vec_t       radius;
+} sphere_t;
+
 #endif // __qtypes_h

libs/util/mathlib.c

@@ -1200,3 +1200,84 @@ BarycentricCoords (const vec_t **points, int num_points, const vec3_t p,
 	}
 	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);
+			sphere->radius = VectorDistance (sphere->center, points[0]);
+			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;
+}

libs/util/test/Makefile.am

@@ -3,13 +3,17 @@ AUTOMAKE_OPTIONS= foreign
 INCLUDES= -I$(top_srcdir)/include
 
 check_PROGRAMS= \
-	test-bary test-dq test-half test-mat3 test-mat4 test-plist test-qfs \
+	test-bary test-cs test-dq test-half test-mat3 test-mat4 \
-	test-quat test-set test-vrect
+	test-plist test-qfs test-quat test-set test-vrect
 
 test_bary_SOURCES=test-bary.c
 test_bary_LDADD=$(top_builddir)/libs/util/libQFutil.la
 test_bary_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la
 
+test_cs_SOURCES=test-cs.c
+test_cs_LDADD=$(top_builddir)/libs/util/libQFutil.la
+test_cs_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la
+
 test_dq_SOURCES=test-dq.c
 test_dq_LDADD=$(top_builddir)/libs/util/libQFutil.la
 test_dq_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la

libs/util/test/test-cs.c

@@ -0,0 +1,102 @@
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
+
+#include "QF/mathlib.h"
+#include "QF/mersenne.h"
+#include "QF/sys.h"
+
+const vec3_t points[] = {
+	{-1, -1,  1},
+	{ 1,  1,  1},
+	{-1,  1, -1},
+	{ 1, -1, -1},
+	{-1, -1, -1},
+	{ 1,  1, -1},
+	{-1,  1,  1},
+	{ 1, -1,  1},
+	{ 0,  0,  0},
+};
+
+struct {
+	const vec3_t *points;
+	int         num_points;
+	sphere_t    expect;
+} tests[] = {
+	{0,      0, {{ 0,  0, 0}, 0}},
+	{points, 1, {{-1, -1, 1}, 0}},
+	{points, 2, {{ 0,  0, 1}, 1.41421356}},
+	{points, 3, {{-0.333333343, 0.333333343, 0.333333343}, 1.63299322}},
+	{points, 4, {{0, 0, 0}, 1.73205081}},
+};
+#define num_tests (sizeof (tests) / sizeof (tests[0]))
+
+static inline float
+rnd (mtstate_t *mt)
+{
+	union {
+		uint32_t    u;
+		float       f;
+	}           uf;
+
+	do {
+		uf.u = mtwist_rand (mt) & 0x007fffff;
+	} while (!uf.u);
+	uf.u |= 0x40000000;
+	return uf.f - 3.0;
+}
+
+int
+main (int argc, const char **argv)
+{
+	int         res = 0;
+	size_t      i, j;
+	sphere_t    sphere;
+	mtstate_t   mt;
+	double      start, end;
+
+	for (i = 0; i < num_tests; i ++) {
+		sphere = BoundingSphere (tests[i].points, tests[i].num_points);
+		if (VectorDistance_fast (sphere.center, tests[i].expect.center) > 1e-4
+			|| fabs (sphere.radius - tests[i].expect.radius) > 1e-4) {
+			res = 1;
+			printf ("test %d failed\n", (int) i);
+			printf ("expect: [%.9g %.9g %.9g],%.9g\n",
+					VectorExpand (tests[i].expect.center),
+					tests[i].expect.radius);
+			printf ("got   : [%.9g %.9g %.9g],%.9g\n",
+					VectorExpand (sphere.center),
+					sphere.radius);
+		}
+	}
+
+	mtwist_seed (&mt, 0);
+	start = Sys_DoubleTime ();
+	for (i = 0; !res && i < 100000; i++) {
+		vec3_t      cloud[4];
+		sphere_t    cc;
+		vec_t       r2;
+
+		for (j = 0; j < 4; j++) {
+			VectorSet (rnd (&mt), rnd (&mt), rnd (&mt), cloud[j]);
+		}
+		if (!CircumSphere ((const vec_t (*)[3]) cloud, 4, &cc))
+			continue;
+		r2 = cc.radius * cc.radius;
+		for (j = 0; j < 4; j++) {
+			if (fabs (VectorDistance_fast (cloud[j], cc.center) - r2)
+				< 1e-4 * r2)
+				continue;
+			printf ("%d %.9g - %.9g = %.9g\n", (int)j,
+					VectorDistance_fast (cloud[j], cc.center), r2,
+					VectorDistance_fast (cloud[j], cc.center) - r2);
+			printf ("[%.9g %.9g %.9g] - [%.9g %.9g %.9g] = %.9g > %.9g\n",
+					VectorExpand (cloud[j]), VectorExpand (cc.center),
+					VectorDistance_fast (cloud[j], cc.center), r2);
+		}
+	}
+	end = Sys_DoubleTime ();
+	printf ("%d itterations in %gs: %g itters/second\n", (int) i, end - start,
+			i / (end - start));
+	return res;
+}