/*
* 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 net.phys2d.math.MathUtil;
import net.phys2d.math.Vector2f;
import net.phys2d.raw.Body;
import net.phys2d.raw.Contact;
import net.phys2d.raw.shapes.Polygon;
import net.phys2d.raw.shapes.Line;
/**
* Collider for a Line and a Convex Polygon.
*
* @author Gideon Smeding
*
*/
public class LinePolygonCollider extends PolygonPolygonCollider {
/**
* @see net.phys2d.raw.collide.Collider#collide(net.phys2d.raw.Contact[], net.phys2d.raw.Body, net.phys2d.raw.Body)
*/
public int collide(Contact[] contacts, Body bodyA, Body bodyB) {
Line line = (Line) bodyA.getShape();
Polygon poly = (Polygon) bodyB.getShape();
// TODO: this can be optimized using matrix multiplications and moving only one shape
// specifically the line, because it has only two vertices
Vector2f[] vertsA = line.getVertices(bodyA.getPosition(), bodyA.getRotation());
Vector2f[] vertsB = poly.getVertices(bodyB.getPosition(), bodyB.getRotation());
Vector2f pos = poly.getCentroid(bodyB.getPosition(), bodyB.getRotation());
// using the z axis of a 3d cross product we determine on what side B is
boolean isLeftOf = 0 > (pos.x - vertsA[0].x) * (vertsA[1].y - vertsA[0].y) - (vertsA[1].x - vertsA[0].x) * (pos.y - vertsA[0].y);
// to get the proper intersection pairs we make sure
// the line's normal is pointing towards the polygon
// TODO: verify that it's not actually pointing in the opposite direction
if ( isLeftOf ) {
Vector2f tmp = vertsA[0];
vertsA[0] = vertsA[1];
vertsA[1] = tmp;
}
// we use the line's normal for our sweepline projection
Vector2f normal = new Vector2f(vertsA[1]);
normal.sub(vertsA[0]);
normal.set(normal.y, -normal.x);
EdgeSweep sweep = new EdgeSweep(normal);
sweep.insert(0, true, vertsA[0].dot(normal));
sweep.insert(0, true, vertsA[1].dot(normal));
sweep.addVerticesToSweep(false, vertsB);
int[][] collEdgeCands = sweep.getOverlappingEdges();
IntersectionGatherer intGath = new IntersectionGatherer(vertsA, vertsB);
for ( int i = 0; i < collEdgeCands.length; i++ )
intGath.intersect(collEdgeCands[i][0], collEdgeCands[i][1]);
Intersection[] intersections = intGath.getIntersections();
return populateContacts(contacts, vertsA, vertsB, intersections);
}
/**
* Given a list of intersections, calculate the collision information and
* set the contacts with that information.
*
* @param contacts The array of contacts to fill
* @param vertsA The vertices of polygon A
* @param vertsB The vertices of polygon B
* @param intersections The array of intersection as returned by
* {@link IntersectionGatherer#getIntersections()}
* @return The number of contacts that have been set in the contact array
*/
public int populateContacts(Contact[] contacts, Vector2f[] vertsA, Vector2f[] vertsB, Intersection[] intersections) {
if ( intersections.length == 0 )
return 0;
int noContacts = 0;
// is the first intersection outgoing?
if ( !intersections[0].isIngoing ) {
setLineEndContact(contacts[noContacts], intersections[intersections.length-1], vertsA, vertsB);
if (contacts[noContacts].getSeparation() < -10 )
System.out.println("first " + contacts[noContacts].getSeparation());
noContacts++;
}
int i = noContacts;
while ( i < intersections.length-1 ) {
if ( noContacts > contacts.length-2 )
return noContacts;
// check if we have an intersection pair
if ( !intersections[i].isIngoing || intersections[i+1].isIngoing ) {
setContact(contacts[noContacts], intersections[i], vertsA, vertsB);
i++;
noContacts++;
continue;
}
setContactPair(
contacts[noContacts],
contacts[noContacts+1],
intersections[i],
intersections[i+1],
vertsA, vertsB);
if (contacts[noContacts].getSeparation() < -10 )
System.out.println("m " + contacts[noContacts].getSeparation());
noContacts += 2;
i += 2;
}
// is there still an ingoing intersection left?
if ( i < intersections.length &&
intersections[intersections.length-1].isIngoing &&
noContacts < contacts.length) {
setLineEndContact(contacts[noContacts], intersections[intersections.length-1], vertsA, vertsB);
if (contacts[noContacts].getSeparation() < -10 )
System.out.println(" last " +contacts[noContacts].getSeparation());
noContacts++;
}
return noContacts;
}
/**
* Set a contact for an intersection where the colliding line's start- or endpoint
* is contained in the colliding polygon.
*
* TODO: The current implementation doesn't work properly: because lines are very
* thin, they can slide into a polygon sideways which gives a very deep penetration
* |
* |->
* | +-----+
* |-> | |
* | | |
* | |
* +-----+
*
* A possible solution would be to use the velocity of the line relative to the
* polygon to construct a collision normal and penetration depth.
* Another possibility is to use the line's normals (both directions) and calculate
* proper intersection distances for them.
* If one has multiple normals/penetration depths to choose from, the one with the
* minimum penetration depth will probably be the best bet.
*
* @param contact The contact to set
* @param intersection The intersection where the line enters or exits the polygon
* @param vertsA The line's vertices
* @param vertsB The polygon's vertices
*/
public void setLineEndContact(Contact contact, Intersection intersection, Vector2f[] vertsA, Vector2f[] vertsB) {
Vector2f separation = new Vector2f(intersection.position);
if ( intersection.isIngoing )
separation.sub(vertsA[1]);
else
separation.sub(vertsA[0]);
float depthA = 0;//separation.length();
contact.setSeparation(-depthA);
contact.setNormal(MathUtil.getNormal(vertsB[(intersection.edgeB + 1) % vertsB.length], vertsB[intersection.edgeB]));
contact.setPosition(intersection.position);
contact.setFeature(new FeaturePair(0, 0, intersection.edgeA, intersection.edgeB));
}
}