3D smallest-circle/sphere Welzl algorithm implementation

Welzl.java

/*
MIT No Attribution

Copyright 2020 Jan Polák

Permission is hereby granted, free of charge, to any person obtaining a copy of this
software and associated documentation files (the "Software"), to deal in the Software
without restriction, including without limitation the rights to use, copy, modify,
merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
permit persons to whom the Software is furnished to do so.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

package welzl;

import java.util.BitSet;
import java.util.Random;
// Uses Queue implementation from libGDX

/**
 * Implements the Welzl smallest-encompassing circle O(n) algorithm described here:
 * https://en.wikipedia.org/wiki/Smallest-circle_problem
 * and here:
 * https://people.inf.ethz.ch/emo/PublFiles/SmallEnclDisk_LNCS555_91.pdf
 * <p>
 * General structure is inspired by http://www.sunshine2k.de/coding/java/Welzl/Welzl.html
 * (especially the trick with explicit array size growing)
 * however the algorithm there tries to be too smart with the end condition and it is 2D only.
 */
public class Welzl {

    /**
     * Implements iterative version of the Welzl algorithm (without explicit recursion),
     * with more-to-front heuristic and minimal allocations.
     */
    public static Sphere minimalEncompassingSphere(Vector3[] inputPoints, Random random) {
        final Queue<Vector3> points = new Queue<>(inputPoints.length, Vector3.class);
        System.arraycopy(inputPoints, 0, points.values, 0, inputPoints.length);

        // Classic shuffle, since Welzl algorithm wants to operate on points in random order
        {
            final Vector3[] v = points.values;
            for (int i = v.length; i > 1; i--) {
                final int aIndex = i - 1;
                final int bIndex = random.nextInt(i);
                final Vector3 a = v[aIndex];
                v[aIndex] = v[bIndex];
                v[bIndex] = a;
            }
        }

        // Each return increments, each recursive call decrements
        int pointCount = 0; // there is no point in the first initial descent which reference impl. does
        boolean calling = true;

        final Vector3[] boundary = new Vector3[4];
        int boundaryCount = 0;
        // This represents the stack - only one bit of info per call is needed
        final BitSet secondRecursionCalled = new BitSet(points.size);
        // The return value
        final Sphere sphere = new Sphere();

        do {
            if (calling) {
                if (pointCount == 0 || boundaryCount == 4) {
                    // Base case hit
                    circumsphere(boundary, boundaryCount, sphere);
                    pointCount++;
                    calling = false;
                } else {
                    // Call first recursion
                    pointCount--;
                    calling = true;
                }
            } else {
                final boolean returningFromSecond = secondRecursionCalled.get(pointCount);
                if (!returningFromSecond) {
                    // Returning from first
                    final Vector3 p = points.get(pointCount - 1);
                    if (sphere.contains(p, 1e-4f)) {
                        // Good, return
                        pointCount++;
                        calling = false;
                    } else {
                        // That failed, we must add it to the boundary and descend
                        boundary[boundaryCount++] = p;
                        secondRecursionCalled.set(pointCount);
                        pointCount--;
                        calling = true;
                    }
                } else { // returning from second
                    secondRecursionCalled.clear(pointCount);

                    points.addFirst(points.removeIndex(pointCount - 1)); // Move-to-front heuristic, gives about 2x speedup
                    boundaryCount--;
                    pointCount++;
                    calling = false;
                }
            }
        } while (pointCount <= points.size);

        return sphere;
    }

