// djm pooling from above
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_vertexCount;
final Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_vertexCount;
final Vec2[] vertices2 = poly2.m_vertices;
final Vec2[] normals2 = poly2.m_normals;
assert (0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Mat22.mulToOut(xf1.R, normals1[edge1], normal1); // temporary
// b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
Mat22.mulTransToOut(xf2.R, normal1, normal1);
// Find the incident edge on poly2.
int index = 0;
float minDot = Float.MAX_VALUE;
for (int i = 0; i < count2; ++i) {
float dot = Vec2.dot(normal1, normals2[i]);
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;
Transform.mulToOut(xf2, vertices2[i1], c[0].v); // = Mul(xf2, vertices2[i1]);
c[0].id.features.referenceEdge = edge1;
c[0].id.features.incidentEdge = i1;
c[0].id.features.incidentVertex = 0;
Transform.mulToOut(xf2, vertices2[i2], c[1].v); // = Mul(xf2, vertices2[i2]);
c[1].id.features.referenceEdge = edge1;
c[1].id.features.incidentEdge = i2;
c[1].id.features.incidentVertex = 1;
}
/**
* Find the separation between poly1 and poly2 for a given edge normal on poly1.
*
* @param poly1
* @param xf1
* @param edge1
* @param poly2
* @param xf2
*/
public final float edgeSeparation(
final PolygonShape poly1,
final Transform xf1,
final int edge1,
final PolygonShape poly2,
final Transform xf2) {
int count1 = poly1.m_vertexCount;
final Vec2[] vertices1 = poly1.m_vertices;
final Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_vertexCount;
final Vec2[] vertices2 = poly2.m_vertices;
assert (0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
// Vec2 normal1World = Mul(xf1.R, normals1[edge1]);
Mat22.mulToOut(xf1.R, normals1[edge1], normal1World);
// Vec2 normal1 = MulT(xf2.R, normal1World);
Mat22.mulTransToOut(xf2.R, normal1World, normal1);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Float.MAX_VALUE;
for (int i = 0; i < count2; ++i) {
float dot = Vec2.dot(vertices2[i], normal1);
if (dot < minDot) {
minDot = dot;
index = i;
}
}
// Vec2 v1 = Mul(xf1, vertices1[edge1]);
// Vec2 v2 = Mul(xf2, vertices2[index]);
Transform.mulToOut(xf1, vertices1[edge1], v1);
Transform.mulToOut(xf2, vertices2[index], v2);
float separation = Vec2.dot(v2.subLocal(v1), normal1World);
return separation;
}
/**
* 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_vertexCount;
final Vec2[] normals1 = poly1.m_normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Transform.mulToOut(xf2, poly2.m_centroid, d);
Transform.mulToOut(xf1, poly1.m_centroid, temp);
d.subLocal(temp);
Mat22.mulTransToOut(xf1.R, d, dLocal1);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float dot;
float maxDot = Float.MIN_VALUE;
for (int i = 0; i < count1; i++) {
dot = Vec2.dot(normals1[i], dLocal1);
if (dot > maxDot) {
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = edgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = edgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = edgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext) {
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
} else if (sNext > s) {
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
} else {
results.edgeIndex = edge;
results.separation = s;
return;
}
// Perform a local search for the best edge normal.
for (; ; ) {
if (increment == -1) {
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
} else {
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = edgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation) {
bestEdge = edge;
bestSeparation = s;
} else {
break;
}
}
results.edgeIndex = bestEdge;
results.separation = bestSeparation;
}
public final void getLocalVectorToOut(Vec2 worldVector, Vec2 out) {
Mat22.mulTransToOut(m_xf.R, worldVector, out);
}