1 /******************************************************************************* 2 * Copyright (c) 2013, Daniel Murphy 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without modification, 6 * are permitted provided that the following conditions are met: 7 * * Redistributions of source code must retain the above copyright notice, 8 * this list of conditions and the following disclaimer. 9 * * Redistributions in binary form must reproduce the above copyright notice, 10 * this list of conditions and the following disclaimer in the documentation 11 * and/or other materials provided with the distribution. 12 * 13 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 16 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 17 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 18 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 19 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 20 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 21 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 22 * POSSIBILITY OF SUCH DAMAGE. 23 ******************************************************************************/ 24 package org.jbox2d.collision; 25 26 import org.jbox2d.collision.Distance.SimplexCache; 27 import org.jbox2d.collision.Manifold.ManifoldType; 28 import org.jbox2d.collision.shapes.CircleShape; 29 import org.jbox2d.collision.shapes.EdgeShape; 30 import org.jbox2d.collision.shapes.PolygonShape; 31 import org.jbox2d.collision.shapes.Shape; 32 import org.jbox2d.common.MathUtils; 33 import org.jbox2d.common.Rot; 34 import org.jbox2d.common.Settings; 35 import org.jbox2d.common.Transform; 36 import org.jbox2d.common.Vec2; 37 import org.jbox2d.pooling.IWorldPool; 38 39 /** 40 * Functions used for computing contact points, distance queries, and TOI queries. Collision methods 41 * are non-static for pooling speed, retrieve a collision object from the {@link SingletonPool}. 42 * Should not be finalructed. 43 * 44 * @author Daniel Murphy 45 */ 46 public class Collision { 47 public static final int NULL_FEATURE = Integer.MAX_VALUE; 48 49 private final IWorldPool pool; 50 51 public Collision(IWorldPool argPool) { 52 incidentEdge[0] = new ClipVertex(); 53 incidentEdge[1] = new ClipVertex(); 54 clipPoints1[0] = new ClipVertex(); 55 clipPoints1[1] = new ClipVertex(); 56 clipPoints2[0] = new ClipVertex(); 57 clipPoints2[1] = new ClipVertex(); 58 pool = argPool; 59 } 60 61 private final DistanceInput input = new DistanceInput(); 62 private final SimplexCache cache = new SimplexCache(); 63 private final DistanceOutput output = new DistanceOutput(); 64 65 /** 66 * Determine if two generic shapes overlap. 67 * 68 * @param shapeA 69 * @param shapeB 70 * @param xfA 71 * @param xfB 72 * @return 73 */ 74 public final boolean testOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB, 75 Transform xfA, Transform xfB) { 76 input.proxyA.set(shapeA, indexA); 77 input.proxyB.set(shapeB, indexB); 78 input.transformA.set(xfA); 79 input.transformB.set(xfB); 80 input.useRadii = true; 81 82 cache.count = 0; 83 84 pool.getDistance().distance(output, cache, input); 85 // djm note: anything significant about 10.0f? 86 return output.distance < 10.0f * Settings.EPSILON; 87 } 88 89 /** 90 * Compute the point states given two manifolds. The states pertain to the transition from 91 * manifold1 to manifold2. So state1 is either persist or remove while state2 is either add or 92 * persist. 93 * 94 * @param state1 95 * @param state2 96 * @param manifold1 97 * @param manifold2 98 */ 99 public static final void getPointStates(final PointState[] state1, final PointState[] state2, 100 final Manifold manifold1, final Manifold manifold2) { 101 102 for (int i = 0; i < Settings.maxManifoldPoints; i++) { 103 state1[i] = PointState.NULL_STATE; 104 state2[i] = PointState.NULL_STATE; 105 } 106 107 // Detect persists and removes. 108 for (int i = 0; i < manifold1.pointCount; i++) { 109 ContactID id = manifold1.points[i].id; 110 111 state1[i] = PointState.REMOVE_STATE; 112 113 for (int j = 0; j < manifold2.pointCount; j++) { 114 if (manifold2.points[j].id.isEqual(id)) { 115 state1[i] = PointState.PERSIST_STATE; 116 break; 117 } 118 } 119 } 120 121 // Detect persists and adds 122 for (int i = 0; i < manifold2.pointCount; i++) { 123 ContactID id = manifold2.points[i].id; 124 125 state2[i] = PointState.ADD_STATE; 126 127 for (int j = 0; j < manifold1.pointCount; j++) { 128 if (manifold1.points[j].id.isEqual(id)) { 129 state2[i] = PointState.PERSIST_STATE; 130 break; 131 } 132 } 133 } 134 } 135 136 /** 137 * Clipping for contact manifolds. Sutherland-Hodgman clipping. 138 * 139 * @param vOut 140 * @param vIn 141 * @param normal 142 * @param offset 143 * @return 144 */ 145 public static final int clipSegmentToLine(final ClipVertex[] vOut, final ClipVertex[] vIn, 146 final Vec2 normal, float offset, int vertexIndexA) { 147 148 // Start with no output points 149 int numOut = 0; 150 final ClipVertex vIn0 = vIn[0]; 151 final ClipVertex vIn1 = vIn[1]; 152 final Vec2 vIn0v = vIn0.v; 153 final Vec2 vIn1v = vIn1.v; 154 155 // Calculate the distance of end points to the line 156 float distance0 = Vec2.dot(normal, vIn0v) - offset; 157 float distance1 = Vec2.dot(normal, vIn1v) - offset; 158 159 // If the points are behind the plane 160 if (distance0 <= 0.0f) { 161 vOut[numOut++].set(vIn0); 162 } 163 if (distance1 <= 0.0f) { 164 vOut[numOut++].set(vIn1); 165 } 166 167 // If the points are on different sides of the plane 168 if (distance0 * distance1 < 0.0f) { 169 // Find intersection point of edge and plane 170 float interp = distance0 / (distance0 - distance1); 171 172 ClipVertex vOutNO = vOut[numOut]; 173 // vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); 174 vOutNO.v.x = vIn0v.x + interp * (vIn1v.x - vIn0v.x); 175 vOutNO.v.y = vIn0v.y + interp * (vIn1v.y - vIn0v.y); 176 177 // VertexA is hitting edgeB. 178 vOutNO.id.indexA = (byte) vertexIndexA; 179 vOutNO.id.indexB = vIn0.id.indexB; 180 vOutNO.id.typeA = (byte) ContactID.Type.VERTEX.ordinal(); 181 vOutNO.id.typeB = (byte) ContactID.Type.FACE.ordinal(); 182 ++numOut; 183 } 184 185 return numOut; 186 } 187 188 // #### COLLISION STUFF (not from collision.h or collision.cpp) #### 189 190 // djm pooling 191 private static Vec2 d = new Vec2(); 192 193 /** 194 * Compute the collision manifold between two circles. 195 * 196 * @param manifold 197 * @param circle1 198 * @param xfA 199 * @param circle2 200 * @param xfB 201 */ 202 public final void collideCircles(Manifold manifold, final CircleShape circle1, 203 final Transform xfA, final CircleShape circle2, final Transform xfB) { 204 manifold.pointCount = 0; 205 // before inline: 206 // Transform.mulToOut(xfA, circle1.m_p, pA); 207 // Transform.mulToOut(xfB, circle2.m_p, pB); 208 // d.set(pB).subLocal(pA); 209 // float distSqr = d.x * d.x + d.y * d.y; 210 211 // after inline: 212 Vec2 circle1p = circle1.m_p; 213 Vec2 circle2p = circle2.m_p; 214 float pAx = (xfA.q.c * circle1p.x - xfA.q.s * circle1p.y) + xfA.p.x; 215 float pAy = (xfA.q.s * circle1p.x + xfA.q.c * circle1p.y) + xfA.p.y; 216 float pBx = (xfB.q.c * circle2p.x - xfB.q.s * circle2p.y) + xfB.p.x; 217 float pBy = (xfB.q.s * circle2p.x + xfB.q.c * circle2p.y) + xfB.p.y; 218 float dx = pBx - pAx; 219 float dy = pBy - pAy; 220 float distSqr = dx * dx + dy * dy; 221 // end inline 222 223 final float radius = circle1.m_radius + circle2.m_radius; 224 if (distSqr > radius * radius) { 225 return; 226 } 227 228 manifold.type = ManifoldType.CIRCLES; 229 manifold.localPoint.set(circle1p); 230 manifold.localNormal.setZero(); 231 manifold.pointCount = 1; 232 233 manifold.points[0].localPoint.set(circle2p); 234 manifold.points[0].id.zero(); 235 } 236 237 // djm pooling, and from above 238 239 /** 240 * Compute the collision manifold between a polygon and a circle. 241 * 242 * @param manifold 243 * @param polygon 244 * @param xfA 245 * @param circle 246 * @param xfB 247 */ 248 public final void collidePolygonAndCircle(Manifold manifold, final PolygonShape polygon, 249 final Transform xfA, final CircleShape circle, final Transform xfB) { 250 manifold.pointCount = 0; 251 // Vec2 v = circle.m_p; 252 253 // Compute circle position in the frame of the polygon. 254 // before inline: 255 // Transform.mulToOutUnsafe(xfB, circle.m_p, c); 256 // Transform.mulTransToOut(xfA, c, cLocal); 257 // final float cLocalx = cLocal.x; 258 // final float cLocaly = cLocal.y; 259 // after inline: 260 final Vec2 circlep = circle.m_p; 261 final Rot xfBq = xfB.q; 262 final Rot xfAq = xfA.q; 263 final float cx = (xfBq.c * circlep.x - xfBq.s * circlep.y) + xfB.p.x; 264 final float cy = (xfBq.s * circlep.x + xfBq.c * circlep.y) + xfB.p.y; 265 final float px = cx - xfA.p.x; 266 final float py = cy - xfA.p.y; 267 final float cLocalx = (xfAq.c * px + xfAq.s * py); 268 final float cLocaly = (-xfAq.s * px + xfAq.c * py); 269 // end inline 270 271 // Find the min separating edge. 272 int normalIndex = 0; 273 float separation = -Float.MAX_VALUE; 274 final float radius = polygon.m_radius + circle.m_radius; 275 final int vertexCount = polygon.m_count; 276 float s; 277 final Vec2[] vertices = polygon.m_vertices; 278 final Vec2[] normals = polygon.m_normals; 279 280 for (int i = 0; i < vertexCount; i++) { 281 // before inline 282 // temp.set(cLocal).subLocal(vertices[i]); 283 // float s = Vec2.dot(normals[i], temp); 284 // after inline 285 final Vec2 vertex = vertices[i]; 286 final float tempx = cLocalx - vertex.x; 287 final float tempy = cLocaly - vertex.y; 288 s = normals[i].x * tempx + normals[i].y * tempy; 289 290 291 if (s > radius) { 292 // early out 293 return; 294 } 295 296 if (s > separation) { 297 separation = s; 298 normalIndex = i; 299 } 300 } 301 302 // Vertices that subtend the incident face. 303 final int vertIndex1 = normalIndex; 304 final int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; 305 final Vec2 v1 = vertices[vertIndex1]; 306 final Vec2 v2 = vertices[vertIndex2]; 307 308 // If the center is inside the polygon ... 309 if (separation < Settings.EPSILON) { 310 manifold.pointCount = 1; 311 manifold.type = ManifoldType.FACE_A; 312 313 // before inline: 314 // manifold.localNormal.set(normals[normalIndex]); 315 // manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f); 316 // manifold.points[0].localPoint.set(circle.m_p); 317 // after inline: 318 final Vec2 normal = normals[normalIndex]; 319 manifold.localNormal.x = normal.x; 320 manifold.localNormal.y = normal.y; 321 manifold.localPoint.x = (v1.x + v2.x) * .5f; 322 manifold.localPoint.y = (v1.y + v2.y) * .5f; 323 final ManifoldPoint mpoint = manifold.points[0]; 324 mpoint.localPoint.x = circlep.x; 325 mpoint.localPoint.y = circlep.y; 326 mpoint.id.zero(); 327 // end inline 328 329 return; 330 } 331 332 // Compute barycentric coordinates 333 // before inline: 334 // temp.set(cLocal).subLocal(v1); 335 // temp2.set(v2).subLocal(v1); 336 // float u1 = Vec2.dot(temp, temp2); 337 // temp.set(cLocal).subLocal(v2); 338 // temp2.set(v1).subLocal(v2); 339 // float u2 = Vec2.dot(temp, temp2); 340 // after inline: 341 final float tempX = cLocalx - v1.x; 342 final float tempY = cLocaly - v1.y; 343 final float temp2X = v2.x - v1.x; 344 final float temp2Y = v2.y - v1.y; 345 final float u1 = tempX * temp2X + tempY * temp2Y; 346 347 final float temp3X = cLocalx - v2.x; 348 final float temp3Y = cLocaly - v2.y; 349 final float temp4X = v1.x - v2.x; 350 final float temp4Y = v1.y - v2.y; 351 final float u2 = temp3X * temp4X + temp3Y * temp4Y; 352 // end inline 353 354 if (u1 <= 0f) { 355 // inlined 356 final float dx = cLocalx - v1.x; 357 final float dy = cLocaly - v1.y; 358 if (dx * dx + dy * dy > radius * radius) { 359 return; 360 } 361 362 manifold.pointCount = 1; 363 manifold.type = ManifoldType.FACE_A; 364 // before inline: 365 // manifold.localNormal.set(cLocal).subLocal(v1); 366 // after inline: 367 manifold.localNormal.x = cLocalx - v1.x; 368 manifold.localNormal.y = cLocaly - v1.y; 369 // end inline 370 manifold.localNormal.normalize(); 371 manifold.localPoint.set(v1); 372 manifold.points[0].localPoint.set(circlep); 373 manifold.points[0].id.zero(); 374 } else if (u2 <= 0.0f) { 375 // inlined 376 final float dx = cLocalx - v2.x; 377 final float dy = cLocaly - v2.y; 378 if (dx * dx + dy * dy > radius * radius) { 379 return; 380 } 381 382 manifold.pointCount = 1; 383 manifold.type = ManifoldType.FACE_A; 384 // before inline: 385 // manifold.localNormal.set(cLocal).subLocal(v2); 386 // after inline: 387 manifold.localNormal.x = cLocalx - v2.x; 388 manifold.localNormal.y = cLocaly - v2.y; 389 // end inline 390 manifold.localNormal.normalize(); 391 manifold.localPoint.set(v2); 392 manifold.points[0].localPoint.set(circlep); 393 manifold.points[0].id.zero(); 394 } else { 395 // Vec2 faceCenter = 0.5f * (v1 + v2); 396 // (temp is faceCenter) 397 // before inline: 398 // temp.set(v1).addLocal(v2).mulLocal(.5f); 399 // 400 // temp2.set(cLocal).subLocal(temp); 401 // separation = Vec2.dot(temp2, normals[vertIndex1]); 402 // if (separation > radius) { 403 // return; 404 // } 405 // after inline: 406 final float fcx = (v1.x + v2.x) * .5f; 407 final float fcy = (v1.y + v2.y) * .5f; 408 409 final float tx = cLocalx - fcx; 410 final float ty = cLocaly - fcy; 411 final Vec2 normal = normals[vertIndex1]; 412 separation = tx * normal.x + ty * normal.y; 413 if (separation > radius) { 414 return; 415 } 416 // end inline 417 418 manifold.pointCount = 1; 419 manifold.type = ManifoldType.FACE_A; 420 manifold.localNormal.set(normals[vertIndex1]); 421 manifold.localPoint.x = fcx; // (faceCenter) 422 manifold.localPoint.y = fcy; 423 manifold.points[0].localPoint.set(circlep); 424 manifold.points[0].id.zero(); 425 } 426 } 427 428 // djm pooling, and from above 429 private final Vec2 temp = new Vec2(); 430 private final Transform xf = new Transform(); 431 private final Vec2 n = new Vec2(); 432 private final Vec2 v1 = new Vec2(); 433 434 /** 435 * Find the max separation between poly1 and poly2 using edge normals from poly1. 436 * 437 * @param edgeIndex 438 * @param poly1 439 * @param xf1 440 * @param poly2 441 * @param xf2 442 * @return 443 */ 444 public final void findMaxSeparation(EdgeResults results, final PolygonShape poly1, 445 final Transform xf1, final PolygonShape poly2, final Transform xf2) { 446 int count1 = poly1.m_count; 447 int count2 = poly2.m_count; 448 Vec2[] n1s = poly1.m_normals; 449 Vec2[] v1s = poly1.m_vertices; 450 Vec2[] v2s = poly2.m_vertices; 451 452 Transform.mulTransToOutUnsafe(xf2, xf1, xf); 453 final Rot xfq = xf.q; 454 455 int bestIndex = 0; 456 float maxSeparation = -Float.MAX_VALUE; 457 for (int i = 0; i < count1; i++) { 458 // Get poly1 normal in frame2. 459 Rot.mulToOutUnsafe(xfq, n1s[i], n); 460 Transform.mulToOutUnsafe(xf, v1s[i], v1); 461 462 // Find deepest point for normal i. 463 float si = Float.MAX_VALUE; 464 for (int j = 0; j < count2; ++j) { 465 Vec2 v2sj = v2s[j]; 466 float sij = n.x * (v2sj.x - v1.x) + n.y * (v2sj.y - v1.y); 467 if (sij < si) { 468 si = sij; 469 } 470 } 471 472 if (si > maxSeparation) { 473 maxSeparation = si; 474 bestIndex = i; 475 } 476 } 477 478 results.edgeIndex = bestIndex; 479 results.separation = maxSeparation; 480 } 481 482 public final void findIncidentEdge(final ClipVertex[] c, final PolygonShape poly1, 483 final Transform xf1, int edge1, final PolygonShape poly2, final Transform xf2) { 484 int count1 = poly1.m_count; 485 final Vec2[] normals1 = poly1.m_normals; 486 487 int count2 = poly2.m_count; 488 final Vec2[] vertices2 = poly2.m_vertices; 489 final Vec2[] normals2 = poly2.m_normals; 490 491 assert (0 <= edge1 && edge1 < count1); 492 493 final ClipVertex c0 = c[0]; 494 final ClipVertex c1 = c[1]; 495 final Rot xf1q = xf1.q; 496 final Rot xf2q = xf2.q; 497 498 // Get the normal of the reference edge in poly2's frame. 499 // Vec2 normal1 = MulT(xf2.R, Mul(xf1.R, normals1[edge1])); 500 // before inline: 501 // Rot.mulToOutUnsafe(xf1.q, normals1[edge1], normal1); // temporary 502 // Rot.mulTrans(xf2.q, normal1, normal1); 503 // after inline: 504 final Vec2 v = normals1[edge1]; 505 final float tempx = xf1q.c * v.x - xf1q.s * v.y; 506 final float tempy = xf1q.s * v.x + xf1q.c * v.y; 507 final float normal1x = xf2q.c * tempx + xf2q.s * tempy; 508 final float normal1y = -xf2q.s * tempx + xf2q.c * tempy; 509 510 // end inline 511 512 // Find the incident edge on poly2. 513 int index = 0; 514 float minDot = Float.MAX_VALUE; 515 for (int i = 0; i < count2; ++i) { 516 Vec2 b = normals2[i]; 517 float dot = normal1x * b.x + normal1y * b.y; 518 if (dot < minDot) { 519 minDot = dot; 520 index = i; 521 } 522 } 523 524 // Build the clip vertices for the incident edge. 525 int i1 = index; 526 int i2 = i1 + 1 < count2 ? i1 + 1 : 0; 527 528 // c0.v = Mul(xf2, vertices2[i1]); 529 Vec2 v1 = vertices2[i1]; 530 Vec2 out = c0.v; 531 out.x = (xf2q.c * v1.x - xf2q.s * v1.y) + xf2.p.x; 532 out.y = (xf2q.s * v1.x + xf2q.c * v1.y) + xf2.p.y; 533 c0.id.indexA = (byte) edge1; 534 c0.id.indexB = (byte) i1; 535 c0.id.typeA = (byte) ContactID.Type.FACE.ordinal(); 536 c0.id.typeB = (byte) ContactID.Type.VERTEX.ordinal(); 537 538 // c1.v = Mul(xf2, vertices2[i2]); 539 Vec2 v2 = vertices2[i2]; 540 Vec2 out1 = c1.v; 541 out1.x = (xf2q.c * v2.x - xf2q.s * v2.y) + xf2.p.x; 542 out1.y = (xf2q.s * v2.x + xf2q.c * v2.y) + xf2.p.y; 543 c1.id.indexA = (byte) edge1; 544 c1.id.indexB = (byte) i2; 545 c1.id.typeA = (byte) ContactID.Type.FACE.ordinal(); 546 c1.id.typeB = (byte) ContactID.Type.VERTEX.ordinal(); 547 } 548 549 private final EdgeResults results1 = new EdgeResults(); 550 private final EdgeResults results2 = new EdgeResults(); 551 private final ClipVertex[] incidentEdge = new ClipVertex[2]; 552 private final Vec2 localTangent = new Vec2(); 553 private final Vec2 localNormal = new Vec2(); 554 private final Vec2 planePoint = new Vec2(); 555 private final Vec2 tangent = new Vec2(); 556 private final Vec2 v11 = new Vec2(); 557 private final Vec2 v12 = new Vec2(); 558 private final ClipVertex[] clipPoints1 = new ClipVertex[2]; 559 private final ClipVertex[] clipPoints2 = new ClipVertex[2]; 560 561 /** 562 * Compute the collision manifold between two polygons. 563 * 564 * @param manifold 565 * @param polygon1 566 * @param xf1 567 * @param polygon2 568 * @param xf2 569 */ 570 public final void collidePolygons(Manifold manifold, final PolygonShape polyA, 571 final Transform xfA, final PolygonShape polyB, final Transform xfB) { 572 // Find edge normal of max separation on A - return if separating axis is found 573 // Find edge normal of max separation on B - return if separation axis is found 574 // Choose reference edge as min(minA, minB) 575 // Find incident edge 576 // Clip 577 578 // The normal points from 1 to 2 579 580 manifold.pointCount = 0; 581 float totalRadius = polyA.m_radius + polyB.m_radius; 582 583 findMaxSeparation(results1, polyA, xfA, polyB, xfB); 584 if (results1.separation > totalRadius) { 585 return; 586 } 587 588 findMaxSeparation(results2, polyB, xfB, polyA, xfA); 589 if (results2.separation > totalRadius) { 590 return; 591 } 592 593 final PolygonShape poly1; // reference polygon 594 final PolygonShape poly2; // incident polygon 595 Transform xf1, xf2; 596 int edge1; // reference edge 597 boolean flip; 598 final float k_tol = 0.1f * Settings.linearSlop; 599 600 if (results2.separation > results1.separation + k_tol) { 601 poly1 = polyB; 602 poly2 = polyA; 603 xf1 = xfB; 604 xf2 = xfA; 605 edge1 = results2.edgeIndex; 606 manifold.type = ManifoldType.FACE_B; 607 flip = true; 608 } else { 609 poly1 = polyA; 610 poly2 = polyB; 611 xf1 = xfA; 612 xf2 = xfB; 613 edge1 = results1.edgeIndex; 614 manifold.type = ManifoldType.FACE_A; 615 flip = false; 616 } 617 final Rot xf1q = xf1.q; 618 619 findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); 620 621 int count1 = poly1.m_count; 622 final Vec2[] vertices1 = poly1.m_vertices; 623 624 final int iv1 = edge1; 625 final int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; 626 v11.set(vertices1[iv1]); 627 v12.set(vertices1[iv2]); 628 localTangent.x = v12.x - v11.x; 629 localTangent.y = v12.y - v11.y; 630 localTangent.normalize(); 631 632 // Vec2 localNormal = Vec2.cross(dv, 1.0f); 633 localNormal.x = 1f * localTangent.y; 634 localNormal.y = -1f * localTangent.x; 635 636 // Vec2 planePoint = 0.5f * (v11+ v12); 637 planePoint.x = (v11.x + v12.x) * .5f; 638 planePoint.y = (v11.y + v12.y) * .5f; 639 640 // Rot.mulToOutUnsafe(xf1.q, localTangent, tangent); 641 tangent.x = xf1q.c * localTangent.x - xf1q.s * localTangent.y; 642 tangent.y = xf1q.s * localTangent.x + xf1q.c * localTangent.y; 643 644 // Vec2.crossToOutUnsafe(tangent, 1f, normal); 645 final float normalx = 1f * tangent.y; 646 final float normaly = -1f * tangent.x; 647 648 649 Transform.mulToOut(xf1, v11, v11); 650 Transform.mulToOut(xf1, v12, v12); 651 // v11 = Mul(xf1, v11); 652 // v12 = Mul(xf1, v12); 653 654 // Face offset 655 // float frontOffset = Vec2.dot(normal, v11); 656 float frontOffset = normalx * v11.x + normaly * v11.y; 657 658 // Side offsets, extended by polytope skin thickness. 659 // float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius; 660 // float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius; 661 float sideOffset1 = -(tangent.x * v11.x + tangent.y * v11.y) + totalRadius; 662 float sideOffset2 = tangent.x * v12.x + tangent.y * v12.y + totalRadius; 663 664 // Clip incident edge against extruded edge1 side edges. 665 // ClipVertex clipPoints1[2]; 666 // ClipVertex clipPoints2[2]; 667 int np; 668 669 // Clip to box side 1 670 // np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1); 671 tangent.negateLocal(); 672 np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1); 673 tangent.negateLocal(); 674 675 if (np < 2) { 676 return; 677 } 678 679 // Clip to negative box side 1 680 np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); 681 682 if (np < 2) { 683 return; 684 } 685 686 // Now clipPoints2 contains the clipped points. 687 manifold.localNormal.set(localNormal); 688 manifold.localPoint.set(planePoint); 689 690 int pointCount = 0; 691 for (int i = 0; i < Settings.maxManifoldPoints; ++i) { 692 // float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset; 693 float separation = normalx * clipPoints2[i].v.x + normaly * clipPoints2[i].v.y - frontOffset; 694 695 if (separation <= totalRadius) { 696 ManifoldPoint cp = manifold.points[pointCount]; 697 // cp.m_localPoint = MulT(xf2, clipPoints2[i].v); 698 Vec2 out = cp.localPoint; 699 final float px = clipPoints2[i].v.x - xf2.p.x; 700 final float py = clipPoints2[i].v.y - xf2.p.y; 701 out.x = (xf2.q.c * px + xf2.q.s * py); 702 out.y = (-xf2.q.s * px + xf2.q.c * py); 703 cp.id.set(clipPoints2[i].id); 704 if (flip) { 705 // Swap features 706 cp.id.flip(); 707 } 708 ++pointCount; 709 } 710 } 711 712 manifold.pointCount = pointCount; 713 } 714 715 private final Vec2 Q = new Vec2(); 716 private final Vec2 e = new Vec2(); 717 private final ContactID cf = new ContactID(); 718 private final Vec2 e1 = new Vec2(); 719 private final Vec2 P = new Vec2(); 720 721 // Compute contact points for edge versus circle. 722 // This accounts for edge connectivity. 723 public void collideEdgeAndCircle(Manifold manifold, final EdgeShape edgeA, final Transform xfA, 724 final CircleShape circleB, final Transform xfB) { 725 manifold.pointCount = 0; 726 727 728 // Compute circle in frame of edge 729 // Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p)); 730 Transform.mulToOutUnsafe(xfB, circleB.m_p, temp); 731 Transform.mulTransToOutUnsafe(xfA, temp, Q); 732 733 final Vec2 A = edgeA.m_vertex1; 734 final Vec2 B = edgeA.m_vertex2; 735 e.set(B).subLocal(A); 736 737 // Barycentric coordinates 738 float u = Vec2.dot(e, temp.set(B).subLocal(Q)); 739 float v = Vec2.dot(e, temp.set(Q).subLocal(A)); 740 741 float radius = edgeA.m_radius + circleB.m_radius; 742 743 // ContactFeature cf; 744 cf.indexB = 0; 745 cf.typeB = (byte) ContactID.Type.VERTEX.ordinal(); 746 747 // Region A 748 if (v <= 0.0f) { 749 final Vec2 P = A; 750 d.set(Q).subLocal(P); 751 float dd = Vec2.dot(d, d); 752 if (dd > radius * radius) { 753 return; 754 } 755 756 // Is there an edge connected to A? 757 if (edgeA.m_hasVertex0) { 758 final Vec2 A1 = edgeA.m_vertex0; 759 final Vec2 B1 = A; 760 e1.set(B1).subLocal(A1); 761 float u1 = Vec2.dot(e1, temp.set(B1).subLocal(Q)); 762 763 // Is the circle in Region AB of the previous edge? 764 if (u1 > 0.0f) { 765 return; 766 } 767 } 768 769 cf.indexA = 0; 770 cf.typeA = (byte) ContactID.Type.VERTEX.ordinal(); 771 manifold.pointCount = 1; 772 manifold.type = Manifold.ManifoldType.CIRCLES; 773 manifold.localNormal.setZero(); 774 manifold.localPoint.set(P); 775 // manifold.points[0].id.key = 0; 776 manifold.points[0].id.set(cf); 777 manifold.points[0].localPoint.set(circleB.m_p); 778 return; 779 } 780 781 // Region B 782 if (u <= 0.0f) { 783 Vec2 P = B; 784 d.set(Q).subLocal(P); 785 float dd = Vec2.dot(d, d); 786 if (dd > radius * radius) { 787 return; 788 } 789 790 // Is there an edge connected to B? 791 if (edgeA.m_hasVertex3) { 792 final Vec2 B2 = edgeA.m_vertex3; 793 final Vec2 A2 = B; 794 final Vec2 e2 = e1; 795 e2.set(B2).subLocal(A2); 796 float v2 = Vec2.dot(e2, temp.set(Q).subLocal(A2)); 797 798 // Is the circle in Region AB of the next edge? 799 if (v2 > 0.0f) { 800 return; 801 } 802 } 803 804 cf.indexA = 1; 805 cf.typeA = (byte) ContactID.Type.VERTEX.ordinal(); 806 manifold.pointCount = 1; 807 manifold.type = Manifold.ManifoldType.CIRCLES; 808 manifold.localNormal.setZero(); 809 manifold.localPoint.set(P); 810 // manifold.points[0].id.key = 0; 811 manifold.points[0].id.set(cf); 812 manifold.points[0].localPoint.set(circleB.m_p); 813 return; 814 } 815 816 // Region AB 817 float den = Vec2.dot(e, e); 818 assert (den > 0.0f); 819 820 // Vec2 P = (1.0f / den) * (u * A + v * B); 821 P.set(A).mulLocal(u).addLocal(temp.set(B).mulLocal(v)); 822 P.mulLocal(1.0f / den); 823 d.set(Q).subLocal(P); 824 float dd = Vec2.dot(d, d); 825 if (dd > radius * radius) { 826 return; 827 } 828 829 n.x = -e.y; 830 n.y = e.x; 831 if (Vec2.dot(n, temp.set(Q).subLocal(A)) < 0.0f) { 832 n.set(-n.x, -n.y); 833 } 834 n.normalize(); 835 836 cf.indexA = 0; 837 cf.typeA = (byte) ContactID.Type.FACE.ordinal(); 838 manifold.pointCount = 1; 839 manifold.type = Manifold.ManifoldType.FACE_A; 840 manifold.localNormal.set(n); 841 manifold.localPoint.set(A); 842 // manifold.points[0].id.key = 0; 843 manifold.points[0].id.set(cf); 844 manifold.points[0].localPoint.set(circleB.m_p); 845 } 846 847 private final EPCollider collider = new EPCollider(); 848 849 public void collideEdgeAndPolygon(Manifold manifold, final EdgeShape edgeA, final Transform xfA, 850 final PolygonShape polygonB, final Transform xfB) { 851 collider.collide(manifold, edgeA, xfA, polygonB, xfB); 852 } 853 854 855 856 /** 857 * Java-specific class for returning edge results 858 */ 859 private static class EdgeResults { 860 public float separation; 861 public int edgeIndex; 862 } 863 864 /** 865 * Used for computing contact manifolds. 866 */ 867 public static class ClipVertex { 868 public final Vec2 v; 869 public final ContactID id; 870 871 public ClipVertex() { 872 v = new Vec2(); 873 id = new ContactID(); 874 } 875 876 public void set(final ClipVertex cv) { 877 Vec2 v1 = cv.v; 878 v.x = v1.x; 879 v.y = v1.y; 880 ContactID c = cv.id; 881 id.indexA = c.indexA; 882 id.indexB = c.indexB; 883 id.typeA = c.typeA; 884 id.typeB = c.typeB; 885 } 886 } 887 888 /** 889 * This is used for determining the state of contact points. 890 * 891 * @author Daniel Murphy 892 */ 893 public static enum PointState { 894 /** 895 * point does not exist 896 */ 897 NULL_STATE, 898 /** 899 * point was added in the update 900 */ 901 ADD_STATE, 902 /** 903 * point persisted across the update 904 */ 905 PERSIST_STATE, 906 /** 907 * point was removed in the update 908 */ 909 REMOVE_STATE 910 } 911 912 /** 913 * This structure is used to keep track of the best separating axis. 914 */ 915 static class EPAxis { 916 enum Type { 917 UNKNOWN, EDGE_A, EDGE_B 918 } 919 920 Type type; 921 int index; 922 float separation; 923 } 924 925 /** 926 * This holds polygon B expressed in frame A. 927 */ 928 static class TempPolygon { 929 final Vec2[] vertices = new Vec2[Settings.maxPolygonVertices]; 930 final Vec2[] normals = new Vec2[Settings.maxPolygonVertices]; 931 int count; 932 933 public TempPolygon() { 934 for (int i = 0; i < vertices.length; i++) { 935 vertices[i] = new Vec2(); 936 normals[i] = new Vec2(); 937 } 938 } 939 } 940 941 /** 942 * Reference face used for clipping 943 */ 944 static class ReferenceFace { 945 int i1, i2; 946 final Vec2 v1 = new Vec2(); 947 final Vec2 v2 = new Vec2(); 948 final Vec2 normal = new Vec2(); 949 950 final Vec2 sideNormal1 = new Vec2(); 951 float sideOffset1; 952 953 final Vec2 sideNormal2 = new Vec2(); 954 float sideOffset2; 955 } 956 957 /** 958 * This class collides and edge and a polygon, taking into account edge adjacency. 959 */ 960 static class EPCollider { 961 enum VertexType { 962 ISOLATED, CONCAVE, CONVEX 963 } 964 965 final TempPolygon m_polygonB = new TempPolygon(); 966 967 final Transform m_xf = new Transform(); 968 final Vec2 m_centroidB = new Vec2(); 969 Vec2 m_v0 = new Vec2(); 970 Vec2 m_v1 = new Vec2(); 971 Vec2 m_v2 = new Vec2(); 972 Vec2 m_v3 = new Vec2(); 973 final Vec2 m_normal0 = new Vec2(); 974 final Vec2 m_normal1 = new Vec2(); 975 final Vec2 m_normal2 = new Vec2(); 976 final Vec2 m_normal = new Vec2(); 977 978 VertexType m_type1, m_type2; 979 980 final Vec2 m_lowerLimit = new Vec2(); 981 final Vec2 m_upperLimit = new Vec2(); 982 float m_radius; 983 boolean m_front; 984 985 public EPCollider() { 986 for (int i = 0; i < 2; i++) { 987 ie[i] = new ClipVertex(); 988 clipPoints1[i] = new ClipVertex(); 989 clipPoints2[i] = new ClipVertex(); 990 } 991 } 992 993 private final Vec2 edge1 = new Vec2(); 994 private final Vec2 temp = new Vec2(); 995 private final Vec2 edge0 = new Vec2(); 996 private final Vec2 edge2 = new Vec2(); 997 private final ClipVertex[] ie = new ClipVertex[2]; 998 private final ClipVertex[] clipPoints1 = new ClipVertex[2]; 999 private final ClipVertex[] clipPoints2 = new ClipVertex[2]; 1000 private final ReferenceFace rf = new ReferenceFace(); 1001 private final EPAxis edgeAxis = new EPAxis(); 1002 private final EPAxis polygonAxis = new EPAxis(); 1003 1004 public void collide(Manifold manifold, final EdgeShape edgeA, final Transform xfA, 1005 final PolygonShape polygonB, final Transform xfB) { 1006 1007 Transform.mulTransToOutUnsafe(xfA, xfB, m_xf); 1008 Transform.mulToOutUnsafe(m_xf, polygonB.m_centroid, m_centroidB); 1009 1010 m_v0 = edgeA.m_vertex0; 1011 m_v1 = edgeA.m_vertex1; 1012 m_v2 = edgeA.m_vertex2; 1013 m_v3 = edgeA.m_vertex3; 1014 1015 boolean hasVertex0 = edgeA.m_hasVertex0; 1016 boolean hasVertex3 = edgeA.m_hasVertex3; 1017 1018 edge1.set(m_v2).subLocal(m_v1); 1019 edge1.normalize(); 1020 m_normal1.set(edge1.y, -edge1.x); 1021 float offset1 = Vec2.dot(m_normal1, temp.set(m_centroidB).subLocal(m_v1)); 1022 float offset0 = 0.0f, offset2 = 0.0f; 1023 boolean convex1 = false, convex2 = false; 1024 1025 // Is there a preceding edge? 1026 if (hasVertex0) { 1027 edge0.set(m_v1).subLocal(m_v0); 1028 edge0.normalize(); 1029 m_normal0.set(edge0.y, -edge0.x); 1030 convex1 = Vec2.cross(edge0, edge1) >= 0.0f; 1031 offset0 = Vec2.dot(m_normal0, temp.set(m_centroidB).subLocal(m_v0)); 1032 } 1033 1034 // Is there a following edge? 1035 if (hasVertex3) { 1036 edge2.set(m_v3).subLocal(m_v2); 1037 edge2.normalize(); 1038 m_normal2.set(edge2.y, -edge2.x); 1039 convex2 = Vec2.cross(edge1, edge2) > 0.0f; 1040 offset2 = Vec2.dot(m_normal2, temp.set(m_centroidB).subLocal(m_v2)); 1041 } 1042 1043 // Determine front or back collision. Determine collision normal limits. 1044 if (hasVertex0 && hasVertex3) { 1045 if (convex1 && convex2) { 1046 m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; 1047 if (m_front) { 1048 m_normal.x = m_normal1.x; 1049 m_normal.y = m_normal1.y; 1050 m_lowerLimit.x = m_normal0.x; 1051 m_lowerLimit.y = m_normal0.y; 1052 m_upperLimit.x = m_normal2.x; 1053 m_upperLimit.y = m_normal2.y; 1054 } else { 1055 m_normal.x = -m_normal1.x; 1056 m_normal.y = -m_normal1.y; 1057 m_lowerLimit.x = -m_normal1.x; 1058 m_lowerLimit.y = -m_normal1.y; 1059 m_upperLimit.x = -m_normal1.x; 1060 m_upperLimit.y = -m_normal1.y; 1061 } 1062 } else if (convex1) { 1063 m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); 1064 if (m_front) { 1065 m_normal.x = m_normal1.x; 1066 m_normal.y = m_normal1.y; 1067 m_lowerLimit.x = m_normal0.x; 1068 m_lowerLimit.y = m_normal0.y; 1069 m_upperLimit.x = m_normal1.x; 1070 m_upperLimit.y = m_normal1.y; 1071 } else { 1072 m_normal.x = -m_normal1.x; 1073 m_normal.y = -m_normal1.y; 1074 m_lowerLimit.x = -m_normal2.x; 1075 m_lowerLimit.y = -m_normal2.y; 1076 m_upperLimit.x = -m_normal1.x; 1077 m_upperLimit.y = -m_normal1.y; 1078 } 1079 } else if (convex2) { 1080 m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); 1081 if (m_front) { 1082 m_normal.x = m_normal1.x; 1083 m_normal.y = m_normal1.y; 1084 m_lowerLimit.x = m_normal1.x; 1085 m_lowerLimit.y = m_normal1.y; 1086 m_upperLimit.x = m_normal2.x; 1087 m_upperLimit.y = m_normal2.y; 1088 } else { 1089 m_normal.x = -m_normal1.x; 1090 m_normal.y = -m_normal1.y; 1091 m_lowerLimit.x = -m_normal1.x; 1092 m_lowerLimit.y = -m_normal1.y; 1093 m_upperLimit.x = -m_normal0.x; 1094 m_upperLimit.y = -m_normal0.y; 1095 } 1096 } else { 1097 m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; 1098 if (m_front) { 1099 m_normal.x = m_normal1.x; 1100 m_normal.y = m_normal1.y; 1101 m_lowerLimit.x = m_normal1.x; 1102 m_lowerLimit.y = m_normal1.y; 1103 m_upperLimit.x = m_normal1.x; 1104 m_upperLimit.y = m_normal1.y; 1105 } else { 1106 m_normal.x = -m_normal1.x; 1107 m_normal.y = -m_normal1.y; 1108 m_lowerLimit.x = -m_normal2.x; 1109 m_lowerLimit.y = -m_normal2.y; 1110 m_upperLimit.x = -m_normal0.x; 1111 m_upperLimit.y = -m_normal0.y; 1112 } 1113 } 1114 } else if (hasVertex0) { 1115 if (convex1) { 1116 m_front = offset0 >= 0.0f || offset1 >= 0.0f; 1117 if (m_front) { 1118 m_normal.x = m_normal1.x; 1119 m_normal.y = m_normal1.y; 1120 m_lowerLimit.x = m_normal0.x; 1121 m_lowerLimit.y = m_normal0.y; 1122 m_upperLimit.x = -m_normal1.x; 1123 m_upperLimit.y = -m_normal1.y; 1124 } else { 1125 m_normal.x = -m_normal1.x; 1126 m_normal.y = -m_normal1.y; 1127 m_lowerLimit.x = m_normal1.x; 1128 m_lowerLimit.y = m_normal1.y; 1129 m_upperLimit.x = -m_normal1.x; 1130 m_upperLimit.y = -m_normal1.y; 1131 } 1132 } else { 1133 m_front = offset0 >= 0.0f && offset1 >= 0.0f; 1134 if (m_front) { 1135 m_normal.x = m_normal1.x; 1136 m_normal.y = m_normal1.y; 1137 m_lowerLimit.x = m_normal1.x; 1138 m_lowerLimit.y = m_normal1.y; 1139 m_upperLimit.x = -m_normal1.x; 1140 m_upperLimit.y = -m_normal1.y; 1141 } else { 1142 m_normal.x = -m_normal1.x; 1143 m_normal.y = -m_normal1.y; 1144 m_lowerLimit.x = m_normal1.x; 1145 m_lowerLimit.y = m_normal1.y; 1146 m_upperLimit.x = -m_normal0.x; 1147 m_upperLimit.y = -m_normal0.y; 1148 } 1149 } 1150 } else if (hasVertex3) { 1151 if (convex2) { 1152 m_front = offset1 >= 0.0f || offset2 >= 0.0f; 1153 if (m_front) { 1154 m_normal.x = m_normal1.x; 1155 m_normal.y = m_normal1.y; 1156 m_lowerLimit.x = -m_normal1.x; 1157 m_lowerLimit.y = -m_normal1.y; 1158 m_upperLimit.x = m_normal2.x; 1159 m_upperLimit.y = m_normal2.y; 1160 } else { 1161 m_normal.x = -m_normal1.x; 1162 m_normal.y = -m_normal1.y; 1163 m_lowerLimit.x = -m_normal1.x; 1164 m_lowerLimit.y = -m_normal1.y; 1165 m_upperLimit.x = m_normal1.x; 1166 m_upperLimit.y = m_normal1.y; 1167 } 1168 } else { 1169 m_front = offset1 >= 0.0f && offset2 >= 0.0f; 1170 if (m_front) { 1171 m_normal.x = m_normal1.x; 1172 m_normal.y = m_normal1.y; 1173 m_lowerLimit.x = -m_normal1.x; 1174 m_lowerLimit.y = -m_normal1.y; 1175 m_upperLimit.x = m_normal1.x; 1176 m_upperLimit.y = m_normal1.y; 1177 } else { 1178 m_normal.x = -m_normal1.x; 1179 m_normal.y = -m_normal1.y; 1180 m_lowerLimit.x = -m_normal2.x; 1181 m_lowerLimit.y = -m_normal2.y; 1182 m_upperLimit.x = m_normal1.x; 1183 m_upperLimit.y = m_normal1.y; 1184 } 1185 } 1186 } else { 1187 m_front = offset1 >= 0.0f; 1188 if (m_front) { 1189 m_normal.x = m_normal1.x; 1190 m_normal.y = m_normal1.y; 1191 m_lowerLimit.x = -m_normal1.x; 1192 m_lowerLimit.y = -m_normal1.y; 1193 m_upperLimit.x = -m_normal1.x; 1194 m_upperLimit.y = -m_normal1.y; 1195 } else { 1196 m_normal.x = -m_normal1.x; 1197 m_normal.y = -m_normal1.y; 1198 m_lowerLimit.x = m_normal1.x; 1199 m_lowerLimit.y = m_normal1.y; 1200 m_upperLimit.x = m_normal1.x; 1201 m_upperLimit.y = m_normal1.y; 1202 } 1203 } 1204 1205 // Get polygonB in frameA 1206 m_polygonB.count = polygonB.m_count; 1207 for (int i = 0; i < polygonB.m_count; ++i) { 1208 Transform.mulToOutUnsafe(m_xf, polygonB.m_vertices[i], m_polygonB.vertices[i]); 1209 Rot.mulToOutUnsafe(m_xf.q, polygonB.m_normals[i], m_polygonB.normals[i]); 1210 } 1211 1212 m_radius = 2.0f * Settings.polygonRadius; 1213 1214 manifold.pointCount = 0; 1215 1216 computeEdgeSeparation(edgeAxis); 1217 1218 // If no valid normal can be found than this edge should not collide. 1219 if (edgeAxis.type == EPAxis.Type.UNKNOWN) { 1220 return; 1221 } 1222 1223 if (edgeAxis.separation > m_radius) { 1224 return; 1225 } 1226 1227 computePolygonSeparation(polygonAxis); 1228 if (polygonAxis.type != EPAxis.Type.UNKNOWN && polygonAxis.separation > m_radius) { 1229 return; 1230 } 1231 1232 // Use hysteresis for jitter reduction. 1233 final float k_relativeTol = 0.98f; 1234 final float k_absoluteTol = 0.001f; 1235 1236 EPAxis primaryAxis; 1237 if (polygonAxis.type == EPAxis.Type.UNKNOWN) { 1238 primaryAxis = edgeAxis; 1239 } else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) { 1240 primaryAxis = polygonAxis; 1241 } else { 1242 primaryAxis = edgeAxis; 1243 } 1244 1245 final ClipVertex ie0 = ie[0]; 1246 final ClipVertex ie1 = ie[1]; 1247 1248 if (primaryAxis.type == EPAxis.Type.EDGE_A) { 1249 manifold.type = Manifold.ManifoldType.FACE_A; 1250 1251 // Search for the polygon normal that is most anti-parallel to the edge normal. 1252 int bestIndex = 0; 1253 float bestValue = Vec2.dot(m_normal, m_polygonB.normals[0]); 1254 for (int i = 1; i < m_polygonB.count; ++i) { 1255 float value = Vec2.dot(m_normal, m_polygonB.normals[i]); 1256 if (value < bestValue) { 1257 bestValue = value; 1258 bestIndex = i; 1259 } 1260 } 1261 1262 int i1 = bestIndex; 1263 int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; 1264 1265 ie0.v.set(m_polygonB.vertices[i1]); 1266 ie0.id.indexA = 0; 1267 ie0.id.indexB = (byte) i1; 1268 ie0.id.typeA = (byte) ContactID.Type.FACE.ordinal(); 1269 ie0.id.typeB = (byte) ContactID.Type.VERTEX.ordinal(); 1270 1271 ie1.v.set(m_polygonB.vertices[i2]); 1272 ie1.id.indexA = 0; 1273 ie1.id.indexB = (byte) i2; 1274 ie1.id.typeA = (byte) ContactID.Type.FACE.ordinal(); 1275 ie1.id.typeB = (byte) ContactID.Type.VERTEX.ordinal(); 1276 1277 if (m_front) { 1278 rf.i1 = 0; 1279 rf.i2 = 1; 1280 rf.v1.set(m_v1); 1281 rf.v2.set(m_v2); 1282 rf.normal.set(m_normal1); 1283 } else { 1284 rf.i1 = 1; 1285 rf.i2 = 0; 1286 rf.v1.set(m_v2); 1287 rf.v2.set(m_v1); 1288 rf.normal.set(m_normal1).negateLocal(); 1289 } 1290 } else { 1291 manifold.type = Manifold.ManifoldType.FACE_B; 1292 1293 ie0.v.set(m_v1); 1294 ie0.id.indexA = 0; 1295 ie0.id.indexB = (byte) primaryAxis.index; 1296 ie0.id.typeA = (byte) ContactID.Type.VERTEX.ordinal(); 1297 ie0.id.typeB = (byte) ContactID.Type.FACE.ordinal(); 1298 1299 ie1.v.set(m_v2); 1300 ie1.id.indexA = 0; 1301 ie1.id.indexB = (byte) primaryAxis.index; 1302 ie1.id.typeA = (byte) ContactID.Type.VERTEX.ordinal(); 1303 ie1.id.typeB = (byte) ContactID.Type.FACE.ordinal(); 1304 1305 rf.i1 = primaryAxis.index; 1306 rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; 1307 rf.v1.set(m_polygonB.vertices[rf.i1]); 1308 rf.v2.set(m_polygonB.vertices[rf.i2]); 1309 rf.normal.set(m_polygonB.normals[rf.i1]); 1310 } 1311 1312 rf.sideNormal1.set(rf.normal.y, -rf.normal.x); 1313 rf.sideNormal2.set(rf.sideNormal1).negateLocal(); 1314 rf.sideOffset1 = Vec2.dot(rf.sideNormal1, rf.v1); 1315 rf.sideOffset2 = Vec2.dot(rf.sideNormal2, rf.v2); 1316 1317 // Clip incident edge against extruded edge1 side edges. 1318 int np; 1319 1320 // Clip to box side 1 1321 np = clipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); 1322 1323 if (np < Settings.maxManifoldPoints) { 1324 return; 1325 } 1326 1327 // Clip to negative box side 1 1328 np = clipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); 1329 1330 if (np < Settings.maxManifoldPoints) { 1331 return; 1332 } 1333 1334 // Now clipPoints2 contains the clipped points. 1335 if (primaryAxis.type == EPAxis.Type.EDGE_A) { 1336 manifold.localNormal.set(rf.normal); 1337 manifold.localPoint.set(rf.v1); 1338 } else { 1339 manifold.localNormal.set(polygonB.m_normals[rf.i1]); 1340 manifold.localPoint.set(polygonB.m_vertices[rf.i1]); 1341 } 1342 1343 int pointCount = 0; 1344 for (int i = 0; i < Settings.maxManifoldPoints; ++i) { 1345 float separation; 1346 1347 separation = Vec2.dot(rf.normal, temp.set(clipPoints2[i].v).subLocal(rf.v1)); 1348 1349 if (separation <= m_radius) { 1350 ManifoldPoint cp = manifold.points[pointCount]; 1351 1352 if (primaryAxis.type == EPAxis.Type.EDGE_A) { 1353 // cp.localPoint = MulT(m_xf, clipPoints2[i].v); 1354 Transform.mulTransToOutUnsafe(m_xf, clipPoints2[i].v, cp.localPoint); 1355 cp.id.set(clipPoints2[i].id); 1356 } else { 1357 cp.localPoint.set(clipPoints2[i].v); 1358 cp.id.typeA = clipPoints2[i].id.typeB; 1359 cp.id.typeB = clipPoints2[i].id.typeA; 1360 cp.id.indexA = clipPoints2[i].id.indexB; 1361 cp.id.indexB = clipPoints2[i].id.indexA; 1362 } 1363 1364 ++pointCount; 1365 } 1366 } 1367 1368 manifold.pointCount = pointCount; 1369 } 1370 1371 1372 public void computeEdgeSeparation(EPAxis axis) { 1373 axis.type = EPAxis.Type.EDGE_A; 1374 axis.index = m_front ? 0 : 1; 1375 axis.separation = Float.MAX_VALUE; 1376 float nx = m_normal.x; 1377 float ny = m_normal.y; 1378 1379 for (int i = 0; i < m_polygonB.count; ++i) { 1380 Vec2 v = m_polygonB.vertices[i]; 1381 float tempx = v.x - m_v1.x; 1382 float tempy = v.y - m_v1.y; 1383 float s = nx * tempx + ny * tempy; 1384 if (s < axis.separation) { 1385 axis.separation = s; 1386 } 1387 } 1388 } 1389 1390 private final Vec2 perp = new Vec2(); 1391 private final Vec2 n = new Vec2(); 1392 1393 public void computePolygonSeparation(EPAxis axis) { 1394 axis.type = EPAxis.Type.UNKNOWN; 1395 axis.index = -1; 1396 axis.separation = -Float.MAX_VALUE; 1397 1398 perp.x = -m_normal.y; 1399 perp.y = m_normal.x; 1400 1401 for (int i = 0; i < m_polygonB.count; ++i) { 1402 Vec2 normalB = m_polygonB.normals[i]; 1403 Vec2 vB = m_polygonB.vertices[i]; 1404 n.x = -normalB.x; 1405 n.y = -normalB.y; 1406 1407 // float s1 = Vec2.dot(n, temp.set(vB).subLocal(m_v1)); 1408 // float s2 = Vec2.dot(n, temp.set(vB).subLocal(m_v2)); 1409 float tempx = vB.x - m_v1.x; 1410 float tempy = vB.y - m_v1.y; 1411 float s1 = n.x * tempx + n.y * tempy; 1412 tempx = vB.x - m_v2.x; 1413 tempy = vB.y - m_v2.y; 1414 float s2 = n.x * tempx + n.y * tempy; 1415 float s = MathUtils.min(s1, s2); 1416 1417 if (s > m_radius) { 1418 // No collision 1419 axis.type = EPAxis.Type.EDGE_B; 1420 axis.index = i; 1421 axis.separation = s; 1422 return; 1423 } 1424 1425 // Adjacency 1426 if (n.x * perp.x + n.y * perp.y >= 0.0f) { 1427 if (Vec2.dot(temp.set(n).subLocal(m_upperLimit), m_normal) < -Settings.angularSlop) { 1428 continue; 1429 } 1430 } else { 1431 if (Vec2.dot(temp.set(n).subLocal(m_lowerLimit), m_normal) < -Settings.angularSlop) { 1432 continue; 1433 } 1434 } 1435 1436 if (s > axis.separation) { 1437 axis.type = EPAxis.Type.EDGE_B; 1438 axis.index = i; 1439 axis.separation = s; 1440 } 1441 } 1442 } 1443 } 1444 } 1445