Collision class
class Collision { static const int NULL_FEATURE = Settings.MAX_INTEGER; final DefaultWorldPool _pool; /** Cache used to help warmstart distance. */ final SimplexCache cache; /** Distance input and output. */ final DistanceInput input; final DistanceOutput output; /** A pool of already constructed objects. */ final EdgeResults results1; final EdgeResults results2; final List<ClipVertex> incidentEdge; final vec2 localTangent; final vec2 localNormal; final vec2 planePoint; final vec2 tangent; final vec2 normal; final vec2 normal1; final vec2 v11; final vec2 v12; final List<ClipVertex> clipPoints1; final List<ClipVertex> clipPoints2; /** * Constructs a new Collision object. Should only be constructed once (in the * pool). Retrieve from the pool to use. */ Collision._construct(DefaultWorldPool pool) : _pool = pool, input = new DistanceInput(), cache = new SimplexCache(), output = new DistanceOutput(), results1 = new EdgeResults(), results2 = new EdgeResults(), incidentEdge = new List<ClipVertex>(2), localTangent = new vec2.zero(), localNormal = new vec2.zero(), planePoint = new vec2.zero(), tangent = new vec2.zero(), normal = new vec2.zero(), normal1 = new vec2.zero(), v11 = new vec2.zero(), v12 = new vec2.zero(), clipPoints1 = new List<ClipVertex>(2), clipPoints2 = new List<ClipVertex>(2) { incidentEdge[0] = new ClipVertex(); incidentEdge[1] = new ClipVertex(); clipPoints1[0] = new ClipVertex(); clipPoints1[1] = new ClipVertex(); clipPoints2[0] = new ClipVertex(); clipPoints2[1] = new ClipVertex(); } /** * Returns true if the two given shapes overlap. */ bool testOverlap(Shape shapeA, Shape shapeB, Transform transformA, Transform transformB) { input.proxyA.setFromShape(shapeA); input.proxyB.setFromShape(shapeB); input.transformA.setFrom(transformA); input.transformB.setFrom(transformB); input.useRadii = true; cache.count = 0; _pool.distance.computeDistance(output, cache, input); return output._distance < 10.0 * 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. */ void getPointStates(List<int> state1, List<int> state2, Manifold manifold1, Manifold manifold2) { for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; ++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. */ static int clipSegmentToLine(List<ClipVertex> vOut, List<ClipVertex> vIn, vec2 norm, num offset) { // Start with no output points int numOut = 0; // Calculate the distance of end points to the line num distance0 = dot(norm, vIn[0].v) - offset; num distance1 = dot(norm, vIn[1].v) - offset; // If the points are behind the plane if (distance0 <= 0.0) vOut[numOut++].setFrom(vIn[0]); if (distance1 <= 0.0) vOut[numOut++].setFrom(vIn[1]); // If the points are on different sides of the plane if (distance0 * distance1 < 0.0) { // Find intersection point of edge and plane num interp = distance0 / (distance0 - distance1); // vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); vOut[numOut].v.copyFrom(vIn[1].v). sub(vIn[0].v).scale(interp).add(vIn[0].v); final ClipVertex vin = (distance0 > 0.0 ? vIn[0] : vIn[1]); vOut[numOut].id.setFrom(vin.id); ++numOut; } return numOut; } /** Compute the collision manifold between two circles. */ void collideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB) { manifold.pointCount = 0; final vec2 v = circle1.position; final num pAy = xfA.position.y + xfA.rotation.col0.y * v.x + xfA.rotation.col1.y * v.y; final num pAx = xfA.position.x + xfA.rotation.col0.x * v.x + xfA.rotation.col1.x * v.y; final vec2 v1 = circle2.position; final num pBy = xfB.position.y + xfB.rotation.col0.y * v1.x + xfB.rotation.col1.y * v1.y; final num pBx = xfB.position.x + xfB.rotation.col0.x * v1.x + xfB.rotation.col1.x * v1.y; final num dx = pBx - pAx; final num dy = pBy - pAy; final num distSqr = dx * dx + dy * dy; final num radius = circle1.radius + circle2.radius; if (distSqr > radius * radius) return; manifold.type = ManifoldType.CIRCLES; manifold.localPoint.copyFrom(circle1.position); manifold.localNormal.splat(0.0); manifold.pointCount = 1; manifold.points[0].localPoint.copyFrom(circle2.position); manifold.points[0].id.zero(); } /** * Compute the collision manifold between a polygon and a circle. */ void collidePolygonAndCircle(Manifold manifold, PolygonShape polygon, Transform xfA, CircleShape circle, Transform xfB) { manifold.pointCount = 0; vec2 v = circle.position; final num cy = xfB.position.y + xfB.rotation.col0.y * v.x + xfB.rotation.col1.y * v.y; final num cx = xfB.position.x + xfB.rotation.col0.x * v.x + xfB.rotation.col1.x * v.y; final num v1x = cx - xfA.position.x; final num v1y = cy - xfA.position.y; final vec2 b = xfA.rotation.col0; final vec2 b1 = xfA.rotation.col1; final num cLocaly = v1x * b1.x + v1y * b1.y; final num cLocalx = v1x * b.x + v1y * b.y; // Find the min separating edge. int normalIndex = 0; num separation = Settings.SMALL_NUMBER; final num radius = polygon.radius + circle.radius; final int vertexCount = polygon.vertexCount; final List<vec2> vertices = polygon.vertices; final List<vec2> normals = polygon.normals; for (int i = 0; i < vertexCount; ++i) { final vec2 vertex = vertices[i]; final num tempx = cLocalx - vertex.x; final num tempy = cLocaly - vertex.y; final vec2 norm = normals[i]; final num s = norm.x * tempx + norm.y * tempy; // early out if (s > radius) 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; vec2 norm = normals[normalIndex]; manifold.localNormal.x = norm.x; manifold.localNormal.y = norm.y; manifold.localPoint.x = (v1.x + v2.x) * .5; manifold.localPoint.y = (v1.y + v2.y) * .5; ManifoldPoint mpoint = manifold.points[0]; mpoint.localPoint.x = circle.position.x; mpoint.localPoint.y = circle.position.y; mpoint.id.zero(); return; } // Compute barycentric coordinates final num tempX = cLocalx - v1.x; final num tempY = cLocaly - v1.y; final num temp2X = v2.x - v1.x; final num temp2Y = v2.y - v1.y; final num u1 = tempX * temp2X + tempY * temp2Y; final num temp3X = cLocalx - v2.x; final num temp3Y = cLocaly - v2.y; final num temp4X = v1.x - v2.x; final num temp4Y = v1.y - v2.y; final num u2 = temp3X * temp4X + temp3Y * temp4Y; if (u1 <= 0) { final num dx = cLocalx - v1.x; final num dy = cLocaly - v1.y; if ( dx * dx + dy * dy > radius * radius) return; manifold.pointCount = 1; manifold.type = ManifoldType.FACE_A; manifold.localNormal.x = cLocalx - v1.x; manifold.localNormal.y = cLocaly - v1.y; manifold.localNormal.normalize(); manifold.localPoint.copyFrom(v1); manifold.points[0].localPoint.copyFrom(circle.position); manifold.points[0].id.zero(); } else if (u2 <= 0.0) { final num dx = cLocalx - v2.x; final num dy = cLocaly - v2.y; if ( dx * dx + dy * dy > radius * radius) return; manifold.pointCount = 1; manifold.type = ManifoldType.FACE_A; manifold.localNormal.x = cLocalx - v2.x; manifold.localNormal.y = cLocaly - v2.y; manifold.localNormal.normalize(); manifold.localPoint.copyFrom(v2); manifold.points[0].localPoint.copyFrom(circle.position); manifold.points[0].id.zero(); } else { // vec2 faceCenter = 0.5 * (v1 + v2); // (temp is faceCenter) final num fcx = (v1.x + v2.x) * .5; final num fcy = (v1.y + v2.y) * .5; final num tx = cLocalx - fcx; final num ty = cLocaly - fcy; final vec2 norm = normals[vertIndex1]; separation = tx * norm.x + ty * norm.y; if (separation > radius) return; manifold.pointCount = 1; manifold.type = ManifoldType.FACE_A; manifold.localNormal.copyFrom(normals[vertIndex1]); manifold.localPoint.x = fcx; manifold.localPoint.y = fcy; manifold.points[0].localPoint.copyFrom(circle.position); manifold.points[0].id.zero(); } } /** * Find the separation between poly1 and poly2 for a given edge normal on * poly1. */ num edgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) { final int count1 = poly1.vertexCount; final List<vec2> vertices1 = poly1.vertices; final List<vec2> normals1 = poly1.normals; final int count2 = poly2.vertexCount; final List<vec2> vertices2 = poly2.vertices; assert (0 <= edge1 && edge1 < count1); // Convert normal from poly1's frame into poly2's frame. final mat2 R = xf1.rotation; final vec2 v = normals1[edge1]; final num normal1Worldy = R.col0.y * v.x + R.col1.y * v.y; final num normal1Worldx = R.col0.x * v.x + R.col1.x * v.y; final mat2 R1 = xf2.rotation; final num normal1x = normal1Worldx * R1.col0.x + normal1Worldy * R1.col0.y; final num normal1y = normal1Worldx * R1.col1.x + normal1Worldy * R1.col1.y; // end inline // Find support vertex on poly2 for -normal. int index = 0; num minDot = Settings.BIG_NUMBER; for (int i = 0; i < count2; ++i) { final vec2 a = vertices2[i]; final num dot_ = a.x * normal1x + a.y * normal1y; if (dot_ < minDot) { minDot = dot_; index = i; } } final vec2 v3 = vertices1[edge1]; final num v1y = xf1.position.y + R.col0.y * v3.x + R.col1.y * v3.y; final num v1x = xf1.position.x + R.col0.x * v3.x + R.col1.x * v3.y; final vec2 v4 = vertices2[index]; final num v2y = xf2.position.y + R1.col0.y * v4.x + R1.col1.y * v4.y - v1y; final num v2x = xf2.position.x + R1.col0.x * v4.x + R1.col1.x * v4.y - v1x; return v2x * normal1Worldx + v2y * normal1Worldy; } /** * Find the max separation between poly1 and poly2 using edge normals from * poly1. */ void findMaxSeparation(EdgeResults results, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2) { int count1 = poly1.vertexCount; final List<vec2> normals1 = poly1.normals; vec2 v = poly2.centroid; final num predy = xf2.position.y + xf2.rotation.col0.y * v.x + xf2.rotation.col1.y * v.y; final num predx = xf2.position.x + xf2.rotation.col0.x * v.x + xf2.rotation.col1.x * v.y; final vec2 v1 = poly1.centroid; final num tempy = xf1.position.y + xf1.rotation.col0.y * v1.x + xf1.rotation.col1.y * v1.y; final num tempx = xf1.position.x + xf1.rotation.col0.x * v1.x + xf1.rotation.col1.x * v1.y; final num dx = predx - tempx; final num dy = predy - tempy; final mat2 R = xf1.rotation; final num dLocal1x = dx * R.col0.x + dy * R.col0.y; final num dLocal1y = dx * R.col1.x + dy * R.col1.y; // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; num maxDot = Settings.SMALL_NUMBER; for (int i = 0; i < count1; i++) { final vec2 norm = normals1[i]; num dot_ = norm.x * dLocal1x + norm.y * dLocal1y; if (dot_ > maxDot) { maxDot = dot_; edge = i; } } // Get the separation for the edge normal. num s = edgeSeparation(poly1, xf1, edge, poly2, xf2); // Check the separation for the previous edge normal. int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; num sPrev = edgeSeparation(poly1, xf1, prevEdge, poly2, xf2); // Check the separation for the next edge normal. int nextEdge = edge + 1 < count1 ? edge + 1 : 0; num sNext = edgeSeparation(poly1, xf1, nextEdge, poly2, xf2); // Find the best edge and the search direction. int bestEdge; num 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. while (true) { 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; } void findIncidentEdge(List<ClipVertex> c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) { int count1 = poly1.vertexCount; final List<vec2> normals1 = poly1.normals; int count2 = poly2.vertexCount; final List<vec2> vertices2 = poly2.vertices; final List<vec2> normals2 = poly2.normals; assert (0 <= edge1 && edge1 < count1); // Get the normal of the reference edge in poly2's frame. xf1.rotation.transformed(normals1[edge1], normal1); xf2.rotation.transposed().transform(normal1); // Find the incident edge on poly2. int index = 0; num minDot = Settings.BIG_NUMBER; for (int i = 0; i < count2; ++i) { num dot_ = 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); 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); c[1].id.features.referenceEdge = edge1; c[1].id.features.incidentEdge = i2; c[1].id.features.incidentVertex = 1; } /** * Compute the collision manifold between two polygons. */ void collidePolygons(Manifold manifold, PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB) { manifold.pointCount = 0; num totalRadius = polyA.radius + polyB.radius; findMaxSeparation(results1, polyA, xfA, polyB, xfB); if (results1.separation > totalRadius) return; findMaxSeparation(results2, polyB, xfB, polyA, xfA); if (results2.separation > totalRadius) return; PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge int flip; num k_relativeTol = 0.98; num k_absoluteTol = 0.001; if (results2.separation > k_relativeTol * results1.separation + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = xfB; xf2 = xfA; edge1 = results2.edgeIndex; manifold.type = ManifoldType.FACE_B; flip = 1; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = results1.edgeIndex; manifold.type = ManifoldType.FACE_A; flip = 0; } findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); int count1 = poly1.vertexCount; List<vec2> vertices1 = poly1.vertices; v11.copyFrom(vertices1[edge1]); v12.copyFrom(edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]); localTangent.copyFrom(v12).sub(v11); localTangent.normalize(); // Vector localNormal = Cross(dv, 1.0); cross(localTangent, 1, localNormal); // Vector planePoint = 0.5 * (v11 + v12) planePoint.copyFrom(v11).add(v12).scale(.5); // Vector sideNormal = Mul(xf1.rotation, v12 - v11); xf1.rotation.transformed(localTangent, tangent); // Vector frontNormal = Cross(sideNormal, 1.0); cross(tangent, 1, normal); // v11 = Mul(xf1, v11); // v12 = Mul(xf1, v12); Transform.mulToOut(xf1, v11, v11); Transform.mulToOut(xf1, v12, v12); // Face offset num frontOffset = dot(normal, v11); // Side offsets, extended by polytope skin thickness. num sideOffset1 = -dot(tangent, v11) + totalRadius; num sideOffset2 = dot(tangent, v12) + 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.negate(); np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1); tangent.negate(); if (np < 2) return; // Clip to negative box side 1 np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2); if (np < 2) return; // Now clipPoints2 contains the clipped points. manifold.localNormal.copyFrom(localNormal); manifold.localPoint.copyFrom(planePoint); int pointCount = 0; for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; ++i) { num separation = dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.points[pointCount]; Transform.mulTransToOut(xf2, clipPoints2[i].v, cp.localPoint); // cp.localPoint = MulT(xf2, clipPoints2[i].v); cp.id.setFrom(clipPoints2[i].id); cp.id.features.flip = flip; ++pointCount; } } manifold.pointCount = pointCount; } }
Static Properties
const int NULL_FEATURE #
NULL_FEATURE = Settings.MAX_INTEGER
Static Methods
int clipSegmentToLine(List<ClipVertex> vOut, List<ClipVertex> vIn, vec2 norm, num offset) #
Clipping for contact manifolds. Sutherland-Hodgman clipping.
static int clipSegmentToLine(List<ClipVertex> vOut, List<ClipVertex> vIn, vec2 norm, num offset) { // Start with no output points int numOut = 0; // Calculate the distance of end points to the line num distance0 = dot(norm, vIn[0].v) - offset; num distance1 = dot(norm, vIn[1].v) - offset; // If the points are behind the plane if (distance0 <= 0.0) vOut[numOut++].setFrom(vIn[0]); if (distance1 <= 0.0) vOut[numOut++].setFrom(vIn[1]); // If the points are on different sides of the plane if (distance0 * distance1 < 0.0) { // Find intersection point of edge and plane num interp = distance0 / (distance0 - distance1); // vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); vOut[numOut].v.copyFrom(vIn[1].v). sub(vIn[0].v).scale(interp).add(vIn[0].v); final ClipVertex vin = (distance0 > 0.0 ? vIn[0] : vIn[1]); vOut[numOut].id.setFrom(vin.id); ++numOut; } return numOut; }
Properties
final SimplexCache cache #
cache
final List<ClipVertex> clipPoints1 #
clipPoints1
final List<ClipVertex> clipPoints2 #
clipPoints2
final List<ClipVertex> incidentEdge #
incidentEdge
final DistanceInput input #
input
final DistanceOutput output #
output
final EdgeResults results1 #
results1
final EdgeResults results2 #
results2
Methods
void collideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB) #
Compute the collision manifold between two circles.
void collideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB) { manifold.pointCount = 0; final vec2 v = circle1.position; final num pAy = xfA.position.y + xfA.rotation.col0.y * v.x + xfA.rotation.col1.y * v.y; final num pAx = xfA.position.x + xfA.rotation.col0.x * v.x + xfA.rotation.col1.x * v.y; final vec2 v1 = circle2.position; final num pBy = xfB.position.y + xfB.rotation.col0.y * v1.x + xfB.rotation.col1.y * v1.y; final num pBx = xfB.position.x + xfB.rotation.col0.x * v1.x + xfB.rotation.col1.x * v1.y; final num dx = pBx - pAx; final num dy = pBy - pAy; final num distSqr = dx * dx + dy * dy; final num radius = circle1.radius + circle2.radius; if (distSqr > radius * radius) return; manifold.type = ManifoldType.CIRCLES; manifold.localPoint.copyFrom(circle1.position); manifold.localNormal.splat(0.0); manifold.pointCount = 1; manifold.points[0].localPoint.copyFrom(circle2.position); manifold.points[0].id.zero(); }
void collidePolygonAndCircle(Manifold manifold, PolygonShape polygon, Transform xfA, CircleShape circle, Transform xfB) #
Compute the collision manifold between a polygon and a circle.
void collidePolygonAndCircle(Manifold manifold, PolygonShape polygon, Transform xfA, CircleShape circle, Transform xfB) { manifold.pointCount = 0; vec2 v = circle.position; final num cy = xfB.position.y + xfB.rotation.col0.y * v.x + xfB.rotation.col1.y * v.y; final num cx = xfB.position.x + xfB.rotation.col0.x * v.x + xfB.rotation.col1.x * v.y; final num v1x = cx - xfA.position.x; final num v1y = cy - xfA.position.y; final vec2 b = xfA.rotation.col0; final vec2 b1 = xfA.rotation.col1; final num cLocaly = v1x * b1.x + v1y * b1.y; final num cLocalx = v1x * b.x + v1y * b.y; // Find the min separating edge. int normalIndex = 0; num separation = Settings.SMALL_NUMBER; final num radius = polygon.radius + circle.radius; final int vertexCount = polygon.vertexCount; final List<vec2> vertices = polygon.vertices; final List<vec2> normals = polygon.normals; for (int i = 0; i < vertexCount; ++i) { final vec2 vertex = vertices[i]; final num tempx = cLocalx - vertex.x; final num tempy = cLocaly - vertex.y; final vec2 norm = normals[i]; final num s = norm.x * tempx + norm.y * tempy; // early out if (s > radius) 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; vec2 norm = normals[normalIndex]; manifold.localNormal.x = norm.x; manifold.localNormal.y = norm.y; manifold.localPoint.x = (v1.x + v2.x) * .5; manifold.localPoint.y = (v1.y + v2.y) * .5; ManifoldPoint mpoint = manifold.points[0]; mpoint.localPoint.x = circle.position.x; mpoint.localPoint.y = circle.position.y; mpoint.id.zero(); return; } // Compute barycentric coordinates final num tempX = cLocalx - v1.x; final num tempY = cLocaly - v1.y; final num temp2X = v2.x - v1.x; final num temp2Y = v2.y - v1.y; final num u1 = tempX * temp2X + tempY * temp2Y; final num temp3X = cLocalx - v2.x; final num temp3Y = cLocaly - v2.y; final num temp4X = v1.x - v2.x; final num temp4Y = v1.y - v2.y; final num u2 = temp3X * temp4X + temp3Y * temp4Y; if (u1 <= 0) { final num dx = cLocalx - v1.x; final num dy = cLocaly - v1.y; if ( dx * dx + dy * dy > radius * radius) return; manifold.pointCount = 1; manifold.type = ManifoldType.FACE_A; manifold.localNormal.x = cLocalx - v1.x; manifold.localNormal.y = cLocaly - v1.y; manifold.localNormal.normalize(); manifold.localPoint.copyFrom(v1); manifold.points[0].localPoint.copyFrom(circle.position); manifold.points[0].id.zero(); } else if (u2 <= 0.0) { final num dx = cLocalx - v2.x; final num dy = cLocaly - v2.y; if ( dx * dx + dy * dy > radius * radius) return; manifold.pointCount = 1; manifold.type = ManifoldType.FACE_A; manifold.localNormal.x = cLocalx - v2.x; manifold.localNormal.y = cLocaly - v2.y; manifold.localNormal.normalize(); manifold.localPoint.copyFrom(v2); manifold.points[0].localPoint.copyFrom(circle.position); manifold.points[0].id.zero(); } else { // vec2 faceCenter = 0.5 * (v1 + v2); // (temp is faceCenter) final num fcx = (v1.x + v2.x) * .5; final num fcy = (v1.y + v2.y) * .5; final num tx = cLocalx - fcx; final num ty = cLocaly - fcy; final vec2 norm = normals[vertIndex1]; separation = tx * norm.x + ty * norm.y; if (separation > radius) return; manifold.pointCount = 1; manifold.type = ManifoldType.FACE_A; manifold.localNormal.copyFrom(normals[vertIndex1]); manifold.localPoint.x = fcx; manifold.localPoint.y = fcy; manifold.points[0].localPoint.copyFrom(circle.position); manifold.points[0].id.zero(); } }
void collidePolygons(Manifold manifold, PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB) #
Compute the collision manifold between two polygons.
void collidePolygons(Manifold manifold, PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB) { manifold.pointCount = 0; num totalRadius = polyA.radius + polyB.radius; findMaxSeparation(results1, polyA, xfA, polyB, xfB); if (results1.separation > totalRadius) return; findMaxSeparation(results2, polyB, xfB, polyA, xfA); if (results2.separation > totalRadius) return; PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge int flip; num k_relativeTol = 0.98; num k_absoluteTol = 0.001; if (results2.separation > k_relativeTol * results1.separation + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = xfB; xf2 = xfA; edge1 = results2.edgeIndex; manifold.type = ManifoldType.FACE_B; flip = 1; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = results1.edgeIndex; manifold.type = ManifoldType.FACE_A; flip = 0; } findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); int count1 = poly1.vertexCount; List<vec2> vertices1 = poly1.vertices; v11.copyFrom(vertices1[edge1]); v12.copyFrom(edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]); localTangent.copyFrom(v12).sub(v11); localTangent.normalize(); // Vector localNormal = Cross(dv, 1.0); cross(localTangent, 1, localNormal); // Vector planePoint = 0.5 * (v11 + v12) planePoint.copyFrom(v11).add(v12).scale(.5); // Vector sideNormal = Mul(xf1.rotation, v12 - v11); xf1.rotation.transformed(localTangent, tangent); // Vector frontNormal = Cross(sideNormal, 1.0); cross(tangent, 1, normal); // v11 = Mul(xf1, v11); // v12 = Mul(xf1, v12); Transform.mulToOut(xf1, v11, v11); Transform.mulToOut(xf1, v12, v12); // Face offset num frontOffset = dot(normal, v11); // Side offsets, extended by polytope skin thickness. num sideOffset1 = -dot(tangent, v11) + totalRadius; num sideOffset2 = dot(tangent, v12) + 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.negate(); np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1); tangent.negate(); if (np < 2) return; // Clip to negative box side 1 np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2); if (np < 2) return; // Now clipPoints2 contains the clipped points. manifold.localNormal.copyFrom(localNormal); manifold.localPoint.copyFrom(planePoint); int pointCount = 0; for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; ++i) { num separation = dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.points[pointCount]; Transform.mulTransToOut(xf2, clipPoints2[i].v, cp.localPoint); // cp.localPoint = MulT(xf2, clipPoints2[i].v); cp.id.setFrom(clipPoints2[i].id); cp.id.features.flip = flip; ++pointCount; } } manifold.pointCount = pointCount; }
num edgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) #
Find the separation between poly1 and poly2 for a given edge normal on poly1.
num edgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) { final int count1 = poly1.vertexCount; final List<vec2> vertices1 = poly1.vertices; final List<vec2> normals1 = poly1.normals; final int count2 = poly2.vertexCount; final List<vec2> vertices2 = poly2.vertices; assert (0 <= edge1 && edge1 < count1); // Convert normal from poly1's frame into poly2's frame. final mat2 R = xf1.rotation; final vec2 v = normals1[edge1]; final num normal1Worldy = R.col0.y * v.x + R.col1.y * v.y; final num normal1Worldx = R.col0.x * v.x + R.col1.x * v.y; final mat2 R1 = xf2.rotation; final num normal1x = normal1Worldx * R1.col0.x + normal1Worldy * R1.col0.y; final num normal1y = normal1Worldx * R1.col1.x + normal1Worldy * R1.col1.y; // end inline // Find support vertex on poly2 for -normal. int index = 0; num minDot = Settings.BIG_NUMBER; for (int i = 0; i < count2; ++i) { final vec2 a = vertices2[i]; final num dot_ = a.x * normal1x + a.y * normal1y; if (dot_ < minDot) { minDot = dot_; index = i; } } final vec2 v3 = vertices1[edge1]; final num v1y = xf1.position.y + R.col0.y * v3.x + R.col1.y * v3.y; final num v1x = xf1.position.x + R.col0.x * v3.x + R.col1.x * v3.y; final vec2 v4 = vertices2[index]; final num v2y = xf2.position.y + R1.col0.y * v4.x + R1.col1.y * v4.y - v1y; final num v2x = xf2.position.x + R1.col0.x * v4.x + R1.col1.x * v4.y - v1x; return v2x * normal1Worldx + v2y * normal1Worldy; }
void findIncidentEdge(List<ClipVertex> c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) #
void findIncidentEdge(List<ClipVertex> c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) { int count1 = poly1.vertexCount; final List<vec2> normals1 = poly1.normals; int count2 = poly2.vertexCount; final List<vec2> vertices2 = poly2.vertices; final List<vec2> normals2 = poly2.normals; assert (0 <= edge1 && edge1 < count1); // Get the normal of the reference edge in poly2's frame. xf1.rotation.transformed(normals1[edge1], normal1); xf2.rotation.transposed().transform(normal1); // Find the incident edge on poly2. int index = 0; num minDot = Settings.BIG_NUMBER; for (int i = 0; i < count2; ++i) { num dot_ = 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); 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); c[1].id.features.referenceEdge = edge1; c[1].id.features.incidentEdge = i2; c[1].id.features.incidentVertex = 1; }
void findMaxSeparation(EdgeResults results, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2) #
Find the max separation between poly1 and poly2 using edge normals from poly1.
void findMaxSeparation(EdgeResults results, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2) { int count1 = poly1.vertexCount; final List<vec2> normals1 = poly1.normals; vec2 v = poly2.centroid; final num predy = xf2.position.y + xf2.rotation.col0.y * v.x + xf2.rotation.col1.y * v.y; final num predx = xf2.position.x + xf2.rotation.col0.x * v.x + xf2.rotation.col1.x * v.y; final vec2 v1 = poly1.centroid; final num tempy = xf1.position.y + xf1.rotation.col0.y * v1.x + xf1.rotation.col1.y * v1.y; final num tempx = xf1.position.x + xf1.rotation.col0.x * v1.x + xf1.rotation.col1.x * v1.y; final num dx = predx - tempx; final num dy = predy - tempy; final mat2 R = xf1.rotation; final num dLocal1x = dx * R.col0.x + dy * R.col0.y; final num dLocal1y = dx * R.col1.x + dy * R.col1.y; // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; num maxDot = Settings.SMALL_NUMBER; for (int i = 0; i < count1; i++) { final vec2 norm = normals1[i]; num dot_ = norm.x * dLocal1x + norm.y * dLocal1y; if (dot_ > maxDot) { maxDot = dot_; edge = i; } } // Get the separation for the edge normal. num s = edgeSeparation(poly1, xf1, edge, poly2, xf2); // Check the separation for the previous edge normal. int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; num sPrev = edgeSeparation(poly1, xf1, prevEdge, poly2, xf2); // Check the separation for the next edge normal. int nextEdge = edge + 1 < count1 ? edge + 1 : 0; num sNext = edgeSeparation(poly1, xf1, nextEdge, poly2, xf2); // Find the best edge and the search direction. int bestEdge; num 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. while (true) { 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; }
void getPointStates(List<int> state1, List<int> state2, Manifold manifold1, Manifold manifold2) #
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.
void getPointStates(List<int> state1, List<int> state2, Manifold manifold1, Manifold manifold2) { for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; ++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; } } } }
bool testOverlap(Shape shapeA, Shape shapeB, Transform transformA, Transform transformB) #
Returns true if the two given shapes overlap.
bool testOverlap(Shape shapeA, Shape shapeB, Transform transformA, Transform transformB) { input.proxyA.setFromShape(shapeA); input.proxyB.setFromShape(shapeB); input.transformA.setFrom(transformA); input.transformB.setFrom(transformB); input.useRadii = true; cache.count = 0; _pool.distance.computeDistance(output, cache, input); return output._distance < 10.0 * Settings.EPSILON; }