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.shapes.ChainShape; 27 import org.jbox2d.collision.shapes.CircleShape; 28 import org.jbox2d.collision.shapes.EdgeShape; 29 import org.jbox2d.collision.shapes.PolygonShape; 30 import org.jbox2d.collision.shapes.Shape; 31 import org.jbox2d.common.MathUtils; 32 import org.jbox2d.common.Rot; 33 import org.jbox2d.common.Settings; 34 import org.jbox2d.common.Vec2; 35 import org.jbox2d.common.Transform; 36 37 // updated to rev 100 38 /** 39 * This is non-static for faster pooling. To get an instance, use the {@link SingletonPool}, don't 40 * construct a distance object. 41 * 42 * @author Daniel Murphy 43 */ 44 public class Distance { 45 public static final int MAX_ITERS = 20; 46 47 public static int GJK_CALLS = 0; 48 public static int GJK_ITERS = 0; 49 public static int GJK_MAX_ITERS = 20; 50 51 /** 52 * GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. 53 */ 54 private class SimplexVertex { 55 public final Vec2 wA = new Vec2(); // support point in shapeA 56 public final Vec2 wB = new Vec2(); // support point in shapeB 57 public final Vec2 w = new Vec2(); // wB - wA 58 public float a; // barycentric coordinate for closest point 59 public int indexA; // wA index 60 public int indexB; // wB index 61 62 public void set(SimplexVertex sv) { 63 wA.set(sv.wA); 64 wB.set(sv.wB); 65 w.set(sv.w); 66 a = sv.a; 67 indexA = sv.indexA; 68 indexB = sv.indexB; 69 } 70 } 71 72 /** 73 * Used to warm start Distance. Set count to zero on first call. 74 * 75 * @author daniel 76 */ 77 public static class SimplexCache { 78 /** length or area */ 79 public float metric; 80 public int count; 81 /** vertices on shape A */ 82 public final int indexA[] = new int[3]; 83 /** vertices on shape B */ 84 public final int indexB[] = new int[3]; 85 86 public SimplexCache() { 87 metric = 0; 88 count = 0; 89 indexA[0] = Integer.MAX_VALUE; 90 indexA[1] = Integer.MAX_VALUE; 91 indexA[2] = Integer.MAX_VALUE; 92 indexB[0] = Integer.MAX_VALUE; 93 indexB[1] = Integer.MAX_VALUE; 94 indexB[2] = Integer.MAX_VALUE; 95 } 96 97 public void set(SimplexCache sc) { 98 System.arraycopy(sc.indexA, 0, indexA, 0, indexA.length); 99 System.arraycopy(sc.indexB, 0, indexB, 0, indexB.length); 100 metric = sc.metric; 101 count = sc.count; 102 } 103 } 104 105 private class Simplex { 106 public final SimplexVertex m_v1 = new SimplexVertex(); 107 public final SimplexVertex m_v2 = new SimplexVertex(); 108 public final SimplexVertex m_v3 = new SimplexVertex(); 109 public final SimplexVertex vertices[] = {m_v1, m_v2, m_v3}; 110 public int m_count; 111 112 public void readCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, 113 DistanceProxy proxyB, Transform transformB) { 114 assert (cache.count <= 3); 115 116 // Copy data from cache. 117 m_count = cache.count; 118 119 for (int i = 0; i < m_count; ++i) { 120 SimplexVertex v = vertices[i]; 121 v.indexA = cache.indexA[i]; 122 v.indexB = cache.indexB[i]; 123 Vec2 wALocal = proxyA.getVertex(v.indexA); 124 Vec2 wBLocal = proxyB.getVertex(v.indexB); 125 Transform.mulToOutUnsafe(transformA, wALocal, v.wA); 126 Transform.mulToOutUnsafe(transformB, wBLocal, v.wB); 127 v.w.set(v.wB).subLocal(v.wA); 128 v.a = 0.0f; 129 } 130 131 // Compute the new simplex metric, if it is substantially different than 132 // old metric then flush the simplex. 133 if (m_count > 1) { 134 float metric1 = cache.metric; 135 float metric2 = getMetric(); 136 if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.EPSILON) { 137 // Reset the simplex. 138 m_count = 0; 139 } 140 } 141 142 // If the cache is empty or invalid ... 143 if (m_count == 0) { 144 SimplexVertex v = vertices[0]; 145 v.indexA = 0; 146 v.indexB = 0; 147 Vec2 wALocal = proxyA.getVertex(0); 148 Vec2 wBLocal = proxyB.getVertex(0); 149 Transform.mulToOutUnsafe(transformA, wALocal, v.wA); 150 Transform.mulToOutUnsafe(transformB, wBLocal, v.wB); 151 v.w.set(v.wB).subLocal(v.wA); 152 m_count = 1; 153 } 154 } 155 156 public void writeCache(SimplexCache cache) { 157 cache.metric = getMetric(); 158 cache.count = m_count; 159 160 for (int i = 0; i < m_count; ++i) { 161 cache.indexA[i] = (vertices[i].indexA); 162 cache.indexB[i] = (vertices[i].indexB); 163 } 164 } 165 166 private final Vec2 e12 = new Vec2(); 167 168 public final void getSearchDirection(final Vec2 out) { 169 switch (m_count) { 170 case 1: 171 out.set(m_v1.w).negateLocal(); 172 return; 173 case 2: 174 e12.set(m_v2.w).subLocal(m_v1.w); 175 // use out for a temp variable real quick 176 out.set(m_v1.w).negateLocal(); 177 float sgn = Vec2.cross(e12, out); 178 179 if (sgn > 0f) { 180 // Origin is left of e12. 181 Vec2.crossToOutUnsafe(1f, e12, out); 182 return; 183 } else { 184 // Origin is right of e12. 185 Vec2.crossToOutUnsafe(e12, 1f, out); 186 return; 187 } 188 default: 189 assert (false); 190 out.setZero(); 191 return; 192 } 193 } 194 195 // djm pooled 196 private final Vec2 case2 = new Vec2(); 197 private final Vec2 case22 = new Vec2(); 198 199 /** 200 * this returns pooled objects. don't keep or modify them 201 * 202 * @return 203 */ 204 public void getClosestPoint(final Vec2 out) { 205 switch (m_count) { 206 case 0: 207 assert (false); 208 out.setZero(); 209 return; 210 case 1: 211 out.set(m_v1.w); 212 return; 213 case 2: 214 case22.set(m_v2.w).mulLocal(m_v2.a); 215 case2.set(m_v1.w).mulLocal(m_v1.a).addLocal(case22); 216 out.set(case2); 217 return; 218 case 3: 219 out.setZero(); 220 return; 221 default: 222 assert (false); 223 out.setZero(); 224 return; 225 } 226 } 227 228 // djm pooled, and from above 229 private final Vec2 case3 = new Vec2(); 230 private final Vec2 case33 = new Vec2(); 231 232 public void getWitnessPoints(Vec2 pA, Vec2 pB) { 233 switch (m_count) { 234 case 0: 235 assert (false); 236 break; 237 238 case 1: 239 pA.set(m_v1.wA); 240 pB.set(m_v1.wB); 241 break; 242 243 case 2: 244 case2.set(m_v1.wA).mulLocal(m_v1.a); 245 pA.set(m_v2.wA).mulLocal(m_v2.a).addLocal(case2); 246 // m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; 247 // *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; 248 case2.set(m_v1.wB).mulLocal(m_v1.a); 249 pB.set(m_v2.wB).mulLocal(m_v2.a).addLocal(case2); 250 251 break; 252 253 case 3: 254 pA.set(m_v1.wA).mulLocal(m_v1.a); 255 case3.set(m_v2.wA).mulLocal(m_v2.a); 256 case33.set(m_v3.wA).mulLocal(m_v3.a); 257 pA.addLocal(case3).addLocal(case33); 258 pB.set(pA); 259 // *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; 260 // *pB = *pA; 261 break; 262 263 default: 264 assert (false); 265 break; 266 } 267 } 268 269 // djm pooled, from above 270 public float getMetric() { 271 switch (m_count) { 272 case 0: 273 assert (false); 274 return 0.0f; 275 276 case 1: 277 return 0.0f; 278 279 case 2: 280 return MathUtils.distance(m_v1.w, m_v2.w); 281 282 case 3: 283 case3.set(m_v2.w).subLocal(m_v1.w); 284 case33.set(m_v3.w).subLocal(m_v1.w); 285 // return Vec2.cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); 286 return Vec2.cross(case3, case33); 287 288 default: 289 assert (false); 290 return 0.0f; 291 } 292 } 293 294 // djm pooled from above 295 /** 296 * Solve a line segment using barycentric coordinates. 297 */ 298 public void solve2() { 299 // Solve a line segment using barycentric coordinates. 300 // 301 // p = a1 * w1 + a2 * w2 302 // a1 + a2 = 1 303 // 304 // The vector from the origin to the closest point on the line is 305 // perpendicular to the line. 306 // e12 = w2 - w1 307 // dot(p, e) = 0 308 // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 309 // 310 // 2-by-2 linear system 311 // [1 1 ][a1] = [1] 312 // [w1.e12 w2.e12][a2] = [0] 313 // 314 // Define 315 // d12_1 = dot(w2, e12) 316 // d12_2 = -dot(w1, e12) 317 // d12 = d12_1 + d12_2 318 // 319 // Solution 320 // a1 = d12_1 / d12 321 // a2 = d12_2 / d12 322 final Vec2 w1 = m_v1.w; 323 final Vec2 w2 = m_v2.w; 324 e12.set(w2).subLocal(w1); 325 326 // w1 region 327 float d12_2 = -Vec2.dot(w1, e12); 328 if (d12_2 <= 0.0f) { 329 // a2 <= 0, so we clamp it to 0 330 m_v1.a = 1.0f; 331 m_count = 1; 332 return; 333 } 334 335 // w2 region 336 float d12_1 = Vec2.dot(w2, e12); 337 if (d12_1 <= 0.0f) { 338 // a1 <= 0, so we clamp it to 0 339 m_v2.a = 1.0f; 340 m_count = 1; 341 m_v1.set(m_v2); 342 return; 343 } 344 345 // Must be in e12 region. 346 float inv_d12 = 1.0f / (d12_1 + d12_2); 347 m_v1.a = d12_1 * inv_d12; 348 m_v2.a = d12_2 * inv_d12; 349 m_count = 2; 350 } 351 352 // djm pooled, and from above 353 private final Vec2 e13 = new Vec2(); 354 private final Vec2 e23 = new Vec2(); 355 private final Vec2 w1 = new Vec2(); 356 private final Vec2 w2 = new Vec2(); 357 private final Vec2 w3 = new Vec2(); 358 359 /** 360 * Solve a line segment using barycentric coordinates.<br/> 361 * Possible regions:<br/> 362 * - points[2]<br/> 363 * - edge points[0]-points[2]<br/> 364 * - edge points[1]-points[2]<br/> 365 * - inside the triangle 366 */ 367 public void solve3() { 368 w1.set(m_v1.w); 369 w2.set(m_v2.w); 370 w3.set(m_v3.w); 371 372 // Edge12 373 // [1 1 ][a1] = [1] 374 // [w1.e12 w2.e12][a2] = [0] 375 // a3 = 0 376 e12.set(w2).subLocal(w1); 377 float w1e12 = Vec2.dot(w1, e12); 378 float w2e12 = Vec2.dot(w2, e12); 379 float d12_1 = w2e12; 380 float d12_2 = -w1e12; 381 382 // Edge13 383 // [1 1 ][a1] = [1] 384 // [w1.e13 w3.e13][a3] = [0] 385 // a2 = 0 386 e13.set(w3).subLocal(w1); 387 float w1e13 = Vec2.dot(w1, e13); 388 float w3e13 = Vec2.dot(w3, e13); 389 float d13_1 = w3e13; 390 float d13_2 = -w1e13; 391 392 // Edge23 393 // [1 1 ][a2] = [1] 394 // [w2.e23 w3.e23][a3] = [0] 395 // a1 = 0 396 e23.set(w3).subLocal(w2); 397 float w2e23 = Vec2.dot(w2, e23); 398 float w3e23 = Vec2.dot(w3, e23); 399 float d23_1 = w3e23; 400 float d23_2 = -w2e23; 401 402 // Triangle123 403 float n123 = Vec2.cross(e12, e13); 404 405 float d123_1 = n123 * Vec2.cross(w2, w3); 406 float d123_2 = n123 * Vec2.cross(w3, w1); 407 float d123_3 = n123 * Vec2.cross(w1, w2); 408 409 // w1 region 410 if (d12_2 <= 0.0f && d13_2 <= 0.0f) { 411 m_v1.a = 1.0f; 412 m_count = 1; 413 return; 414 } 415 416 // e12 417 if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) { 418 float inv_d12 = 1.0f / (d12_1 + d12_2); 419 m_v1.a = d12_1 * inv_d12; 420 m_v2.a = d12_2 * inv_d12; 421 m_count = 2; 422 return; 423 } 424 425 // e13 426 if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) { 427 float inv_d13 = 1.0f / (d13_1 + d13_2); 428 m_v1.a = d13_1 * inv_d13; 429 m_v3.a = d13_2 * inv_d13; 430 m_count = 2; 431 m_v2.set(m_v3); 432 return; 433 } 434 435 // w2 region 436 if (d12_1 <= 0.0f && d23_2 <= 0.0f) { 437 m_v2.a = 1.0f; 438 m_count = 1; 439 m_v1.set(m_v2); 440 return; 441 } 442 443 // w3 region 444 if (d13_1 <= 0.0f && d23_1 <= 0.0f) { 445 m_v3.a = 1.0f; 446 m_count = 1; 447 m_v1.set(m_v3); 448 return; 449 } 450 451 // e23 452 if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) { 453 float inv_d23 = 1.0f / (d23_1 + d23_2); 454 m_v2.a = d23_1 * inv_d23; 455 m_v3.a = d23_2 * inv_d23; 456 m_count = 2; 457 m_v1.set(m_v3); 458 return; 459 } 460 461 // Must be in triangle123 462 float inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); 463 m_v1.a = d123_1 * inv_d123; 464 m_v2.a = d123_2 * inv_d123; 465 m_v3.a = d123_3 * inv_d123; 466 m_count = 3; 467 } 468 } 469 470 /** 471 * A distance proxy is used by the GJK algorithm. It encapsulates any shape. TODO: see if we can 472 * just do assignments with m_vertices, instead of copying stuff over 473 * 474 * @author daniel 475 */ 476 public static class DistanceProxy { 477 public final Vec2[] m_vertices; 478 public int m_count; 479 public float m_radius; 480 public final Vec2[] m_buffer; 481 482 public DistanceProxy() { 483 m_vertices = new Vec2[Settings.maxPolygonVertices]; 484 for (int i = 0; i < m_vertices.length; i++) { 485 m_vertices[i] = new Vec2(); 486 } 487 m_buffer = new Vec2[2]; 488 m_count = 0; 489 m_radius = 0f; 490 } 491 492 /** 493 * Initialize the proxy using the given shape. The shape must remain in scope while the proxy is 494 * in use. 495 */ 496 public final void set(final Shape shape, int index) { 497 switch (shape.getType()) { 498 case CIRCLE: 499 final CircleShape circle = (CircleShape) shape; 500 m_vertices[0].set(circle.m_p); 501 m_count = 1; 502 m_radius = circle.m_radius; 503 504 break; 505 case POLYGON: 506 final PolygonShape poly = (PolygonShape) shape; 507 m_count = poly.m_count; 508 m_radius = poly.m_radius; 509 for (int i = 0; i < m_count; i++) { 510 m_vertices[i].set(poly.m_vertices[i]); 511 } 512 break; 513 case CHAIN: 514 final ChainShape chain = (ChainShape) shape; 515 assert (0 <= index && index < chain.m_count); 516 517 m_buffer[0] = chain.m_vertices[index]; 518 if (index + 1 < chain.m_count) { 519 m_buffer[1] = chain.m_vertices[index + 1]; 520 } else { 521 m_buffer[1] = chain.m_vertices[0]; 522 } 523 524 m_vertices[0].set(m_buffer[0]); 525 m_vertices[1].set(m_buffer[1]); 526 m_count = 2; 527 m_radius = chain.m_radius; 528 break; 529 case EDGE: 530 EdgeShape edge = (EdgeShape) shape; 531 m_vertices[0].set(edge.m_vertex1); 532 m_vertices[1].set(edge.m_vertex2); 533 m_count = 2; 534 m_radius = edge.m_radius; 535 break; 536 default: 537 assert (false); 538 } 539 } 540 541 /** 542 * Get the supporting vertex index in the given direction. 543 * 544 * @param d 545 * @return 546 */ 547 public final int getSupport(final Vec2 d) { 548 int bestIndex = 0; 549 float bestValue = Vec2.dot(m_vertices[0], d); 550 for (int i = 1; i < m_count; i++) { 551 float value = Vec2.dot(m_vertices[i], d); 552 if (value > bestValue) { 553 bestIndex = i; 554 bestValue = value; 555 } 556 } 557 558 return bestIndex; 559 } 560 561 /** 562 * Get the supporting vertex in the given direction. 563 * 564 * @param d 565 * @return 566 */ 567 public final Vec2 getSupportVertex(final Vec2 d) { 568 int bestIndex = 0; 569 float bestValue = Vec2.dot(m_vertices[0], d); 570 for (int i = 1; i < m_count; i++) { 571 float value = Vec2.dot(m_vertices[i], d); 572 if (value > bestValue) { 573 bestIndex = i; 574 bestValue = value; 575 } 576 } 577 578 return m_vertices[bestIndex]; 579 } 580 581 /** 582 * Get the vertex count. 583 * 584 * @return 585 */ 586 public final int getVertexCount() { 587 return m_count; 588 } 589 590 /** 591 * Get a vertex by index. Used by Distance. 592 * 593 * @param index 594 * @return 595 */ 596 public final Vec2 getVertex(int index) { 597 assert (0 <= index && index < m_count); 598 return m_vertices[index]; 599 } 600 } 601 602 private Simplex simplex = new Simplex(); 603 private int[] saveA = new int[3]; 604 private int[] saveB = new int[3]; 605 private Vec2 closestPoint = new Vec2(); 606 private Vec2 d = new Vec2(); 607 private Vec2 temp = new Vec2(); 608 private Vec2 normal = new Vec2(); 609 610 /** 611 * Compute the closest points between two shapes. Supports any combination of: CircleShape and 612 * PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to 613 * zero. 614 * 615 * @param output 616 * @param cache 617 * @param input 618 */ 619 public final void distance(final DistanceOutput output, final SimplexCache cache, 620 final DistanceInput input) { 621 GJK_CALLS++; 622 623 final DistanceProxy proxyA = input.proxyA; 624 final DistanceProxy proxyB = input.proxyB; 625 626 Transform transformA = input.transformA; 627 Transform transformB = input.transformB; 628 629 // Initialize the simplex. 630 simplex.readCache(cache, proxyA, transformA, proxyB, transformB); 631 632 // Get simplex vertices as an array. 633 SimplexVertex[] vertices = simplex.vertices; 634 635 // These store the vertices of the last simplex so that we 636 // can check for duplicates and prevent cycling. 637 // (pooled above) 638 int saveCount = 0; 639 640 simplex.getClosestPoint(closestPoint); 641 float distanceSqr1 = closestPoint.lengthSquared(); 642 float distanceSqr2 = distanceSqr1; 643 644 // Main iteration loop 645 int iter = 0; 646 while (iter < MAX_ITERS) { 647 648 // Copy simplex so we can identify duplicates. 649 saveCount = simplex.m_count; 650 for (int i = 0; i < saveCount; i++) { 651 saveA[i] = vertices[i].indexA; 652 saveB[i] = vertices[i].indexB; 653 } 654 655 switch (simplex.m_count) { 656 case 1: 657 break; 658 case 2: 659 simplex.solve2(); 660 break; 661 case 3: 662 simplex.solve3(); 663 break; 664 default: 665 assert (false); 666 } 667 668 // If we have 3 points, then the origin is in the corresponding triangle. 669 if (simplex.m_count == 3) { 670 break; 671 } 672 673 // Compute closest point. 674 simplex.getClosestPoint(closestPoint); 675 distanceSqr2 = closestPoint.lengthSquared(); 676 677 // ensure progress 678 if (distanceSqr2 >= distanceSqr1) { 679 // break; 680 } 681 distanceSqr1 = distanceSqr2; 682 683 // get search direction; 684 simplex.getSearchDirection(d); 685 686 // Ensure the search direction is numerically fit. 687 if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON) { 688 // The origin is probably contained by a line segment 689 // or triangle. Thus the shapes are overlapped. 690 691 // We can't return zero here even though there may be overlap. 692 // In case the simplex is a point, segment, or triangle it is difficult 693 // to determine if the origin is contained in the CSO or very close to it. 694 break; 695 } 696 /* 697 * SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA = 698 * proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA, 699 * proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB = 700 * proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB, 701 * proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; 702 */ 703 704 // Compute a tentative new simplex vertex using support points. 705 SimplexVertex vertex = vertices[simplex.m_count]; 706 707 Rot.mulTransUnsafe(transformA.q, d.negateLocal(), temp); 708 vertex.indexA = proxyA.getSupport(temp); 709 Transform.mulToOutUnsafe(transformA, proxyA.getVertex(vertex.indexA), vertex.wA); 710 // Vec2 wBLocal; 711 Rot.mulTransUnsafe(transformB.q, d.negateLocal(), temp); 712 vertex.indexB = proxyB.getSupport(temp); 713 Transform.mulToOutUnsafe(transformB, proxyB.getVertex(vertex.indexB), vertex.wB); 714 vertex.w.set(vertex.wB).subLocal(vertex.wA); 715 716 // Iteration count is equated to the number of support point calls. 717 ++iter; 718 ++GJK_ITERS; 719 720 // Check for duplicate support points. This is the main termination criteria. 721 boolean duplicate = false; 722 for (int i = 0; i < saveCount; ++i) { 723 if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i]) { 724 duplicate = true; 725 break; 726 } 727 } 728 729 // If we found a duplicate support point we must exit to avoid cycling. 730 if (duplicate) { 731 break; 732 } 733 734 // New vertex is ok and needed. 735 ++simplex.m_count; 736 } 737 738 GJK_MAX_ITERS = MathUtils.max(GJK_MAX_ITERS, iter); 739 740 // Prepare output. 741 simplex.getWitnessPoints(output.pointA, output.pointB); 742 output.distance = MathUtils.distance(output.pointA, output.pointB); 743 output.iterations = iter; 744 745 // Cache the simplex. 746 simplex.writeCache(cache); 747 748 // Apply radii if requested. 749 if (input.useRadii) { 750 float rA = proxyA.m_radius; 751 float rB = proxyB.m_radius; 752 753 if (output.distance > rA + rB && output.distance > Settings.EPSILON) { 754 // Shapes are still no overlapped. 755 // Move the witness points to the outer surface. 756 output.distance -= rA + rB; 757 normal.set(output.pointB).subLocal(output.pointA); 758 normal.normalize(); 759 temp.set(normal).mulLocal(rA); 760 output.pointA.addLocal(temp); 761 temp.set(normal).mulLocal(rB); 762 output.pointB.subLocal(temp); 763 } else { 764 // Shapes are overlapped when radii are considered. 765 // Move the witness points to the middle. 766 // Vec2 p = 0.5f * (output.pointA + output.pointB); 767 output.pointA.addLocal(output.pointB).mulLocal(.5f); 768 output.pointB.set(output.pointA); 769 output.distance = 0.0f; 770 } 771 } 772 } 773 } 774