Commits

Michael Ludwig committed 2deb361

Implement single axis and temporal SAP

Comments (0)

Files changed (9)

ferox-demos/src/main/java/com/ferox/physics/PhysicsApplicationStub.java

 import com.ferox.physics.controller.ConstraintSolvingTask;
 import com.ferox.physics.controller.ForcesTask;
 import com.ferox.physics.controller.MotionTask;
-import com.ferox.physics.controller.SpatialIndexCollisionController;
+import com.ferox.physics.controller.TemporalSAPCollisionController;
 import com.ferox.renderer.OnscreenSurface;
 import com.ferox.renderer.impl.lwjgl.LwjglFramework;
 import com.ferox.scene.Camera;
                            .createJob("physics",
                                       Timers.fixedDelta(1 / 60.0),
                                       new ForcesTask(),
-                                      new SpatialIndexCollisionController(new QuadTree<Entity>(worldBounds,
-                                                                                               6),
-                                                                          new DefaultCollisionAlgorithmProvider()),
+                                      //                                      new SpatialIndexCollisionController(new QuadTree<Entity>(worldBounds,
+                                      //                                                                                               6),
+                                      //                                                                          new DefaultCollisionAlgorithmProvider()),
+                                      //                                      new SingleAxisSAPCollisionController(new DefaultCollisionAlgorithmProvider()),
+                                      new TemporalSAPCollisionController(new DefaultCollisionAlgorithmProvider()),
                                       new ConstraintSolvingTask(), new MotionTask(),
                                       new TransformController());
 

