From 6eec76dd491f421c2ffb17ccbd018867295e5c72 Mon Sep 17 00:00:00 2001 From: Bill Currie Date: Wed, 13 Mar 2013 17:10:55 +0900 Subject: [PATCH] Implement Fischer's SEB algorithm (for 3d). Now we can get tight (<1e-6 * radius_squared error) bounding spheres. More importantly (for qfvis, anyway) very quickly: 1.7Mspheres/second for a 5 point cloud on my 2.33GHz Core 2 :) --- include/QF/mathlib.h | 1 + libs/util/mathlib.c | 129 +++++++++++++++++++++++++++++++++++++ libs/util/test/Makefile.am | 8 ++- libs/util/test/test-seb.c | 102 +++++++++++++++++++++++++++++ 4 files changed, 238 insertions(+), 2 deletions(-) create mode 100644 libs/util/test/test-seb.c diff --git a/include/QF/mathlib.h b/include/QF/mathlib.h index 10f93f27e..75c427ced 100644 --- a/include/QF/mathlib.h +++ b/include/QF/mathlib.h @@ -213,6 +213,7 @@ R_CullSphere (const vec3_t origin, const float radius) return false; } +sphere_t SmallestEnclosingBall (const vec3_t points[], int num_points); 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); diff --git a/libs/util/mathlib.c b/libs/util/mathlib.c index 18f2fab3f..9f1053025 100644 --- a/libs/util/mathlib.c +++ b/libs/util/mathlib.c @@ -1281,3 +1281,132 @@ CircumSphere (const vec3_t points[], int num_points, sphere_t *sphere) VectorZero (sphere->center); return 1; } + +static void +closest_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; + } +} + +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 itters = 0; + + if (num_points < 3) { + CircumSphere (points, num_points, &sphere); + return sphere; + } + + for (i = 0; i < 4; i++) + support[i] = 0; + + VectorCopy (points[0], sphere.center); + best_dist = dist = 0; + best = 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 (1) { + 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 (itters++ > 10) + Sys_Error ("stuck SEB"); + best = 0; + + if (num_support == 4) { + vec_t lambda[4]; + int dropped = 0; + + BarycentricCoords (support, 4, sphere.center, lambda); + for (i = 0; i < 4; i++) { + support[i - dropped] = support[i]; + if (lambda[i] < 0) { + dropped++; + num_support--; + } + } + if (!dropped) + break; + for (i = 4 - dropped; i < 4; i++) + support[i] = 0; + } + closest_point (support, num_support, sphere.center, affine); + VectorSubtract (affine, sphere.center, center_to_affine); + affine_dist = DotProduct (center_to_affine, center_to_affine); + if (affine_dist < 1e-2 * sphere.radius) + break; + 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); + if (!best) + break; + sphere.radius = VectorDistance_fast (sphere.center, 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; +} diff --git a/libs/util/test/Makefile.am b/libs/util/test/Makefile.am index 2960b1f0e..17b64d3a7 100644 --- a/libs/util/test/Makefile.am +++ b/libs/util/test/Makefile.am @@ -3,8 +3,8 @@ AUTOMAKE_OPTIONS= foreign INCLUDES= -I$(top_srcdir)/include check_PROGRAMS= \ - test-bary test-cs test-dq test-half test-mat3 test-mat4 \ - test-plist test-qfs test-quat test-set test-vrect + test-bary test-cs test-dq test-half test-mat3 test-mat4 test-plist \ + test-qfs test-quat test-seb test-set test-vrect test_bary_SOURCES=test-bary.c test_bary_LDADD=$(top_builddir)/libs/util/libQFutil.la @@ -42,6 +42,10 @@ test_quat_SOURCES=test-quat.c test_quat_LDADD=$(top_builddir)/libs/util/libQFutil.la test_quat_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la +test_seb_SOURCES=test-seb.c +test_seb_LDADD=$(top_builddir)/libs/util/libQFutil.la +test_seb_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la + test_set_SOURCES=test-set.c test_set_LDADD=$(top_builddir)/libs/util/libQFutil.la test_set_DEPENDENCIES=$(top_builddir)/libs/util/libQFutil.la diff --git a/libs/util/test/test-seb.c b/libs/util/test/test-seb.c new file mode 100644 index 000000000..801e1ff88 --- /dev/null +++ b/libs/util/test/test-seb.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 - 1.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 = SmallestEnclosingBall (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 < 1000000; i++) { + vec3_t cloud[10]; + sphere_t seb; + vec_t r2; + + for (j = 0; j < 5; j++) { + VectorSet (rnd (&mt), rnd (&mt), rnd (&mt), cloud[j]); + } + seb = SmallestEnclosingBall ((const vec_t (*)[3]) cloud, 5); + r2 = seb.radius * seb.radius; + for (j = 0; j < 5; j++) { + if (VectorDistance_fast (cloud[j], seb.center) - r2 + > 1e-5 * r2) { + res = 1; + printf ("%d %.9g - %.9g = %.9g\n", (int)j, + VectorDistance_fast (cloud[j], seb.center), r2, + VectorDistance_fast (cloud[j], seb.center) - r2); + printf ("[%.9g %.9g %.9g] - [%.9g %.9g %.9g] = %.9g > %.9g\n", + VectorExpand (cloud[j]), VectorExpand (seb.center), + VectorDistance_fast (cloud[j], seb.center), r2); + } + } + } + end = Sys_DoubleTime (); + printf ("%d itterations in %gs: %g itters/second\n", (int) i, end - start, + i / (end - start)); + return res; +}