Commits

Michael Ludwig  committed e4adcc9

Remove old GJK/EPA implementation in favor of new rewrite.

  • Participants
  • Parent commits 0aaae1e

Comments (0)

Files changed (9)

File ferox-physics/src/main/java/com/ferox/physics/collision/DefaultCollisionAlgorithmProvider.java

 import java.util.List;
 import java.util.Map;
 
-import com.ferox.physics.collision.algorithm.GjkEpaCollisionAlgorithm;
+import com.ferox.physics.collision.algorithm.GjkEpaCollisionAlgorithm2;
 import com.ferox.physics.collision.algorithm.SphereSphereCollisionAlgorithm;
 import com.ferox.physics.collision.algorithm.SwappingCollisionAlgorithm;
 
         algorithmCache = new HashMap<TypePair, CollisionAlgorithm<?,?>>();
         lookup = new TypePair(null, null);
         
-        register(new com.ferox.physics.collision.algorithm.GjkEpaCollisionAlgorithm2());
+        register(new GjkEpaCollisionAlgorithm2());
         register(new SphereSphereCollisionAlgorithm());
     }
 

File ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/EPA.java

-package com.ferox.physics.collision.algorithm;
-
-import com.ferox.math.Const;
-import com.ferox.math.Vector3;
-import com.ferox.physics.collision.algorithm.Simplex.Vertex;
-import com.ferox.util.Bag;
-
-
-/**
- * <p>
- * EPA is a low-level implementation of the Expanding Polytope Algorithm for
- * detecting the closest pairs between two intersecting convex hulls. The
- * algorithm was originally presented in
- * "Proximity Queries and Penetration Depth Computation on 3D Game Objects" by
- * Gino van den Bergen (as far as I can this was the first).
- * </p>
- * <p>
- * The code within this class is a Java port, refactoring and clean-up of the
- * EPA implementation contained in the Bullet physics engine. Specifically, the
- * code in "/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp" by Nathanael
- * Presson.
- * </p>
- * 
- * @author Michael Ludwig
- * @author Nathanael Presson
- */
-public class EPA {
-    /**
-     * The Status enum represents the various states that an evaluation of EPA
-     * can take. Only the state VALID represents a correct run of the EPA
-     * algorithm.
-     */
-    public static enum Status {
-        VALID, DEGENERATED, NON_CONVEX, INVALID_HULL, FAILED
-    }
-    
-    private static final int EPA_MAX_ITERATIONS = 255;
-    private static final double EPA_ACCURACY = .00001;
-    private static final double EPA_PLANE_EPS = .00001;
-    private static final double EPA_INSIDE_EPS = .00001;
-    
-    private static final int[] I1M3 = new int[] { 1, 2, 0 };
-    private static final int[] I2M3 = new int[] { 2, 0, 1 };
-    
-
-    private MinkowskiDifference shape;
-    private Simplex gjkSimplex;
-    
-    private Vector3 normal;
-    private Bag<Face> hull;
-    
-    private Simplex simplex;
-    private double depth;
-    private Status status;
-
-    /**
-     * Construct an EPA instance that will use the end result of the given GJK
-     * to determine the correct intersection between the two convex hulls.
-     * 
-     * @param gjk GJK whose evaluated simplex is used to start the EPA algorithm
-     * @throws NullPointerException if gjk is null
-     */
-    public EPA(GJK gjk) {
-        this(gjk.getMinkowskiDifference(), gjk.getSimplex());
-    }
-    
-    public EPA(MinkowskiDifference shape, Simplex gjk) {
-        depth = 0f;
-        this.shape = shape;
-        this.gjkSimplex = gjk;
-    }
-
-    /**
-     * Return the simplex containing the information needed to reconstruct the closest
-     * pair, if the status returned by {@link #getStatus()} is VALID. Any other
-     * status implies that the Simplex may be inaccurate or invalid.
-     * 
-     * @return The Simplex after running the EPA algorithm.
-     */
-    public Simplex getSimplex() {
-        return simplex;
-    }
-
-    /**
-     * Return the depth of penetration after {@link #evaluate(Vector3)}
-     * is invoked. If the status of EPA is not VALID, the depth is invalid.
-     * 
-     * @return The penetration depth
-     */
-    public double getDepth() {
-        return depth;
-    }
-
-    /**
-     * Return the contact normal between the two intersecting convex hulls from
-     * the last invocation of {@link #evaluate(Vector3)}. If the status
-     * is not VALID, the returned normal is invalid and possibly null.
-     * 
-     * @return The contact normal from A to B
-     */
-    public @Const Vector3 getNormal() {
-        return normal;
-    }
-
-    /**
-     * <p>
-     * Evaluate the EPA algorithm using the given vector as the initial guess
-     * for contact normal between the two intersecting shapes. The EPA is
-     * performed using the simplex of the GJK passed into the constructor. It is
-     * assumed that said simplex has been left unmodified since the GJK was
-     * evaluated.
-     * </p>
-     * <p>
-     * After this method is invoked, {@link #getStatus()} returns the Status
-     * enum specifying the correctness or validity of the EPA computaitons. If
-     * it is not VALID, then the simplex returned by {@link #getSimplex()} is
-     * invalid, otherwise it can be used to reconstruct the closest pair of
-     * points on each convex hull.
-     * </p>
-     * 
-     * @param guess The initial guess
-     * @return The Status of the evaluation
-     * @throws NullPointerException if guess is null
-     */
-    public Status evaluate(@Const Vector3 guess) {
-        if (guess == null)
-            throw new NullPointerException("Guess cannot be null");
-        
-        // we assume that the simplex of the GJK contains
-        // the origin, otherwise behavior is undefined
-        Simplex simplex = gjkSimplex;
-        MinkowskiDifference function = shape;
-        
-        normal = new Vector3();
-        hull = new Bag<Face>();
-        status = Status.FAILED;
-        
-        if (simplex.getRank() > 1 && simplex.encloseOrigin(function)) {
-            status = Status.VALID;
-            
-            // build initial hull
-            Face f1 = new Face(simplex.getVertex(0), simplex.getVertex(1), simplex.getVertex(2), true);
-            Face f2 = new Face(simplex.getVertex(1), simplex.getVertex(0), simplex.getVertex(3), true);
-            Face f3 = new Face(simplex.getVertex(2), simplex.getVertex(1), simplex.getVertex(3), true);
-            Face f4 = new Face(simplex.getVertex(0), simplex.getVertex(2), simplex.getVertex(3), true);
-            
-            if (hull.size() == 4) {
-                Face best = findBest();
-                Face outer = best;
-                int pass = 0;
-                
-                bind(f1, 0, f2, 0);
-                bind(f1, 1, f3, 0);
-                bind(f1, 2, f4, 0);
-                bind(f2, 1, f4, 2);
-                bind(f2, 2, f3, 1);
-                bind(f3, 2, f4, 1);
-                
-                Horizon horizon = new Horizon();
-                boolean valid;
-                double wdist;
-                for (int iter = 0; iter < EPA_MAX_ITERATIONS; iter++) {
-                    // reset the horizon for the next iteration
-                    horizon.cf = null; 
-                    horizon.ff = null; 
-                    horizon.numFaces = 0;
-                    
-                    // calculate the next vertex to go into the hull
-                    Vertex w = new Vertex();
-                    valid = true;
-                    best.pass = ++pass;
-                    
-                    w.getInputVector().normalize(best.normal);
-                    function.getSupport(w.getInputVector(), w.getVertex());
-                    wdist = best.normal.dot(w.getVertex()) - best.d;
-                    
-                    if (wdist > EPA_ACCURACY) {
-                        for (int j = 0; j < 3 && valid; j++) {
-                            valid &= expand(pass, w, best.adjacent[j], 
-                                            best.faceIndex[j], horizon);
-                        }
-                        
-                        if (valid && horizon.numFaces >= 3) {
-                            bind(horizon.cf, 1, horizon.ff, 2);
-                            best.remove();
-                            best = findBest();
-                                outer = best;
-                        } else {
-                            status = Status.INVALID_HULL;
-                            break;
-                        }
-                    } else {
-                        // accuracy reached, but the simplex should be valid
-                        break;
-                    }
-                }
-                
-                Vector3 projection = new Vector3().scale(outer.normal, outer.d);
-                
-                normal.set(outer.normal);
-                depth = outer.d;
-                
-                Vector3 t1 = new Vector3();
-                Vector3 t2 = new Vector3();
-                
-                double w1 = t1.sub(outer.vertices[1].getVertex(), projection).cross(t2.sub(outer.vertices[2].getVertex(), projection)).length();
-                double w2 = t1.sub(outer.vertices[2].getVertex(), projection).cross(t2.sub(outer.vertices[0].getVertex(), projection)).length();
-                double w3 = t1.sub(outer.vertices[0].getVertex(), projection).cross(t2.sub(outer.vertices[1].getVertex(), projection)).length();
-
-                double sum = w1 + w2 + w3;
-                
-                outer.vertices[0].setWeight(w1 / sum);
-                outer.vertices[1].setWeight(w2 / sum);
-                outer.vertices[2].setWeight(w3 / sum);
-                this.simplex = new Simplex(outer.vertices[0], outer.vertices[1], outer.vertices[2]);
-                return status;
-            }
-        }
-        
-        return Status.FAILED;
-    }
-        
-    private Face findBest() {
-        Face minf = hull.get(0);
-        double mind = minf.d * minf.d;
-        
-        Face f;
-        double sqd;
-        int ct = hull.size();
-        for (int i = 1; i < ct; i++) {
-            f = hull.get(i);
-            sqd = f.d * f.d;
-            if (sqd < mind) {
-                minf = f;
-                mind = sqd;
-            }
-        }
-        
-        return minf;
-    }
-    
-    private boolean expand(int pass, Vertex w, Face face, int index, Horizon horizon) {
-        if (face.pass != pass) {
-            int e1 = I1M3[index];
-            if (face.normal.dot(w.getVertex()) - face.d < -EPA_PLANE_EPS) {
-                Face nf = new Face(face.vertices[e1], face.vertices[index], w, false);
-                if (nf.hullIndex >= 0) {
-                    bind(nf, 0, face, index);
-                    if (horizon.cf != null)
-                        bind(horizon.cf, 1, nf, 2);
-                    else
-                        horizon.ff = nf;
-                    
-                    horizon.cf = nf;
-                    horizon.numFaces++;
-                    return true;
-                }
-            } else {
-                int e2 = I2M3[index];
-                face.pass = pass;
-                if (expand(pass, w, face.adjacent[e1], face.faceIndex[e1], horizon) &&
-                    expand(pass, w, face.adjacent[e2], face.faceIndex[e2], horizon)) {
-                    face.remove();
-                    return true;
-                }
-            }
-        }
-        
-        return false;
-    }
-    
-    private static double edgeDistance(@Const Vector3 va, @Const Vector3 vb, @Const Vector3 normal) {
-        Vector3 ba = new Vector3().sub(vb, va);
-        Vector3 nab = new Vector3().cross(ba, normal); // outward facing edge normal direction on triangle plane
-        
-        double aDotNAB = va.dot(nab); // only care about sign to determine inside/outside, no normalization required
-        
-        if (aDotNAB < 0) {
-            // outside of edge a->b
-            double aDotBA = va.dot(ba);
-            double bDotBA = vb.dot(ba);
-            
-            if (aDotBA > 0) {
-                // pick distance to vertex a
-                return va.length();
-            } else if (bDotBA < 0) {
-                // pick distance to vertex b
-                return vb.length();
-            } else {
-                // pick distance to edge a->b
-                double aDotB = va.dot(vb);
-                double d2 = (va.lengthSquared() * vb.lengthSquared() - aDotB * aDotB) / ba.lengthSquared();
-                return Math.sqrt(Math.max(d2, 0.0));
-            }
-        } else {
-            return -1.0;
-        }
-    }
-    
-    private static class Horizon {
-        Face cf;
-        Face ff;
-        int numFaces;
-    }
-    
-    private class Face {
-        final Vector3 normal;
-        double d;
-        double p;
-        
-        final Vertex[] vertices;
-        final Face[] adjacent;
-        final int[] faceIndex;
-        
-        int pass;
-        int hullIndex;
-        
-        public Face(Vertex a, Vertex b, Vertex c, boolean force) {
-            adjacent = new Face[3];
-            faceIndex = new int[3];
-            pass = 0;
-            
-            vertices = new Vertex[] { a, b, c };
-            normal = new Vector3().sub(b.getVertex(), a.getVertex()).cross(new Vector3().sub(c.getVertex(), a.getVertex()));
-            double l = normal.length();
-            boolean v = l > EPA_ACCURACY;
-            
-            double invL = 1.0 / l;
-            double d1 = a.getVertex().dot(new Vector3().cross(normal, new Vector3().sub(a.getVertex(), b.getVertex())));
-            double d2 = b.getVertex().dot(new Vector3().cross(normal, new Vector3().sub(b.getVertex(), c.getVertex())));
-            double d3 = c.getVertex().dot(new Vector3().cross(normal, new Vector3().sub(c.getVertex(), a.getVertex())));
-
-            p = Math.min(Math.min(d1, d2), d3) * (v ? invL : 1.0);
-            if (p >= -EPA_INSIDE_EPS)
-                p = 0.0;
-            
-            hullIndex = -1;
-            if (v) {
-                d = edgeDistance(a.getVertex(), b.getVertex(), normal);
-                if (d < 0) {
-                    d = edgeDistance(b.getVertex(), c.getVertex(), normal);
-                    if (d < 0) {
-                        d = edgeDistance(c.getVertex(), a.getVertex(), normal);
-                        if (d < 0) {
-                            // origin projects to the interior of the triangle,
-                            // so use the distance to the triangle plane
-                            d = a.getVertex().dot(normal) / l;
-                        }
-                    }
-                }
-                
-                normal.scale(1 / l);
-                if (force || d >= -EPA_PLANE_EPS) {
-                    hull.add(this);
-                    hullIndex = hull.size() - 1;
-                } else
-                    status = Status.NON_CONVEX;
-            } else
-                status = Status.DEGENERATED;
-        }
-        
-        public void remove() {
-            hull.remove(hullIndex);
-            if (hullIndex != hull.size())
-                hull.get(hullIndex).hullIndex = hullIndex;
-        }
-    }
-    
-    private static void bind(Face f1, int i1, Face f2, int i2) {
-        f1.faceIndex[i1] = i2;
-        f1.adjacent[i1] = f2;
-        
-        f2.faceIndex[i2] = i1;
-        f2.adjacent[i2] = f1;
-    }
-}

File ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/GJK.java

-package com.ferox.physics.collision.algorithm;
-
-import com.ferox.math.Const;
-import com.ferox.math.Vector3;
-
-/**
- * <p>
- * GJK is a low-level implementation of the Gilbert-Johnson-Keerth algorithm for
- * detecting the closest pairs between two non-intersecting convex hulls. The
- * algorithm was originally presented in a paper titled: "A Fast Procedure for
- * Computing the Distance Between Complex Objects in Three-Dimensional Space".
- * </p>
- * <p>
- * The code within this class is a Java port, refactoring and clean-up of the
- * GJK implementation contained in the Bullet physics engine. Specifically, the
- * code in "/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp" by Nathanael
- * Presson.
- * </p>
- * 
- * @author Michael Ludwig
- * @author Nathanael Presson
- */
-public class GJK {
-    public static final int GJK_MAX_ITERATIONS = 128;
-    public static final double GJK_MIN_DISTANCE = .00001;
-    public static final double GJK_DUPLICATE_EPS = .0001;
-    public static final double GJK_ACCURACY = .00001;
-
-    /**
-     * Status represents the three states that a GJK evaluation can take. A
-     * state of VALID means that the convex shapes involved are not
-     * intersecting. A state of INSIDE means the shapes are intersecting, but
-     * contact points are unknown. A state of FAILED implies inconsistent data
-     * means the GJK evaluation failed.
-     */
-    public static enum Status {
-        VALID, INSIDE, FAILED
-    }
-    
-    private final MinkowskiDifference function;
-    private Simplex simplex;
-
-    /**
-     * Create a new GJK instance that will evaluate the pair of convex shapes
-     * represented by the given {@link MinkowskiDifference}.
-     * 
-     * @param support The MinkowskiDifference to evaluate
-     * @throws NullPointerException if shape is null
-     */
-    public GJK(MinkowskiDifference support) {
-        if (support == null)
-            throw new NullPointerException("MinkowskiDifference cannot be null");
-        
-        function = support;
-        simplex = null;
-    }
-    
-    /**
-     * Return the MinkowskiDifference used to build this GJK's Simplex.
-     * 
-     * @return The MinkowskiDifference
-     */
-    public MinkowskiDifference getMinkowskiDifference() {
-        return function;
-    }
-
-    /**
-     * Return the Simplex containing the results of the last run to
-     * {@link #evaluate(ReadOnlyVector3f)}. If the GJK evaluation was VALID, the
-     * returned Simplex can be used to reconstruct the closest pair. A status of
-     * INSIDE means the returned Simplex can be used with {@link EPA}. A status
-     * of FAILURE means the Simplex may be suitable for use with an EPA, but
-     * numerical errors may result.
-     * 
-     * @return The Simplex formed from the last evaluation
-     */
-    public Simplex getSimplex() {
-        return simplex;
-    }
-    
-    /**
-     * Run the GJK algorithm on the {@link MinkowskiDifference} specified when
-     * the instance was constructed.
-     * 
-     * @param guess An initial guess as to the contact normal between the two
-     *            convex shapes
-     * @return The status of the evaluation
-     * @throws NullPointerException if guess is null
-     */
-    public Status evaluate(@Const Vector3 guess) {
-        if (guess == null)
-            throw new NullPointerException("Guess cannot be null");
-        
-        Status status = Status.VALID;
-        simplex = new Simplex();
-        Vector3 ray = new Vector3(guess);
-        if (ray.lengthSquared() < GJK_MIN_DISTANCE * GJK_MIN_DISTANCE)
-            ray.set(-1, 0, 0); // for 0-vector guess, choose arbitrary vector to start
-        
-        int iterations = 0;
-        double alpha = 0.0;
-        
-        simplex.addVertex(function, ray, true);
-        simplex.getVertex(0).setWeight(1);
-        ray.set(simplex.getVertex(0).getVertex());
-
-        // old support values are tracked so we can terminate when a duplicate is 
-        // returned in subsequent iterations
-        Vector3[] oldSupports = new Vector3[] { new Vector3(ray), new Vector3(ray), 
-                                                new Vector3(ray), new Vector3(ray) };
-        
-        int lastSupportIndex = 0;
-
-        double rayLength;
-        Vector3 support;
-        while(status == Status.VALID) {
-            rayLength = ray.length();
-            if (rayLength < GJK_MIN_DISTANCE) {
-                // touching or inside
-                status = Status.INSIDE;
-                break;
-            }
-
-            // add the new vertex
-            simplex.addVertex(function, ray, true);
-            support = simplex.getVertex(simplex.getRank() - 1).getVertex();
-            
-            // check for duplicates
-            boolean duplicate = false;
-            for (int i = 0; i < 4; i++) {
-                if (support.epsilonEquals(oldSupports[i], GJK_DUPLICATE_EPS)) {
-                    duplicate = true;
-                    break;
-                }
-            }
-            
-            if (duplicate) {
-                // return the old simplex since we didn't add any vertices
-                simplex.removeVertex();
-                break;
-            } else {
-                // update the old support storage
-                lastSupportIndex = (lastSupportIndex + 1) & 3;
-                oldSupports[lastSupportIndex].set(support);
-            }
-            
-            // check for termination
-            alpha = Math.max(ray.dot(support) / rayLength, alpha);
-            if ((rayLength - alpha) - (GJK_ACCURACY * rayLength) <= 0.0) {
-                // error threshold is small enough so we can terminate
-                simplex.removeVertex();
-                break;
-            }
-            
-            // reduce the simplex
-            if (simplex.reduce()) {
-                // the simplex is valid, compute next guess
-                ray.set(0.0, 0.0, 0.0);
-                for (int i = 0; i < simplex.getRank(); i++)
-                    ray.add(new Vector3().scale(simplex.getVertex(i).getVertex(), simplex.getVertex(i).getWeight()));
-                
-                // terminate if the simplex is full
-                if (simplex.getRank() == 4)
-                    status = Status.INSIDE;
-            } else {
-                // terminate using the old simplex
-                simplex.removeVertex();
-//                status = Status.FAILED;
-                break;
-            }
-            
-            iterations++;
-            if (iterations > GJK_MAX_ITERATIONS)
-                status = Status.FAILED;
-        }
-        
-        return status;
-    }
-}

File ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/GjkEpaCollisionAlgorithm.java

-package com.ferox.physics.collision.algorithm;
-
-import com.ferox.math.Const;
-import com.ferox.math.Matrix4;
-import com.ferox.math.Vector3;
-import com.ferox.physics.collision.ClosestPair;
-import com.ferox.physics.collision.CollisionAlgorithm;
-import com.ferox.physics.collision.shape.ConvexShape;
-
-/**
- * <p>
- * The GjkEpaCollisionAlgorithm is a CollisionAlgorithm implementation that uses
- * {@link GJK} and {@link EPA} to compute accurate closest pair information
- * between two convex hulls. If the two objects are separated, it can rely
- * solely on the GJK algorithm otherwise it will automatically use the EPA
- * algorithm and report an intersection.
- * </p>
- * <p>
- * This pair detector implementation also uses a trick to improve efficiency.
- * The GJK algorithm is much faster than the EPA algorithm. When testing pairs
- * of objects, the detector 'shrinks' each object so that in many cases
- * intersection between the original pair of objects is a separation between the
- * smaller pair. This separation can be accurately computed with only GJK and
- * the GjkEpaCollisionAlgorithm can quickly transform the pair back to the
- * original collision space.
- * </p>
- * <p>
- * This CollisionAlgorithm only supports collisions between {@link ConvexShape
- * ConvexShapes}.
- * </p>
- * 
- * @author Michael Ludwig
- */
-public class GjkEpaCollisionAlgorithm implements CollisionAlgorithm<ConvexShape, ConvexShape> {
-    private static final int MAX_EPA_CHECKS = 4;
-    
-    @Override
-    public ClosestPair getClosestPair(ConvexShape shapeA, @Const Matrix4 transA,
-                                      ConvexShape shapeB, @Const Matrix4 transB) {
-        // MinkowskiDifference does the error checking for GjkEpaCollisionAlgorithm
-        MinkowskiDifference support = new MinkowskiDifference(shapeA, transA, shapeB, transB);
-        support.setNumAppliedMargins(0);
-        GJK gjk = new GJK(support);
-        
-        // FIXME this code is duplicated in a number of places, particularly MinkowskiDifference,
-        // but in MD it needs to mutate the vectors, another place is in SphereSphereCollisionAlgorithm
-        Vector3 pa = new Vector3(transA.m03, transA.m13, transA.m23);
-        Vector3 pb = new Vector3(transB.m03, transB.m13, transB.m23);
-        
-        ClosestPair p = null;
-        Vector3 guess = new Vector3().sub(pb, pa);
-        if (gjk.evaluate(guess) == GJK.Status.VALID) {
-            // non-intersecting pair
-           p = support.getClosestPair(gjk.getSimplex(), null);
-           if (p != null)
-               return p;
-        } 
-        
-        EPA epa = new EPA(gjk);
-        for (int i = 1; i <= MAX_EPA_CHECKS; i++) {
-            // intersection or failure, fall back onto EPA
-            // must re-run the GJK with scaling so that the simplex is in the correct space
-            support.setNumAppliedMargins(i);
-
-            if (gjk.evaluate(guess) == GJK.Status.VALID) {
-                p = support.getClosestPair(gjk.getSimplex(), null);
-                if (p != null)
-                    return p;
-            }
-
-            EPA.Status status = epa.evaluate(guess);
-            if (status == EPA.Status.VALID) {
-                // epa successfully determined an intersection
-                p = support.getClosestPair(epa.getSimplex(), epa.getNormal());
-                if (p != null)
-                    return p;
-            }
-        }
-        
-        return null;
-    }
-
-    @Override
-    public Class<ConvexShape> getShapeTypeA() {
-        return ConvexShape.class;
-    }
-
-    @Override
-    public Class<ConvexShape> getShapeTypeB() {
-        return ConvexShape.class;
-    }
-}

File ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/MinkowskiDifference.java

-package com.ferox.physics.collision.algorithm;
-
-import com.ferox.math.Const;
-import com.ferox.math.Matrix4;
-import com.ferox.math.Vector3;
-import com.ferox.math.Vector4;
-import com.ferox.physics.collision.ClosestPair;
-import com.ferox.physics.collision.CollisionBody;
-import com.ferox.physics.collision.shape.ConvexShape;
-
-/**
- * MinkowskiDifference represents the mathematical concept of a Minkowski sum
- * between two convex shapes. Because {@link GJK} and {@link EPA} are interested
- * in the difference between two shapes, the second shape in the sum is just the
- * negation of one convex shape. This implementation also automatically handles
- * converting the local support functions of {@link ConvexShape ConvexShapes}
- * into world space by applying the world transform of {@link CollisionBody
- * Collidables} involved.
- * 
- * @author Michael Ludwig
- */
-public class MinkowskiDifference {
-    private static final double CONTACT_NORMAL_ACCURACY = .0001;
-    
-    private ConvexShape shapeA;
-    private ConvexShape shapeB;
-    
-    private Matrix4 transA; // transforms a to world
-    private Matrix4 transB; // transforms b to world
-    
-    private int appliedMargins;
-    
-    // final variables to reuse during computations, should be faster than thread-local
-//    private final Vector3 supportCache;
-//    private final Vector3 dirCache;
-//    private final Vector4 transformCache;
-
-    /**
-     * Create a new MinkowskiDifference between the two Shape objects. The
-     * MinkowskiDifference will automatically apply the provided world
-     * transforms associated with each shape, and take into account the margins
-     * of the Shapes.
-     * 
-     * @param shapeA The first shape involved in the minkowski difference
-     * @param transA The first shape's world transform
-     * @param shapeB The second object involved in the minkowski difference
-     * @param transB The second shape's world transform
-     * @throws NullPointerException if any arguments are null
-     */
-    public MinkowskiDifference(ConvexShape shapeA, @Const Matrix4 transA,
-                               ConvexShape shapeB, @Const Matrix4 transB) {
-        if (shapeA == null || shapeB == null || transA == null || transB == null)
-            throw new NullPointerException("Arguments cannot be null");
-//        supportCache = new Vector3();
-//        dirCache = new Vector3();
-//        transformCache = new Vector4();
-        
-        this.shapeA = shapeA;
-        this.shapeB = shapeB;
-        this.transA = transA;
-        this.transB = transB;
-        
-        appliedMargins = 1;
-    }
-
-    public MinkowskiShape asShape() {
-        MinkowskiShape s = new MinkowskiShape(shapeA, transA, shapeB, transB);
-        s.setAppliedMargins(appliedMargins);
-        return s;
-    }
-    
-    /**
-     * Set the number of times the MinkowskiDifference will apply each
-     * ConvexShape's margin when
-     * {@link #getSupport(ReadOnlyVector3f, MutableVector3f)} is called. This
-     * number does not affect the result of
-     * {@link #getClosestPair(Simplex, ReadOnlyVector3f)}, which always applies
-     * a single margin for each shape.
-     * 
-     * @param num The number of margin applications
-     * @throws IllegalArgumentException if num is less than 0
-     */
-    public void setNumAppliedMargins(int num) {
-        if (num < 0)
-            throw new IllegalArgumentException("Number of applied margins must be at least 0, not: " + num);
-        appliedMargins = num;
-    }
-
-    /**
-     * Build a closest pair from the given Simplex and this MinkowskiDifference.
-     * The specified simplex should have been built by the GJK or EPA algorithms
-     * using this MinkowskiDifference. The returned pair will always applied a
-     * single margin to each ConvexShape, regardless of
-     * {@link #setNumAppliedMargins(int)}.
-     * 
-     * @param simplex The simplex used to build the closest pair
-     * @param zeroNormal An optional normal vector to use when the closest
-     *            pair's distance is 0
-     * @return A ClosestPair formed by the simplex, or null if it could not be
-     *         computed
-     * @throws NullPointerException if simplex is null
-     */
-    public ClosestPair getClosestPair(Simplex simplex, @Const Vector3 zeroNormal) {
-        Vector3 pa = new Vector3(transA.m03, transA.m13, transA.m23);
-        Vector3 pb = new Vector3(transB.m03, transB.m13, transB.m23);
-        
-        Vector3 ma = getClosestPointA(simplex);
-        Vector3 mb = getClosestPointB(simplex);
-        
-        return constructPair(ma, mb, pa, pb, zeroNormal);
-    }
-    
-    private ClosestPair constructPair(Vector3 wA, Vector3 wB, 
-                                      Vector3 pA, Vector3 pB,
-                                      @Const Vector3 zeroNormal) {
-        boolean intersecting = (pA.distanceSquared(wB) < pA.distanceSquared(wA)) ||
-                               (pB.distanceSquared(wA) < pB.distanceSquared(wB));
-        // after this, pA and pB are free vectors to be mutated
-        
-//        Vector3 normal = wB.sub(wA); // wB becomes the normal here
-        Vector3 normal = new Vector3().sub(wB, wA);
-        double distance = normal.length() * (intersecting ? -1.0 : 1.0);
-
-        if (Math.abs(distance) < CONTACT_NORMAL_ACCURACY) {
-            // special case for very close contact points, where the normal might become inaccurate
-            if (zeroNormal != null) {
-                normal.normalize(zeroNormal);
-                if (intersecting)
-                    normal.scale(-1f);
-            } else
-                return null;
-        } else {
-            // normalize, and possibly flip the normal based on intersection
-            normal.scale(1.0 / distance);
-        }
-        
-        if (appliedMargins != 1) {
-            int scale = Math.abs(appliedMargins - 1);
-            double distDelta = scale * (shapeA.getMargin() + shapeB.getMargin());
-            if (intersecting)
-                distDelta *= -1.0;
-            
-//            wA.add(pA.scale(normal, (1 - appliedMargins) * shapeA.getMargin()));
-            wA.add(new Vector3().scale(normal, (1 - appliedMargins) * shapeA.getMargin()));
-
-            if ((appliedMargins == 0 && intersecting) || (appliedMargins > 1 && !intersecting)) {
-                // moving to one margin increases distance
-                distance += distDelta;
-            } else {
-                // moving to one margin decreases distance
-                distance -= distDelta;
-            }
-        }
-
-        return new ClosestPair(wA, normal, distance);
-    }
-
-
-    private Vector3 getClosestPointA(Simplex simplex) {
-        Vector3 result = new Vector3();
-        for (int i = 0; i < simplex.getRank(); i++) {
-            // sum weighted supports from simplex
-            Vector3 s = new Vector3();
-            getAffineSupport(shapeA, transA, simplex.getVertex(i).getInputVector(), s);
-            s.scale(simplex.getVertex(i).getWeight());
-            result.add(s);
-//            getAffineSupport(shapeA, transA, simplex.getVertex(i).getInputVector(), supportCache);
-//            result.add(supportCache.scale(simplex.getVertex(i).getWeight()));
-        }
-        return result;
-    }
-
-    private Vector3 getClosestPointB(Simplex simplex) {
-        Vector3 result = new Vector3();
-        for (int i = 0; i < simplex.getRank(); i++) {
-            // sum weighted supports from simplex
-            Vector3 s = new Vector3();
-            getAffineSupport(shapeB, transB, new Vector3().scale(simplex.getVertex(i).getInputVector(), -1.0), s);
-            s.scale(simplex.getVertex(i).getWeight());
-            result.add(s);
-//            getAffineSupport(shapeB, transB, supportCache.scale(simplex.getVertex(i).getInputVector(), -1.0), supportCache);
-//            result.add(supportCache.scale(simplex.getVertex(i).getWeight()));
-        }
-        return result;
-    }
-
-    /**
-     * <p>
-     * Compute the support function of this MinkowskiDifference. The support of
-     * a minkowski difference is <code>Sa(d) - Sb(-d)</code> where <tt>Sa</tt>
-     * and <tt>Sb</tt> are the support functions of the convex shapes after
-     * applying their world transforms. Each ConvexShape will have their
-     * configured margins applied a number of times depending on the last call
-     * to {@link #setNumAppliedMargins(int)}.
-     * </p>
-     * <p>
-     * The support is stored in result if result is non-null. If result is null
-     * a new vector is created and returned instead. When result is non-null,
-     * result is returned.
-     * </p>
-     * 
-     * @param d The input to the support function
-     * @param result The result to store the support in, may be null
-     * @return The support, stored in result or a new vector if result was null
-     * @throws NullPointerException if d is null
-     */
-    public Vector3 getSupport(@Const Vector3 d, Vector3 result) {
-        if (result == null)
-            result = new Vector3();
-        
-        Vector3 a = new Vector3();
-        Vector3 b = new Vector3();
-        getAffineSupport(shapeA, transA, d, a);
-        getAffineSupport(shapeB, transB, new Vector3().scale(d, -1.0), b);
-//        System.out.println("get support " + d + " " + a + " " + b);
-        return result.sub(a, b);
-    }
-    
-    private void getAffineSupport(ConvexShape shape, @Const Matrix4 t, @Const Vector3 d, Vector3 result) {
-        // first step is to transform d by the transpose of the upper 3x3
-        // we do this by wrapping d in a 4-vector and setting w = 0
-//        transformCache.set(d.x, d.y, d.z, 0.0).mul(transformCache, t);
-        Vector4 transformed = new Vector4(d.x, d.y, d.z, 0.0);
-        transformed.mul(transformed, t);
-        
-        // second step is to compute the local support
-        Vector3 dir = new Vector3(transformed.x, transformed.y, transformed.z);
-//        dirCache.set(transformCache.x, transformCache.y, transformCache.z);
-//        shape.computeSupport(dirCache, result);
-        shape.computeSupport(dir, result);
-        
-        if (appliedMargins > 0) {
-            // apply a number of margin offsets, as if a sphere is added to the convex shape
-//            result.add(dirCache.scale(appliedMargins * shape.getMargin()));
-            result.add(new Vector3().scale(dir, appliedMargins * shape.getMargin()));
-        }
-        
-        // then transform that by the complete affine transform, so w = 1
-//        transformCache.set(result.x, result.y, result.z, 1.0).mul(t, transformCache);
-        transformed.set(result.x, result.y, result.z, 1.0);
-        transformed.mul(t, transformed);
-//        result.set(transformCache.x, transformCache.y, transformCache.z);
-        result.set(transformed.x, transformed.y, transformed.z);
-    }
-}

File ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/Simplex.java

-package com.ferox.physics.collision.algorithm;
-
-import java.util.Arrays;
-
-import com.ferox.math.Const;
-import com.ferox.math.Vector3;
-
-/**
- * <p>
- * In the mathematical sense, a Simplex is the generalization of a triangle to
- * arbitrary dimensions. In other terms, it's the smallest convex set containing
- * a set of vertices: a 0-simplex is a point, a 1-simplex is a line, a 2-simplex
- * is a triangle, etc. This implementation of Simplex is tied to the details of
- * the {@link GJK} and {@link EPA} algorithms and provides the storage they need
- * to represent their solutions.
- * </p>
- * <p>
- * Because of this, Simplex is not heavily configurable. It is not possible to
- * add arbitrary vertices but vertices are added by evaluating the support of
- * the MinkowskiDifference involved in the algorithms' iterations.
- * </p>
- * <p>
- * Like the GJK and EPA classes, much of this code is a port and clean-up of the
- * code from "BulletCollision/NarrowPhase/btGjkEpa2.cpp" written by Nathanael
- * Presson for the Bullet physics engine.
- * </p>
- * 
- * @author Michael Ludwig
- * @author Nathanael Presson
- */
-public class Simplex {
-    /**
-     * <p>
-     * Vertex is a utility class for Simplex that encapsulates the data used to
-     * create a vertex within the simplex. It contains the vertex location, the
-     * input vector used in the support function, and an assignable weight.
-     * </p>
-     * <p>
-     * Each vertex's weight in a simplex must be at least 0, and there is the
-     * assumed contract that they sum to 1.
-     * </p>
-     */
-    public static class Vertex {
-        private final Vector3 input;
-        private final Vector3 vertex;
-        private double weight;
-        
-        public Vertex() {
-            input = new Vector3();
-            vertex = new Vector3();
-            weight = 0;
-        }
-        
-        /**
-         * @return The input vector used to evaluate the support function
-         */
-        public @Const Vector3 getInputVector() {
-            return input;
-        }
-        
-        /**
-         * @return The vertex of the simplex
-         */
-        public @Const Vector3 getVertex() {
-            return vertex;
-        }
-        
-        /**
-         * @return The current weight of the simplex
-         */
-        public double getWeight() {
-            return weight;
-        }
-
-        /**
-         * Assign a new weight to the vertex. It is the callers responsibility
-         * to preserve the invariant that all vertices within a simplex have
-         * weights summing to 1.
-         * 
-         * @param w The new weight
-         * @throws IllegalArgumentException if w is less than 0
-         */
-        public void setWeight(double w) {
-            if (w < 0.0)
-                throw new IllegalArgumentException("Weight must be positive, not: " + w);
-            if (w < .0000001)
-                w = 0.0;
-            weight = w;
-        }
-        
-        public String toString() {
-            return "(i: " + input + ", v: " + vertex + ", w: " + weight + ")";
-        }
-    }
-    
-    public String toString() {
-        StringBuilder sb = new StringBuilder();
-        sb.append("[mask: ");
-        sb.append(mask);
-        sb.append(",\n");
-        for (int i = 0; i < rank; i++) {
-            if (i > 0)
-                sb.append(",\n");
-            sb.append(vertices[i]);
-        }
-        sb.append("]");
-        return sb.toString();
-    }
-    
-    private final Vertex[] vertices;
-    private int rank; // 0 -> 3
-    
-    private int mask;
-    // for use with SimplexUtil
-    private final int[] maskCache;
-    private final double[] weightCache;
-    
-    /**
-     * Create a Simplex that is initially empty and has a rank of 0.
-     */
-    public Simplex() {
-        vertices = new Vertex[4];
-        rank = 0;
-        
-        maskCache = new int[1];
-        weightCache = new double[4];
-    }
-    
-    public Simplex(Simplex2 o) {
-        this();
-        
-        rank = o.getRank();
-        for (int i = 0; i < rank; i++) {
-            vertices[i] = new Vertex();
-            vertices[i].weight = o.getWeight(i);
-            vertices[i].vertex.set(o.getVertex(i));
-            vertices[i].input.set(o.getInput(i));
-        }
-    }
-
-    /**
-     * Create a Simplex that uses the given vertice's as its three
-     * vertices. It will have a rank of 3, and it is assumed that the three
-     * provided weights sum to 1.
-     * 
-     * @param c1 The first vertex sample
-     * @param c2 The second vertex sample
-     * @param c3 The third vertex sample
-     * @param w1 The first vertex's weight
-     * @param w2 The second vertex's weight
-     * @param w3 The third vertex's weight
-     * @throws NullPointerException if c1, c2, or c3 are null
-     */
-    public Simplex(Vertex c1, Vertex c2, Vertex c3) {
-        if (c1 == null || c2 == null || c3 == null)
-            throw new NullPointerException("SupportSamples cannot be null");
-        vertices = new Vertex[] { c1, c2, c3, null};
-        rank = 3;
-        
-        maskCache = new int[1];
-        weightCache = new double[4];
-    }
-    
-    public void clear() {
-        rank = 0;
-    }
-
-    /**
-     * Return the vertex at the given index within this Simplex.
-     * @param i The vertex to return, assumed to be in [0, rank - 1]
-     * @return The SupportSample instance holding the support data for the given
-     *         vertex index
-     * @throws IndexOutOfBoundsException if i is invalid
-     */
-    public Vertex getVertex(int i) {
-        if (i < 0 || i >= rank)
-            throw new IndexOutOfBoundsException("Invalid index: " + i);
-        return vertices[i];
-    }
-
-    /**
-     * Return the rank, or number of vertices, of the Simplex. The rank can
-     * range from 0 to 4. A rank of 0 is an empty simplex, a rank of 1 is a
-     * point, 2 is a line, 3 is a triangle, and 4 is a tetrahedron.
-     * 
-     * @return The current rank
-     */
-    public int getRank() {
-        return rank;
-    }
-
-    /**
-     * Manipulate this simplex so that its vertices will enclose the origin.
-     * This is a necessary initial condition of the EPA algorithm after the GJK
-     * algorithm has been applied. The specified MinkowskiDifference is assumed
-     * to be the same shape that seeded the vertices of this Simplex.
-     * 
-     * @param shape The MinkowskiDifference controlling the support evaluation
-     *            for this Simplex
-     * @return True if the simplex could be modified to enclose the origin
-     * @throws NullPointerException if shape is null
-     */
-    public boolean encloseOrigin(MinkowskiDifference shape) {
-        if (shape == null)
-            throw new NullPointerException("MinkowskiDifference cannot be null");
-        
-        if (encloseOriginImpl(shape)) {
-            orient();
-            return true;
-        } else
-            return false;
-    }
-    
-    private boolean encloseOriginImpl(MinkowskiDifference shape) {
-        switch(rank) {
-        case 1: {
-            Vector3 axis = new Vector3();
-            for (int i = 0; i < 3; i++) {
-                addVertex(shape, axis.set(0.0, 0.0, 0.0).set(i, 1.0));
-                if (encloseOriginImpl(shape))
-                    return true;
-                removeVertex();
-                addVertex(shape, axis.scale(-1));
-                if (encloseOriginImpl(shape))
-                    return true;
-                removeVertex();
-            }
-            break; }
-        case 2: {
-            Vector3 d = new Vector3().sub(vertices[1].vertex, vertices[0].vertex);
-            Vector3 axis = new Vector3();
-            
-            for (int i = 0; i < 3; i++) {
-                axis.set(0, 0, 0).set(i, 1.0);
-                axis.cross(d, axis);
-                if (axis.lengthSquared() > 0) {
-                    addVertex(shape, axis);
-                    if (encloseOriginImpl(shape))
-                        return true;
-                    removeVertex();
-                    addVertex(shape, axis.scale(-1.0));
-                    if (encloseOriginImpl(shape))
-                        return true;
-                    removeVertex();
-                }
-            }
-            break; }
-        case 3: {
-            Vector3 n = new Vector3().sub(vertices[1].vertex, vertices[0].vertex).cross(new Vector3().sub(vertices[2].vertex, vertices[0].vertex));
-            if (n.lengthSquared() > 0.0) {
-                addVertex(shape, n);
-                if (encloseOriginImpl(shape))
-                    return true;
-                removeVertex();
-                addVertex(shape, n.scale(-1.0));
-                if (encloseOriginImpl(shape))
-                    return true;
-                removeVertex();
-            }
-            break; }
-        case 4: {
-            if (Math.abs(SimplexUtil.det(new Vector3().sub(vertices[0].vertex, vertices[3].vertex),
-                                         new Vector3().sub(vertices[1].vertex, vertices[3].vertex),
-                                         new Vector3().sub(vertices[2].vertex, vertices[3].vertex))) > 0.0)
-                return true;
-            break; }
-        }
-        return false;
-    }
-    
-    private void orient() {
-        if (SimplexUtil.det(new Vector3().sub(vertices[0].vertex, vertices[3].vertex),
-                            new Vector3().sub(vertices[1].vertex, vertices[3].vertex),
-                            new Vector3().sub(vertices[2].vertex, vertices[3].vertex)) < 0.0) {
-            Vector3 t = new Vector3();
-            
-            t.set(vertices[0].input);
-            vertices[0].input.set(vertices[1].input);
-            vertices[1].input.set(t);
-            
-            t.set(vertices[0].vertex);
-            vertices[0].vertex.set(vertices[1].vertex);
-            vertices[1].vertex.set(t);
-            
-            double tt = vertices[0].weight;
-            vertices[0].weight = vertices[1].weight;
-            vertices[1].weight = tt;
-        }
-    }
-
-    /**
-     * Add a new vertex to the Simplex by evaluating the support of the given
-     * MinkowskiDifference with the input vector <tt>d</tt>. The new vertex is
-     * added to the end and the simplex's rank is increased by 1.
-     * 
-     * @param s The MinkowskiDifference whose support function is evaluated
-     * @param d The input to the support function
-     * @throws NullPointerException if s or d are null
-     */
-    public void addVertex(MinkowskiDifference s, @Const Vector3 d) {
-        addVertex(s, d, false);
-    }
-
-    /**
-     * Equivalent to {@link #addVertex(MinkowskiDifference, ReadOnlyVector3f)}
-     * except that if <tt>negate</tt> is true, the negation of the input vector
-     * is used to evaluate the support function.
-     * 
-     * @param s The MinkowskiDifference whose support function is evaluated
-     * @param d The support function input
-     * @param negate True if <tt>d</tt> should be negated before being passed to
-     *            the support function
-     */
-    public void addVertex(MinkowskiDifference s, @Const Vector3 d, boolean negate) {
-        if (rank == 4)
-            throw new IllegalStateException("Cannot add a vertex to a full simplex");
-        if (vertices[rank] == null)
-            vertices[rank] = new Vertex();
-        Vertex v = vertices[rank++];
-        
-        v.weight = 0.0;
-        if (negate)
-            v.input.scale(d, -1.0).normalize();
-        else
-            v.input.normalize(d);
-        s.getSupport(v.input, v.vertex);
-    }
-    
-    /**
-     * Remove the last added vertex from the Simplex and reduce its rank by one.
-     */
-    public void removeVertex() {
-        rank--;
-    }
-    
-    /**
-     * Update the Simplex so that it represents the smallest sub-simplex of its
-     * vertices that still contains the last added vertex. If false is returned,
-     * the simplex could not be reduced further.
-     * 
-     * @return True if the simplex was successfully reduced, i.e. can be used
-     *         again in another iteration of the GJK algorithm
-     */
-    public boolean reduce() {
-        if (projectOrigin()) {
-            // the simplex is still valid, so compact it
-            int m = mask;
-            for (int i = 0; i < rank; i++) {
-                if ((m & (1 << i)) != 0) {
-                    // find lowest empty vertex slot
-                    int low = -1;
-                    for (int j = 0; j < i; j++) {
-                        if ((m & (1 << j)) == 0) {
-                            low = j;
-                            break;
-                        }
-                    }
-                    
-                    if (low >= 0) {
-                        // copy the ith vertex into low
-                        vertices[low].input.set(vertices[i].input);
-                        vertices[low].vertex.set(vertices[i].vertex);
-                        vertices[low].weight = vertices[i].weight;
-                        
-                        m |= (1 << low);
-                        m &= ~(1 << i);
-                    }
-                }
-            }
-            
-            // determine new rank after compaction
-            rank = 0;
-            for (int i = 3; i >= 0; i--) {
-                if ((m & (1 << i)) != 0) {
-                    rank = i + 1;
-                    break;
-                }
-            }
-            
-            return true;
-        } else
-            return false;
-    }
-    
-    /*
-     * Update the weights and mask to perform the actual math needed by reduce()
-     */
-    private boolean projectOrigin() {
-        Arrays.fill(weightCache, 0.0);
-        maskCache[0] = 0;
-        
-        double sqdist = 0.0;
-        switch(rank) {
-        case 2:
-            sqdist = simplexUtil.get().projectOrigin2(vertices[0].vertex, vertices[1].vertex, 
-                                                      weightCache, maskCache);
-            break;
-        case 3:
-            sqdist = simplexUtil.get().projectOrigin3(vertices[0].vertex, vertices[1].vertex,
-                                                      vertices[2].vertex, weightCache, maskCache);
-            break;
-        case 4:
-            sqdist = simplexUtil.get().projectOrigin4(vertices[0].vertex, vertices[1].vertex, 
-                                                      vertices[2].vertex, vertices[3].vertex, weightCache, maskCache);
-            break;
-        }
-        
-        if (sqdist >= 0.0) {
-            // copy weights back into member variables
-            for (int i = 0; i < rank; i++)
-                vertices[i].weight = weightCache[i];
-            mask = maskCache[0];
-            return true;
-        } else
-            return false;
-    }
-    
-    // because projecting takes up so much extra storage, we cache a SimplexUtil per thread
-    private static final ThreadLocal<SimplexUtil> simplexUtil = new ThreadLocal<SimplexUtil>() {
-        @Override
-        protected SimplexUtil initialValue() {
-            return new SimplexUtil();
-        }
-    };
-    
-    private static class SimplexUtil {
-        private static final int[] IMD3 = new int[] {1, 2, 0};
-
-        // used only in projectOrigin2
-        private final Vector3 p2 = new Vector3(); 
-        
-        // used only in projectOrigin3
-        private final Vector3[] vt3 = new Vector3[3];
-        private final Vector3[] dl3 = new Vector3[] { new Vector3(), new Vector3(), new Vector3() };
-        private final Vector3 n3 = new Vector3();
-        private final Vector3 p3 = new Vector3();
-        
-        private final double[] subw3 = new double[2];
-        private final int[] subm3 = new int[1];
-        
-        // used only in projectOrigin4
-        private final Vector3[] vt4 = new Vector3[4];
-        private final Vector3[] dl4 = new Vector3[] { new Vector3(), new Vector3(), new Vector3() };
-        private final Vector3 n4 = new Vector3();
-        private final Vector3 p4 = new Vector3();
-        
-        private final double[] subw4 = new double[3];
-        private final int[] subm4 = new int[1];
-        
-        public double projectOrigin2(@Const Vector3 a, @Const Vector3 b, double[] weights, int[] mask) {
-            Vector3 d = p2.sub(b, a);
-            double l = d.lengthSquared();
-            
-            if (l > 0.0) {
-                double t = -a.dot(d) / l;
-                if (t >= 1.0) {
-                    weights[0] = 0.0; 
-                    weights[1] = 1.0; 
-                    mask[0] = 2; 
-                    return b.lengthSquared();
-                } else if (t <= 0) {
-                    weights[0] = 1f; 
-                    weights[1] = 0f;
-                    mask[0] = 1; 
-                    return a.lengthSquared();
-                } else {
-                    weights[0] = 1 - t; 
-                    weights[1] = t; 
-                    mask[0] = 3;
-                    return d.scale(t).add(a).lengthSquared();
-                }
-            } else
-                return -1.0;
-        }
-        
-        public double projectOrigin3(@Const Vector3 a, @Const Vector3 b, @Const Vector3 c, double[] weights, int[] mask) {
-            vt3[0] = a; vt3[1] = b; vt3[2] = c;
-            dl3[0].sub(a, b);
-            dl3[1].sub(b, c);
-            dl3[2].sub(c, a);
-            
-            n3.cross(dl3[0], dl3[1]);
-            double l = n3.lengthSquared();
-            
-            if (l > 0.0) {
-                double minDist = -1.0;
-                subw3[0] = 0.0; subw3[1] = 0.0;
-                subm3[0] = 0;
-                
-                for (int i = 0; i < 3; i++) {
-                    if (vt3[i].dot(p3.cross(dl3[i], n3)) > 0.0) {
-                        int j = IMD3[i];
-                        double subd = projectOrigin2(vt3[i], vt3[j], subw3, subm3);
-                        if (minDist < 0.0 || subd < minDist) {
-                            minDist = subd;
-                            mask[0] = ((subm3[0] & 1) != 0 ? (1 << i) : 0) + 
-                                      ((subm3[0] & 2) != 0 ? (1 << j) : 0);
-                            weights[i] = subw3[0];
-                            weights[j] = subw3[1];
-                            weights[IMD3[j]] = 0.0;
-                        }
-                    }
-                }
-                
-                if (minDist < 0.0) {
-                    double d = a.dot(n3);
-                    double s = Math.sqrt(l);
-                    n3.scale(d / l);
-                    
-                    minDist = n3.lengthSquared();
-                    mask[0] = 7;
-                    weights[0] = dl3[1].cross(p3.sub(b, n3)).length() / s; // at this point dl[1] and p3 are throwaway
-                    weights[1] = dl3[2].cross(p3.sub(c, n3)).length() / s; // at this point dl[2] and p3 throwaway
-                    weights[2] = 1 - weights[0] - weights[1];
-                }
-                return minDist;
-            } else
-                return -1.0;
-        }
-        
-        public double projectOrigin4(@Const Vector3 a, @Const Vector3 b, @Const Vector3 c, @Const Vector3 d, double[] weights, int[] mask) {
-            vt4[0] = a; vt4[1] = b; vt4[2] = c; vt4[3] = d;
-            dl4[0].sub(a, d);
-            dl4[1].sub(b, d);
-            dl4[2].sub(c, d);
-            
-            double vl = det(dl4[0], dl4[1], dl4[2]);
-            boolean ng = (vl * a.dot(n4.sub(b, c).cross(p4.sub(a, b)))) <= 0.0;
-            
-            if (ng && Math.abs(vl) > 0.0) {
-                double minDist = -1.0;
-                subw4[0] = 0.0; subw4[1] = 0.0; subw4[2] = 0.0;
-                subm4[0] = 0;
-                
-                for (int i = 0; i < 3; i++) {
-                    int j = IMD3[i];
-                    double s = vl * d.dot(n4.cross(dl4[i], dl4[j]));
-                    if (s > 0.0) {
-                        double subd = projectOrigin3(vt4[i], vt4[j], d, subw4, subm4);
-                        if (minDist < 0.0 || subd < minDist) {
-                            minDist = subd;
-                            mask[0] = ((subm4[0] & 1) != 0 ? (1 << i) : 0) +
-                                      ((subm4[0] & 2) != 0 ? (1 << j) : 0) +
-                                      ((subm4[0] & 4) != 0 ? 8 : 0);
-                            weights[i] = subw4[0];
-                            weights[j] = subw4[1];
-                            weights[IMD3[j]] = 0.0;
-                            weights[3] = subw4[2];
-                        }
-                    }
-                }
-                
-                if (minDist < 0.0) {
-                    minDist = 0.0;
-                    mask[0] = 15;
-                    weights[0] = det(c, b, d) / vl;
-                    weights[1] = det(a, c, d) / vl;
-                    weights[2] = det(b, a, d) / vl;
-                    weights[3] = 1 - weights[0] - weights[1] - weights[2];
-                }
-                return minDist;
-            } else
-                return -1.0;
-        }
-        
-        public static double det(@Const Vector3 a, @Const Vector3 b, @Const Vector3 c) {
-            return a.y * b.z * c.x + a.z * b.x * c.y -
-                   a.x * b.z * c.y - a.y * b.x * c.z +
-                   a.x * b.y * c.z - a.z * b.y * c.x;
-        }
-    }
-}

File ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/Simplex2.java

         isIntersection = false;
     }
     
-    public Simplex2(MinkowskiShape shape, Simplex s) {
-        this(shape);
-        rank = s.getRank();
-        for (int i = 0; i < rank; i++) {
-            inputs[i].set(s.getVertex(i).getInputVector());
-            vertices[i].set(s.getVertex(i).getVertex());
-            weights[i] = s.getVertex(i).getWeight();
-        }
-    }
-    
     public MinkowskiShape getShape() {
         return shape;
     }

File ferox-physics/src/main/java/com/ferox/physics/collision/shape/ConvexShape.java

 import com.ferox.math.Const;
 import com.ferox.math.Vector3;
 import com.ferox.physics.collision.Shape;
-import com.ferox.physics.collision.algorithm.GjkEpaCollisionAlgorithm;
 
 /**
  * ConvexShape is a Shape type that represents a convex hull. It itself is not a

File ferox-physics/src/test/java/com/ferox/physics/CollisionTest.java

 import com.ferox.physics.collision.CollisionAlgorithm;
 import com.ferox.physics.collision.CollisionBody;
 import com.ferox.physics.collision.Shape;
-import com.ferox.physics.collision.algorithm.GjkEpaCollisionAlgorithm;
 import com.ferox.physics.collision.algorithm.GjkEpaCollisionAlgorithm2;
 import com.ferox.physics.collision.shape.ConvexShape;
 import com.ferox.renderer.Framework;
             }
         }, new KeyPressedCondition(KeyCode.SPACE));
         
-        // toggle algorithm
-        io.addTrigger(new Trigger() {
-            @Override
-            public void onTrigger(InputState prev, InputState next) {
-                setup.toggleAlgorithm();
-            }
-        }, new KeyPressedCondition(KeyCode.C));
-        
         io.addTrigger(new Trigger() {
             @Override
             public void onTrigger(InputState prev, InputState next) {
             this.collision = collision;
             
             active = a;
-            algo = new GjkEpaCollisionAlgorithm();
+            algo = new GjkEpaCollisionAlgorithm2();
             collision.get(Renderable.ID, true).setEnabled(false);
         }
         
             }
         }
         
-        public void toggleAlgorithm() {
-            if (algo instanceof GjkEpaCollisionAlgorithm) {
-                algo = new GjkEpaCollisionAlgorithm2();
-            } else if (algo instanceof GjkEpaCollisionAlgorithm2) {
-                algo = new GjkEpaCollisionAlgorithm();
-            }
-            System.out.println("Changed algorithm to " + algo.getClass());
-            updateCollision();
-        }
-        
         public void toggleActive() {
             if (active == a)
                 active = b;