ferox-math/src/main/java/com/ferox/math/Functions.java

  * @author Michael Ludwig
  */
 public final class Functions {
+    public static final long LONG_SIGN_BIT = 0x8000000000000000L;
+    public static final int INT_SIGN_BIT = 0x80000000;
+
+    public static final long LONG_SIGN_MASK = ~LONG_SIGN_BIT;
+    public static final int INT_SIGN_MASK = ~INT_SIGN_BIT;
+
     private Functions() {}
 
     /**
         }
         return (num & (num - 1)) == 0;
     }
+
+    public static int sortableFloatToIntBits(float v) {
+        int bits = Float.floatToIntBits(v);
+        if ((bits & INT_SIGN_BIT) != 0) {
+            // reverse negative numbers
+            return (~bits) | INT_SIGN_BIT;
+        } else {
+            // flip sign for positive numbers
+            return bits;
+        }
+    }
+
+    public static float sortableIntBitsToFloat(int v) {
+        if ((v & INT_SIGN_BIT) == 0) {
+            // flip sign
+            return Float.intBitsToFloat(~(v & INT_SIGN_MASK));
+        } else {
+            // reverse bits
+            return Float.intBitsToFloat(~v);
+        }
+    }
+
+    public static long sortableDoubleToLongBits(double v) {
+        long bits = Double.doubleToLongBits(v);
+        if ((bits & LONG_SIGN_BIT) != 0) {
+            // reverse negative numbers
+            return (~bits) | LONG_SIGN_BIT;
+        } else {
+            // flip sign for positive numbers
+            return bits;
+        }
+    }
+
+    public static double sortableLongBitsToDouble(long v) {
+        if ((v & LONG_SIGN_BIT) == 0) {
+            // flip sign
+            return Double.longBitsToDouble(v);
+        } else {
+            // reverse bits
+            return Double.longBitsToDouble(~(v & LONG_SIGN_MASK));
+        }
+    }
 }

ferox-physics/src/main/java/com/ferox/physics/collision/CollisionPair.java

+package com.ferox.physics.collision;
+
+import com.lhkbob.entreri.Component;
+
+public class CollisionPair {
+    private Component<CollisionBody> a;
+    private Component<CollisionBody> b;
+
+    public CollisionPair() {}
+
+    public CollisionPair(Component<CollisionBody> a, Component<CollisionBody> b) {
+        set(a, b);
+    }
+
+    public void set(Component<CollisionBody> a, Component<CollisionBody> b) {
+        if (a == null || b == null) {
+            throw new NullPointerException("Bodies cannot be null");
+        }
+        this.a = a;
+        this.b = b;
+    }
+
+    public Component<CollisionBody> getBodyA() {
+        return a;
+    }
+
+    public Component<CollisionBody> getBodyB() {
+        return b;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (!(o instanceof CollisionPair)) {
+            return false;
+        }
+        CollisionPair t = (CollisionPair) o;
+        return (t.a == a && t.b == b) || (t.b == a && t.a == b);
+    }
+
+    @Override
+    public int hashCode() {
+        // sum of hashes -> follow Set hashcode since a pair is just a 2 element set
+        return a.hashCode() + b.hashCode();
+    }
+}

ferox-physics/src/main/java/com/ferox/physics/controller/CollisionTask.java

 import java.util.Set;
 
 import com.ferox.physics.collision.ClosestPair;
+import com.ferox.physics.collision.CollisionAlgorithm;
+import com.ferox.physics.collision.CollisionAlgorithmProvider;
 import com.ferox.physics.collision.CollisionBody;
 import com.ferox.physics.dynamics.ContactManifoldPool;
 import com.ferox.physics.dynamics.LinearConstraintPool;
+import com.ferox.physics.dynamics.RigidBody;
 import com.ferox.util.profile.Profiler;
 import com.lhkbob.entreri.ComponentData;
 import com.lhkbob.entreri.EntitySystem;
 import com.lhkbob.entreri.task.Task;
 
 public abstract class CollisionTask implements Task {
+    private final CollisionAlgorithmProvider algorithms;
+
     private final ContactManifoldPool manifolds;
 
     private final LinearConstraintPool contactGroup;
 
     protected double dt;
 
-    public CollisionTask() {
+    public CollisionTask(CollisionAlgorithmProvider algorithms) {
+        if (algorithms == null) {
+            throw new NullPointerException("Algorithm provider cannot be null");
+        }
+
+        this.algorithms = algorithms;
         manifolds = new ContactManifoldPool();
         contactGroup = new LinearConstraintPool(null);
         frictionGroup = new LinearConstraintPool(contactGroup);
         job.report(new ConstraintResult(frictionGroup));
     }
 
-    protected void notifyContact(CollisionBody bodyA, CollisionBody bodyB,
-                                 ClosestPair contact) {
-        manifolds.addContact(bodyA, bodyB, contact);
+    @SuppressWarnings({"rawtypes", "unchecked"})
+    protected void notifyPotentialContact(CollisionBody bodyA, CollisionBody bodyB) {
+        // test collision groups and masks
+        if (!bodyA.canCollide(bodyB)) {
+            return;
+        }
+        // collisions must have at least one rigid body to act on
+        if (bodyA.getEntity().get(RigidBody.class) == null && bodyB.getEntity()
+                                                                   .get(RigidBody.class) == null) {
+            return;
+        }
+
+        // get the appropriate algorithm
+        CollisionAlgorithm algorithm = algorithms.getAlgorithm(bodyA.getShape()
+                                                                    .getClass(),
+                                                               bodyB.getShape()
+                                                                    .getClass());
+
+        if (algorithm != null) {
+            // compute closest pair between the two shapes
+            ClosestPair pair = algorithm.getClosestPair(bodyA.getShape(),
+                                                        bodyA.getTransform(),
+                                                        bodyB.getShape(),
+                                                        bodyB.getTransform());
+
+            if (pair != null && pair.isIntersecting()) {
+                // add to manifold only when there is an intersection
+                manifolds.addContact(bodyA, bodyB, pair);
+            }
+        }
     }
 
     private class WarmstartTask implements Task, ParallelAware {

ferox-physics/src/main/java/com/ferox/physics/controller/SingleAxisSAPCollisionController.java

  */
 package com.ferox.physics.controller;
 
-public class SingleAxisSAPCollisionController extends CollisionTask {
+import java.util.Collections;
+import java.util.HashSet;
+import java.util.Set;
 
+import com.ferox.math.AxisAlignedBox;
+import com.ferox.math.Functions;
+import com.ferox.physics.collision.CollisionAlgorithmProvider;
+import com.ferox.physics.collision.CollisionBody;
+import com.ferox.physics.dynamics.RigidBody;
+import com.ferox.util.Bag;
+import com.ferox.util.profile.Profiler;
+import com.lhkbob.entreri.ComponentData;
+import com.lhkbob.entreri.ComponentIterator;
+import com.lhkbob.entreri.Entity;
+import com.lhkbob.entreri.EntitySystem;
+import com.lhkbob.entreri.task.Job;
+import com.lhkbob.entreri.task.ParallelAware;
+import com.lhkbob.entreri.task.Task;
+
+public class SingleAxisSAPCollisionController extends CollisionTask implements ParallelAware {
+    private static final Set<Class<? extends ComponentData<?>>> COMPONENTS;
+    static {
+        Set<Class<? extends ComponentData<?>>> types = new HashSet<Class<? extends ComponentData<?>>>();
+        types.add(CollisionBody.class);
+        types.add(RigidBody.class);
+        COMPONENTS = Collections.unmodifiableSet(types);
+    }
+
+    private final Bag<Entity> bodies;
+
+    // cached instances that are normally local to process()
+    private CollisionBody bodyA;
+    private CollisionBody bodyB;
+
+    private ComponentIterator iterator;
+
+    public SingleAxisSAPCollisionController(CollisionAlgorithmProvider algorithms) {
+        super(algorithms);
+        bodies = new Bag<Entity>();
+    }
+
+    @Override
+    public void reset(EntitySystem system) {
+        super.reset(system);
+
+        if (bodyA == null) {
+            bodyA = system.createDataInstance(CollisionBody.class);
+            bodyB = system.createDataInstance(CollisionBody.class);
+
+            iterator = new ComponentIterator(system).addRequired(bodyA);
+        }
+
+        iterator.reset();
+        bodies.clear(true);
+    }
+
+    @Override
+    public Task process(EntitySystem system, Job job) {
+        Profiler.push("detect-collisions");
+
+        // build up axis lists to sort
+        Profiler.push("prepare-axis");
+        while (iterator.next()) {
+            bodies.add(bodyA.getEntity());
+        }
+
+        int[] edges = new int[bodies.size() * 2];
+        int[] edgeLabels = new int[bodies.size() * 2];
+
+        for (int i = 0; i < bodies.size(); i++) {
+            bodies.get(i).get(bodyA);
+            AxisAlignedBox aabb = bodyA.getWorldBounds();
+            edges[(i << 1)] = Functions.sortableFloatToIntBits((float) aabb.min.x);
+            edges[(i << 1) + 1] = Functions.sortableFloatToIntBits((float) aabb.max.x);
+            edgeLabels[(i << 1)] = i;
+            edgeLabels[(i << 1) + 1] = (0x80000000) | i;
+        }
+        Profiler.pop();
+
+        // sort edges, keeping edgeLabels in sync with swaps
+        Profiler.push("sort-axis");
+        quickSort(edges, edgeLabels, 0, edges.length);
+        Profiler.pop();
+
+        // iterate edges, checking overlapping pairs
+        Profiler.push("prune");
+        int openIndex = -1;
+        int nextIndex = -1;
+
+        int currLabel;
+        boolean isMax;
+        for (int i = 0; i < edges.length; i++) {
+            // check the next label
+            isMax = edgeLabels[i] < 0;
+            currLabel = (~0x80000000) & edgeLabels[i];
+
+            if (isMax) {
+                if (openIndex >= 0 && currLabel == edgeLabels[openIndex]) {
+                    // reached the end of the current box, update loop if
+                    // we know of another min edge to visit
+                    if (nextIndex >= 0) {
+                        i = nextIndex;
+                    }
+
+                    openIndex = nextIndex;
+                    nextIndex = -1;
+                }
+                // otherwise we're skipping past the max edges of already
+                // processed boxes
+            } else {
+                if (openIndex < 0) {
+                    // open another box
+                    openIndex = i;
+                } else {
+                    // perform intersection test with open box and current box
+                    bodies.get(edgeLabels[openIndex]).get(bodyA);
+                    bodies.get(currLabel).get(bodyB);
+
+                    if (bodyA.getWorldBounds().intersects(bodyB.getWorldBounds())) {
+                        notifyPotentialContact(bodyA, bodyB);
+                    }
+
+                    if (nextIndex < 0) {
+                        // earliest next min edge we've found
+                        nextIndex = i;
+                    }
+                }
+            }
+        }
+        Profiler.pop();
+
+        // generate constraints
+        Profiler.push("generate-constraints");
+        reportConstraints(job);
+        Profiler.pop();
+
+        Profiler.pop();
+        return super.process(system, job);
+    }
+
+    @Override
+    public Set<Class<? extends ComponentData<?>>> getAccessedComponents() {
+        return COMPONENTS;
+    }
+
+    @Override
+    public boolean isEntitySetModified() {
+        return false;
+    }
+
+    // use quick sort to sort elements in x, swapping y along with it
+    private void quickSort(int[] x, int[] y, int off, int len) {
+        // insertion sort on smallest arrays
+        if (len < 7) {
+            for (int i = off; i < len + off; i++) {
+                for (int j = i; j > off && x[j - 1] > x[j]; j--) {
+                    swap(x, y, j, j - 1);
+                }
+            }
+            return;
+        }
+
+        // choose a partition element, v
+        int m = off + (len >> 1); // small arrays, middle element
+        if (len > 7) {
+            int l = off;
+            int n = off + len - 1;
+            if (len > 40) { // big arrays, pseudomedian of 9
+                int s = len / 8;
+                l = med3(x, l, l + s, l + 2 * s);
+                m = med3(x, m - s, m, m + s);
+                n = med3(x, n - 2 * s, n - s, n);
+            }
+            m = med3(x, l, m, n); // mid-size, med of 3
+        }
+        int v = x[m];
+
+        int a = off, b = a, c = off + len - 1, d = c;
+        while (true) {
+            while (b <= c && x[b] <= v) {
+                if (v == x[b]) {
+                    swap(x, y, a++, b);
+                }
+                b++;
+            }
+            while (c >= b && x[c] >= v) {
+                if (v == x[c]) {
+                    swap(x, y, c, d--);
+                }
+                c--;
+            }
+            if (b > c) {
+                break;
+            }
+            swap(x, y, b++, c--);
+        }
+
+        // swap partition elements back to middle
+        int s, n = off + len;
+        s = Math.min(a - off, b - a);
+        vecswap(x, y, off, b - s, s);
+        s = Math.min(d - c, n - d - 1);
+        vecswap(x, y, b, n - s, s);
+
+        // recursively sort non-partition-elements
+        if ((s = b - a) > 1) {
+            quickSort(x, y, off, s);
+        }
+        if ((s = d - c) > 1) {
+            quickSort(x, y, n - s, s);
+        }
+    }
+
+    // swaps the elements at indices a and b, along with the hashes in x
+    private void swap(int[] x, int[] y, int a, int b) {
+        int k = x[a];
+        x[a] = x[b];
+        x[b] = k;
+
+        int l = y[a];
+        y[a] = y[b];
+        y[b] = l;
+    }
+
+    // swaps n elements starting at a and b, such that (a,b), (a+1, b+1), etc. are swapped
+    private void vecswap(int[] x, int[] y, int a, int b, int n) {
+        for (int i = 0; i < n; i++, a++, b++) {
+            swap(x, y, a, b);
+        }
+    }
+
+    // returns the index of the median of the three indexed elements
+    private static int med3(int[] x, int a, int b, int c) {
+        return (x[a] < x[b] ? (x[b] < x[c] ? b : x[a] < x[c] ? c : a) : (x[b] > x[c] ? b : x[a] > x[c] ? c : a));
+    }
 }

ferox-physics/src/main/java/com/ferox/physics/controller/SpatialIndexCollisionController.java

 import com.ferox.math.bounds.BoundedSpatialIndex;
 import com.ferox.math.bounds.IntersectionCallback;
 import com.ferox.math.bounds.SpatialIndex;
-import com.ferox.physics.collision.ClosestPair;
-import com.ferox.physics.collision.CollisionAlgorithm;
 import com.ferox.physics.collision.CollisionAlgorithmProvider;
 import com.ferox.physics.collision.CollisionBody;
 import com.ferox.physics.dynamics.RigidBody;
     }
 
     private final SpatialIndex<Entity> index;
-    private final CollisionAlgorithmProvider algorithms;
 
     // cached instances that are normally local to process()
     private CollisionBody bodyA;
 
     public SpatialIndexCollisionController(SpatialIndex<Entity> index,
                                            CollisionAlgorithmProvider algorithms) {
-        if (index == null || algorithms == null) {
-            throw new NullPointerException("Arguments cannot be null");
+        super(algorithms);
+        if (index == null) {
+            throw new NullPointerException("SpatialIndex cannot be null");
         }
         this.index = index;
-        this.algorithms = algorithms;
     }
 
     @Override
         }
 
         @Override
-        @SuppressWarnings({"rawtypes", "unchecked"})
         public void process(Entity a, AxisAlignedBox boundsA, Entity b,
                             AxisAlignedBox boundsB) {
             // at this point we know the world bounds of a and b intersect, but
             a.get(bodyA);
             b.get(bodyB);
 
-            CollisionAlgorithm algorithm = algorithms.getAlgorithm(bodyA.getShape()
-                                                                        .getClass(),
-                                                                   bodyB.getShape()
-                                                                        .getClass());
-
-            if (algorithm != null) {
-                // have a valid algorithm to test
-                ClosestPair pair = algorithm.getClosestPair(bodyA.getShape(),
-                                                            bodyA.getTransform(),
-                                                            bodyB.getShape(),
-                                                            bodyB.getTransform());
-
-                if (pair != null && pair.isIntersecting() && (a.get(RigidBody.class) != null || b.get(RigidBody.class) != null)) {
-                    notifyContact(bodyA, bodyB, pair);
-                }
-            }
+            notifyPotentialContact(bodyA, bodyB);
         }
     }
 

