This is a continuation of our look into the CollidePolgyons method.
We have delved into the Lite implementation in quite some detail and the full version is very similar in structure. Hence this will be more of a refresher of the logic.
There is however, one very important difference between the two implementations.
Lite implementation returns the clipped points in world space coordinates.
Full implementation returns the clipped points in local space coordinates of the reference body.As I understand it, the preference is to keep as much of the calculations as possible in local space coordinates.
ClipVertex
As a reminder, as with the Lite implementation, an object called ClipVertex is used to identify the vertices to be clipped.
/// Used for computing contact manifolds.
class b2ClipVertex {
constructor() {
this.v = new Vec2(); //b2Vec2
this.id = new b2ContactID(); //b2ContactID
}
};CollidePolygon method - continued
In the previous post we looked at the collision detection part, where we identified:
the polygons are overlapping
the significant faces
Now we want to identify the incident vs the reference edges, before we start the clipping process.
Identifying the Incident Edge vs Reference Edge
// if get to here, means polys are colliding
let poly1; // reference polygon
let poly2; // incident polygon
let xf1, xf2;
let edge1; // reference edge
let flip;
const k_tol = 0.1 b2_linearSlop;
if (separationB.separation > separationA.separation + k_tol) {
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = separationB.edgeIndex;
manifold.type = b2Manifold.e_faceB;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = separationA.edgeIndex;
manifold.type = b2Manifold.e_faceA;
flip = false;
}
let incidentEdge = [new b2ClipVertex(), new b2ClipVertex()]; //b2ClipVertex
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
Finding the Incident Edge
/**
Find Incident Edge
@param b2ClipVertex c[2]
@param const b2ClipVertex vIn[2]
@param const const b2Vec2& normal
@param float offset
@param int32 vertexIndexA
/
function b2FindIncidentEdge(c, poly1, xf1, edge1, poly2, xf2) {
const normals1 = poly1.m_normals;
let count2 = poly2.m_count;
const vertices2 = poly2.m_vertices;
const normals2 = poly2.m_normals;
b2Assert(0 <= edge1 && edge1 < poly1.m_count);
// Get the normal of the reference edge in poly2's frame.
let normal1 = Rot.MulQTV(xf2.q, Rot.MulQV(xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
let index = 0;
let minDot = Number.MAX_VALUE;
for (let i = 0; i < count2; i++) {
let dot = Vec2.b2Dot(normal1, normals2[i]);
if (dot < minDot) {
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
let i1 = index;
let i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c[0].v = b2Mul(xf2, vertices2[i1]);
c[0].id.cf.indexA = edge1;
c[0].id.cf.indexB = i1;
c[0].id.cf.typeA = b2ContactFeature.e_face;
c[0].id.cf.typeB = b2ContactFeature.e_vertex;
c[1].v = b2Mul(xf2, vertices2[i2]);
c[1].id.cf.indexA = edge1;
c[1].id.cf.indexB = i2;
c[1].id.cf.typeA = b2ContactFeature.e_face;
c[1].id.cf.typeB = b2ContactFeature.e_vertex;
}
ClipSegmentToLine
/**
* Clipping for contact manifolds.
* @param b2ClipVertex vOut[2]
* @param const b2ClipVertex vIn[2]
* @param const const b2Vec2& normal
* @param float offset
* @param int32 vertexIndexA
*/
// Sutherland-Hodgman clipping.
function b2ClipSegmentToLine(vOut, vIn, normal, offset, vertexIndexA) {
// Start with no output points
let count = 0;
// Calculate the distance of end points to the line
let distance0 = Vec2.b2Dot(normal, vIn[0].v) - offset;
let distance1 = Vec2.b2Dot(normal, vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0) vOut[count++] = vIn[0];
if (distance1 <= 0.0) vOut[count++] = vIn[1];
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0) {
// Find intersection point of edge and plane
let interp = distance0 / (distance0 - distance1);
vOut[count].v = Vec2.AddScaled(vIn[0].v, Vec2.Subtract(vIn[1].v, vIn[0].v), interp);
// VertexA is hitting edgeB.
vOut[count].id.cf.indexA = vertexIndexA;
vOut[count].id.cf.indexB = vIn[0].id.cf.indexB;
vOut[count].id.cf.typeA = b2ContactFeature.e_vertex;
vOut[count].id.cf.typeB = b2ContactFeature.e_face;
count++;
b2Assert(count == 2);
}
return count;
}
The actual clipping
Going back to the CollidePolygons class, we now start the actual clipping process.
Remember, there are 3 main stages to the clipping process:
Clip against the positive side of the reference edge
Clip against the negative side of the reference edge
Clip against the reference edge itself
Clipping against the adjacent sides
The code goes onto prepare for the clipping process.
let count1 = poly1.m_count;
const vertices1 = poly1.m_vertices;
let iv1 = edge1;
let iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
let v11 = vertices1[iv1];
let v12 = vertices1[iv2];
let localTangent = Vec2.Subtract(v12, v11);
localTangent.normalize();
let localNormal = Vec2.b2Cross(localTangent, 1.0); // vector
let planePoint = Vec2.Scale(Vec2.Add(v11, v12), 0.5); // vector
let tangent = Rot.MulQV(xf1.q, localTangent); // vector
let normal = Vec2.b2Cross(tangent, 1.0); // vector
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
// Face offset.
let frontOffset = Vec2.b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
let sideOffset1 = -Vec2.b2Dot(tangent, v11) + totalRadius;
let sideOffset2 = Vec2.b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
let clipPoints1 = [new b2ClipVertex(), new b2ClipVertex()];
let clipPoints2 = [new b2ClipVertex(), new b2ClipVertex()];
let np; // integer
Clip against positive side of reference edge
Clip the incident edge against the positive side of reference edge
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, Vec2.Negate(tangent), sideOffset1, iv1);
if (np < 2)
return;
Clip against negative side of reference edge
Now clip the incident edge against the negative side of reference edge
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < 2) {
return;
}
// Now clipPoints2 contains the clipped points.Final Clip
The code that deals with step 3 - clipping against the reference edge itself - is shown below. Unlike clipping the incident edge as in the previous stage, we now just remove all points that lie inside the clipping region.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
let pointCount = 0;
for (let i = 0; i < b2_maxManifoldPoints; i++) {
let separation = Vec2.b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius) {
let cp = manifold.points[pointCount]; // b2ManifoldPoint*
// cp.localPoint = Transform.MulTTV(xf2, clipPoints2[i].v);
cp.localPoint = b2MulT(xf2, clipPoints2[i].v);
//cp.localPoint = clipPoints2[i].v
cp.id = clipPoints2[i].id;
if (flip) {
// Swap features
let cf = cp.id.cf; // b2ContactFeature
cp.id.cf.indexA = cf.indexB;
cp.id.cf.indexB = cf.indexA;
cp.id.cf.typeA = cf.typeB;
cp.id.cf.typeB = cf.typeA;
}
pointCount++;
}
}
manifold.pointCount = pointCount;What remains from this final clipping process are copied from clipPoints2 to the manifold.
Note, that the above code is a direct port of Box2D method. As noted earlier, the CollidePolygons method gives you the contact points and the normal in local space of the reference polygon.