// Find the max separation between poly1 and poly2 using edge normals from poly1. static btScalar FindMaxSeparation(int* edgeIndex, const btBox2dShape* poly1, const btTransform& xf1, const btBox2dShape* poly2, const btTransform& xf2) { int count1 = poly1->getVertexCount(); const btVector3* normals1 = poly1->getNormals(); // Vector pointing from the centroid of poly1 to the centroid of poly2. btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid()); btVector3 dLocal1 = b2MulT(xf1.getBasis(), d); // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; btScalar maxDot; if( count1 > 0 ) edge = (int) dLocal1.maxDot( normals1, count1, maxDot); // Get the separation for the edge normal. btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); if (s > 0.0f) { return s; } // Check the separation for the previous edge normal. int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); if (sPrev > 0.0f) { return sPrev; } // Check the separation for the next edge normal. int nextEdge = edge + 1 < count1 ? edge + 1 : 0; btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); if (sNext > 0.0f) { return sNext; } // Find the best edge and the search direction. int bestEdge; btScalar 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 { *edgeIndex = edge; return s; } // 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 > 0.0f) { return s; } if (s > bestSeparation) { bestEdge = edge; bestSeparation = s; } else { break; } } *edgeIndex = bestEdge; return bestSeparation; }
// Find the max separation between poly1 and poly2 using edge normals from poly1. static float32 FindMaxSeparation(int32* edgeIndex, const b2PolygonShape* poly1, const b2XForm& xf1, const b2PolygonShape* poly2, const b2XForm& xf2) { int32 count1 = poly1->GetVertexCount(); const b2Vec2* normals1 = poly1->GetNormals(); // Vector pointing from the centroid of poly1 to the centroid of poly2. b2Vec2 d = b2Mul(xf2, poly2->GetCentroid()) - b2Mul(xf1, poly1->GetCentroid()); b2Vec2 dLocal1 = b2MulT(xf1.R, d); // Find edge normal on poly1 that has the largest projection onto d. int32 edge = 0; float32 maxDot = -B2_FLT_MAX; for (int32 i = 0; i < count1; ++i) { float32 dot = b2Dot(normals1[i], dLocal1); if (dot > maxDot) { maxDot = dot; edge = i; } } // Get the separation for the edge normal. float32 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); if (s > 0.0f) { return s; } // Check the separation for the previous edge normal. int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; float32 sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); if (sPrev > 0.0f) { return sPrev; } // Check the separation for the next edge normal. int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0; float32 sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); if (sNext > 0.0f) { return sNext; } // Find the best edge and the search direction. int32 bestEdge; float32 bestSeparation; int32 increment; if (sPrev > s && sPrev > sNext) { increment = -1; bestEdge = prevEdge; bestSeparation = sPrev; } else if (sNext > s) { increment = 1; bestEdge = nextEdge; bestSeparation = sNext; } else { *edgeIndex = edge; return s; } // 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 > 0.0f) { return s; } if (s > bestSeparation) { bestEdge = edge; bestSeparation = s; } else { break; } } *edgeIndex = bestEdge; return bestSeparation; }