ferox-physics/src/main/java/com/ferox/physics/controller/TemporalSAPCollisionController.java

  */
 package com.ferox.physics.controller;
 
+import java.util.Arrays;
+import java.util.HashSet;
+import java.util.Set;
+
+import com.ferox.math.AxisAlignedBox;
+import com.ferox.math.Functions;
+import com.ferox.physics.collision.CollisionAlgorithmProvider;
+import com.ferox.physics.collision.CollisionBody;
+import com.ferox.physics.collision.CollisionPair;
+import com.ferox.util.profile.Profiler;
+import com.lhkbob.entreri.Component;
+import com.lhkbob.entreri.ComponentIterator;
+import com.lhkbob.entreri.EntitySystem;
+import com.lhkbob.entreri.property.IntProperty;
+import com.lhkbob.entreri.task.Job;
+import com.lhkbob.entreri.task.Task;
+
 public class TemporalSAPCollisionController extends CollisionTask {
+    private final EdgeStore[] edges;
+    private final Set<CollisionPair> overlappingPairCache;
 
+    // cached local instances
+    private CollisionBody bodyA;
+    private CollisionBody bodyB;
+    private ComponentIterator bodyIterator; // iterates bodyA
+
+    public TemporalSAPCollisionController(CollisionAlgorithmProvider algorithms) {
+        super(algorithms);
+
+        overlappingPairCache = new HashSet<CollisionPair>();
+
+        edges = new EdgeStore[3];
+        for (int i = 0; i < edges.length; i++) {
+            edges[i] = new EdgeStore(overlappingPairCache);
+        }
+
+        // link edge axis
+        edges[0].axis1 = edges[1];
+        edges[0].axis2 = edges[2];
+        edges[1].axis1 = edges[2];
+        edges[1].axis2 = edges[0];
+        edges[2].axis1 = edges[0];
+        edges[2].axis2 = edges[1];
+    }
+
+    @Override
+    public void reset(EntitySystem system) {
+        super.reset(system);
+
+        if (bodyA == null) {
+            bodyA = system.createDataInstance(CollisionBody.class);
+            bodyB = system.createDataInstance(CollisionBody.class);
+            bodyIterator = new ComponentIterator(system).addRequired(bodyA);
+
+            for (int i = 0; i < edges.length; i++) {
+                edges[i].maxBodyEdges = system.decorate(CollisionBody.class,
+                                                        new IntProperty.Factory(-1));
+                edges[i].minBodyEdges = system.decorate(CollisionBody.class,
+                                                        new IntProperty.Factory(-1));
+            }
+        }
+
+        bodyIterator.reset();
+    }
+
+    @Override
+    public Task process(EntitySystem system, Job job) {
+        Profiler.push("detect-collisions");
+
+        // update all edges, sorting as necessary, and keeping track of
+        // overlapping pairs
+        Profiler.push("update-overlaps");
+        for (int i = 0; i < edges.length; i++) {
+            edges[i].removeDeadEdges();
+        }
+
+        while (bodyIterator.next()) {
+            AxisAlignedBox aabb = bodyA.getWorldBounds();
+            Component<CollisionBody> c = bodyA.getComponent();
+
+            if (edges[0].minBodyEdges.get(bodyA.getIndex()) < 0) {
+                // add body to edge lists
+                edges[0].addEdges(c, aabb.min.x, aabb.max.x, false);
+                edges[1].addEdges(c, aabb.min.y, aabb.max.y, false);
+                edges[2].addEdges(c, aabb.min.z, aabb.max.z, true);
+            } else {
+                edges[0].updateEdges(c, aabb.min.x, aabb.max.x);
+                edges[1].updateEdges(c, aabb.min.y, aabb.max.y);
+                edges[2].updateEdges(c, aabb.min.z, aabb.max.z);
+            }
+        }
+        Profiler.pop();
+
+        // iterate through pairs, removing those that have dead components,
+        // and performing narrow-phase collisions on the remaining
+        Profiler.push("process-overlaps");
+        for (CollisionPair pair : overlappingPairCache) {
+            if (bodyA.set(pair.getBodyA()) && bodyB.set(pair.getBodyB()) && bodyA.isEnabled() && bodyB.isEnabled()) {
+                // both components are still valid
+                notifyPotentialContact(bodyA, bodyB);
+            }
+        }
+        Profiler.pop();
+
+        // report constraints for accumulated collisions
+        Profiler.push("generate-constraints");
+        reportConstraints(job);
+        Profiler.pop();
+
+        Profiler.pop();
+
+        return super.process(system, job);
+    }
+
+    private static class EdgeStore {
+        private final Set<CollisionPair> overlappingPairCache;
+        private final CollisionPair query; // mutable, don't put in set
+
+        private int[] edges;
+        private Component<CollisionBody>[] edgeLabels;
+        private boolean[] edgeIsMax;
+
+        private IntProperty minBodyEdges;
+        private IntProperty maxBodyEdges;
+
+        private int edgeCount;
+
+        private EdgeStore axis1;
+        private EdgeStore axis2;
+
+        @SuppressWarnings("unchecked")
+        public EdgeStore(Set<CollisionPair> overlappingPairCache) {
+            this.overlappingPairCache = overlappingPairCache;
+            query = new CollisionPair();
+
+            edges = new int[2];
+            edgeLabels = new Component[2];
+            edgeIsMax = new boolean[2];
+
+            edgeCount = 0;
+
+            // sentinel values
+            edges[0] = Integer.MIN_VALUE;
+            edges[1] = Integer.MAX_VALUE;
+        }
+
+        public void removeDeadEdges() {
+            // remove disabled component edges by shifting everything to the left
+            for (int i = 1; i < 1 + edgeCount; i++) {
+                if (!edgeLabels[i].isEnabled()) {
+                    System.arraycopy(edgeLabels, i + 1, edgeLabels, i, edgeCount - i);
+                    System.arraycopy(edges, i + 1, edges, i, edgeCount - i);
+                    System.arraycopy(edgeIsMax, i + 1, edges, i, edgeCount - i);
+                    edgeCount--;
+
+                    if (edgeLabels[i].isLive()) {
+                        // still have component data so invalidate edge index
+                        maxBodyEdges.set(-1, edgeLabels[i].getIndex());
+                        minBodyEdges.set(-1, edgeLabels[i].getIndex());
+                    }
+                }
+            }
+
+            // sync component edge index after everything has been moved
+            for (int i = 1; i < 1 + edgeCount; i++) {
+                if (edgeIsMax[i]) {
+                    maxBodyEdges.set(i, edgeLabels[i].getIndex());
+                } else {
+                    minBodyEdges.set(i, edgeLabels[i].getIndex());
+                }
+            }
+        }
+
+        public void addEdges(Component<CollisionBody> body, double min, double max,
+                             boolean updateOverlaps) {
+            // offset by 1 for sentinel value stored in 0th index
+            int minIndex = 1 + edgeCount++;
+            int maxIndex = 1 + edgeCount++;
+            if (maxIndex >= edges.length - 2) {
+                int newLength = (edges.length) * 2 + 2;
+                edges = Arrays.copyOf(edges, newLength);
+                edgeLabels = Arrays.copyOf(edgeLabels, newLength);
+                edgeIsMax = Arrays.copyOf(edgeIsMax, newLength);
+            }
+
+            edges[minIndex] = Functions.sortableFloatToIntBits((float) min);
+            edgeIsMax[minIndex] = false;
+            edgeLabels[minIndex] = body;
+
+            edges[maxIndex] = Functions.sortableFloatToIntBits((float) max);
+            edgeIsMax[maxIndex] = true;
+            edgeLabels[maxIndex] = body;
+
+            minBodyEdges.set(minIndex, body.getIndex());
+            maxBodyEdges.set(maxIndex, body.getIndex());
+
+            // preserve ending sentinel value
+            edges[maxIndex + 1] = Integer.MAX_VALUE;
+            edgeIsMax[maxIndex + 1] = true;
+
+            // sort to proper position
+            sortMinDown(minIndex, updateOverlaps);
+            sortMaxDown(maxIndex, updateOverlaps);
+        }
+
+        public void updateEdges(Component<CollisionBody> body, double newMin,
+                                double newMax) {
+            int minIndex = minBodyEdges.get(body.getIndex());
+            int maxIndex = maxBodyEdges.get(body.getIndex());
+
+            int sNewMin = Functions.sortableFloatToIntBits((float) newMin);
+            int sNewMax = Functions.sortableFloatToIntBits((float) newMax);
+
+            // calculate change along axis
+            int dmin = sNewMin - edges[minIndex];
+            int dmax = sNewMax - edges[maxIndex];
+
+            // assign edge values
+            edges[minIndex] = sNewMin;
+            edges[maxIndex] = sNewMax;
+
+            // expand (only adds overlaps)
+            if (dmin < 0) {
+                sortMinDown(minIndex, true);
+            }
+            if (dmax > 0) {
+                sortMaxUp(maxIndex, true);
+            }
+
+            // shrink (only removes overlaps)
+            if (dmin > 0) {
+                sortMinUp(minIndex, true);
+            }
+            if (dmax < 0) {
+                sortMaxDown(maxIndex, true);
+            }
+        }
+
+        public boolean test2DOverlap(int bodyA, int bodyB) {
+            // optimization: check the array index instead of the actual
+            // coordinate value, since we know that they're sorted
+            boolean a1A = axis1.maxBodyEdges.get(bodyA) < axis1.minBodyEdges.get(bodyB);
+            boolean a1B = axis1.maxBodyEdges.get(bodyB) < axis1.minBodyEdges.get(bodyA);
+            boolean a2A = axis2.maxBodyEdges.get(bodyA) < axis2.minBodyEdges.get(bodyB);
+            boolean a2B = axis2.maxBodyEdges.get(bodyB) < axis2.minBodyEdges.get(bodyA);
+
+            return !(a1A || a1B || a2A || a2B);
+        }
+
+        public void sortMinDown(int edge, boolean updateOverlaps) {
+            int prevEdge = edge - 1;
+            while (edges[edge] < edges[prevEdge]) {
+                if (edgeIsMax[prevEdge]) {
+                    // previous edge is a maximum so check the bounds for overlap,
+                    // and if they do, add an overlap
+                    if (updateOverlaps && test2DOverlap(edgeLabels[edge].getIndex(),
+                                                        edgeLabels[prevEdge].getIndex())) {
+                        query.set(edgeLabels[edge], edgeLabels[prevEdge]);
+                        if (!overlappingPairCache.contains(query)) {
+                            // allocate new pair
+                            overlappingPairCache.add(new CollisionPair(edgeLabels[edge],
+                                                                       edgeLabels[prevEdge]));
+                        } // else already overlapping
+                    }
+
+                    // update edge reference
+                    maxBodyEdges.set(edge, edgeLabels[prevEdge].getIndex());
+                } else {
+                    minBodyEdges.set(edge, edgeLabels[prevEdge].getIndex());
+                }
+
+                // swap
+                Component<CollisionBody> sb = edgeLabels[edge];
+                int se = edges[edge];
+                boolean sm = edgeIsMax[edge];
+
+                edgeLabels[edge] = edgeLabels[prevEdge];
+                edges[edge] = edges[prevEdge];
+                edgeIsMax[edge] = edgeIsMax[prevEdge];
+
+                edgeLabels[prevEdge] = sb;
+                edges[prevEdge] = se;
+                edgeIsMax[prevEdge] = sm;
+
+                // sync up the body to the final position of the edge
+                minBodyEdges.set(prevEdge, sb.getIndex());
+
+                // decrement
+                edge = prevEdge;
+                prevEdge--;
+            }
+        }
+
+        public void sortMaxDown(int edge, boolean updateOverlaps) {
+            int prevEdge = edge - 1;
+            while (edges[edge] < edges[prevEdge]) {
+                if (!edgeIsMax[prevEdge]) {
+                    // previous edge is a minimum so remove all overlaps between
+                    // current and previous bodies
+                    if (updateOverlaps && test2DOverlap(edgeLabels[edge].getIndex(),
+                                                        edgeLabels[prevEdge].getIndex())) {
+                        query.set(edgeLabels[edge], edgeLabels[prevEdge]);
+                        overlappingPairCache.remove(query);
+                    }
+
+                    // update edge reference
+                    minBodyEdges.set(edge, edgeLabels[prevEdge].getIndex());
+                } else {
+                    maxBodyEdges.set(edge, edgeLabels[prevEdge].getIndex());
+                }
+
+                // swap
+                Component<CollisionBody> sb = edgeLabels[edge];
+                int se = edges[edge];
+                boolean sm = edgeIsMax[edge];
+
+                edgeLabels[edge] = edgeLabels[prevEdge];
+                edges[edge] = edges[prevEdge];
+                edgeIsMax[edge] = edgeIsMax[prevEdge];
+
+                edgeLabels[prevEdge] = sb;
+                edges[prevEdge] = se;
+                edgeIsMax[prevEdge] = sm;
+
+                // sync up the body to the final position of the edge
+                maxBodyEdges.set(prevEdge, sb.getIndex());
+
+                // decrement
+                edge = prevEdge;
+                prevEdge--;
+            }
+        }
+
+        public void sortMinUp(int edge, boolean updateOverlaps) {
+            int nextEdge = edge + 1;
+            while (edges[edge] > edges[nextEdge]) {
+                if (edgeIsMax[nextEdge]) {
+                    // next edge is a maximum so remove any overlaps between
+                    // the two bodies
+                    if (updateOverlaps && test2DOverlap(edgeLabels[edge].getIndex(),
+                                                        edgeLabels[nextEdge].getIndex())) {
+                        query.set(edgeLabels[edge], edgeLabels[nextEdge]);
+                        overlappingPairCache.remove(query);
+                    }
+
+                    // update edge reference
+                    maxBodyEdges.set(edge, edgeLabels[nextEdge].getIndex());
+                } else {
+                    minBodyEdges.set(edge, edgeLabels[nextEdge].getIndex());
+                }
+
+                // swap
+                Component<CollisionBody> sb = edgeLabels[edge];
+                int se = edges[edge];
+                boolean sm = edgeIsMax[edge];
+
+                edgeLabels[edge] = edgeLabels[nextEdge];
+                edges[edge] = edges[nextEdge];
+                edgeIsMax[edge] = edgeIsMax[nextEdge];
+
+                edgeLabels[nextEdge] = sb;
+                edges[nextEdge] = se;
+                edgeIsMax[nextEdge] = sm;
+
+                // sync up the body to the final position of the edge
+                minBodyEdges.set(nextEdge, sb.getIndex());
+
+                // increment
+                edge = nextEdge;
+                nextEdge++;
+            }
+        }
+
+        public void sortMaxUp(int edge, boolean updateOverlaps) {
+            int nextEdge = edge + 1;
+            while (edges[edge] > edges[nextEdge]) {
+                if (!edgeIsMax[nextEdge]) {
+                    // next edge is a minimum so test for overlap and possibly
+                    // report the new pair
+                    if (updateOverlaps && test2DOverlap(edgeLabels[edge].getIndex(),
+                                                        edgeLabels[nextEdge].getIndex())) {
+                        query.set(edgeLabels[edge], edgeLabels[nextEdge]);
+                        if (!overlappingPairCache.contains(query)) {
+                            // allocate new pair
+                            overlappingPairCache.add(new CollisionPair(edgeLabels[edge],
+                                                                       edgeLabels[nextEdge]));
+                        } // else already overlapping
+                    }
+
+                    // update edge reference
+                    minBodyEdges.set(edge, edgeLabels[nextEdge].getIndex());
+                } else {
+                    maxBodyEdges.set(edge, edgeLabels[nextEdge].getIndex());
+                }
+
+                // swap
+                Component<CollisionBody> sb = edgeLabels[edge];
+                int se = edges[edge];
+                boolean sm = edgeIsMax[edge];
+
+                edgeLabels[edge] = edgeLabels[nextEdge];
+                edges[edge] = edges[nextEdge];
+                edgeIsMax[edge] = edgeIsMax[nextEdge];
+
+                edgeLabels[nextEdge] = sb;
+                edges[nextEdge] = se;
+                edgeIsMax[nextEdge] = sm;
+
+                // sync up the body to the final position of the edge
+                maxBodyEdges.set(nextEdge, sb.getIndex());
+
+                // increment
+                edge = nextEdge;
+                nextEdge++;
+            }
+        }
+    }
 }

