/* * Phys2D - a 2D physics engine based on the work of Erin Catto. * * This source is provided under the terms of the BSD License. * * Copyright (c) 2006, Phys2D * All rights reserved. * * Redistribution and use in source and binary forms, with or * without modification, are permitted provided that the following * conditions are met: * * * Redistributions of source code must retain the above * copyright notice, this list of conditions and the * following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the Phys2D/New Dawn Software nor the names of * its contributors may be used to endorse or promote products * derived from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY * OF SUCH DAMAGE. */ package net.phys2d.raw.collide; import java.util.Arrays; import java.util.Comparator; import java.util.LinkedList; import net.phys2d.math.Vector2f; /** *
This is a very important class for collision detection between polygons * for several reasons: *
While intersection tests are rather straight forward and well known problems, * the selection of the intersections to return is quite complicated. * It suprised me that I was completely unable to find applicable information on * this particular problem on the net.
* *Suppose we have two colliding polygons A and B. * Observe that if we trace around the polygons in counterclockwise order starting * on an edge of the polygon that is outside the other, we will encounter first * an intersection that goes into the other polygon and then one that comes out * again.
* ** ---- * out| B |in * ---+----+--- * |A | | | * | ->-- | * ->---------- ** *
These in- and outgoing intersections can be used to determine two very * important things: *
Things get complicated when polygons don't just intersect but totally * penetrate eachother. First of all, the collision normals would (depending on the * specific polygons and their intersection) oppose eachother, this has * unpredictable but possibly still acceptible results.
* *The biggest problem however is the penetration depth that will be completely * off. Take for example the intersection pair 1 and 2 in the example. * The penetration depth will be determined by the maximum vertical distance * between the edges of A between in and out and the edges of B between out and in * (both counterclockwise). * Clearly this gives very large penetration depths which often are completely * wrong.
* *In the example the final result will not be very problematic: the forces * are most likely to cancel eachother out. But even here it's simple to see that * the penetration depth for the intersection pair (3,4) will be much bigger than * the other. There are many examples that occur frequently in practice which will * cause bodies to be shot in orbit (if the physics would've allowed it ;) ).
* ** ---- * in|A |out * ---3----2-------- * |B | | | * | | | | * ->-4----1-------- * out| |in * ->-- ** *
The/A solution actually is quite straightforward: if we throw out one of the * two intersection pairs, the penetration depth will be correct and there won't be * any normals that cancel eachother out while they shouldn't. Aditionally this * technique will decrease the number of feature pairs, which is good for speed. *
* *Throwing out the right pairs still is quite a challange, but we can apply a * technique similar to the one we used to get the intersection pairs. Then we * traced the contour of A, now we trace the contour of B starting with an * outgoing intersection. If we encounter an ingoing intersection of the same pair * we accept the pair (i.e. not throw it out) and move on to the next intersection. * If we encounter an ingoing intersection belonging to a different pair or an * outgoing intersection we throw it out.
* *We still are not done just yet. The following example shows that it matters * a great deal which intersection we start with. If we start at intersection 2, * all the other intersection pairs will be thrown out and B will be pushed down. * If we had chosen to start with 4 or 6, the result would be more acceptable; * only (1,2) would be thrown out and B would be pushed up.
* ** -------------- * |B | * | ---- | * out| in| |out |in * ---6----5----4----3-- * |A | | | | | * | | | ---- | * ->-1----2------------ * in| |out * ->-- ** *
This problem is not really solved currently. There is a heuristic in place * that will select the outgoing edge that has the largest number of vertices * outside A. In other words, the largest number of vertices between the chosen * outgoing intersection and the last ingoing intersection (last as in clockwise * as opposed to counterclockwise).
* *The heuristic is based on the idea that we are more likely to be pushed * from A into the direction where the largest part of B is. For the example * this would not work, but for many examples in practice it will.
* * TODO: When we're stacking a lot of triangles the heuristic fails occationaly. * The triangles will sometimes jump in directions they shouldn't. * *If we manage to get all intersections between polygons, we should always * encounter alternating ingoing and outgoing intersections when tracing either * A or B. In practice this isn't really the case for two reasons:
*In stead of trying to fix this issue or throw out the faulty intersections, * I've made some adjustments that will simply skip these intersections. I am * afraid that these issues might cause trouble somewhere sometime, so if possible * this should be fixed some time.
* * * TODO: This class could use some specialized data structures in stead of the * countless arrays which clutter the code with modulo indices. * * @author Gideon Smeding * */ public class IntersectionGatherer { /** The minimum distance two intersections have to be apart to be considered a pair */ public static float MIN_PAIR_DIST = 0.5f; /** The size of the intersections array, thus determening the maximum number * of intersections that the IntersectionGatherer can accept. */ public static int MAX_INTERSECTIONS = 50; /** The array of intersecting edges which is unsorted while gathering the * intersections, but will be sorted in order of occurence along the contour * of polygon A. */ private SortableIntersection[] intersections; /** The amount of intersections gathered */ private int noIntersections = 0; /** The vertices of polygon A */ private Vector2f[] vertsA; /** The vertices of polygon B */ private Vector2f[] vertsB; /** * Construct an IntersectionGatherer for a specific pair of polygons. * * @param vertsA The 'first' polygon involved in this collision check * @param vertsB The 'second' polygon involved in this collision check */ public IntersectionGatherer(Vector2f[] vertsA, Vector2f[] vertsB) { this.intersections = new SortableIntersection[MAX_INTERSECTIONS]; this.noIntersections = 0; this.vertsA = vertsA; this.vertsB = vertsB; } /** * Intersect two edges of the two polygons and stores the intersecting lines * along with their position. * * @param a The edge of polygon A to check for intersection with B * @param b The edge of polygon B to check for intersection with A */ public void intersect(int a, int b) { if ( noIntersections >= MAX_INTERSECTIONS ) return; Vector2f startA = vertsA[a]; Vector2f endA = vertsA[(a+1) % vertsA.length ]; Vector2f startB = vertsB[b]; Vector2f endB = vertsB[(b+1) % vertsB.length ]; //TODO: reuse mathutil.intersect float d = (endB.y - startB.y) * (endA.x - startA.x) - (endB.x - startB.x) * (endA.y - startA.y); if ( d == 0 ) // parallel lines return; float uA = (endB.x - startB.x) * (startA.y - startB.y) - (endB.y - startB.y) * (startA.x - startB.x); uA /= d; float uB = (endA.x - startA.x) * (startA.y - startB.y) - (endA.y - startA.y) * (startA.x - startB.x); uB /= d; if ( uA < 0 || uA > 1 || uB < 0 || uB > 1 ) return; // intersection point isn't between the start and endpoints Vector2f position = new Vector2f( startA.x + uA * (endA.x - startA.x), startA.y + uA * (endA.y - startA.y)); Vector2f dist = new Vector2f(position); dist.sub(startA); float distFromVertA = dist.lengthSquared(); dist = new Vector2f(position); dist.sub(startB); float distFromVertB = dist.lengthSquared(); // z axis of 3d cross product float sA = (startA.x - startB.x) * (endB.y - startB.y) - (endB.x - startB.x) * (startA.y - startB.y); if ( sA > 0 ) { intersections[noIntersections] = new SortableIntersection(a, b, position, true, distFromVertA, distFromVertB); } else { intersections[noIntersections] = new SortableIntersection(a, b, position, false, distFromVertA, distFromVertB); } noIntersections++; } /** * Get the list of intersections, sorted by the order defined by * {@link IntersectionComparator}. * * @return A sorted list of intersections */ public Intersection[] getIntersections() { Intersection[] out = new Intersection[noIntersections]; for ( int i = 0; i < noIntersections; i++ ) out[i] = intersections[i]; Arrays.sort(out, new IntersectionComparator()); return out; } /** * Get the pairs of ingoing and outgoing intersections encountered when tracing * the contour of polygon A. Some pairs will be filtered out as described * in detail in this class's documentation. * * @return An array with the intersection pairs which has the dimensions * [n][2] or [n][1], where n is the number of intersections. * For a pair i getIntersectionPairs()[i][0] will contain the ingoing intersection * and getIntersectionPairs()[i][1] the outgoing intersection. */ public Intersection[][] getIntersectionPairs() { if ( noIntersections < 2 ) return new Intersection[0][2]; // sort the array for a trace Arrays.sort(intersections, 0, noIntersections, new IntersectionComparator()); // sort a pointer table which uses the indices in the intersections array Integer[] pointers = new Integer[noIntersections]; for ( int i = 0; i < noIntersections; i++ ) pointers[i] = new Integer(i); Arrays.sort(pointers, new PointerTableComparator()); int referenceVertB = getReferencePointer(pointers); filterIntersections(referenceVertB, pointers); // make sure we're starting with an ingoing edge int first = intersections[0].isIngoing ? 0 : 1; // now copy our results to a new array LinkedList outIntersections = new LinkedList(); for ( int i = first; i < noIntersections + first; ) { SortableIntersection in = intersections[i % noIntersections]; SortableIntersection out = intersections[(i+1) % noIntersections]; if ( in == null ) { i += 1; continue; } if ( out != null && in.isIngoing && !out.isIngoing ) { // pairs that are too close to eachother will // often cause problems, so don't create them if ( !in.position.equalsDelta(out.position, MIN_PAIR_DIST) ) { Intersection[] pair = {in, out}; outIntersections.add(pair); i += 2; continue; } } Intersection[] inArr = {in}; outIntersections.add(inArr); i += 1; } return (Intersection[][]) outIntersections.toArray(new Intersection[outIntersections.size()][]); } /** * Gets an outgoing intersection that seems appropriate to start the * intersection filter with. This implements the heuristic described in the * class documentation. * * @param pointers An array of pointers which are the indices of the * intersections array. The list should be sorted with the order defined by * {@link PointerTableComparator}. * @return The reference pointer which is an outgoing intersection. */ private int getReferencePointer(Integer[] pointers) { // we want to find an ingoing edge with the largest number of edges outside of A // these edges should be between the in and the nex out, when tracing the contour of B int first = intersections[pointers[0].intValue()].isIngoing ? 0 : 1; int maxInOutDist = 0; int maxInIndex = first + 1 % noIntersections;//intersections[pointers[first].intValue()].edgeB; int lastInEdgeB = -1; for ( int i = first; i < noIntersections + first; i++ ) { int k = pointers[i % noIntersections].intValue(); SortableIntersection intersection = intersections[k]; if ( intersection.isIngoing ) { lastInEdgeB = intersection.edgeB; } else if ( lastInEdgeB >= 0 ) { int inOutDist = (intersection.edgeB - lastInEdgeB + vertsB.length) % vertsB.length; // did we find a new max dist? if ( inOutDist > maxInOutDist ) { maxInOutDist = inOutDist; maxInIndex = i % noIntersections; } lastInEdgeB = -1; } } return maxInIndex; } /** * This function filters out intersection pairs to remedy the situation where * polygons totally penetrate eachother. See this class's documentation for * a more thorough description. * * @param referencePointer The vertex of B that lies outside of A, we will start * at the first outgoing intersection after the reference vertex. * @param pointers An array of pointers which are the indices of the * intersections array. The list should be sorted with the order defined by * {@link PointerTableComparator}. */ private void filterIntersections(int referencePointer, Integer[] pointers) { // make sure the reference vertex is real if ( referencePointer >= noIntersections && referencePointer < 0 ) throw new RuntimeException("The reference vertex cannot be correct since B does not have that many vertices."); // now throw out the total penetrating intersections int topOut = -2; // -2 + 1 will never give an edge number for ( int i = referencePointer; i < noIntersections + referencePointer; i++ ) { int j = i % noIntersections; int k = pointers[j].intValue(); SortableIntersection intersection = intersections[k]; // note that we go backwards with respect to A so we expect an outgoing edge first if ( intersection.isIngoing ) { if ( (topOut - 1 + noIntersections) % noIntersections == k ) { // the closing 'in' intersection topOut = -2; // reset our top 'out' intersection } else { intersections[k] = null; // remove the 'in' } } else { if ( topOut < 0 ) { topOut = k; // we encountered our new 'out' intersection } else { intersections[k] = null; // remove the out } } } // now get rid of the nullified intersections int noRemoved = 0; for ( int i = 0; i < noIntersections; i++ ) { if ( intersections[i] == null ) { noRemoved++; } else { intersections[i-noRemoved] = intersections[i]; } } noIntersections -= noRemoved; } /** Class representing a single intersection. It also contains some information used * to sort the edges along the counters of the polygons A and B. */ class SortableIntersection extends Intersection { /** The squared distance from the vertice that starts edgeA */ public float distFromVertA; /** The squared distance from the vertice that starts edgeB */ public float distFromVertB; /** * Construct an Intersection object, immediately setting all the attributes. * * @param edgeA The edge of polygon A that intersects * @param edgeB The edge of polygon B that intersects * @param position The position of the intersection in world (absolute) coordinates * @param isIngoing True iff this is an intersection where polygon A enters B * @param distFromVertA The squared distance from the vertice that starts edgeA * @param distFromVertB The squared distance from the vertice that starts edgeB */ public SortableIntersection(int edgeA, int edgeB, Vector2f position, boolean isIngoing, float distFromVertA, float distFromVertB) { super(edgeA, edgeB, position, isIngoing); this.distFromVertA = distFromVertA; this.distFromVertB = distFromVertB; } } /** Comparator used to sort intersections by their distance from A's first * vertex. */ class IntersectionComparator implements Comparator { /** Compares two intersections. Note that this function will/should never * return 0 because no two intersections can have the same distance from * vertex 0. However, due to the finite precision of floating points this * situation does occur. In those cases we try to put ingoing edges first. * * @see Comparator#compare(Object, Object) */ public int compare(Object first, Object second) { SortableIntersection one = (SortableIntersection) first; SortableIntersection other = (SortableIntersection) second; if ( one.edgeA < other.edgeA ) { return -1; } else if ( one.edgeA == other.edgeA ) { if ( one.distFromVertA < other.distFromVertA ) return -1; else if ( one.distFromVertA == other.distFromVertA && one.isIngoing ) return -1; } return 1; } } /** Comparator used to sort intersections by their distance from B's first * vertex. This sorts an array of pointers which are the indices of the * intersections array. So the actual intersections are retrieved via an * indirection. */ class PointerTableComparator implements Comparator { /** Compares two intersections. Note that this function will/should never * return 0 because no two intersections can have the same distance from * vertex 0. However, due to the finite precision of floating points this * situation does occur. In those cases we try to put outgoing edges first. * * @see Comparator#compare(Object, Object) */ public int compare(Object first, Object second) { SortableIntersection one = intersections[((Integer) first).intValue()]; SortableIntersection other = intersections[((Integer) second).intValue()]; if ( one.edgeB < other.edgeB ) { return -1; } else if ( one.edgeB == other.edgeB ) { if ( one.distFromVertB < other.distFromVertB ) return -1; else if ( one.distFromVertB == other.distFromVertB && !one.isIngoing ) return -1; } return 1; } } }