# Villane/scalabox2d

Switch branches/tags
Nothing to show
Fetching contributors…
Cannot retrieve contributors at this time
310 lines (259 sloc) 9.77 KB
 package org.villane.box2d.collision import vecmath._ import vecmath.Preamble._ import shapes._ import collision._ object PolygonCollider extends Collider[Polygon, Polygon] { private case class MaxSeparation(bestFaceIndex: Int, bestSeparation: Float) private case class ClipVertex(v: Vector2, id: ContactID) // 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 def collide(polyA: Polygon, xfA: Transform2, polyB: Polygon, xfB: Transform2): Option[Manifold] = { //testbed.PTest.debugCount++; val sepA = findMaxSeparation(polyA, xfA, polyB, xfB) if (sepA.bestSeparation > 0.0f) return None val sepB = findMaxSeparation(polyB, xfB, polyA, xfA) if (sepB.bestSeparation > 0.0f) return None val k_relativeTol = 0.98f val k_absoluteTol = 0.001f val (poly1, poly2, xf1, xf2, edge1, flip) = // TODO_ERIN use "radius" of poly for absolute tolerance. if (sepB.bestSeparation > k_relativeTol * sepA.bestSeparation + k_absoluteTol) (polyB, polyA, xfB, xfA, sepB.bestFaceIndex, true) else (polyA, polyB, xfA, xfB, sepA.bestFaceIndex, false) val incidentEdge = findIncidentEdge(poly1, xf1, edge1, poly2, xf2) val count1 = poly1.vertexCount val vertices1 = poly1.vertices val v11 = vertices1(edge1) val v12 = if (edge1 + 1 < count1) vertices1(edge1 + 1) else vertices1(0) //val sideNormal = (xf1.rot * (v12 - v11)).normalize //val frontNormal = sideNormal.normal val dvx = v12.x - v11.x val dvy = v12.y - v11.y val tdvx = xf1.rot.a11 * dvx + xf1.rot.a12 * dvy val tdvy = xf1.rot.a21 * dvx + xf1.rot.a22 * dvy val dvl = sqrt(tdvx * tdvx + tdvy * tdvy) val snx = tdvx / dvl val sny = tdvy / dvl val fnx = sny val fny = -snx //v11 = XForm.mul(xf1, v11); //v12 = XForm.mul(xf1, v12); //val Vector2(v11x, v11y) = xf1 * v11 //val Vector2(v12x, v12y) = xf1 * v12 val v11x = xf1.pos.x + xf1.rot.a11 * v11.x + xf1.rot.a12 * v11.y val v11y = xf1.pos.y + xf1.rot.a21 * v11.x + xf1.rot.a22 * v11.y val v12x = xf1.pos.x + xf1.rot.a11 * v12.x + xf1.rot.a12 * v12.y val v12y = xf1.pos.y + xf1.rot.a21 * v12.x + xf1.rot.a22 * v12.y val frontOffset = fnx * v11x + fny * v11y val sideOffset1 = -(snx * v11x + sny * v11y) val sideOffset2 = snx * v12x + sny * v12y // Clip incident edge against extruded edge1 side edges. // Clip to box side 1 val clipPoints1 = clipSegmentToLine(incidentEdge, -snx, -sny, sideOffset1) if (clipPoints1.length < 2) return None // Clip to negative box side 1 val clipPoints2 = clipSegmentToLine(clipPoints1, snx, sny, sideOffset2) val cp2len = clipPoints2.length if (cp2len < 2) return None // Now clipPoints2 contains the clipped points. val points = new collection.mutable.ListBuffer[ManifoldPoint] for (cp2 <- clipPoints2) { val separation = (fnx * cp2.v.x + fny * cp2.v.y) - frontOffset if (separation <= 0.0f) { // should not inline this xf ** v creates just one vector anyway points += ManifoldPoint( xfA ** cp2.v, xfB ** cp2.v, separation, if (flip == cp2.id.flip) cp2.id else cp2.id withFlip flip ) } } if (points.length == 0) None else Some(Manifold(points.toList, if (flip) -Vector2(fnx,fny) else Vector2(fnx,fny))) } private def clipSegmentToLine(vIn: List[ClipVertex], normalx: Float, normaly: Float, offset: Float): List[ClipVertex] = (vIn: @unchecked) match { case vIn0 :: vIn1 :: _ => // Calculate the distance of end points to the line val distance0 = (normalx * vIn0.v.x + normaly * vIn0.v.y) - offset val distance1 = (normalx * vIn1.v.x + normaly * vIn1.v.y) - offset val vOut = new collection.mutable.ListBuffer[ClipVertex] // If the points are behind the plane if (distance0 <= 0.0f) { vOut += vIn0 } if (distance1 <= 0.0f) { vOut += vIn1 } // If the points are on different sides of the plane if (distance0 * distance1 < 0.0f) { // Find intersection point of edge and plane val interp = distance0 / (distance0 - distance1) val id = if (distance0 > 0.0f) vIn0.id else vIn1.id vOut += ClipVertex(vIn0.v + interp * (vIn1.v - vIn0.v), id) } vOut.toList } def edgeSeparation(poly1: Polygon, xf1: Transform2, edge1: Int, poly2: Polygon, xf2: Transform2): Float = { val count1 = poly1.vertexCount val vertices1 = poly1.vertices val normals1 = poly1.normals val count2 = poly2.vertexCount val vertices2 = poly2.vertices assert(0 <= edge1 && edge1 < count1) // Convert normal from poly1's frame into poly2's frame. //val normal1World = xf1.rot * normals1(edge1) //val normal1 = xf2.rot ** normal1World val n1e = normals1(edge1) val n1wx = xf1.rot.a11 * n1e.x + xf1.rot.a12 * n1e.y val n1wy = xf1.rot.a21 * n1e.x + xf1.rot.a22 * n1e.y val n1x = xf2.rot.a11 * n1wx + xf2.rot.a21 * n1wy val n1y = xf2.rot.a12 * n1wx + xf2.rot.a22 * n1wy // Find support vertex on poly2 for -normal. var index = 0 var minDot = Float.MaxValue var i = 0 while (i < count2) { //val dot = vertices2(i) ∙ normal1 val dot = vertices2(i).x * n1x + vertices2(i).y * n1y if (dot < minDot) { minDot = dot index = i } i += 1 } //val v1 = xf1 * vertices1(edge1) //val v2 = xf2 * vertices2(index) val v1e = vertices1(edge1) val v2e = vertices2(index) val v1x = xf1.pos.x + xf1.rot.a11 * v1e.x + xf1.rot.a12 * v1e.y val v1y = xf1.pos.y + xf1.rot.a21 * v1e.x + xf1.rot.a22 * v1e.y val v2x = xf2.pos.x + xf2.rot.a11 * v2e.x + xf2.rot.a12 * v2e.y val v2y = xf2.pos.y + xf2.rot.a21 * v2e.x + xf2.rot.a22 * v2e.y val dvx = v2x - v1x val dvy = v2y - v1y //(v2 - v1) ∙ normal1World dvx * n1wx + dvy * n1wy } // Find the max separation between poly1 and poly2 using face normals // from poly1. private def findMaxSeparation(poly1: Polygon, xf1: Transform2, poly2: Polygon, xf2: Transform2): MaxSeparation = { val count1 = poly1.vertexCount val normals1 = poly1.normals val v = poly1.centroid val v1 = poly2.centroid // Vector pointing from the centroid of poly1 to the centroid of poly2. //val d = (xf2 * v1) - (xf1 * v) //val dLocal1 = xf1.rot ** d val dx = (xf2.pos.x + xf2.rot.a11 * v1.x + xf2.rot.a12 * v1.y) - (xf1.pos.x + xf1.rot.a11 * v.x + xf1.rot.a12 * v.y) val dy = (xf2.pos.y + xf2.rot.a21 * v1.x + xf2.rot.a22 * v1.y) - (xf1.pos.y + xf1.rot.a21 * v.x + xf1.rot.a22 * v.y) val dlx = xf1.rot.a11 * dx + xf1.rot.a21 * dy val dly = xf1.rot.a12 * dx + xf1.rot.a22 * dy // Find edge normal on poly1 that has the largest projection onto d. var edge = 0 var maxDot = -Float.MaxValue var i = 0 while (i < count1) { //val dot = normals1(i) ∙ dLocal1 val dot = normals1(i).x * dlx + normals1(i).y * dly if (dot > maxDot) { maxDot = dot edge = i } i += 1 } // Get the separation for the edge normal. var s = edgeSeparation(poly1, xf1, edge, poly2, xf2) if (s > 0.0f){ return MaxSeparation(0, s) } // Check the separation for the previous edge normal. val prevEdge = if (edge - 1 >= 0) edge - 1 else count1 - 1 val sPrev = edgeSeparation(poly1, xf1, prevEdge, poly2, xf2) if (sPrev > 0.0f) { return MaxSeparation(0, sPrev) } val nextEdge = if (edge + 1 < count1) edge + 1 else 0 val sNext = edgeSeparation(poly1, xf1, nextEdge, poly2, xf2) if (sNext > 0.0f){ return MaxSeparation(0, sNext) } // Find the best edge and the search direction. var (bestEdge, bestSeparation, increment) = if (sPrev > s && sPrev > sNext) { (prevEdge, sPrev, -1) } else if (sNext > s){ (nextEdge, sNext, 1) } else { return MaxSeparation(edge, s) } // Perform a local search for the best edge normal. var loop = true while (loop) { if (increment == -1) edge = if (bestEdge - 1 >= 0) bestEdge - 1 else count1 - 1 else edge = if (bestEdge + 1 < count1) bestEdge + 1 else 0 s = edgeSeparation(poly1, xf1, edge, poly2, xf2) if (s > 0.0f) { return MaxSeparation(0, s) } if (s > bestSeparation) { bestEdge = edge bestSeparation = s } else { loop = false } } MaxSeparation(bestEdge, bestSeparation) } private def findIncidentEdge(poly1: Polygon, xf1: Transform2, edge1: Int, poly2: Polygon, xf2: Transform2) = { val count1 = poly1.vertexCount val normals1 = poly1.normals val count2 = poly2.vertexCount val vertices2 = poly2.vertices val normals2 = poly2.normals assert(0 <= edge1 && edge1 < count1) // Get the normal of the reference edge in poly2's frame. val normal1 = xf2.rot ** (xf1.rot * normals1(edge1)) // Find the incident edge on poly2. var index = 0 var minDot = Float.MaxValue var i = 0 while (i < count2) { val dot = normal1 ∙ normals2(i) if (dot < minDot) { minDot = dot index = i } i += 1 } // Build the clip vertices for the incident edge. val i1 = index val i2 = if (i1 + 1 < count2) i1 + 1 else 0 List( ClipVertex(xf2 * vertices2(i1), ContactID(edge1, i1, 0, false)), ClipVertex(xf2 * vertices2(i2), ContactID(edge1, i2, 1, false)) ) } }