ferox-physics/src/main/java/com/ferox/physics/dynamics/ContactManifoldPool.java

 import com.ferox.math.bounds.Plane;
 import com.ferox.physics.collision.ClosestPair;
 import com.ferox.physics.collision.CollisionBody;
+import com.ferox.physics.collision.CollisionPair;
 import com.lhkbob.entreri.Component;
 import com.lhkbob.entreri.Entity;
 import com.lhkbob.entreri.EntitySystem;
     private int toVector3Index(int manifold, int point) {
         return toIndex(manifold, point) * 3;
     }
-
-    private static class CollisionPair {
-        private Component<CollisionBody> a;
-        private Component<CollisionBody> b;
-
-        public CollisionPair() {}
-
-        public CollisionPair(Component<CollisionBody> a, Component<CollisionBody> b) {
-            set(a, b);
-        }
-
-        public void set(Component<CollisionBody> a, Component<CollisionBody> b) {
-            this.a = a;
-            this.b = b;
-        }
-
-        @Override
-        public boolean equals(Object o) {
-            if (!(o instanceof CollisionPair)) {
-                return false;
-            }
-            CollisionPair t = (CollisionPair) o;
-            return (t.a == a && t.b == b) || (t.b == a && t.a == b);
-        }
-
-        @Override
-        public int hashCode() {
-            // sum of hashes -> follow Set hashcode since a pair is just a 2 element set
-            return a.hashCode() + b.hashCode();
-        }
-    }
 }

ferox-util/src/main/java/com/ferox/util/Bag.java

      */
     public E get(int index) {
         if (index < 0 || index >= size) {
-            throw new IndexOutOfBoundsException("Index must be in [0, " + (size - 1) + "]");
+            throw new IndexOutOfBoundsException("Index must be in [0, " + (size - 1) + "], not: " + index);
         }
         return elements[index];
     }