/*******************************************************************************
* Copyright (c) 2013, Daniel Murphy
* 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.
*
* 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 HOLDER 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 org.jbox2d.collision;
import org.jbox2d.collision.Distance.SimplexCache;
import org.jbox2d.collision.Manifold.ManifoldType;
import org.jbox2d.collision.shapes.CircleShape;
import org.jbox2d.collision.shapes.EdgeShape;
import org.jbox2d.collision.shapes.PolygonShape;
import org.jbox2d.collision.shapes.Shape;
import org.jbox2d.common.MathUtils;
import org.jbox2d.common.Rot;
import org.jbox2d.common.Settings;
import org.jbox2d.common.Transform;
import org.jbox2d.common.Vec2;
import org.jbox2d.pooling.IWorldPool;
/**
* Functions used for computing contact points, distance queries, and TOI queries. Collision methods
* are non-static for pooling speed, retrieve a collision object from the {@link SingletonPool}.
* Should not be finalructed.
*
* @author Daniel Murphy
*/
public class Collision {
public static final int NULL_FEATURE = Integer.MAX_VALUE;
private final IWorldPool pool;
public Collision(IWorldPool argPool) {
incidentEdge[0] = new ClipVertex();
incidentEdge[1] = new ClipVertex();
clipPoints1[0] = new ClipVertex();
clipPoints1[1] = new ClipVertex();
clipPoints2[0] = new ClipVertex();
clipPoints2[1] = new ClipVertex();
pool = argPool;
}
private final DistanceInput input = new DistanceInput();
private final SimplexCache cache = new SimplexCache();
private final DistanceOutput output = new DistanceOutput();
/**
* Determine if two generic shapes overlap.
*
* @param shapeA
* @param shapeB
* @param xfA
* @param xfB
* @return
*/
public final boolean testOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB,
Transform xfA, Transform xfB) {
input.proxyA.set(shapeA, indexA);
input.proxyB.set(shapeB, indexB);
input.transformA.set(xfA);
input.transformB.set(xfB);
input.useRadii = true;
cache.count = 0;
pool.getDistance().distance(output, cache, input);
// djm note: anything significant about 10.0f?
return output.distance < 10.0f * Settings.EPSILON;
}
/**
* Compute the point states given two manifolds. The states pertain to the transition from
* manifold1 to manifold2. So state1 is either persist or remove while state2 is either add or
* persist.
*
* @param state1
* @param state2
* @param manifold1
* @param manifold2
*/
public static final void getPointStates(final PointState[] state1, final PointState[] state2,
final Manifold manifold1, final Manifold manifold2) {
for (int i = 0; i < Settings.maxManifoldPoints; i++) {
state1[i] = PointState.NULL_STATE;
state2[i] = PointState.NULL_STATE;
}
// Detect persists and removes.
for (int i = 0; i < manifold1.pointCount; i++) {
ContactID id = manifold1.points[i].id;
state1[i] = PointState.REMOVE_STATE;
for (int j = 0; j < manifold2.pointCount; j++) {
if (manifold2.points[j].id.isEqual(id)) {
state1[i] = PointState.PERSIST_STATE;
break;
}
}
}
// Detect persists and adds
for (int i = 0; i < manifold2.pointCount; i++) {
ContactID id = manifold2.points[i].id;
state2[i] = PointState.ADD_STATE;
for (int j = 0; j < manifold1.pointCount; j++) {
if (manifold1.points[j].id.isEqual(id)) {
state2[i] = PointState.PERSIST_STATE;
break;
}
}
}
}
/**
* Clipping for contact manifolds. Sutherland-Hodgman clipping.
*
* @param vOut
* @param vIn
* @param normal
* @param offset
* @return
*/
public static final int clipSegmentToLine(final ClipVertex[] vOut, final ClipVertex[] vIn,
final Vec2 normal, float offset, int vertexIndexA) {
// Start with no output points
int numOut = 0;
final ClipVertex vIn0 = vIn[0];
final ClipVertex vIn1 = vIn[1];
final Vec2 vIn0v = vIn0.v;
final Vec2 vIn1v = vIn1.v;
// Calculate the distance of end points to the line
float distance0 = Vec2.dot(normal, vIn0v) - offset;
float distance1 = Vec2.dot(normal, vIn1v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) {
vOut[numOut++].set(vIn0);
}
if (distance1 <= 0.0f) {
vOut[numOut++].set(vIn1);
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f) {
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
ClipVertex vOutNO = vOut[numOut];
// vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
vOutNO.v.x = vIn0v.x + interp * (vIn1v.x - vIn0v.x);
vOutNO.v.y = vIn0v.y + interp * (vIn1v.y - vIn0v.y);
// VertexA is hitting edgeB.
vOutNO.id.indexA = (byte) vertexIndexA;
vOutNO.id.indexB = vIn0.id.indexB;
vOutNO.id.typeA = (byte) ContactID.Type.VERTEX.ordinal();
vOutNO.id.typeB = (byte) ContactID.Type.FACE.ordinal();
++numOut;
}
return numOut;
}
// #### COLLISION STUFF (not from collision.h or collision.cpp) ####
// djm pooling
private static Vec2 d = new Vec2();
/**
* Compute the collision manifold between two circles.
*
* @param manifold
* @param circle1
* @param xfA
* @param circle2
* @param xfB
*/
public final void collideCircles(Manifold manifold, final CircleShape circle1,
final Transform xfA, final CircleShape circle2, final Transform xfB) {
manifold.pointCount = 0;
// before inline:
// Transform.mulToOut(xfA, circle1.m_p, pA);
// Transform.mulToOut(xfB, circle2.m_p, pB);
// d.set(pB).subLocal(pA);
// float distSqr = d.x * d.x + d.y * d.y;
// after inline:
Vec2 circle1p = circle1.m_p;
Vec2 circle2p = circle2.m_p;
float pAx = (xfA.q.c * circle1p.x - xfA.q.s * circle1p.y) + xfA.p.x;
float pAy = (xfA.q.s * circle1p.x + xfA.q.c * circle1p.y) + xfA.p.y;
float pBx = (xfB.q.c * circle2p.x - xfB.q.s * circle2p.y) + xfB.p.x;
float pBy = (xfB.q.s * circle2p.x + xfB.q.c * circle2p.y) + xfB.p.y;
float dx = pBx - pAx;
float dy = pBy - pAy;
float distSqr = dx * dx + dy * dy;
// end inline
final float radius = circle1.m_radius + circle2.m_radius;
if (distSqr > radius * radius) {
return;
}
manifold.type = ManifoldType.CIRCLES;
manifold.localPoint.set(circle1p);
manifold.localNormal.setZero();
manifold.pointCount = 1;
manifold.points[0].localPoint.set(circle2p);
manifold.points[0].id.zero();
}
// djm pooling, and from above
/**
* Compute the collision manifold between a polygon and a circle.
*
* @param manifold
* @param polygon
* @param xfA
* @param circle
* @param xfB
*/
public final void collidePolygonAndCircle(Manifold manifold, final PolygonShape polygon,
final Transform xfA, final CircleShape circle, final Transform xfB) {
manifold.pointCount = 0;
// Vec2 v = circle.m_p;
// Compute circle position in the frame of the polygon.
// before inline:
// Transform.mulToOutUnsafe(xfB, circle.m_p, c);
// Transform.mulTransToOut(xfA, c, cLocal);
// final float cLocalx = cLocal.x;
// final float cLocaly = cLocal.y;
// after inline:
final Vec2 circlep = circle.m_p;
final Rot xfBq = xfB.q;
final Rot xfAq = xfA.q;
final float cx = (xfBq.c * circlep.x - xfBq.s * circlep.y) + xfB.p.x;
final float cy = (xfBq.s * circlep.x + xfBq.c * circlep.y) + xfB.p.y;
final float px = cx - xfA.p.x;
final float py = cy - xfA.p.y;
final float cLocalx = (xfAq.c * px + xfAq.s * py);
final float cLocaly = (-xfAq.s * px + xfAq.c * py);
// end inline
// Find the min separating edge.
int normalIndex = 0;
float separation = -Float.MAX_VALUE;
final float radius = polygon.m_radius + circle.m_radius;
final int vertexCount = polygon.m_count;
float s;
final Vec2[] vertices = polygon.m_vertices;
final Vec2[] normals = polygon.m_normals;
for (int i = 0; i < vertexCount; i++) {
// before inline
// temp.set(cLocal).subLocal(vertices[i]);
// float s = Vec2.dot(normals[i], temp);
// after inline
final Vec2 vertex = vertices[i];
final float tempx = cLocalx - vertex.x;
final float tempy = cLocaly - vertex.y;
s = normals[i].x * tempx + normals[i].y * tempy;
if (s > radius) {
// early out
return;
}
if (s > separation) {
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
final int vertIndex1 = normalIndex;
final int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
final Vec2 v1 = vertices[vertIndex1];
final Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.EPSILON) {
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
// before inline:
// manifold.localNormal.set(normals[normalIndex]);
// manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
// manifold.points[0].localPoint.set(circle.m_p);
// after inline:
final Vec2 normal = normals[normalIndex];
manifold.localNormal.x = normal.x;
manifold.localNormal.y = normal.y;
manifold.localPoint.x = (v1.x + v2.x) * .5f;
manifold.localPoint.y = (v1.y + v2.y) * .5f;
final ManifoldPoint mpoint = manifold.points[0];
mpoint.localPoint.x = circlep.x;
mpoint.localPoint.y = circlep.y;
mpoint.id.zero();
// end inline
return;
}
// Compute barycentric coordinates
// before inline:
// temp.set(cLocal).subLocal(v1);
// temp2.set(v2).subLocal(v1);
// float u1 = Vec2.dot(temp, temp2);
// temp.set(cLocal).subLocal(v2);
// temp2.set(v1).subLocal(v2);
// float u2 = Vec2.dot(temp, temp2);
// after inline:
final float tempX = cLocalx - v1.x;
final float tempY = cLocaly - v1.y;
final float temp2X = v2.x - v1.x;
final float temp2Y = v2.y - v1.y;
final float u1 = tempX * temp2X + tempY * temp2Y;
final float temp3X = cLocalx - v2.x;
final float temp3Y = cLocaly - v2.y;
final float temp4X = v1.x - v2.x;
final float temp4Y = v1.y - v2.y;
final float u2 = temp3X * temp4X + temp3Y * temp4Y;
// end inline
if (u1 <= 0f) {
// inlined
final float dx = cLocalx - v1.x;
final float dy = cLocaly - v1.y;
if (dx * dx + dy * dy > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
// before inline:
// manifold.localNormal.set(cLocal).subLocal(v1);
// after inline:
manifold.localNormal.x = cLocalx - v1.x;
manifold.localNormal.y = cLocaly - v1.y;
// end inline
manifold.localNormal.normalize();
manifold.localPoint.set(v1);
manifold.points[0].localPoint.set(circlep);
manifold.points[0].id.zero();
} else if (u2 <= 0.0f) {
// inlined
final float dx = cLocalx - v2.x;
final float dy = cLocaly - v2.y;
if (dx * dx + dy * dy > radius * radius) {
return;
}
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
// before inline:
// manifold.localNormal.set(cLocal).subLocal(v2);
// after inline:
manifold.localNormal.x = cLocalx - v2.x;
manifold.localNormal.y = cLocaly - v2.y;
// end inline
manifold.localNormal.normalize();
manifold.localPoint.set(v2);
manifold.points[0].localPoint.set(circlep);
manifold.points[0].id.zero();
} else {
// Vec2 faceCenter = 0.5f * (v1 + v2);
// (temp is faceCenter)
// before inline:
// temp.set(v1).addLocal(v2).mulLocal(.5f);
//
// temp2.set(cLocal).subLocal(temp);
// separation = Vec2.dot(temp2, normals[vertIndex1]);
// if (separation > radius) {
// return;
// }
// after inline:
final float fcx = (v1.x + v2.x) * .5f;
final float fcy = (v1.y + v2.y) * .5f;
final float tx = cLocalx - fcx;
final float ty = cLocaly - fcy;
final Vec2 normal = normals[vertIndex1];
separation = tx * normal.x + ty * normal.y;
if (separation > radius) {
return;
}
// end inline
manifold.pointCount = 1;
manifold.type = ManifoldType.FACE_A;
manifold.localNormal.set(normals[vertIndex1]);
manifold.localPoint.x = fcx; // (faceCenter)
manifold.localPoint.y = fcy;
manifold.points[0].localPoint.set(circlep);
manifold.points[0].id.zero();
}
}
// djm pooling, and from above
private final Vec2 temp = new Vec2();
private final Transform xf = new Transform();
private final Vec2 n = new Vec2();
private final Vec2 v1 = new Vec2();
/**
* Find the max separation between poly1 and poly2 using edge normals from poly1.
*
* @param edgeIndex
* @param poly1
* @param xf1
* @param poly2
* @param xf2
* @return
*/
public final void findMaxSeparation(EdgeResults results, final PolygonShape poly1,
final Transform xf1, final PolygonShape poly2, final Transform xf2) {
int count1 = poly1.m_count;
int count2 = poly2.m_count;
Vec2[] n1s = poly1.m_normals;
Vec2[] v1s = poly1.m_vertices;
Vec2[] v2s = poly2.m_vertices;
Transform.mulTransToOutUnsafe(xf2, xf1, xf);
final Rot xfq = xf.q;
int bestIndex = 0;
float maxSeparation = -Float.MAX_VALUE;
for (int i = 0; i < count1; i++) {
// Get poly1 normal in frame2.
Rot.mulToOutUnsafe(xfq, n1s[i], n);
Transform.mulToOutUnsafe(xf, v1s[i], v1);
// Find deepest point for normal i.
float si = Float.MAX_VALUE;
for (int j = 0; j < count2; ++j) {
Vec2 v2sj = v2s[j];
float sij = n.x * (v2sj.x - v1.x) + n.y * (v2sj.y - v1.y);
if (sij < si) {
si = sij;
}
}
if (si > maxSeparation) {
maxSeparation = si;
bestIndex = i;
}
}
results.edgeIndex = bestIndex;
results.separation = maxSeparation;
}
public final void findIncidentEdge(final ClipVertex[] c, final PolygonShape poly1,
final Transform xf1, int edge1, final PolygonShape poly2, final Transform xf2) {
int count1 = poly1.m_count;
final Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_count;
final Vec2[] vertices2 = poly2.m_vertices;
final Vec2[] normals2 = poly2.m_normals;
assert (0 <= edge1 && edge1 < count1);
final ClipVertex c0 = c[0];
final ClipVertex c1 = c[1];
final Rot xf1q = xf1.q;
final Rot xf2q = xf2.q;
// Get the normal of the reference edge in poly2's frame.
// Vec2 normal1 = MulT(xf2.R, Mul(xf1.R, normals1[edge1]));
// before inline:
// Rot.mulToOutUnsafe(xf1.q, normals1[edge1], normal1); // temporary
// Rot.mulTrans(xf2.q, normal1, normal1);
// after inline:
final Vec2 v = normals1[edge1];
final float tempx = xf1q.c * v.x - xf1q.s * v.y;
final float tempy = xf1q.s * v.x + xf1q.c * v.y;
final float normal1x = xf2q.c * tempx + xf2q.s * tempy;
final float normal1y = -xf2q.s * tempx + xf2q.c * tempy;
// end inline
// Find the incident edge on poly2.
int index = 0;
float minDot = Float.MAX_VALUE;
for (int i = 0; i < count2; ++i) {
Vec2 b = normals2[i];
float dot = normal1x * b.x + normal1y * b.y;
if (dot < minDot) {
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
// c0.v = Mul(xf2, vertices2[i1]);
Vec2 v1 = vertices2[i1];
Vec2 out = c0.v;
out.x = (xf2q.c * v1.x - xf2q.s * v1.y) + xf2.p.x;
out.y = (xf2q.s * v1.x + xf2q.c * v1.y) + xf2.p.y;
c0.id.indexA = (byte) edge1;
c0.id.indexB = (byte) i1;
c0.id.typeA = (byte) ContactID.Type.FACE.ordinal();
c0.id.typeB = (byte) ContactID.Type.VERTEX.ordinal();
// c1.v = Mul(xf2, vertices2[i2]);
Vec2 v2 = vertices2[i2];
Vec2 out1 = c1.v;
out1.x = (xf2q.c * v2.x - xf2q.s * v2.y) + xf2.p.x;
out1.y = (xf2q.s * v2.x + xf2q.c * v2.y) + xf2.p.y;
c1.id.indexA = (byte) edge1;
c1.id.indexB = (byte) i2;
c1.id.typeA = (byte) ContactID.Type.FACE.ordinal();
c1.id.typeB = (byte) ContactID.Type.VERTEX.ordinal();
}
private final EdgeResults results1 = new EdgeResults();
private final EdgeResults results2 = new EdgeResults();
private final ClipVertex[] incidentEdge = new ClipVertex[2];
private final Vec2 localTangent = new Vec2();
private final Vec2 localNormal = new Vec2();
private final Vec2 planePoint = new Vec2();
private final Vec2 tangent = new Vec2();
private final Vec2 v11 = new Vec2();
private final Vec2 v12 = new Vec2();
private final ClipVertex[] clipPoints1 = new ClipVertex[2];
private final ClipVertex[] clipPoints2 = new ClipVertex[2];
/**
* Compute the collision manifold between two polygons.
*
* @param manifold
* @param polygon1
* @param xf1
* @param polygon2
* @param xf2
*/
public final void collidePolygons(Manifold manifold, final PolygonShape polyA,
final Transform xfA, final PolygonShape polyB, final Transform xfB) {
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
manifold.pointCount = 0;
float totalRadius = polyA.m_radius + polyB.m_radius;
findMaxSeparation(results1, polyA, xfA, polyB, xfB);
if (results1.separation > totalRadius) {
return;
}
findMaxSeparation(results2, polyB, xfB, polyA, xfA);
if (results2.separation > totalRadius) {
return;
}
final PolygonShape poly1; // reference polygon
final PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
boolean flip;
final float k_tol = 0.1f * Settings.linearSlop;
if (results2.separation > results1.separation + k_tol) {
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = results2.edgeIndex;
manifold.type = ManifoldType.FACE_B;
flip = true;
} else {
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = results1.edgeIndex;
manifold.type = ManifoldType.FACE_A;
flip = false;
}
final Rot xf1q = xf1.q;
findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_count;
final Vec2[] vertices1 = poly1.m_vertices;
final int iv1 = edge1;
final int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
v11.set(vertices1[iv1]);
v12.set(vertices1[iv2]);
localTangent.x = v12.x - v11.x;
localTangent.y = v12.y - v11.y;
localTangent.normalize();
// Vec2 localNormal = Vec2.cross(dv, 1.0f);
localNormal.x = 1f * localTangent.y;
localNormal.y = -1f * localTangent.x;
// Vec2 planePoint = 0.5f * (v11+ v12);
planePoint.x = (v11.x + v12.x) * .5f;
planePoint.y = (v11.y + v12.y) * .5f;
// Rot.mulToOutUnsafe(xf1.q, localTangent, tangent);
tangent.x = xf1q.c * localTangent.x - xf1q.s * localTangent.y;
tangent.y = xf1q.s * localTangent.x + xf1q.c * localTangent.y;
// Vec2.crossToOutUnsafe(tangent, 1f, normal);
final float normalx = 1f * tangent.y;
final float normaly = -1f * tangent.x;
Transform.mulToOut(xf1, v11, v11);
Transform.mulToOut(xf1, v12, v12);
// v11 = Mul(xf1, v11);
// v12 = Mul(xf1, v12);
// Face offset
// float frontOffset = Vec2.dot(normal, v11);
float frontOffset = normalx * v11.x + normaly * v11.y;
// Side offsets, extended by polytope skin thickness.
// float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius;
// float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius;
float sideOffset1 = -(tangent.x * v11.x + tangent.y * v11.y) + totalRadius;
float sideOffset2 = tangent.x * v12.x + tangent.y * v12.y + totalRadius;
// Clip incident edge against extruded edge1 side edges.
// ClipVertex clipPoints1[2];
// ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
// np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
tangent.negateLocal();
np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1);
tangent.negateLocal();
if (np < 2) {
return;
}
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < 2) {
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal.set(localNormal);
manifold.localPoint.set(planePoint);
int pointCount = 0;
for (int i = 0; i < Settings.maxManifoldPoints; ++i) {
// float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset;
float separation = normalx * clipPoints2[i].v.x + normaly * clipPoints2[i].v.y - frontOffset;
if (separation <= totalRadius) {
ManifoldPoint cp = manifold.points[pointCount];
// cp.m_localPoint = MulT(xf2, clipPoints2[i].v);
Vec2 out = cp.localPoint;
final float px = clipPoints2[i].v.x - xf2.p.x;
final float py = clipPoints2[i].v.y - xf2.p.y;
out.x = (xf2.q.c * px + xf2.q.s * py);
out.y = (-xf2.q.s * px + xf2.q.c * py);
cp.id.set(clipPoints2[i].id);
if (flip) {
// Swap features
cp.id.flip();
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
private final Vec2 Q = new Vec2();
private final Vec2 e = new Vec2();
private final ContactID cf = new ContactID();
private final Vec2 e1 = new Vec2();
private final Vec2 P = new Vec2();
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
public void collideEdgeAndCircle(Manifold manifold, final EdgeShape edgeA, final Transform xfA,
final CircleShape circleB, final Transform xfB) {
manifold.pointCount = 0;
// Compute circle in frame of edge
// Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p));
Transform.mulToOutUnsafe(xfB, circleB.m_p, temp);
Transform.mulTransToOutUnsafe(xfA, temp, Q);
final Vec2 A = edgeA.m_vertex1;
final Vec2 B = edgeA.m_vertex2;
e.set(B).subLocal(A);
// Barycentric coordinates
float u = Vec2.dot(e, temp.set(B).subLocal(Q));
float v = Vec2.dot(e, temp.set(Q).subLocal(A));
float radius = edgeA.m_radius + circleB.m_radius;
// ContactFeature cf;
cf.indexB = 0;
cf.typeB = (byte) ContactID.Type.VERTEX.ordinal();
// Region A
if (v <= 0.0f) {
final Vec2 P = A;
d.set(Q).subLocal(P);
float dd = Vec2.dot(d, d);
if (dd > radius * radius) {
return;
}
// Is there an edge connected to A?
if (edgeA.m_hasVertex0) {
final Vec2 A1 = edgeA.m_vertex0;
final Vec2 B1 = A;
e1.set(B1).subLocal(A1);
float u1 = Vec2.dot(e1, temp.set(B1).subLocal(Q));
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f) {
return;
}
}
cf.indexA = 0;
cf.typeA = (byte) ContactID.Type.VERTEX.ordinal();
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.CIRCLES;
manifold.localNormal.setZero();
manifold.localPoint.set(P);
// manifold.points[0].id.key = 0;
manifold.points[0].id.set(cf);
manifold.points[0].localPoint.set(circleB.m_p);
return;
}
// Region B
if (u <= 0.0f) {
Vec2 P = B;
d.set(Q).subLocal(P);
float dd = Vec2.dot(d, d);
if (dd > radius * radius) {
return;
}
// Is there an edge connected to B?
if (edgeA.m_hasVertex3) {
final Vec2 B2 = edgeA.m_vertex3;
final Vec2 A2 = B;
final Vec2 e2 = e1;
e2.set(B2).subLocal(A2);
float v2 = Vec2.dot(e2, temp.set(Q).subLocal(A2));
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f) {
return;
}
}
cf.indexA = 1;
cf.typeA = (byte) ContactID.Type.VERTEX.ordinal();
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.CIRCLES;
manifold.localNormal.setZero();
manifold.localPoint.set(P);
// manifold.points[0].id.key = 0;
manifold.points[0].id.set(cf);
manifold.points[0].localPoint.set(circleB.m_p);
return;
}
// Region AB
float den = Vec2.dot(e, e);
assert (den > 0.0f);
// Vec2 P = (1.0f / den) * (u * A + v * B);
P.set(A).mulLocal(u).addLocal(temp.set(B).mulLocal(v));
P.mulLocal(1.0f / den);
d.set(Q).subLocal(P);
float dd = Vec2.dot(d, d);
if (dd > radius * radius) {
return;
}
n.x = -e.y;
n.y = e.x;
if (Vec2.dot(n, temp.set(Q).subLocal(A)) < 0.0f) {
n.set(-n.x, -n.y);
}
n.normalize();
cf.indexA = 0;
cf.typeA = (byte) ContactID.Type.FACE.ordinal();
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
manifold.localNormal.set(n);
manifold.localPoint.set(A);
// manifold.points[0].id.key = 0;
manifold.points[0].id.set(cf);
manifold.points[0].localPoint.set(circleB.m_p);
}
private final EPCollider collider = new EPCollider();
public void collideEdgeAndPolygon(Manifold manifold, final EdgeShape edgeA, final Transform xfA,
final PolygonShape polygonB, final Transform xfB) {
collider.collide(manifold, edgeA, xfA, polygonB, xfB);
}
/**
* Java-specific class for returning edge results
*/
private static class EdgeResults {
public float separation;
public int edgeIndex;
}
/**
* Used for computing contact manifolds.
*/
public static class ClipVertex {
public final Vec2 v;
public final ContactID id;
public ClipVertex() {
v = new Vec2();
id = new ContactID();
}
public void set(final ClipVertex cv) {
Vec2 v1 = cv.v;
v.x = v1.x;
v.y = v1.y;
ContactID c = cv.id;
id.indexA = c.indexA;
id.indexB = c.indexB;
id.typeA = c.typeA;
id.typeB = c.typeB;
}
}
/**
* This is used for determining the state of contact points.
*
* @author Daniel Murphy
*/
public static enum PointState {
/**
* point does not exist
*/
NULL_STATE,
/**
* point was added in the update
*/
ADD_STATE,
/**
* point persisted across the update
*/
PERSIST_STATE,
/**
* point was removed in the update
*/
REMOVE_STATE
}
/**
* This structure is used to keep track of the best separating axis.
*/
static class EPAxis {
enum Type {
UNKNOWN, EDGE_A, EDGE_B
}
Type type;
int index;
float separation;
}
/**
* This holds polygon B expressed in frame A.
*/
static class TempPolygon {
final Vec2[] vertices = new Vec2[Settings.maxPolygonVertices];
final Vec2[] normals = new Vec2[Settings.maxPolygonVertices];
int count;
public TempPolygon() {
for (int i = 0; i < vertices.length; i++) {
vertices[i] = new Vec2();
normals[i] = new Vec2();
}
}
}
/**
* Reference face used for clipping
*/
static class ReferenceFace {
int i1, i2;
final Vec2 v1 = new Vec2();
final Vec2 v2 = new Vec2();
final Vec2 normal = new Vec2();
final Vec2 sideNormal1 = new Vec2();
float sideOffset1;
final Vec2 sideNormal2 = new Vec2();
float sideOffset2;
}
/**
* This class collides and edge and a polygon, taking into account edge adjacency.
*/
static class EPCollider {
enum VertexType {
ISOLATED, CONCAVE, CONVEX
}
final TempPolygon m_polygonB = new TempPolygon();
final Transform m_xf = new Transform();
final Vec2 m_centroidB = new Vec2();
Vec2 m_v0 = new Vec2();
Vec2 m_v1 = new Vec2();
Vec2 m_v2 = new Vec2();
Vec2 m_v3 = new Vec2();
final Vec2 m_normal0 = new Vec2();
final Vec2 m_normal1 = new Vec2();
final Vec2 m_normal2 = new Vec2();
final Vec2 m_normal = new Vec2();
VertexType m_type1, m_type2;
final Vec2 m_lowerLimit = new Vec2();
final Vec2 m_upperLimit = new Vec2();
float m_radius;
boolean m_front;
public EPCollider() {
for (int i = 0; i < 2; i++) {
ie[i] = new ClipVertex();
clipPoints1[i] = new ClipVertex();
clipPoints2[i] = new ClipVertex();
}
}
private final Vec2 edge1 = new Vec2();
private final Vec2 temp = new Vec2();
private final Vec2 edge0 = new Vec2();
private final Vec2 edge2 = new Vec2();
private final ClipVertex[] ie = new ClipVertex[2];
private final ClipVertex[] clipPoints1 = new ClipVertex[2];
private final ClipVertex[] clipPoints2 = new ClipVertex[2];
private final ReferenceFace rf = new ReferenceFace();
private final EPAxis edgeAxis = new EPAxis();
private final EPAxis polygonAxis = new EPAxis();
public void collide(Manifold manifold, final EdgeShape edgeA, final Transform xfA,
final PolygonShape polygonB, final Transform xfB) {
Transform.mulTransToOutUnsafe(xfA, xfB, m_xf);
Transform.mulToOutUnsafe(m_xf, polygonB.m_centroid, m_centroidB);
m_v0 = edgeA.m_vertex0;
m_v1 = edgeA.m_vertex1;
m_v2 = edgeA.m_vertex2;
m_v3 = edgeA.m_vertex3;
boolean hasVertex0 = edgeA.m_hasVertex0;
boolean hasVertex3 = edgeA.m_hasVertex3;
edge1.set(m_v2).subLocal(m_v1);
edge1.normalize();
m_normal1.set(edge1.y, -edge1.x);
float offset1 = Vec2.dot(m_normal1, temp.set(m_centroidB).subLocal(m_v1));
float offset0 = 0.0f, offset2 = 0.0f;
boolean convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0) {
edge0.set(m_v1).subLocal(m_v0);
edge0.normalize();
m_normal0.set(edge0.y, -edge0.x);
convex1 = Vec2.cross(edge0, edge1) >= 0.0f;
offset0 = Vec2.dot(m_normal0, temp.set(m_centroidB).subLocal(m_v0));
}
// Is there a following edge?
if (hasVertex3) {
edge2.set(m_v3).subLocal(m_v2);
edge2.normalize();
m_normal2.set(edge2.y, -edge2.x);
convex2 = Vec2.cross(edge1, edge2) > 0.0f;
offset2 = Vec2.dot(m_normal2, temp.set(m_centroidB).subLocal(m_v2));
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3) {
if (convex1 && convex2) {
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front) {
m_normal.x = m_normal1.x;
m_normal.y = m_normal1.y;
m_lowerLimit.x = m_normal0.x;
m_lowerLimit.y = m_normal0.y;
m_upperLimit.x = m_normal2.x;
m_upperLimit.y = m_normal2.y;
} else {
m_normal.x = -m_normal1.x;
m_normal.y = -m_normal1.y;
m_lowerLimit.x = -m_normal1.x;
m_lowerLimit.y = -m_normal1.y;
m_upperLimit.x = -m_normal1.x;
m_upperLimit.y = -m_normal1.y;
}
} else if (convex1) {
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front) {
m_normal.x = m_normal1.x;
m_normal.y = m_normal1.y;
m_lowerLimit.x = m_normal0.x;
m_lowerLimit.y = m_normal0.y;
m_upperLimit.x = m_normal1.x;
m_upperLimit.y = m_normal1.y;
} else {
m_normal.x = -m_normal1.x;
m_normal.y = -m_normal1.y;
m_lowerLimit.x = -m_normal2.x;
m_lowerLimit.y = -m_normal2.y;
m_upperLimit.x = -m_normal1.x;
m_upperLimit.y = -m_normal1.y;
}
} else if (convex2) {
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front) {
m_normal.x = m_normal1.x;
m_normal.y = m_normal1.y;
m_lowerLimit.x = m_normal1.x;
m_lowerLimit.y = m_normal1.y;
m_upperLimit.x = m_normal2.x;
m_upperLimit.y = m_normal2.y;
} else {
m_normal.x = -m_normal1.x;
m_normal.y = -m_normal1.y;
m_lowerLimit.x = -m_normal1.x;
m_lowerLimit.y = -m_normal1.y;
m_upperLimit.x = -m_normal0.x;
m_upperLimit.y = -m_normal0.y;
}
} else {
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front) {
m_normal.x = m_normal1.x;
m_normal.y = m_normal1.y;
m_lowerLimit.x = m_normal1.x;
m_lowerLimit.y = m_normal1.y;
m_upperLimit.x = m_normal1.x;
m_upperLimit.y = m_normal1.y;
} else {
m_normal.x = -m_normal1.x;
m_normal.y = -m_normal1.y;
m_lowerLimit.x = -m_normal2.x;
m_lowerLimit.y = -m_normal2.y;
m_upperLimit.x = -m_normal0.x;
m_upperLimit.y = -m_normal0.y;
}
}
} else if (hasVertex0) {
if (convex1) {
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front) {
m_normal.x = m_normal1.x;
m_normal.y = m_normal1.y;
m_lowerLimit.x = m_normal0.x;
m_lowerLimit.y = m_normal0.y;
m_upperLimit.x = -m_normal1.x;
m_upperLimit.y = -m_normal1.y;
} else {
m_normal.x = -m_normal1.x;
m_normal.y = -m_normal1.y;
m_lowerLimit.x = m_normal1.x;
m_lowerLimit.y = m_normal1.y;
m_upperLimit.x = -m_normal1.x;
m_upperLimit.y = -m_normal1.y;
}
} else {
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front) {
m_normal.x = m_normal1.x;
m_normal.y = m_normal1.y;
m_lowerLimit.x = m_normal1.x;
m_lowerLimit.y = m_normal1.y;
m_upperLimit.x = -m_normal1.x;
m_upperLimit.y = -m_normal1.y;
} else {
m_normal.x = -m_normal1.x;
m_normal.y = -m_normal1.y;
m_lowerLimit.x = m_normal1.x;
m_lowerLimit.y = m_normal1.y;
m_upperLimit.x = -m_normal0.x;
m_upperLimit.y = -m_normal0.y;
}
}
} else if (hasVertex3) {
if (convex2) {
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front) {
m_normal.x = m_normal1.x;
m_normal.y = m_normal1.y;
m_lowerLimit.x = -m_normal1.x;
m_lowerLimit.y = -m_normal1.y;
m_upperLimit.x = m_normal2.x;
m_upperLimit.y = m_normal2.y;
} else {
m_normal.x = -m_normal1.x;
m_normal.y = -m_normal1.y;
m_lowerLimit.x = -m_normal1.x;
m_lowerLimit.y = -m_normal1.y;
m_upperLimit.x = m_normal1.x;
m_upperLimit.y = m_normal1.y;
}
} else {
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front) {
m_normal.x = m_normal1.x;
m_normal.y = m_normal1.y;
m_lowerLimit.x = -m_normal1.x;
m_lowerLimit.y = -m_normal1.y;
m_upperLimit.x = m_normal1.x;
m_upperLimit.y = m_normal1.y;
} else {
m_normal.x = -m_normal1.x;
m_normal.y = -m_normal1.y;
m_lowerLimit.x = -m_normal2.x;
m_lowerLimit.y = -m_normal2.y;
m_upperLimit.x = m_normal1.x;
m_upperLimit.y = m_normal1.y;
}
}
} else {
m_front = offset1 >= 0.0f;
if (m_front) {
m_normal.x = m_normal1.x;
m_normal.y = m_normal1.y;
m_lowerLimit.x = -m_normal1.x;
m_lowerLimit.y = -m_normal1.y;
m_upperLimit.x = -m_normal1.x;
m_upperLimit.y = -m_normal1.y;
} else {
m_normal.x = -m_normal1.x;
m_normal.y = -m_normal1.y;
m_lowerLimit.x = m_normal1.x;
m_lowerLimit.y = m_normal1.y;
m_upperLimit.x = m_normal1.x;
m_upperLimit.y = m_normal1.y;
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB.m_count;
for (int i = 0; i < polygonB.m_count; ++i) {
Transform.mulToOutUnsafe(m_xf, polygonB.m_vertices[i], m_polygonB.vertices[i]);
Rot.mulToOutUnsafe(m_xf.q, polygonB.m_normals[i], m_polygonB.normals[i]);
}
m_radius = 2.0f * Settings.polygonRadius;
manifold.pointCount = 0;
computeEdgeSeparation(edgeAxis);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == EPAxis.Type.UNKNOWN) {
return;
}
if (edgeAxis.separation > m_radius) {
return;
}
computePolygonSeparation(polygonAxis);
if (polygonAxis.type != EPAxis.Type.UNKNOWN && polygonAxis.separation > m_radius) {
return;
}
// Use hysteresis for jitter reduction.
final float k_relativeTol = 0.98f;
final float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.type == EPAxis.Type.UNKNOWN) {
primaryAxis = edgeAxis;
} else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) {
primaryAxis = polygonAxis;
} else {
primaryAxis = edgeAxis;
}
final ClipVertex ie0 = ie[0];
final ClipVertex ie1 = ie[1];
if (primaryAxis.type == EPAxis.Type.EDGE_A) {
manifold.type = Manifold.ManifoldType.FACE_A;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vec2.dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i) {
float value = Vec2.dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue) {
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie0.v.set(m_polygonB.vertices[i1]);
ie0.id.indexA = 0;
ie0.id.indexB = (byte) i1;
ie0.id.typeA = (byte) ContactID.Type.FACE.ordinal();
ie0.id.typeB = (byte) ContactID.Type.VERTEX.ordinal();
ie1.v.set(m_polygonB.vertices[i2]);
ie1.id.indexA = 0;
ie1.id.indexB = (byte) i2;
ie1.id.typeA = (byte) ContactID.Type.FACE.ordinal();
ie1.id.typeB = (byte) ContactID.Type.VERTEX.ordinal();
if (m_front) {
rf.i1 = 0;
rf.i2 = 1;
rf.v1.set(m_v1);
rf.v2.set(m_v2);
rf.normal.set(m_normal1);
} else {
rf.i1 = 1;
rf.i2 = 0;
rf.v1.set(m_v2);
rf.v2.set(m_v1);
rf.normal.set(m_normal1).negateLocal();
}
} else {
manifold.type = Manifold.ManifoldType.FACE_B;
ie0.v.set(m_v1);
ie0.id.indexA = 0;
ie0.id.indexB = (byte) primaryAxis.index;
ie0.id.typeA = (byte) ContactID.Type.VERTEX.ordinal();
ie0.id.typeB = (byte) ContactID.Type.FACE.ordinal();
ie1.v.set(m_v2);
ie1.id.indexA = 0;
ie1.id.indexB = (byte) primaryAxis.index;
ie1.id.typeA = (byte) ContactID.Type.VERTEX.ordinal();
ie1.id.typeB = (byte) ContactID.Type.FACE.ordinal();
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1.set(m_polygonB.vertices[rf.i1]);
rf.v2.set(m_polygonB.vertices[rf.i2]);
rf.normal.set(m_polygonB.normals[rf.i1]);
}
rf.sideNormal1.set(rf.normal.y, -rf.normal.x);
rf.sideNormal2.set(rf.sideNormal1).negateLocal();
rf.sideOffset1 = Vec2.dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Vec2.dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
int np;
// Clip to box side 1
np = clipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings.maxManifoldPoints) {
return;
}
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings.maxManifoldPoints) {
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == EPAxis.Type.EDGE_A) {
manifold.localNormal.set(rf.normal);
manifold.localPoint.set(rf.v1);
} else {
manifold.localNormal.set(polygonB.m_normals[rf.i1]);
manifold.localPoint.set(polygonB.m_vertices[rf.i1]);
}
int pointCount = 0;
for (int i = 0; i < Settings.maxManifoldPoints; ++i) {
float separation;
separation = Vec2.dot(rf.normal, temp.set(clipPoints2[i].v).subLocal(rf.v1));
if (separation <= m_radius) {
ManifoldPoint cp = manifold.points[pointCount];
if (primaryAxis.type == EPAxis.Type.EDGE_A) {
// cp.localPoint = MulT(m_xf, clipPoints2[i].v);
Transform.mulTransToOutUnsafe(m_xf, clipPoints2[i].v, cp.localPoint);
cp.id.set(clipPoints2[i].id);
} else {
cp.localPoint.set(clipPoints2[i].v);
cp.id.typeA = clipPoints2[i].id.typeB;
cp.id.typeB = clipPoints2[i].id.typeA;
cp.id.indexA = clipPoints2[i].id.indexB;
cp.id.indexB = clipPoints2[i].id.indexA;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public void computeEdgeSeparation(EPAxis axis) {
axis.type = EPAxis.Type.EDGE_A;
axis.index = m_front ? 0 : 1;
axis.separation = Float.MAX_VALUE;
float nx = m_normal.x;
float ny = m_normal.y;
for (int i = 0; i < m_polygonB.count; ++i) {
Vec2 v = m_polygonB.vertices[i];
float tempx = v.x - m_v1.x;
float tempy = v.y - m_v1.y;
float s = nx * tempx + ny * tempy;
if (s < axis.separation) {
axis.separation = s;
}
}
}
private final Vec2 perp = new Vec2();
private final Vec2 n = new Vec2();
public void computePolygonSeparation(EPAxis axis) {
axis.type = EPAxis.Type.UNKNOWN;
axis.index = -1;
axis.separation = -Float.MAX_VALUE;
perp.x = -m_normal.y;
perp.y = m_normal.x;
for (int i = 0; i < m_polygonB.count; ++i) {
Vec2 normalB = m_polygonB.normals[i];
Vec2 vB = m_polygonB.vertices[i];
n.x = -normalB.x;
n.y = -normalB.y;
// float s1 = Vec2.dot(n, temp.set(vB).subLocal(m_v1));
// float s2 = Vec2.dot(n, temp.set(vB).subLocal(m_v2));
float tempx = vB.x - m_v1.x;
float tempy = vB.y - m_v1.y;
float s1 = n.x * tempx + n.y * tempy;
tempx = vB.x - m_v2.x;
tempy = vB.y - m_v2.y;
float s2 = n.x * tempx + n.y * tempy;
float s = MathUtils.min(s1, s2);
if (s > m_radius) {
// No collision
axis.type = EPAxis.Type.EDGE_B;
axis.index = i;
axis.separation = s;
return;
}
// Adjacency
if (n.x * perp.x + n.y * perp.y >= 0.0f) {
if (Vec2.dot(temp.set(n).subLocal(m_upperLimit), m_normal) < -Settings.angularSlop) {
continue;
}
} else {
if (Vec2.dot(temp.set(n).subLocal(m_lowerLimit), m_normal) < -Settings.angularSlop) {
continue;
}
}
if (s > axis.separation) {
axis.type = EPAxis.Type.EDGE_B;
axis.index = i;
axis.separation = s;
}
}
}
}
}