/* * JBox2D - A Java Port of Erin Catto's Box2D * * JBox2D homepage: http://jbox2d.sourceforge.net/ * Box2D homepage: http://www.box2d.org * * This software is provided 'as-is', without any express or implied * warranty. In no event will the authors be held liable for any damages * arising from the use of this software. * * Permission is granted to anyone to use this software for any purpose, * including commercial applications, and to alter it and redistribute it * freely, subject to the following restrictions: * * 1. The origin of this software must not be misrepresented; you must not * claim that you wrote the original software. If you use this software * in a product, an acknowledgment in the product documentation would be * appreciated but is not required. * 2. Altered source versions must be plainly marked as such, and must not be * misrepresented as being the original software. * 3. This notice may not be removed or altered from any source distribution. */ package org.jbox2d.collision; import org.jbox2d.common.*; //Updated to rev 55->108->139 of b2CollidePoly.cpp /** Polygon overlap solver - for internal use. */ public class CollidePoly { static class ClipVertex { Vec2 v; ContactID id; public ClipVertex() { v = new Vec2(); id = new ContactID(); } public ClipVertex(ContactID id) { v = new Vec2(); this.id = id; } } static int clipSegmentToLine(ClipVertex vOut[], ClipVertex vIn[], Vec2 normal, float offset) { // Start with no output points int numOut = 0; // Calculate the distance of end points to the line float distance0 = Vec2.dot(normal, vIn[0].v) - offset; float distance1 = Vec2.dot(normal, vIn[1].v) - offset; // Clipvertex constructors are optimised // If the points are behind the plane if (distance0 <= 0.0f) { vOut[numOut] = new ClipVertex(new ContactID(vIn[0].id)); //vOut[numOut] = new ClipVertex(); //vOut[numOut].id = new ContactID(vIn[0].id); vOut[numOut].v.x = vIn[0].v.x; vOut[numOut++].v.y = vIn[0].v.y; //vOut[numOut++].v = vIn[0].v.clone(); } if (distance1 <= 0.0f) { vOut[numOut] = new ClipVertex(new ContactID(vIn[1].id)); //vOut[numOut] = new ClipVertex(); //vOut[numOut].id = new ContactID(vIn[1].id); vOut[numOut].v.x = vIn[1].v.x; vOut[numOut++].v.y = vIn[1].v.y; //vOut[numOut++].v = vIn[1].v.clone(); } // If the points are on different sides of the plane if (distance0 * distance1 < 0.0f) { // Find intersection point of edge and plane float interp = distance0 / (distance0 - distance1); if (distance0 > 0.0f) { vOut[numOut] = new ClipVertex(new ContactID(vIn[0].id)); } else { vOut[numOut] = new ClipVertex(new ContactID(vIn[1].id)); } vOut[numOut].v.x = vIn[0].v.x + interp * (vIn[1].v.x - vIn[0].v.x); vOut[numOut].v.y = vIn[0].v.y + interp * (vIn[1].v.y - vIn[0].v.y); ++numOut; } return numOut; } static float edgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) { // direct field access instead of getters (midp) int count1 = poly1.m_vertexCount; Vec2[] vertices1 = poly1.m_vertices; Vec2[] normals1 = poly1.m_normals; int count2 = poly2.m_vertexCount; Vec2[] vertices2 = poly2.m_vertices; //int count1 = poly1.getVertexCount(); //Vec2[] vertices1 = poly1.getVertices(); //Vec2[] normals1 = poly1.getNormals(); //int count2 = poly2.getVertexCount(); //Vec2[] vertices2 = poly2.getVertices(); //assert(0 <= edge1 && edge1 < count1); // Convert normal from poly1's frame into poly2's frame. Vec2 normal1World = Mat22.mul(xf1.R, normals1[edge1]); float normal1x = Vec2.dot(normal1World, xf2.R.col1); float normal1y = Vec2.dot(normal1World, xf2.R.col2); // Find support vertex on poly2 for -normal. int index = 0; float minDot = Float.MAX_VALUE; for (int i = 0; i < count2; ++i) { float dot = vertices2[i].x * normal1x + vertices2[i].y * normal1y; //Vec2.dot(poly2.m_vertices[i], normal1); if (dot < minDot) { minDot = dot; index = i; } } //Vec2 v1 = XForm.mul(xf1, poly1.m_vertices[edge1]); Vec2 v = vertices1[edge1]; float v1x = xf1.position.x + xf1.R.col1.x * v.x + xf1.R.col2.x * v.y; float v1y = xf1.position.y + xf1.R.col1.y * v.x + xf1.R.col2.y * v.y; Vec2 v3 = vertices2[index]; //Vec2 v2 = XForm.mul(xf2, poly2.m_vertices[index]); float v2x = xf2.position.x + xf2.R.col1.x * v3.x + xf2.R.col2.x * v3.y; float v2y = xf2.position.y + xf2.R.col1.y * v3.x + xf2.R.col2.y * v3.y; //float separation = Vec2.dot(v2.sub(v1), normal1World); float separation = (v2x-v1x) * normal1World.x + (v2y-v1y) * normal1World.y; return separation; } // Find the max separation between poly1 and poly2 using face normals // from poly1. static MaxSeparation findMaxSeparation(PolygonShape poly1, XForm xf1, PolygonShape poly2, XForm xf2) { MaxSeparation separation = new MaxSeparation(); int count1 = poly1.getVertexCount(); Vec2[] normals1 = poly1.getNormals(); Vec2 v = poly1.getCentroid(); Vec2 v1 = poly2.getCentroid(); // Vector pointing from the centroid of poly1 to the centroid of poly2. //Vec2 d = XForm.mul(xf2, poly2.m_centroid).subLocal(XForm.mul(xf1, poly1.m_centroid)); //Vec2 dLocal1 = Mat22.mulT(xf1.R, d); float dx = xf2.position.x + xf2.R.col1.x * v1.x + xf2.R.col2.x * v1.y - (xf1.position.x + xf1.R.col1.x * v.x + xf1.R.col2.x * v.y); float dy = xf2.position.y + xf2.R.col1.y * v1.x + xf2.R.col2.y * v1.y - (xf1.position.y + xf1.R.col1.y * v.x + xf1.R.col2.y * v.y); Vec2 b = xf1.R.col1; Vec2 b1 = xf1.R.col2; Vec2 dLocal1 = new Vec2((dx * b.x + dy * b.y), (dx * b1.x + dy * b1.y)); // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; float maxDot = -Float.MAX_VALUE; for (int i = 0; i < count1; ++i) { float 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); if (s > 0.0f){ separation.bestSeparation = s; return separation; } // 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); if (sPrev > 0.0f) { separation.bestSeparation = sPrev; return separation; } int nextEdge = edge + 1 < count1 ? edge + 1 : 0; float sNext = edgeSeparation(poly1, xf1, nextEdge, poly2, xf2); if (sNext > 0.0f){ separation.bestSeparation = sNext; return separation; } // 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 { separation.bestFaceIndex = edge; separation.bestSeparation = s; return separation; } // 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 > 0.0f) { separation.bestSeparation = s; return separation; } if (s > bestSeparation){ bestEdge = edge; bestSeparation = s; } else { break; } } separation.bestFaceIndex = bestEdge; separation.bestSeparation = bestSeparation; return separation; } static void findIncidentEdge(ClipVertex c[], PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) { int count1 = poly1.getVertexCount(); Vec2[] normals1 = poly1.getNormals(); int count2 = poly2.getVertexCount(); Vec2[] vertices2 = poly2.getVertices(); Vec2[] normals2 = poly2.getNormals(); //assert(0 <= edge1 && edge1 < count1); // Get the normal of the reference edge in poly2's frame. Vec2 normal1 = Mat22.mulT(xf2.R, Mat22.mul(xf1.R, normals1[edge1])); // 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; c[0] = new ClipVertex(); c[1] = new ClipVertex(); c[0].v = XForm.mul(xf2, vertices2[i1]); c[0].id.features.referenceEdge = edge1; c[0].id.features.incidentEdge = i1; c[0].id.features.incidentVertex = 0; c[1].v = XForm.mul(xf2, vertices2[i2]); c[1].id.features.referenceEdge = edge1; c[1].id.features.incidentEdge = i2; c[1].id.features.incidentVertex = 1; } // Find edge normal of max separation on A - return if separating axis is // found // Find edge normal of max separation on B - return if separation axis is // found // Choose reference edge as min(minA, minB) // Find incident edge // Clip // The normal points from 1 to 2 public static void collidePolygons(Manifold manif, PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB) { //testbed.PTest.debugCount++; manif.pointCount = 0; // Fixed a problem with contacts MaxSeparation sepA = findMaxSeparation(polyA, xfA, polyB, xfB); if (sepA.bestSeparation > 0.0f) { return; } MaxSeparation sepB = findMaxSeparation(polyB, xfB, polyA, xfA); if (sepB.bestSeparation > 0.0f) { return; } PolygonShape poly1; // reference poly PolygonShape poly2; // incident poly XForm xf1 = new XForm(); XForm xf2 = new XForm(); int edge1; // reference edge byte flip; float k_relativeTol = 0.98f; float k_absoluteTol = 0.001f; // TODO_ERIN use "radius" of poly for absolute tolerance. if (sepB.bestSeparation > k_relativeTol * sepA.bestSeparation + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1.set(xfB); xf2.set(xfA); edge1 = sepB.bestFaceIndex; flip = 1; } else { poly1 = polyA; poly2 = polyB; xf1.set(xfA); xf2.set(xfB); edge1 = sepA.bestFaceIndex; flip = 0; } ClipVertex incidentEdge[] = new ClipVertex[2]; findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); int count1 = poly1.getVertexCount(); Vec2[] vertices1 = poly1.getVertices(); Vec2 v11 = vertices1[edge1]; Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; Vec2 sideNormal = Mat22.mul(xf1.R, v12.sub(v11)); sideNormal.normalize(); Vec2 frontNormal = Vec2.cross(sideNormal, 1.0f); //v11 = XForm.mul(xf1, v11); //v12 = XForm.mul(xf1, v12); float v11x = xf1.position.x + xf1.R.col1.x * v11.x + xf1.R.col2.x * v11.y; float v11y = xf1.position.y + xf1.R.col1.y * v11.x + xf1.R.col2.y * v11.y; float v12x = xf1.position.x + xf1.R.col1.x * v12.x + xf1.R.col2.x * v12.y; float v12y = xf1.position.y + xf1.R.col1.y * v12.x + xf1.R.col2.y * v12.y; float frontOffset = frontNormal.x * v11x + frontNormal.y * v11y; float sideOffset1 = -(sideNormal.x * v11x + sideNormal.y * v11y); float sideOffset2 = sideNormal.x * v12x + sideNormal.y * v12y; // Clip incident edge against extruded edge1 side edges. ClipVertex clipPoints1[] = new ClipVertex[2]; ClipVertex clipPoints2[] = new ClipVertex[2]; int np; // Clip to box side 1 np = clipSegmentToLine(clipPoints1, incidentEdge, sideNormal.negate(), sideOffset1); if (np < 2) { return; } // Clip to negative box side 1 np = clipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manif.normal = (flip != 0) ? frontNormal.negate() : frontNormal.clone(); int pointCount = 0; for (int i = 0; i < Settings.maxManifoldPoints; ++i) { float separation = Vec2.dot(frontNormal, clipPoints2[i].v) - frontOffset; if (separation <= 0.0f) { ManifoldPoint cp = manif.points[pointCount]; cp.separation = separation; //cp.localPoint1 = XForm.mulT(xfA, clipPoints2[i].v); //cp.localPoint2 = XForm.mulT(xfB, clipPoints2[i].v); Vec2 vec = clipPoints2[i].v; float v1x = vec.x-xfA.position.x; float v1y = vec.y-xfA.position.y; cp.localPoint1.x = (v1x * xfA.R.col1.x + v1y * xfA.R.col1.y); cp.localPoint1.y = (v1x * xfA.R.col2.x + v1y * xfA.R.col2.y); v1x = vec.x-xfB.position.x; v1y = vec.y-xfB.position.y; cp.localPoint2.x = (v1x * xfB.R.col1.x + v1y * xfB.R.col1.y); cp.localPoint2.y = (v1x * xfB.R.col2.x + v1y * xfB.R.col2.y); cp.id = new ContactID(clipPoints2[i].id); cp.id.features.flip = flip; ++pointCount; } } manif.pointCount = pointCount; return; } } /** Holder class used internally in CollidePoly. */ class MaxSeparation { public int bestFaceIndex; public float bestSeparation; }