34 #define PLAYRHO_MAGIC(x) (x)
41 #ifdef DEFINE_GET_MANIFOLD
56 using VertexCounterPair = std::pair<VertexCounter, VertexCounter>;
59 GetMostAntiParallelEdge(UnitVec shape0_rel_n0,
const Transformation& xf0,
60 const DistanceProxy& shape1,
const Transformation& xf1,
69 const auto count = shape1.GetVertexCount();
71 const auto prevDot =
Dot(normal, shape1.GetNormal(prevIdx));
72 const auto currDot =
Dot(normal, shape1.GetNormal(firstIdx));
73 return (prevDot < currDot)?
74 std::make_pair(prevIdx, firstIdx):
77 return ((secondIdx > firstIdx) && ((firstIdx + 1) == secondIdx))?
78 std::make_pair(firstIdx, secondIdx): std::make_pair(secondIdx, firstIdx);
82 UnitVec shape0_abs_e0_dir,
83 Length2 shape1_abs_v0,
Length2 shape1_abs_v1, VertexCounterPair shape1_e)
91 const auto shape0_dp_v0_e0 = -
Dot(shape0_abs_e0_dir, shape0_abs_v0);
92 const auto shape0_dp_v1_e0 = +
Dot(shape0_abs_e0_dir, shape0_abs_v1);
94 const auto points =
ClipSegmentToLine(ie, -shape0_abs_e0_dir, shape0_dp_v0_e0, shape0_e.first);
95 return ClipSegmentToLine(points, +shape0_abs_e0_dir, shape0_dp_v1_e0, shape0_e.second);
111 const auto totalRadius =
Length{r0 + r1};
115 const auto shape0_rel_v0 = shape0.
GetVertex(idx0);
116 const auto shape0_rel_v1 = shape0.
GetVertex(idx0Next);
117 const auto shape0_abs_v0 =
Transform(shape0_rel_v0, xf0);
118 const auto shape0_abs_v1 =
Transform(shape0_rel_v1, xf0);
125 const auto shape0_rel_n0 = shape0.
GetNormal(idx0);
127 const auto shape1_e = GetMostAntiParallelEdge(shape0_rel_n0, xf0, shape1, xf1, indices1);
128 const auto shape1_rel_v0 = shape1.
GetVertex(shape1_e.first);
129 const auto shape1_abs_v0 =
Transform(shape1_rel_v0, xf1);
130 const auto shape1_rel_v1 = shape1.
GetVertex(shape1_e.second);
131 const auto shape1_abs_v1 =
Transform(shape1_rel_v1, xf1);
133 const auto shape0_abs_e0_dir =
Rotate(shape0_rel_e0_dir, xf0.
q);
134 const auto clipPoints = GetClipPoints(shape0_abs_v0, shape0_abs_v1, std::make_pair(idx0, idx0Next),
136 shape1_abs_v0, shape1_abs_v1, shape1_e);
137 if (
size(clipPoints) == 2)
140 const auto rel_midpoint = (shape0_rel_v0 + shape0_rel_v1) / 2;
141 const auto abs_offset =
Dot(abs_normal, shape0_abs_v0);
147 for (
auto&& cp: clipPoints)
149 if ((
Dot(abs_normal, cp.v) - abs_offset) <= totalRadius)
158 for (
auto&& cp: clipPoints)
160 if ((
Dot(abs_normal, cp.v) - abs_offset) <= totalRadius)
166 if (manifold.GetPointCount() > 0)
181 const auto totalRadiusSquared =
Square(totalRadius);
191 if (mustUseFaceManifold)
195 shape1_e.first, shape1_rel_v0);
200 if (mustUseFaceManifold)
204 shape1_e.first, shape1_rel_v0);
213 if (mustUseFaceManifold)
217 shape1_e.second, shape1_rel_v1);
221 if (mustUseFaceManifold)
225 shape1_e.second, shape1_rel_v1);
234 if (mustUseFaceManifold)
238 shape1_e.second, shape1_rel_v1);
242 if (mustUseFaceManifold)
246 shape1_e.second, shape1_rel_v1);
255 if (mustUseFaceManifold)
259 shape1_e.first, shape1_rel_v0);
263 if (mustUseFaceManifold)
267 shape1_e.first, shape1_rel_v0);
284 auto indexOfMax = decltype(vertexCount){0};
287 for (
auto i = decltype(vertexCount){0}; i < vertexCount; ++i)
296 if (maxSeparation < s)
303 const auto indexOfMax2 =
GetModuloNext(indexOfMax, vertexCount);
304 assert(maxSeparation <= totalRadius);
307 const auto v1 = shape.
GetVertex(indexOfMax);
308 const auto v2 = shape.
GetVertex(indexOfMax2);
310 if (maxSeparation < 0_m)
312 const auto faceCenter = (v1 + v2) /
Real{2};
326 const auto cLocalV1 = cLocal - v1;
327 if (
Dot(cLocalV1, v2 - v1) <= 0_m2)
341 const auto ClocalV2 = cLocal - v2;
342 if (
Dot(ClocalV2, v1 - v2) <= 0_m2)
357 const auto faceCenter = (v1 + v2) /
Real{2};
358 if (
Dot(cLocal - faceCenter, shape.
GetNormal(indexOfMax)) > totalRadius)
373 Length totalRadius) noexcept
375 const auto pA =
Transform(locationA, xfA);
376 const auto pB =
Transform(locationB, xfB);
379 const auto totSq =
Square(totalRadius);
403 enum:
unsigned { ZeroOneVert = 0x0u, OneVertA = 0x1u, OneVertB = 0x2u };
404 switch (((countA == 1)? OneVertA: ZeroOneVert) | ((countB == 1)? OneVertB: ZeroOneVert))
406 case OneVertA|OneVertB:
414 const auto do4x4 = (countA == 4) && (countB == 4);
416 const auto edgeSepA = do4x4?
419 if (edgeSepA.distance > totalRadius)
424 const auto edgeSepB = do4x4?
427 if (edgeSepB.distance > totalRadius)
433 return (edgeSepB.distance > (edgeSepA.distance + k_tol))?
435 shapeB, xfB, edgeSepB.firstShape,
436 shapeA, xfA, edgeSepB.secondShape,
439 shapeA, xfA, edgeSepA.firstShape,
440 shapeB, xfB, edgeSepA.secondShape,
445 Manifold CollideCached(
const DistanceProxy& shapeA,
const Transformation& xfA,
446 const DistanceProxy& shapeB,
const Transformation& xfB,
455 const auto vertexCountShapeA = shapeA.GetVertexCount();
456 const auto vertexCountShapeB = shapeB.GetVertexCount();
457 if (vertexCountShapeA == 1)
459 if (vertexCountShapeB > 1)
462 shapeA.GetVertex(0), shapeA.GetVertexRadius(), xfA);
464 return CollideShapes(shapeA.GetVertex(0), shapeA.GetVertexRadius(), xfA,
465 shapeB.GetVertex(0), shapeB.GetVertexRadius(), xfB);
467 if (vertexCountShapeB == 1)
469 if (vertexCountShapeA > 1)
472 shapeB.GetVertex(0), shapeB.GetVertexRadius(), xfB);
474 return CollideShapes(shapeA.GetVertex(0), shapeA.GetVertexRadius(), xfA,
475 shapeB.GetVertex(0), shapeB.GetVertexRadius(), xfB);
478 const auto totalRadius = shapeA.GetVertexRadius() + shapeB.GetVertexRadius();
480 IndexPairSeparation edgeSepA;
481 IndexPairSeparation edgeSepB;
483 if (vertexCountShapeA == 4 && vertexCountShapeB == 4)
490 verticesA[0] =
Transform(shapeA.GetVertex(0), xfA);
491 verticesA[1] =
Transform(shapeA.GetVertex(1), xfA);
492 verticesA[2] =
Transform(shapeA.GetVertex(2), xfA);
493 verticesA[3] =
Transform(shapeA.GetVertex(3), xfA);
495 normalsA[0] =
Rotate(shapeA.GetNormal(0), xfA.q);
496 normalsA[1] =
Rotate(shapeA.GetNormal(1), xfA.q);
497 normalsA[2] =
Rotate(shapeA.GetNormal(2), xfA.q);
498 normalsA[3] =
Rotate(shapeA.GetNormal(3), xfA.q);
500 verticesB[0] =
Transform(shapeB.GetVertex(0), xfB);
501 verticesB[1] =
Transform(shapeB.GetVertex(1), xfB);
502 verticesB[2] =
Transform(shapeB.GetVertex(2), xfB);
503 verticesB[3] =
Transform(shapeB.GetVertex(3), xfB);
505 normalsB[0] =
Rotate(shapeB.GetNormal(0), xfB.q);
506 normalsB[1] =
Rotate(shapeB.GetNormal(1), xfB.q);
507 normalsB[2] =
Rotate(shapeB.GetNormal(2), xfB.q);
508 normalsB[3] =
Rotate(shapeB.GetNormal(3), xfB.q);
510 const auto dpA = DistanceProxy{shapeA.GetVertexRadius(), vertexCountShapeA, verticesA, normalsA};
511 const auto dpB = DistanceProxy{shapeB.GetVertexRadius(), vertexCountShapeB, verticesB, normalsB};
513 if (edgeSepA.separation > totalRadius)
518 if (edgeSepB.separation > totalRadius)
526 if (edgeSepA.separation > totalRadius)
531 if (edgeSepB.separation > totalRadius)
538 return (edgeSepB.separation > (edgeSepA.separation + k_tol))?
540 shapeB, xfB, edgeSepB.index1,
541 shapeA, xfA, edgeSepB.index2,
544 shapeA, xfA, edgeSepA.index1,
545 shapeB, xfB, edgeSepA.index2,
550 #ifdef DEFINE_GET_MANIFOLD
551 Manifold
GetManifold(
const DistanceProxy& proxyA,
const Transformation& transformA,
552 const DistanceProxy& proxyB,
const Transformation& transformB)
554 const auto distanceInfo =
Distance(proxyA, transformA, proxyB, transformB);
555 const auto totalRadius = proxyA.GetVertexRadius() + proxyB.GetVertexRadius();
559 if (distance > totalRadius)
565 const auto a_count = proxyA.GetVertexCount();
566 const auto b_count = proxyB.GetVertexCount();
568 index_type a_indices_array[Simplex::MaxEdges];
569 index_type b_indices_array[Simplex::MaxEdges];
570 auto uniqA = std::size_t{0};
571 auto uniqB = std::size_t{0};
573 std::bitset<MaxShapeVertices> a_indices_set;
574 std::bitset<MaxShapeVertices> b_indices_set;
575 for (
auto&& e: distanceInfo.simplex.GetEdges())
577 const auto indexA = e.GetIndexA();
578 if (!a_indices_set[indexA])
580 a_indices_set[indexA] =
true;
581 a_indices_array[uniqA] = indexA;
584 const auto indexB = e.GetIndexB();
585 if (!b_indices_set[indexB])
587 b_indices_set[indexB] =
true;
588 b_indices_array[uniqB] = indexB;
594 assert(uniqA > 0 && uniqB > 0);
596 std::sort(a_indices_array, a_indices_array + uniqA);
597 std::sort(b_indices_array, b_indices_array + uniqB);
605 const auto b_idx0 = GetEdgeIndex(b_indices_array[0], b_indices_array[1], b_count);
608 const auto b_v0 = proxyB.GetVertex(b_idx0);
609 const auto b_v1 = proxyB.GetVertex(b_idx1);
610 const auto lp = (b_v0 + b_v1) /
Real{2};
612 const auto mp0 = Manifold::Point{
613 proxyA.GetVertex(a_indices_array[0]),
625 auto mp0 = Manifold::Point{};
626 auto mp1 = Manifold::Point{};
629 const auto v0 = proxyA.GetVertex(a_indices_array[0]);
630 const auto v1 = proxyA.GetVertex(a_indices_array[1]);
631 const auto lp = (v0 + v1) /
Real{2};
632 const auto count = proxyA.GetVertexCount();
633 if ((a_indices_array[1] - a_indices_array[0]) == 1)
635 mp0.contactFeature.indexA = a_indices_array[0];
636 mp1.contactFeature.indexA = a_indices_array[0];
640 else if (
GetModuloNext(a_indices_array[1], count) == a_indices_array[0])
642 mp0.contactFeature.indexA = a_indices_array[1];
643 mp1.contactFeature.indexA = a_indices_array[1];
657 else if (uniqB < uniqA)
663 const auto a_idx0 = GetEdgeIndex(a_indices_array[0],a_indices_array[1], a_count);
666 const auto a_v0 = proxyA.GetVertex(a_idx0);
667 const auto a_v1 = proxyA.GetVertex(a_idx1);
668 const auto lp = (a_v0 + a_v1) /
Real{2};
670 const auto mp0 = Manifold::Point{
671 proxyB.GetVertex(b_indices_array[0]),
683 auto mp0 = Manifold::Point{};
684 auto mp1 = Manifold::Point{};
687 const auto v0 = proxyB.GetVertex(b_indices_array[0]);
688 const auto v1 = proxyB.GetVertex(b_indices_array[1]);
689 const auto lp = (v0 + v1) /
Real{2};
690 const auto count = proxyB.GetVertexCount();
691 if ((b_indices_array[1] - b_indices_array[0]) == 1)
693 mp0.contactFeature.indexB = b_indices_array[0];
694 mp1.contactFeature.indexB = b_indices_array[0];
698 else if (
GetModuloNext(b_indices_array[1], count) == b_indices_array[0])
700 mp0.contactFeature.indexB = b_indices_array[1];
701 mp1.contactFeature.indexB = b_indices_array[1];
722 proxyB.GetVertex(b_indices_array[0]), b_indices_array[0]);
726 const auto v0 = proxyA.GetVertex(a_indices_array[0]);
727 const auto v1 = proxyA.GetVertex(a_indices_array[1]);
728 const auto lp = (v0 + v1) /
Real{2};
729 const auto count = proxyA.GetVertexCount();
730 auto mp0 = Manifold::Point{};
731 auto mp1 = Manifold::Point{};
733 mp0.contactFeature.indexB = b_indices_array[0];
734 mp0.localPoint = proxyB.GetVertex(mp0.contactFeature.indexB);
736 mp1.contactFeature.indexB = b_indices_array[1];
737 mp1.localPoint = proxyB.GetVertex(mp1.contactFeature.indexB);
738 if ((a_indices_array[1] - a_indices_array[0]) == 1)
741 mp0.contactFeature.indexA = a_indices_array[0];
743 mp1.contactFeature.indexA = a_indices_array[0];
747 if (
GetModuloNext(a_indices_array[1], count) == a_indices_array[0])
750 mp0.contactFeature.indexA = a_indices_array[1];
752 mp1.contactFeature.indexA = a_indices_array[1];
789 if (lhs.localPoint != rhs.localPoint)
793 if (lhs.contactFeature != rhs.contactFeature)
797 if (lhs.normalImpulse != rhs.normalImpulse)
801 if (lhs.tangentImpulse != rhs.tangentImpulse)
810 return !(lhs == rhs);
815 if (lhs.GetType() != rhs.GetType())
819 if (lhs.GetPointCount() != rhs.GetPointCount())
824 switch (lhs.GetType())
829 if (lhs.GetLocalPoint() != rhs.GetLocalPoint())
835 if (lhs.GetLocalPoint() != rhs.GetLocalPoint())
839 if (lhs.GetLocalNormal() != rhs.GetLocalNormal())
845 if (lhs.GetLocalPoint() != rhs.GetLocalPoint())
849 if (lhs.GetLocalNormal() != rhs.GetLocalNormal())
856 const auto count = lhs.GetPointCount();
863 if (lhs.GetPoint(0) != rhs.GetPoint(0))
869 if (lhs.GetPoint(0) != rhs.GetPoint(0))
871 if (lhs.GetPoint(0) != rhs.GetPoint(1))
875 if (lhs.GetPoint(1) != rhs.GetPoint(0))
880 else if (lhs.GetPoint(1) != rhs.GetPoint(1))
892 return !(lhs == rhs);
902 return proxy.GetVertex(index);
905 return proxy.GetVertex(index);
908 return GetInvalid<Length2>();