Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
GJKClosestPoint.h
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
11
12//#define JPH_GJK_DEBUG
13#ifdef JPH_GJK_DEBUG
16#endif
17
19
23{
24private:
36 template <bool LastPointPartOfClosestFeature>
37 bool GetClosest(float inPrevVLenSq, Vec3 &outV, float &outVLenSq, uint32 &outSet) const
38 {
39#ifdef JPH_GJK_DEBUG
40 for (int i = 0; i < mNumPoints; ++i)
41 Trace("y[%d] = [%s], |y[%d]| = %g", i, ConvertToString(mY[i]).c_str(), i, (double)mY[i].Length());
42#endif
43
44 uint32 set;
45 Vec3 v;
46
47 switch (mNumPoints)
48 {
49 case 1:
50 // Single point
51 set = 0b0001;
52 v = mY[0];
53 break;
54
55 case 2:
56 // Line segment
57 v = ClosestPoint::GetClosestPointOnLine(mY[0], mY[1], set);
58 break;
59
60 case 3:
61 // Triangle
62 v = ClosestPoint::GetClosestPointOnTriangle<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], set);
63 break;
64
65 case 4:
66 // Tetrahedron
67 v = ClosestPoint::GetClosestPointOnTetrahedron<LastPointPartOfClosestFeature>(mY[0], mY[1], mY[2], mY[3], set);
68 break;
69
70 default:
71 JPH_ASSERT(false);
72 return false;
73 }
74
75#ifdef JPH_GJK_DEBUG
76 Trace("GetClosest: set = 0b%s, v = [%s], |v| = %g", NibbleToBinary(set), ConvertToString(v).c_str(), (double)v.Length());
77#endif
78
79 float v_len_sq = v.LengthSq();
80 if (v_len_sq < inPrevVLenSq) // Note, comparison order important: If v_len_sq is NaN then this expression will be false so we will return false
81 {
82 // Return closest point
83 outV = v;
84 outVLenSq = v_len_sq;
85 outSet = set;
86 return true;
87 }
88
89 // No better match found
90#ifdef JPH_GJK_DEBUG
91 Trace("New closer point is further away, failed to converge");
92#endif
93 return false;
94 }
95
96 // Get max(|Y_0|^2 .. |Y_n|^2)
97 float GetMaxYLengthSq() const
98 {
99 float y_len_sq = mY[0].LengthSq();
100 for (int i = 1; i < mNumPoints; ++i)
101 y_len_sq = max(y_len_sq, mY[i].LengthSq());
102 return y_len_sq;
103 }
104
105 // Remove points that are not in the set, only updates mY
106 void UpdatePointSetY(uint32 inSet)
107 {
108 int num_points = 0;
109 for (int i = 0; i < mNumPoints; ++i)
110 if ((inSet & (1 << i)) != 0)
111 {
112 mY[num_points] = mY[i];
113 ++num_points;
114 }
115 mNumPoints = num_points;
116 }
117
118 // GCC 11.3 thinks the assignments to mP, mQ and mY below may use uninitialized variables
119 JPH_SUPPRESS_WARNING_PUSH
120 JPH_GCC_SUPPRESS_WARNING("-Wmaybe-uninitialized")
121
122 // Remove points that are not in the set, only updates mP
123 void UpdatePointSetP(uint32 inSet)
124 {
125 int num_points = 0;
126 for (int i = 0; i < mNumPoints; ++i)
127 if ((inSet & (1 << i)) != 0)
128 {
129 mP[num_points] = mP[i];
130 ++num_points;
131 }
132 mNumPoints = num_points;
133 }
134
135 // Remove points that are not in the set, only updates mP and mQ
136 void UpdatePointSetPQ(uint32 inSet)
137 {
138 int num_points = 0;
139 for (int i = 0; i < mNumPoints; ++i)
140 if ((inSet & (1 << i)) != 0)
141 {
142 mP[num_points] = mP[i];
143 mQ[num_points] = mQ[i];
144 ++num_points;
145 }
146 mNumPoints = num_points;
147 }
148
149 // Remove points that are not in the set, updates mY, mP and mQ
150 void UpdatePointSetYPQ(uint32 inSet)
151 {
152 int num_points = 0;
153 for (int i = 0; i < mNumPoints; ++i)
154 if ((inSet & (1 << i)) != 0)
155 {
156 mY[num_points] = mY[i];
157 mP[num_points] = mP[i];
158 mQ[num_points] = mQ[i];
159 ++num_points;
160 }
161 mNumPoints = num_points;
162 }
163
164 JPH_SUPPRESS_WARNING_POP
165
166 // Calculate closest points on A and B
167 void CalculatePointAAndB(Vec3 &outPointA, Vec3 &outPointB) const
168 {
169 switch (mNumPoints)
170 {
171 case 1:
172 outPointA = mP[0];
173 outPointB = mQ[0];
174 break;
175
176 case 2:
177 {
178 float u, v;
179 ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], u, v);
180 outPointA = u * mP[0] + v * mP[1];
181 outPointB = u * mQ[0] + v * mQ[1];
182 }
183 break;
184
185 case 3:
186 {
187 float u, v, w;
188 ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], u, v, w);
189 outPointA = u * mP[0] + v * mP[1] + w * mP[2];
190 outPointB = u * mQ[0] + v * mQ[1] + w * mQ[2];
191 }
192 break;
193
194 case 4:
195 #ifdef _DEBUG
196 memset(&outPointA, 0xcd, sizeof(outPointA));
197 memset(&outPointB, 0xcd, sizeof(outPointB));
198 #endif
199 break;
200 }
201 }
202
203public:
213 template <typename A, typename B>
214 bool Intersects(const A &inA, const B &inB, float inTolerance, Vec3 &ioV)
215 {
216 float tolerance_sq = Square(inTolerance);
217
218 // Reset state
219 mNumPoints = 0;
220
221#ifdef JPH_GJK_DEBUG
222 for (int i = 0; i < 4; ++i)
223 mY[i] = Vec3::sZero();
224#endif
225
226 // Previous length^2 of v
227 float prev_v_len_sq = FLT_MAX;
228
229 for (;;)
230 {
231#ifdef JPH_GJK_DEBUG
232 Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints);
233#endif
234
235 // Get support points for shape A and B in the direction of v
236 Vec3 p = inA.GetSupport(ioV);
237 Vec3 q = inB.GetSupport(-ioV);
238
239 // Get support point of the minkowski sum A - B of v
240 Vec3 w = p - q;
241
242 // If the support point sA-B(v) is in the opposite direction as v, then we have found a separating axis and there is no intersection
243 if (ioV.Dot(w) < 0.0f)
244 {
245 // Separating axis found
246#ifdef JPH_GJK_DEBUG
247 Trace("Separating axis");
248#endif
249 return false;
250 }
251
252 // Store the point for later use
253 mY[mNumPoints] = w;
254 ++mNumPoints;
255
256#ifdef JPH_GJK_DEBUG
257 Trace("w = [%s]", ConvertToString(w).c_str());
258#endif
259
260 // Determine the new closest point
261 float v_len_sq; // Length^2 of v
262 uint32 set; // Set of points that form the new simplex
263 if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
264 return false;
265
266 // If there are 4 points, the origin is inside the tetrahedron and we're done
267 if (set == 0xf)
268 {
269#ifdef JPH_GJK_DEBUG
270 Trace("Full simplex");
271#endif
272 ioV = Vec3::sZero();
273 return true;
274 }
275
276 // If v is very close to zero, we consider this a collision
277 if (v_len_sq <= tolerance_sq)
278 {
279#ifdef JPH_GJK_DEBUG
280 Trace("Distance zero");
281#endif
282 ioV = Vec3::sZero();
283 return true;
284 }
285
286 // If v is very small compared to the length of y, we also consider this a collision
287 if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq())
288 {
289#ifdef JPH_GJK_DEBUG
290 Trace("Machine precision reached");
291#endif
292 ioV = Vec3::sZero();
293 return true;
294 }
295
296 // The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin
297 // Note: This must be done before terminating as converged since the separating axis is -v
298 ioV = -ioV;
299
300 // If the squared length of v is not changing enough, we've converged and there is no collision
301 JPH_ASSERT(prev_v_len_sq >= v_len_sq);
302 if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq)
303 {
304 // v is a separating axis
305#ifdef JPH_GJK_DEBUG
306 Trace("Converged");
307#endif
308 return false;
309 }
310 prev_v_len_sq = v_len_sq;
311
312 // Update the points of the simplex
313 UpdatePointSetY(set);
314 }
315 }
316
333 template <typename A, typename B>
334 float GetClosestPoints(const A &inA, const B &inB, float inTolerance, float inMaxDistSq, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB)
335 {
336 float tolerance_sq = Square(inTolerance);
337
338 // Reset state
339 mNumPoints = 0;
340
341#ifdef JPH_GJK_DEBUG
342 // Generate the hull of the Minkowski difference for visualization
343 MinkowskiDifference diff(inA, inB);
344 mGeometry = DebugRenderer::sInstance->CreateTriangleGeometryForConvex([&diff](Vec3Arg inDirection) { return diff.GetSupport(inDirection); });
345
346 for (int i = 0; i < 4; ++i)
347 {
348 mY[i] = Vec3::sZero();
349 mP[i] = Vec3::sZero();
350 mQ[i] = Vec3::sZero();
351 }
352#endif
353
354 // Length^2 of v
355 float v_len_sq = ioV.LengthSq();
356
357 // Previous length^2 of v
358 float prev_v_len_sq = FLT_MAX;
359
360 for (;;)
361 {
362#ifdef JPH_GJK_DEBUG
363 Trace("v = [%s], num_points = %d", ConvertToString(ioV).c_str(), mNumPoints);
364#endif
365
366 // Get support points for shape A and B in the direction of v
367 Vec3 p = inA.GetSupport(ioV);
368 Vec3 q = inB.GetSupport(-ioV);
369
370 // Get support point of the minkowski sum A - B of v
371 Vec3 w = p - q;
372
373 float dot = ioV.Dot(w);
374
375#ifdef JPH_GJK_DEBUG
376 // Draw -ioV to show the closest point to the origin from the previous simplex
377 DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f);
378
379 // Draw ioV to show where we're probing next
380 DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + ioV, Color::sCyan, 0.05f);
381
382 // Draw w, the support point
383 DebugRenderer::sInstance->DrawArrow(mOffset, mOffset + w, Color::sGreen, 0.05f);
385
386 // Draw the simplex and the Minkowski difference around it
387 DrawState();
388#endif
389
390 // Test if we have a separation of more than inMaxDistSq, in which case we terminate early
391 if (dot < 0.0f && dot * dot > v_len_sq * inMaxDistSq)
392 {
393#ifdef JPH_GJK_DEBUG
394 Trace("Distance bigger than max");
395#endif
396#ifdef _DEBUG
397 memset(&outPointA, 0xcd, sizeof(outPointA));
398 memset(&outPointB, 0xcd, sizeof(outPointB));
399#endif
400 return FLT_MAX;
401 }
402
403 // Store the point for later use
404 mY[mNumPoints] = w;
405 mP[mNumPoints] = p;
406 mQ[mNumPoints] = q;
407 ++mNumPoints;
408
409#ifdef JPH_GJK_DEBUG
410 Trace("w = [%s]", ConvertToString(w).c_str());
411#endif
412
413 uint32 set;
414 if (!GetClosest<true>(prev_v_len_sq, ioV, v_len_sq, set))
415 {
416 --mNumPoints; // Undo add last point
417 break;
418 }
419
420 // If there are 4 points, the origin is inside the tetrahedron and we're done
421 if (set == 0xf)
422 {
423#ifdef JPH_GJK_DEBUG
424 Trace("Full simplex");
425#endif
426 ioV = Vec3::sZero();
427 v_len_sq = 0.0f;
428 break;
429 }
430
431 // Update the points of the simplex
432 UpdatePointSetYPQ(set);
433
434 // If v is very close to zero, we consider this a collision
435 if (v_len_sq <= tolerance_sq)
436 {
437#ifdef JPH_GJK_DEBUG
438 Trace("Distance zero");
439#endif
440 ioV = Vec3::sZero();
441 v_len_sq = 0.0f;
442 break;
443 }
444
445 // If v is very small compared to the length of y, we also consider this a collision
446#ifdef JPH_GJK_DEBUG
447 Trace("Check v small compared to y: %g <= %g", (double)v_len_sq, (double)(FLT_EPSILON * GetMaxYLengthSq()));
448#endif
449 if (v_len_sq <= FLT_EPSILON * GetMaxYLengthSq())
450 {
451#ifdef JPH_GJK_DEBUG
452 Trace("Machine precision reached");
453#endif
454 ioV = Vec3::sZero();
455 v_len_sq = 0.0f;
456 break;
457 }
458
459 // The next separation axis to test is the negative of the closest point of the Minkowski sum to the origin
460 // Note: This must be done before terminating as converged since the separating axis is -v
461 ioV = -ioV;
462
463 // If the squared length of v is not changing enough, we've converged and there is no collision
464#ifdef JPH_GJK_DEBUG
465 Trace("Check v not changing enough: %g <= %g", (double)(prev_v_len_sq - v_len_sq), (double)(FLT_EPSILON * prev_v_len_sq));
466#endif
467 JPH_ASSERT(prev_v_len_sq >= v_len_sq);
468 if (prev_v_len_sq - v_len_sq <= FLT_EPSILON * prev_v_len_sq)
469 {
470 // v is a separating axis
471#ifdef JPH_GJK_DEBUG
472 Trace("Converged");
473#endif
474 break;
475 }
476 prev_v_len_sq = v_len_sq;
477 }
478
479 // Get the closest points
480 CalculatePointAAndB(outPointA, outPointB);
481
482#ifdef JPH_GJK_DEBUG
483 Trace("Return: v = [%s], |v| = %g", ConvertToString(ioV).c_str(), (double)ioV.Length());
484
485 // Draw -ioV to show the closest point to the origin from the previous simplex
486 DebugRenderer::sInstance->DrawArrow(mOffset, mOffset - ioV, Color::sOrange, 0.05f);
487
488 // Draw the closest points
489 DebugRenderer::sInstance->DrawMarker(mOffset + outPointA, Color::sGreen, 1.0f);
490 DebugRenderer::sInstance->DrawMarker(mOffset + outPointB, Color::sPurple, 1.0f);
491
492 // Draw the simplex and the Minkowski difference around it
493 DrawState();
494#endif
495
496 JPH_ASSERT(ioV.LengthSq() == v_len_sq);
497 return v_len_sq;
498 }
499
502 void GetClosestPointsSimplex(Vec3 *outY, Vec3 *outP, Vec3 *outQ, uint &outNumPoints) const
503 {
504 uint size = sizeof(Vec3) * mNumPoints;
505 memcpy(outY, mY, size);
506 memcpy(outP, mP, size);
507 memcpy(outQ, mQ, size);
508 outNumPoints = mNumPoints;
509 }
510
522 template <typename A>
523 bool CastRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inTolerance, const A &inA, float &ioLambda)
524 {
525 float tolerance_sq = Square(inTolerance);
526
527 // Reset state
528 mNumPoints = 0;
529
530 float lambda = 0.0f;
531 Vec3 x = inRayOrigin;
532 Vec3 v = x - inA.GetSupport(Vec3::sZero());
533 float v_len_sq = FLT_MAX;
534 bool allow_restart = false;
535
536 for (;;)
537 {
538#ifdef JPH_GJK_DEBUG
539 Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints);
540#endif
541
542 // Get new support point
543 Vec3 p = inA.GetSupport(v);
544 Vec3 w = x - p;
545
546#ifdef JPH_GJK_DEBUG
547 Trace("w = [%s]", ConvertToString(w).c_str());
548#endif
549
550 float v_dot_w = v.Dot(w);
551#ifdef JPH_GJK_DEBUG
552 Trace("v . w = %g", (double)v_dot_w);
553#endif
554 if (v_dot_w > 0.0f)
555 {
556 // If ray and normal are in the same direction, we've passed A and there's no collision
557 float v_dot_r = v.Dot(inRayDirection);
558#ifdef JPH_GJK_DEBUG
559 Trace("v . r = %g", (double)v_dot_r);
560#endif
561 if (v_dot_r >= 0.0f)
562 return false;
563
564 // Update the lower bound for lambda
565 float delta = v_dot_w / v_dot_r;
566 float old_lambda = lambda;
567 lambda -= delta;
568#ifdef JPH_GJK_DEBUG
569 Trace("lambda = %g, delta = %g", (double)lambda, (double)delta);
570#endif
571
572 // If lambda didn't change, we cannot converge any further and we assume a hit
573 if (old_lambda == lambda)
574 break;
575
576 // If lambda is bigger or equal than max, we don't have a hit
577 if (lambda >= ioLambda)
578 return false;
579
580 // Update x to new closest point on the ray
581 x = inRayOrigin + lambda * inRayDirection;
582
583 // We've shifted x, so reset v_len_sq so that it is not used as early out for GetClosest
584 v_len_sq = FLT_MAX;
585
586 // We allow rebuilding the simplex once after x changes because the simplex was built
587 // for another x and numerical round off builds up as you keep adding points to an
588 // existing simplex
589 allow_restart = true;
590 }
591
592 // Add p to set P: P = P U {p}
593 mP[mNumPoints] = p;
594 ++mNumPoints;
595
596 // Calculate Y = {x} - P
597 for (int i = 0; i < mNumPoints; ++i)
598 mY[i] = x - mP[i];
599
600 // Determine the new closest point from Y to origin
601 uint32 set; // Set of points that form the new simplex
602 if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
603 {
604#ifdef JPH_GJK_DEBUG
605 Trace("Failed to converge");
606#endif
607
608 // Only allow 1 restart, if we still can't get a closest point
609 // we're so close that we return this as a hit
610 if (!allow_restart)
611 break;
612
613 // If we fail to converge, we start again with the last point as simplex
614#ifdef JPH_GJK_DEBUG
615 Trace("Restarting");
616#endif
617 allow_restart = false;
618 mP[0] = p;
619 mNumPoints = 1;
620 v = x - p;
621 v_len_sq = FLT_MAX;
622 continue;
623 }
624 else if (set == 0xf)
625 {
626#ifdef JPH_GJK_DEBUG
627 Trace("Full simplex");
628#endif
629
630 // We're inside the tetrahedron, we have a hit (verify that length of v is 0)
631 JPH_ASSERT(v_len_sq == 0.0f);
632 break;
633 }
634
635 // Update the points P to form the new simplex
636 // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
637 UpdatePointSetP(set);
638
639 // Check if x is close enough to inA
640 if (v_len_sq <= tolerance_sq)
641 {
642#ifdef JPH_GJK_DEBUG
643 Trace("Converged");
644#endif
645 break;
646 }
647 }
648
649 // Store hit fraction
650 ioLambda = lambda;
651 return true;
652 }
653
664 template <typename A, typename B>
665 bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float &ioLambda)
666 {
667 // Transform the shape to be cast to the starting position
668 TransformedConvexObject transformed_a(inStart, inA);
669
670 // Calculate the minkowski difference inB - inA
671 // inA is moving, so we need to add the back side of inB to the front side of inA
672 MinkowskiDifference difference(inB, transformed_a);
673
674 // Do a raycast against the Minkowski difference
675 return CastRay(Vec3::sZero(), inDirection, inTolerance, difference, ioLambda);
676 }
677
695 template <typename A, typename B>
696 bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outSeparatingAxis)
697 {
698 float tolerance_sq = Square(inTolerance);
699
700 // Calculate how close A and B (without their convex radius) need to be to each other in order for us to consider this a collision
701 float sum_convex_radius = inConvexRadiusA + inConvexRadiusB;
702
703 // Transform the shape to be cast to the starting position
704 TransformedConvexObject transformed_a(inStart, inA);
705
706 // Reset state
707 mNumPoints = 0;
708
709 float lambda = 0.0f;
710 Vec3 x = Vec3::sZero(); // Since A is already transformed we can start the cast from zero
711 Vec3 v = -inB.GetSupport(Vec3::sZero()) + transformed_a.GetSupport(Vec3::sZero()); // See CastRay: v = x - inA.GetSupport(Vec3::sZero()) where inA is the Minkowski difference inB - transformed_a (see CastShape above) and x is zero
712 float v_len_sq = FLT_MAX;
713 bool allow_restart = false;
714
715 // Keeps track of separating axis of the previous iteration.
716 // Initialized at zero as we don't know if our first v is actually a separating axis.
717 Vec3 prev_v = Vec3::sZero();
718
719 for (;;)
720 {
721#ifdef JPH_GJK_DEBUG
722 Trace("v = [%s], num_points = %d", ConvertToString(v).c_str(), mNumPoints);
723#endif
724
725 // Calculate the minkowski difference inB - inA
726 // inA is moving, so we need to add the back side of inB to the front side of inA
727 // Keep the support points on A and B separate so that in the end we can calculate a contact point
728 Vec3 p = transformed_a.GetSupport(-v);
729 Vec3 q = inB.GetSupport(v);
730 Vec3 w = x - (q - p);
731
732#ifdef JPH_GJK_DEBUG
733 Trace("w = [%s]", ConvertToString(w).c_str());
734#endif
735
736 // Difference from article to this code:
737 // We did not include the convex radius in p and q in order to be able to calculate a good separating axis at the end of the algorithm.
738 // However when moving forward along inDirection we do need to take this into account so that we keep A and B separated by the sum of their convex radii.
739 // From p we have to subtract: inConvexRadiusA * v / |v|
740 // To q we have to add: inConvexRadiusB * v / |v|
741 // This means that to w we have to add: -(inConvexRadiusA + inConvexRadiusB) * v / |v|
742 // So to v . w we have to add: v . (-(inConvexRadiusA + inConvexRadiusB) * v / |v|) = -(inConvexRadiusA + inConvexRadiusB) * |v|
743 float v_dot_w = v.Dot(w) - sum_convex_radius * v.Length();
744#ifdef JPH_GJK_DEBUG
745 Trace("v . w = %g", (double)v_dot_w);
746#endif
747 if (v_dot_w > 0.0f)
748 {
749 // If ray and normal are in the same direction, we've passed A and there's no collision
750 float v_dot_r = v.Dot(inDirection);
751#ifdef JPH_GJK_DEBUG
752 Trace("v . r = %g", (double)v_dot_r);
753#endif
754 if (v_dot_r >= 0.0f)
755 return false;
756
757 // Update the lower bound for lambda
758 float delta = v_dot_w / v_dot_r;
759 float old_lambda = lambda;
760 lambda -= delta;
761#ifdef JPH_GJK_DEBUG
762 Trace("lambda = %g, delta = %g", (double)lambda, (double)delta);
763#endif
764
765 // If lambda didn't change, we cannot converge any further and we assume a hit
766 if (old_lambda == lambda)
767 break;
768
769 // If lambda is bigger or equal than max, we don't have a hit
770 if (lambda >= ioLambda)
771 return false;
772
773 // Update x to new closest point on the ray
774 x = lambda * inDirection;
775
776 // We've shifted x, so reset v_len_sq so that it is not used as early out when GetClosest returns false
777 v_len_sq = FLT_MAX;
778
779 // Now that we've moved, we know that A and B are not intersecting at lambda = 0, so we can update our tolerance to stop iterating
780 // as soon as A and B are inConvexRadiusA + inConvexRadiusB apart
781 tolerance_sq = Square(inTolerance + sum_convex_radius);
782
783 // We allow rebuilding the simplex once after x changes because the simplex was built
784 // for another x and numerical round off builds up as you keep adding points to an
785 // existing simplex
786 allow_restart = true;
787 }
788
789 // Add p to set P, q to set Q: P = P U {p}, Q = Q U {q}
790 mP[mNumPoints] = p;
791 mQ[mNumPoints] = q;
792 ++mNumPoints;
793
794 // Calculate Y = {x} - (Q - P)
795 for (int i = 0; i < mNumPoints; ++i)
796 mY[i] = x - (mQ[i] - mP[i]);
797
798 // Determine the new closest point from Y to origin
799 uint32 set; // Set of points that form the new simplex
800 if (!GetClosest<false>(v_len_sq, v, v_len_sq, set))
801 {
802#ifdef JPH_GJK_DEBUG
803 Trace("Failed to converge");
804#endif
805
806 // Only allow 1 restart, if we still can't get a closest point
807 // we're so close that we return this as a hit
808 if (!allow_restart)
809 break;
810
811 // If we fail to converge, we start again with the last point as simplex
812#ifdef JPH_GJK_DEBUG
813 Trace("Restarting");
814#endif
815 allow_restart = false;
816 mP[0] = p;
817 mQ[0] = q;
818 mNumPoints = 1;
819 v = x - q;
820 v_len_sq = FLT_MAX;
821 continue;
822 }
823 else if (set == 0xf)
824 {
825#ifdef JPH_GJK_DEBUG
826 Trace("Full simplex");
827#endif
828
829 // We're inside the tetrahedron, we have a hit (verify that length of v is 0)
830 JPH_ASSERT(v_len_sq == 0.0f);
831 break;
832 }
833
834 // Update the points P and Q to form the new simplex
835 // Note: We're not updating Y as Y will shift with x so we have to calculate it every iteration
836 UpdatePointSetPQ(set);
837
838 // Check if A and B are touching according to our tolerance
839 if (v_len_sq <= tolerance_sq)
840 {
841#ifdef JPH_GJK_DEBUG
842 Trace("Converged");
843#endif
844 break;
845 }
846
847 // Store our v to return as separating axis
848 prev_v = v;
849 }
850
851 // Calculate Y = {x} - (Q - P) again so we can calculate the contact points
852 for (int i = 0; i < mNumPoints; ++i)
853 mY[i] = x - (mQ[i] - mP[i]);
854
855 // Calculate the offset we need to apply to A and B to correct for the convex radius
856 Vec3 normalized_v = v.NormalizedOr(Vec3::sZero());
857 Vec3 convex_radius_a = inConvexRadiusA * normalized_v;
858 Vec3 convex_radius_b = inConvexRadiusB * normalized_v;
859
860 // Get the contact point
861 // Note that A and B will coincide when lambda > 0. In this case we calculate only B as it is more accurate as it contains less terms.
862 switch (mNumPoints)
863 {
864 case 1:
865 outPointB = mQ[0] + convex_radius_b;
866 outPointA = lambda > 0.0f? outPointB : mP[0] - convex_radius_a;
867 break;
868
869 case 2:
870 {
871 float bu, bv;
872 ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], bu, bv);
873 outPointB = bu * mQ[0] + bv * mQ[1] + convex_radius_b;
874 outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] - convex_radius_a;
875 }
876 break;
877
878 case 3:
879 case 4: // A full simplex, we can't properly determine a contact point! As contact point we take the closest point of the previous iteration.
880 {
881 float bu, bv, bw;
882 ClosestPoint::GetBaryCentricCoordinates(mY[0], mY[1], mY[2], bu, bv, bw);
883 outPointB = bu * mQ[0] + bv * mQ[1] + bw * mQ[2] + convex_radius_b;
884 outPointA = lambda > 0.0f? outPointB : bu * mP[0] + bv * mP[1] + bw * mP[2] - convex_radius_a;
885 }
886 break;
887 }
888
889 // Store separating axis, in case we have a convex radius we can just return v,
890 // otherwise v will be very small and we resort to returning previous v as an approximation.
891 outSeparatingAxis = sum_convex_radius > 0.0f? -v : -prev_v;
892
893 // Store hit fraction
894 ioLambda = lambda;
895 return true;
896 }
897
898private:
899#ifdef JPH_GJK_DEBUG
901 void DrawState()
902 {
903 RMat44 origin = RMat44::sTranslation(mOffset);
904
905 // Draw origin
907
908 // Draw the hull
909 DebugRenderer::sInstance->DrawGeometry(origin, mGeometry->mBounds.Transformed(origin), mGeometry->mBounds.GetExtent().LengthSq(), Color::sYellow, mGeometry);
910
911 // Draw Y
912 for (int i = 0; i < mNumPoints; ++i)
913 {
914 // Draw support point
915 RVec3 y_i = origin * mY[i];
917 for (int j = i + 1; j < mNumPoints; ++j)
918 {
919 // Draw edge
920 RVec3 y_j = origin * mY[j];
922 for (int k = j + 1; k < mNumPoints; ++k)
923 {
924 // Make sure triangle faces the origin
925 RVec3 y_k = origin * mY[k];
926 RVec3 center = (y_i + y_j + y_k) / Real(3);
927 RVec3 normal = (y_j - y_i).Cross(y_k - y_i);
928 if (normal.Dot(center) < Real(0))
929 DebugRenderer::sInstance->DrawTriangle(y_i, y_j, y_k, Color::sLightGrey);
930 else
932 }
933 }
934 }
935
936 // Offset to the right
937 mOffset += Vec3(mGeometry->mBounds.GetSize().GetX() + 2.0f, 0, 0);
938 }
939#endif // JPH_GJK_DEBUG
940
941 Vec3 mY[4];
942 Vec3 mP[4];
943 Vec3 mQ[4];
944 int mNumPoints = 0;
945
946#ifdef JPH_GJK_DEBUG
948 RVec3 mOffset = RVec3::sZero();
949#endif
950};
951
#define JPH_GCC_SUPPRESS_WARNING(w)
Definition: Core.h:264
unsigned int uint
Definition: Core.h:439
#define JPH_NAMESPACE_END
Definition: Core.h:367
std::uint32_t uint32
Definition: Core.h:442
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
TraceFunction Trace
Definition: IssueReporting.cpp:18
#define JPH_ASSERT(...)
Definition: IssueReporting.h:33
constexpr T Square(T inV)
Square a value.
Definition: Math.h:52
float Real
Definition: Real.h:27
const char * NibbleToBinary(uint32 inNibble)
Converts the lower 4 bits of inNibble to a string that represents the number in binary format.
Definition: StringTools.cpp:95
String ConvertToString(const T &inValue)
Convert type to string.
Definition: StringTools.h:15
static const Color sPurple
Definition: Color.h:58
static const Color sGreen
Definition: Color.h:54
static const Color sLightGrey
Definition: Color.h:63
static const Color sOrange
Definition: Color.h:60
static const Color sCyan
Definition: Color.h:59
static const Color sRed
Definition: Color.h:52
static const Color sYellow
Definition: Color.h:57
GeometryRef CreateTriangleGeometryForConvex(SupportFunction inGetSupport)
Definition: DebugRenderer.cpp:688
void DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize)
Draw a marker on a position.
Definition: DebugRenderer.cpp:172
void DrawCoordinateSystem(RMat44Arg inTransform, float inSize=1.0f)
Draw coordinate system (3 arrows, x = red, y = green, z = blue)
Definition: DebugRenderer.cpp:206
virtual void DrawGeometry(RMat44Arg inModelMatrix, const AABox &inWorldSpaceBounds, float inLODScaleSq, ColorArg inModelColor, const GeometryRef &inGeometry, ECullMode inCullMode=ECullMode::CullBackFace, ECastShadow inCastShadow=ECastShadow::On, EDrawMode inDrawMode=EDrawMode::Solid)=0
static DebugRenderer * sInstance
Singleton instance.
Definition: DebugRenderer.h:168
virtual void DrawLine(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor)=0
Draw line.
void DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize)
Draw an arrow.
Definition: DebugRenderer.cpp:184
virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow=ECastShadow::Off)=0
Draw a single back face culled triangle.
Definition: GJKClosestPoint.h:23
bool Intersects(const A &inA, const B &inB, float inTolerance, Vec3 &ioV)
Definition: GJKClosestPoint.h:214
bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float &ioLambda)
Definition: GJKClosestPoint.h:665
void GetClosestPointsSimplex(Vec3 *outY, Vec3 *outP, Vec3 *outQ, uint &outNumPoints) const
Definition: GJKClosestPoint.h:502
bool CastShape(Mat44Arg inStart, Vec3Arg inDirection, float inTolerance, const A &inA, const B &inB, float inConvexRadiusA, float inConvexRadiusB, float &ioLambda, Vec3 &outPointA, Vec3 &outPointB, Vec3 &outSeparatingAxis)
Definition: GJKClosestPoint.h:696
float GetClosestPoints(const A &inA, const B &inB, float inTolerance, float inMaxDistSq, Vec3 &ioV, Vec3 &outPointA, Vec3 &outPointB)
Definition: GJKClosestPoint.h:334
bool CastRay(Vec3Arg inRayOrigin, Vec3Arg inRayDirection, float inTolerance, const A &inA, float &ioLambda)
Definition: GJKClosestPoint.h:523
Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
Definition: Mat44.h:13
static JPH_INLINE Mat44 sTranslation(Vec3Arg inV)
Get matrix that translates.
Definition: Mat44.inl:144
Class that makes another class non-copyable. Usage: Inherit from NonCopyable.
Definition: NonCopyable.h:11
Definition: Vec3.h:16
JPH_INLINE float Dot(Vec3Arg inV2) const
Dot product.
Definition: Vec3.inl:637
JPH_INLINE float Length() const
Length of vector.
Definition: Vec3.inl:669
JPH_INLINE Vec3 NormalizedOr(Vec3Arg inZeroValue) const
Normalize vector or return inZeroValue if the length of the vector is zero.
Definition: Vec3.inl:708
JPH_INLINE float LengthSq() const
Squared length of vector.
Definition: Vec3.inl:653
static JPH_INLINE Vec3 sZero()
Vector with all zeros.
Definition: Vec3.inl:107
bool GetBaryCentricCoordinates(Vec3Arg inA, Vec3Arg inB, float &outU, float &outV)
Definition: ClosestPoint.h:18
Vec3 GetClosestPointOnLine(Vec3Arg inA, Vec3Arg inB, uint32 &outSet)
Definition: ClosestPoint.h:132
Structure that performs a Minkowski difference A - B.
Definition: ConvexSupport.h:67
Vec3 GetSupport(Vec3Arg inDirection) const
Calculate the support vector for this convex shape.
Definition: ConvexSupport.h:75
Definition: ConvexSupport.h:15
Vec3 GetSupport(Vec3Arg inDirection) const
Calculate the support vector for this convex shape.
Definition: ConvexSupport.h:24