findIncidentEdge method

void findIncidentEdge(
1. c,
2. PolygonShape poly1,
3. Transform xf1,
4. int edge1,
5. PolygonShape poly2,
6. Transform xf2
)

Implementation

``````void findIncidentEdge(
final List<ClipVertex> c,
final PolygonShape poly1,
final Transform xf1,
int edge1,
final PolygonShape poly2,
final Transform xf2) {
int count1 = poly1.count;
final List<Vector2> normals1 = poly1.normals;

int count2 = poly2.count;
final List<Vector2> vertices2 = poly2.vertices;
final List<Vector2> normals2 = poly2.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 Vector2 v = normals1[edge1];
final double tempx = xf1q.c * v.x - xf1q.s * v.y;
final double tempy = xf1q.s * v.x + xf1q.c * v.y;
final double normal1x = xf2q.c * tempx + xf2q.s * tempy;
final double normal1y = -xf2q.s * tempx + xf2q.c * tempy;

// end inline

// Find the incident edge on poly2.
int index = 0;
double minDot = double.maxFinite;
for (int i = 0; i < count2; ++i) {
Vector2 b = normals2[i];
double 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]);
Vector2 v1 = vertices2[i1];
Vector2 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 = edge1 & 0xFF;
c0.id.indexB = i1 & 0xFF;
c0.id.typeA = ContactIDType.FACE.index & 0xFF;
c0.id.typeB = ContactIDType.VERTEX.index & 0xFF;

// c1.v = Mul(xf2, vertices2[i2]);
Vector2 v2 = vertices2[i2];
Vector2 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 = edge1 & 0xFF;
c1.id.indexB = i2 & 0xFF;
c1.id.typeA = ContactIDType.FACE.index & 0xFF;
c1.id.typeB = ContactIDType.VERTEX.index & 0xFF;
}``````