    /**
     * Construct a circumsphere for given boundary points, that is a minimal sphere which touches all given points.
     * Note that this may be very different from the minimal encompassing sphere for these points, at least for 3 and 4 points.
     *
     * @param boundary      the points to consider, first boundaryCount points must be valid
     * @param boundaryCount how many points to use from boundary array
     * @param out           sphere into which the result is written to (to prevent extra allocations)
     */
    private static void circumsphere(Vector3[] boundary, int boundaryCount, Sphere out) {
        if (boundaryCount == 0) {
            out.centerX = Float.NaN;
            out.centerY = Float.NaN;
            out.centerZ = Float.NaN;
            out.radius = Float.NaN;
        } else if (boundaryCount == 1) {
            out.centerX = boundary[0].x;
            out.centerY = boundary[0].y;
            out.centerZ = boundary[0].z;
            out.radius = 0;
        } else if (boundaryCount == 2) {
            out.centerX = (boundary[0].x + boundary[1].x) * 0.5f;
            out.centerY = (boundary[0].y + boundary[1].y) * 0.5f;
            out.centerZ = (boundary[0].z + boundary[1].z) * 0.5f;
            out.radius = (float) Math.sqrt(Vector3.distanceSquared(boundary[0], boundary[1])) * 0.5f;
        } else if (boundaryCount == 3) {
            // https://en.wikipedia.org/wiki/Circumscribed_circle#Higher_dimensions
            final Vector3 a = boundary[0].sub(boundary[2]);
            final Vector3 b = boundary[1].sub(boundary[2]);
            final Vector3 aCrossB = a.cross(b);
            final Vector3 top = b.scale(a.magnitudeSquared()).sub(a.scale(b.magnitudeSquared())).cross(aCrossB);
            final float bottom = 0.5f / aCrossB.magnitudeSquared();
            final Vector3 center = top.scale(bottom);

            out.centerX = boundary[2].x + center.x;
            out.centerY = boundary[2].y + center.y;
            out.centerZ = boundary[2].z + center.z;
            out.radius = (float) Math.sqrt(center.magnitudeSquared());
        } else if (boundaryCount == 4) {
            // Using https://gamedev.stackexchange.com/a/175387
            // More resources: https://mathworld.wolfram.com/Circumsphere.html
            // https://gamedev.stackexchange.com/questions/162731/welzl-algorithm-to-find-the-smallest-bounding-sphere
            final Vector3 r1 = boundary[1].sub(boundary[0]);
            final Vector3 r2 = boundary[2].sub(boundary[0]);
            final Vector3 r3 = boundary[3].sub(boundary[0]);
            final float sqLength1 = r1.magnitudeSquared();
            final float sqLength2 = r2.magnitudeSquared();
            final float sqLength3 = r3.magnitudeSquared();

            final float determinant =
                    r1.x * (r2.y * r3.z - r3.y * r2.z)
                            - r2.x * (r1.y * r3.z - r3.y * r1.z)
                            + r3.x * (r1.y * r2.z - r2.y * r1.z);

            final float f = 0.5f / determinant;

            final float offsetX = f * ((r2.y * r3.z - r3.y * r2.z) * sqLength1 - (r1.y * r3.z - r3.y * r1.z) * sqLength2 + (r1.y * r2.z - r2.y * r1.z) * sqLength3);
            final float offsetY = f * (-(r2.x * r3.z - r3.x * r2.z) * sqLength1 + (r1.x * r3.z - r3.x * r1.z) * sqLength2 - (r1.x * r2.z - r2.x * r1.z) * sqLength3);
            final float offsetZ = f * ((r2.x * r3.y - r3.x * r2.y) * sqLength1 - (r1.x * r3.y - r3.x * r1.y) * sqLength2 + (r1.x * r2.y - r2.x * r1.y) * sqLength3);

            out.centerX = boundary[0].x + offsetX;
            out.centerY = boundary[0].y + offsetY;
            out.centerZ = boundary[0].z + offsetZ;

            out.radius = (float) Math.sqrt(offsetX * offsetX + offsetY * offsetY + offsetZ * offsetZ);
        } else {
            throw new IllegalArgumentException("Invalid amount of boundary points: " + boundaryCount);
        }
    }

    public static final class Sphere {

        public float centerX, centerY, centerZ;
        public float radius;

        public final boolean contains(final Vector3 p, final float bias) {
            final float x = p.x - this.centerX;
            final float y = p.y - this.centerY;
            final float z = p.z - this.centerZ;
            final float radius = this.radius;
            return x * x + y * y + z * z - bias <= radius * radius;
        }

        @Override
        public String toString() {
            return String.format("Sphere(%4f, %4f, %4f, %4f)", centerX, centerY, centerZ, radius);
        }
    }

    public static final class Vector3 {

        public float x, y, z;

        public Vector3(float x, float y, float z) {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public static float distanceSquared(Vector3 t1, Vector3 t2) {
            final float x = t1.x - t2.x;
            final float y = t1.y - t2.y;
            final float z = t1.z - t2.z;
            return x * x + y * y + z * z;
        }

        public Vector3 sub(Vector3 rhs) {
            return new Vector3(this.x - rhs.x, this.y - rhs.y, this.z - rhs.z);
        }

        public Vector3 scale(float rhs) {
            return new Vector3(this.x * rhs, this.y * rhs, this.z * rhs);
        }

        public float magnitudeSquared() {
            return this.x * this.x + this.y * this.y + this.z * this.z;
        }

        public Vector3 cross(Vector3 other) {
            float a = this.y * other.z - this.z * other.y;
            float b = this.z * other.x - this.x * other.z;
            float c = this.x * other.y - this.y * other.x;
            return new Vector3(a, b, c);
        }

        @Override
        public String toString() {
            return String.format("V3(%3.4f, %3.4f, %3.4f)", x, y, z);
        }
    }
}