Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
EPAConvexHullBuilder.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
7// Define to validate the integrity of the hull structure
8//#define JPH_EPA_CONVEX_BUILDER_VALIDATE
9
10// Define to draw the building of the hull for debugging purposes
11//#define JPH_EPA_CONVEX_BUILDER_DRAW
12
14
15#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
18#endif
19
21
24{
25private:
26#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
28 static constexpr Real cDrawScale = 10;
29#endif
30
31public:
32 // Due to the Euler characteristic (https://en.wikipedia.org/wiki/Euler_characteristic) we know that Vertices - Edges + Faces = 2
33 // In our case we only have triangles and they are always fully connected, so each edge is shared exactly between 2 faces: Edges = Faces * 3 / 2
34 // Substituting: Vertices = Faces / 2 + 2 which is approximately Faces / 2.
35 static constexpr int cMaxTriangles = 256;
36 static constexpr int cMaxPoints = cMaxTriangles / 2;
37
38 // Constants
39 static constexpr int cMaxEdgeLength = 128;
40 static constexpr float cMinTriangleArea = 1.0e-10f;
41 static constexpr float cBarycentricEpsilon = 1.0e-3f;
42
43 // Forward declare
44 class Triangle;
45
47 class Edge
48 {
49 public:
53
55 };
56
59
61 class Triangle : public NonCopyable
62 {
63 public:
65 inline Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions);
66
68 inline bool IsFacing(Vec3Arg inPosition) const
69 {
71 return mNormal.Dot(inPosition - mCentroid) > 0.0f;
72 }
73
75 inline bool IsFacingOrigin() const
76 {
78 return mNormal.Dot(mCentroid) < 0.0f;
79 }
80
82 inline const Edge & GetNextEdge(int inIndex) const
83 {
84 return mEdge[(inIndex + 1) % 3];
85 }
86
90 float mClosestLenSq = FLT_MAX;
91 float mLambda[2];
93 bool mClosestPointInterior = false;
94 bool mRemoved = false;
95 bool mInQueue = false;
96#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
97 int mIteration;
98#endif
99 };
100
103 {
104 private:
106 union alignas(Triangle) Block
107 {
108 uint8 mTriangle[sizeof(Triangle)];
109 Block * mNextFree;
110 };
111
113 Block mTriangles[cMaxTriangles];
114 Block * mNextFree = nullptr;
115 int mHighWatermark = 0;
116
117 public:
119 void Clear()
120 {
121 mNextFree = nullptr;
122 mHighWatermark = 0;
123 }
124
126 Triangle * CreateTriangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
127 {
128 Triangle *t;
129 if (mNextFree != nullptr)
130 {
131 // Entry available from the free list
132 t = reinterpret_cast<Triangle *>(&mNextFree->mTriangle);
133 mNextFree = mNextFree->mNextFree;
134 }
135 else
136 {
137 // Allocate from never used before triangle store
138 if (mHighWatermark >= cMaxTriangles)
139 return nullptr; // Buffer full
140 t = reinterpret_cast<Triangle *>(&mTriangles[mHighWatermark].mTriangle);
141 ++mHighWatermark;
142 }
143
144 // Call constructor
145 new (t) Triangle(inIdx0, inIdx1, inIdx2, inPositions);
146
147 return t;
148 }
149
152 {
153 // Destruct triangle
154 inT->~Triangle();
155#ifdef _DEBUG
156 memset(inT, 0xcd, sizeof(Triangle));
157#endif
158
159 // Add triangle to the free list
160 Block *tu = reinterpret_cast<Block *>(inT);
161 tu->mNextFree = mNextFree;
162 mNextFree = tu;
163 }
164 };
165
166 // Typedefs
169
171 class Points : public PointsBase
172 {
173 public:
175 {
176 return mSize;
177 }
178 };
179
182 {
183 public:
185 static bool sTriangleSorter(const Triangle *inT1, const Triangle *inT2)
186 {
187 return inT1->mClosestLenSq > inT2->mClosestLenSq;
188 }
189
192 {
193 // Add to base
194 Triangles::push_back(inT);
195
196 // Mark in queue
197 inT->mInQueue = true;
198
199 // Resort heap
200 std::push_heap(begin(), end(), sTriangleSorter);
201 }
202
205 {
206 return front();
207 }
208
211 {
212 // Move closest to end
213 std::pop_heap(begin(), end(), sTriangleSorter);
214
215 // Remove last triangle
216 Triangle *t = back();
217 pop_back();
218 return t;
219 }
220 };
221
223 explicit EPAConvexHullBuilder(const Points &inPositions) :
224 mPositions(inPositions)
225 {
226#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
227 mIteration = 0;
228 mOffset = RVec3::sZero();
229#endif
230 }
231
233 void Initialize(int inIdx1, int inIdx2, int inIdx3)
234 {
235 // Release triangles
236 mFactory.Clear();
237
238 // Create triangles (back to back)
239 Triangle *t1 = CreateTriangle(inIdx1, inIdx2, inIdx3);
240 Triangle *t2 = CreateTriangle(inIdx1, inIdx3, inIdx2);
241
242 // Link triangles edges
243 sLinkTriangle(t1, 0, t2, 2);
244 sLinkTriangle(t1, 1, t2, 1);
245 sLinkTriangle(t1, 2, t2, 0);
246
247 // Always add both triangles to the priority queue
248 mTriangleQueue.push_back(t1);
249 mTriangleQueue.push_back(t2);
250
251#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
252 // Draw current state
253 DrawState();
254
255 // Increment iteration counter
256 ++mIteration;
257#endif
258 }
259
261 bool HasNextTriangle() const
262 {
263 return !mTriangleQueue.empty();
264 }
265
268 {
269 return mTriangleQueue.PeekClosest();
270 }
271
274 {
275 return mTriangleQueue.PopClosest();
276 }
277
280 Triangle * FindFacingTriangle(Vec3Arg inPosition, float &outBestDistSq)
281 {
282 Triangle *best = nullptr;
283 float best_dist_sq = 0.0f;
284
285 for (Triangle *t : mTriangleQueue)
286 if (!t->mRemoved)
287 {
288 float dot = t->mNormal.Dot(inPosition - t->mCentroid);
289 if (dot > 0.0f)
290 {
291 float dist_sq = dot * dot / t->mNormal.LengthSq();
292 if (dist_sq > best_dist_sq)
293 {
294 best = t;
295 best_dist_sq = dist_sq;
296 }
297 }
298 }
299
300 outBestDistSq = best_dist_sq;
301 return best;
302 }
303
305 bool AddPoint(Triangle *inFacingTriangle, int inIdx, float inClosestDistSq, NewTriangles &outTriangles)
306 {
307 // Get position
308 Vec3 pos = mPositions[inIdx];
309
310#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
311 // Draw new support point
312 DrawMarker(pos, Color::sYellow, 1.0f);
313#endif
314
315#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
316 // Check if structure is intact
317 ValidateTriangles();
318#endif
319
320 // Find edge of convex hull of triangles that are not facing the new vertex w
321 Edges edges;
322 if (!FindEdge(inFacingTriangle, pos, edges))
323 return false;
324
325 // Create new triangles
326 int num_edges = edges.size();
327 for (int i = 0; i < num_edges; ++i)
328 {
329 // Create new triangle
330 Triangle *nt = CreateTriangle(edges[i].mStartIdx, edges[(i + 1) % num_edges].mStartIdx, inIdx);
331 if (nt == nullptr)
332 return false;
333 outTriangles.push_back(nt);
334
335 // Check if we need to put this triangle in the priority queue
336 if ((nt->mClosestPointInterior && nt->mClosestLenSq < inClosestDistSq) // For the main algorithm
337 || nt->mClosestLenSq < 0.0f) // For when the origin is not inside the hull yet
338 mTriangleQueue.push_back(nt);
339 }
340
341 // Link edges
342 for (int i = 0; i < num_edges; ++i)
343 {
344 sLinkTriangle(outTriangles[i], 0, edges[i].mNeighbourTriangle, edges[i].mNeighbourEdge);
345 sLinkTriangle(outTriangles[i], 1, outTriangles[(i + 1) % num_edges], 2);
346 }
347
348#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
349 // Check if structure is intact
350 ValidateTriangles();
351#endif
352
353#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
354 // Draw state of the hull
355 DrawState();
356
357 // Increment iteration counter
358 ++mIteration;
359#endif
360
361 return true;
362 }
363
366 {
367#ifdef JPH_ENABLE_ASSERTS
368 // Make sure that this triangle is not connected
369 JPH_ASSERT(inT->mRemoved);
370 for (const Edge &e : inT->mEdge)
371 JPH_ASSERT(e.mNeighbourTriangle == nullptr);
372#endif
373
374#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
375 // Remove from list of all triangles
376 Triangles::iterator i = std::find(mTriangles.begin(), mTriangles.end(), inT);
377 JPH_ASSERT(i != mTriangles.end());
378 mTriangles.erase(i);
379#endif
380
381 mFactory.FreeTriangle(inT);
382 }
383
384private:
386 Triangle * CreateTriangle(int inIdx1, int inIdx2, int inIdx3)
387 {
388 // Call provider to create triangle
389 Triangle *t = mFactory.CreateTriangle(inIdx1, inIdx2, inIdx3, mPositions.data());
390 if (t == nullptr)
391 return nullptr;
392
393#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
394 // Remember iteration counter
395 t->mIteration = mIteration;
396#endif
397
398#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
399 // Add to list of triangles for debugging purposes
400 mTriangles.push_back(t);
401#endif
402
403 return t;
404 }
405
407 static void sLinkTriangle(Triangle *inT1, int inEdge1, Triangle *inT2, int inEdge2)
408 {
409 JPH_ASSERT(inEdge1 >= 0 && inEdge1 < 3);
410 JPH_ASSERT(inEdge2 >= 0 && inEdge2 < 3);
411 Edge &e1 = inT1->mEdge[inEdge1];
412 Edge &e2 = inT2->mEdge[inEdge2];
413
414 // Check not connected yet
415 JPH_ASSERT(e1.mNeighbourTriangle == nullptr);
416 JPH_ASSERT(e2.mNeighbourTriangle == nullptr);
417
418 // Check vertices match
419 JPH_ASSERT(e1.mStartIdx == inT2->GetNextEdge(inEdge2).mStartIdx);
420 JPH_ASSERT(e2.mStartIdx == inT1->GetNextEdge(inEdge1).mStartIdx);
421
422 // Link up
423 e1.mNeighbourTriangle = inT2;
424 e1.mNeighbourEdge = inEdge2;
425 e2.mNeighbourTriangle = inT1;
426 e2.mNeighbourEdge = inEdge1;
427 }
428
430 void UnlinkTriangle(Triangle *inT)
431 {
432 // Unlink from neighbours
433 for (int i = 0; i < 3; ++i)
434 {
435 Edge &edge = inT->mEdge[i];
436 if (edge.mNeighbourTriangle != nullptr)
437 {
438 Edge &neighbour_edge = edge.mNeighbourTriangle->mEdge[edge.mNeighbourEdge];
439
440 // Validate that neighbour points to us
441 JPH_ASSERT(neighbour_edge.mNeighbourTriangle == inT);
442 JPH_ASSERT(neighbour_edge.mNeighbourEdge == i);
443
444 // Unlink
445 neighbour_edge.mNeighbourTriangle = nullptr;
446 edge.mNeighbourTriangle = nullptr;
447 }
448 }
449
450 // If this triangle is not in the priority queue, we can delete it now
451 if (!inT->mInQueue)
452 FreeTriangle(inT);
453 }
454
457 bool FindEdge(Triangle *inFacingTriangle, Vec3Arg inVertex, Edges &outEdges)
458 {
459 // Assert that we were given an empty array
460 JPH_ASSERT(outEdges.empty());
461
462 // Should start with a facing triangle
463 JPH_ASSERT(inFacingTriangle->IsFacing(inVertex));
464
465 // Flag as removed
466 inFacingTriangle->mRemoved = true;
467
468 // Instead of recursing, we build our own stack with the information we need
469 struct StackEntry
470 {
471 Triangle * mTriangle;
472 int mEdge;
473 int mIter;
474 };
475 StackEntry stack[cMaxEdgeLength];
476 int cur_stack_pos = 0;
477
478 // Start with the triangle / edge provided
479 stack[0].mTriangle = inFacingTriangle;
480 stack[0].mEdge = 0;
481 stack[0].mIter = -1; // Start with edge 0 (is incremented below before use)
482
483 // Next index that we expect to find, if we don't then there are 'islands'
484 int next_expected_start_idx = -1;
485
486 for (;;)
487 {
488 StackEntry &cur_entry = stack[cur_stack_pos];
489
490 // Next iteration
491 if (++cur_entry.mIter >= 3)
492 {
493 // This triangle needs to be removed, unlink it now
494 UnlinkTriangle(cur_entry.mTriangle);
495
496 // Pop from stack
497 if (--cur_stack_pos < 0)
498 break;
499 }
500 else
501 {
502 // Visit neighbour
503 Edge &e = cur_entry.mTriangle->mEdge[(cur_entry.mEdge + cur_entry.mIter) % 3];
504 Triangle *n = e.mNeighbourTriangle;
505 if (n != nullptr && !n->mRemoved)
506 {
507 // Check if vertex is on the front side of this triangle
508 if (n->IsFacing(inVertex))
509 {
510 // Vertex on front, this triangle needs to be removed
511 n->mRemoved = true;
512
513 // Add element to the stack of elements to visit
514 cur_stack_pos++;
515 JPH_ASSERT(cur_stack_pos < cMaxEdgeLength);
516 StackEntry &new_entry = stack[cur_stack_pos];
517 new_entry.mTriangle = n;
518 new_entry.mEdge = e.mNeighbourEdge;
519 new_entry.mIter = 0; // Is incremented before use, we don't need to test this edge again since we came from it
520 }
521 else
522 {
523 // Detect if edge doesn't connect to previous edge, if this happens we have found and 'island' which means
524 // the newly added point is so close to the triangles of the hull that we classified some (nearly) coplanar
525 // triangles as before and some behind the point. At this point we just abort adding the point because
526 // we've reached numerical precision.
527 // Note that we do not need to test if the first and last edge connect, since when there are islands
528 // there should be at least 2 disconnects.
529 if (e.mStartIdx != next_expected_start_idx && next_expected_start_idx != -1)
530 return false;
531
532 // Next expected index is the start index of our neighbour's edge
533 next_expected_start_idx = n->mEdge[e.mNeighbourEdge].mStartIdx;
534
535 // Vertex behind, keep edge
536 outEdges.push_back(e);
537 }
538 }
539 }
540 }
541
542 // Assert that we have a fully connected loop
543 JPH_ASSERT(outEdges.empty() || outEdges[0].mStartIdx == next_expected_start_idx);
544
545#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
546 // Draw edge of facing triangles
547 for (int i = 0; i < (int)outEdges.size(); ++i)
548 {
549 RVec3 edge_start = cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]);
550 DebugRenderer::sInstance->DrawArrow(edge_start, cDrawScale * (mOffset + mPositions[outEdges[(i + 1) % outEdges.size()].mStartIdx]), Color::sYellow, 0.01f);
551 DebugRenderer::sInstance->DrawText3D(edge_start, ConvertToString(outEdges[i].mStartIdx), Color::sWhite);
552 }
553
554 // Draw the state with the facing triangles removed
555 DrawState();
556#endif
557
558 // When we start with two triangles facing away from each other and adding a point that is on the plane,
559 // sometimes we consider the point in front of both causing both triangles to be removed resulting in an empty edge list.
560 // In this case we fail to add the point which will result in no collision reported (the shapes are contacting in 1 point so there's 0 penetration)
561 return outEdges.size() >= 3;
562 }
563
564#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
566 void ValidateTriangle(const Triangle *inT) const
567 {
568 if (inT->mRemoved)
569 {
570 // Validate that removed triangles are not connected to anything
571 for (const Edge &my_edge : inT->mEdge)
572 JPH_ASSERT(my_edge.mNeighbourTriangle == nullptr);
573 }
574 else
575 {
576 for (int i = 0; i < 3; ++i)
577 {
578 const Edge &my_edge = inT->mEdge[i];
579
580 // Assert that we have a neighbour
581 const Triangle *nb = my_edge.mNeighbourTriangle;
582 JPH_ASSERT(nb != nullptr);
583
584 if (nb != nullptr)
585 {
586 // Assert that our neighbours edge points to us
587 const Edge &nb_edge = nb->mEdge[my_edge.mNeighbourEdge];
588 JPH_ASSERT(nb_edge.mNeighbourTriangle == inT);
589 JPH_ASSERT(nb_edge.mNeighbourEdge == i);
590
591 // Assert that the next edge of the neighbour points to the same vertex as this edge's vertex
592 const Edge &nb_next_edge = nb->GetNextEdge(my_edge.mNeighbourEdge);
593 JPH_ASSERT(nb_next_edge.mStartIdx == my_edge.mStartIdx);
594
595 // Assert that my next edge points to the same vertex as my neighbours vertex
596 const Edge &my_next_edge = inT->GetNextEdge(i);
597 JPH_ASSERT(my_next_edge.mStartIdx == nb_edge.mStartIdx);
598 }
599 }
600 }
601 }
602
604 void ValidateTriangles() const
605 {
606 for (const Triangle *t : mTriangles)
607 ValidateTriangle(t);
608 }
609#endif
610
611#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
612public:
614 void DrawState()
615 {
616 // Draw origin
618
619 // Draw triangles
620 for (const Triangle *t : mTriangles)
621 if (!t->mRemoved)
622 {
623 // Calculate the triangle vertices
624 RVec3 p1 = cDrawScale * (mOffset + mPositions[t->mEdge[0].mStartIdx]);
625 RVec3 p2 = cDrawScale * (mOffset + mPositions[t->mEdge[1].mStartIdx]);
626 RVec3 p3 = cDrawScale * (mOffset + mPositions[t->mEdge[2].mStartIdx]);
627
628 // Draw triangle
631
632 // Draw normal
633 RVec3 centroid = cDrawScale * (mOffset + t->mCentroid);
634 float len = t->mNormal.Length();
635 if (len > 0.0f)
636 DebugRenderer::sInstance->DrawArrow(centroid, centroid + t->mNormal / len, Color::sDarkGreen, 0.01f);
637 }
638
639 // Determine max position
640 float min_x = FLT_MAX;
641 float max_x = -FLT_MAX;
642 for (Vec3 p : mPositions)
643 {
644 min_x = min(min_x, p.GetX());
645 max_x = max(max_x, p.GetX());
646 }
647
648 // Offset to the right
649 mOffset += Vec3(max_x - min_x + 0.5f, 0.0f, 0.0f);
650 }
651
653 void DrawLabel(const string_view &inText)
654 {
655 DebugRenderer::sInstance->DrawText3D(cDrawScale * mOffset, inText, Color::sWhite, 0.1f * cDrawScale);
656
657 mOffset += Vec3(5.0f, 0.0f, 0.0f);
658 }
659
661 void DrawGeometry(const DebugRenderer::GeometryRef &inGeometry, ColorArg inColor)
662 {
663 RMat44 origin = RMat44::sScale(Vec3::sReplicate(cDrawScale)) * RMat44::sTranslation(mOffset);
664 DebugRenderer::sInstance->DrawGeometry(origin, inGeometry->mBounds.Transformed(origin), inGeometry->mBounds.GetExtent().LengthSq(), inColor, inGeometry);
665
666 mOffset += Vec3(inGeometry->mBounds.GetSize().GetX(), 0, 0);
667 }
668
670 void DrawWireTriangle(const Triangle &inTriangle, ColorArg inColor)
671 {
672 RVec3 prev = cDrawScale * (mOffset + mPositions[inTriangle.mEdge[2].mStartIdx]);
673 for (const Edge &edge : inTriangle.mEdge)
674 {
675 RVec3 cur = cDrawScale * (mOffset + mPositions[edge.mStartIdx]);
676 DebugRenderer::sInstance->DrawArrow(prev, cur, inColor, 0.01f);
677 prev = cur;
678 }
679 }
680
682 void DrawMarker(Vec3Arg inPosition, ColorArg inColor, float inSize)
683 {
684 DebugRenderer::sInstance->DrawMarker(cDrawScale * (mOffset + inPosition), inColor, inSize);
685 }
686
688 void DrawArrow(Vec3Arg inFrom, Vec3Arg inTo, ColorArg inColor, float inArrowSize)
689 {
690 DebugRenderer::sInstance->DrawArrow(cDrawScale * (mOffset + inFrom), cDrawScale * (mOffset + inTo), inColor, inArrowSize);
691 }
692#endif
693
694private:
695 TriangleFactory mFactory;
696 const Points & mPositions;
697 TriangleQueue mTriangleQueue;
698
699#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
700 Triangles mTriangles;
701#endif
702
703#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
704 int mIteration;
705 RVec3 mOffset;
706#endif
707};
708
709// The determinant that is calculated in the Triangle constructor is really sensitive
710// to numerical round off, disable the fmadd instructions to maintain precision.
711JPH_PRECISE_MATH_ON
712
713EPAConvexHullBuilder::Triangle::Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
714{
715 // Fill in indexes
716 JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2);
717 mEdge[0].mStartIdx = inIdx0;
718 mEdge[1].mStartIdx = inIdx1;
719 mEdge[2].mStartIdx = inIdx2;
720
721 // Clear links
722 mEdge[0].mNeighbourTriangle = nullptr;
723 mEdge[1].mNeighbourTriangle = nullptr;
724 mEdge[2].mNeighbourTriangle = nullptr;
725
726 // Get vertex positions
727 Vec3 y0 = inPositions[inIdx0];
728 Vec3 y1 = inPositions[inIdx1];
729 Vec3 y2 = inPositions[inIdx2];
730
731 // Calculate centroid
732 mCentroid = (y0 + y1 + y2) / 3.0f;
733
734 // Calculate edges
735 Vec3 y10 = y1 - y0;
736 Vec3 y20 = y2 - y0;
737 Vec3 y21 = y2 - y1;
738
739 // The most accurate normal is calculated by using the two shortest edges
740 // See: https://box2d.org/posts/2014/01/troublesome-triangle/
741 // The difference in normals is most pronounced when one edge is much smaller than the others (in which case the other 2 must have roughly the same length).
742 // Therefore we can suffice by just picking the shortest from 2 edges and use that with the 3rd edge to calculate the normal.
743 // We first check which of the edges is shorter.
744 float y20_dot_y20 = y20.Dot(y20);
745 float y21_dot_y21 = y21.Dot(y21);
746 if (y20_dot_y20 < y21_dot_y21)
747 {
748 // We select the edges y10 and y20
749 mNormal = y10.Cross(y20);
750
751 // Check if triangle is degenerate
752 float normal_len_sq = mNormal.LengthSq();
753 if (normal_len_sq > cMinTriangleArea)
754 {
755 // Determine distance between triangle and origin: distance = (centroid - origin) . normal / |normal|
756 // Note that this way of calculating the closest point is much more accurate than first calculating barycentric coordinates and then calculating the closest
757 // point based on those coordinates. Note that we preserve the sign of the distance to check on which side the origin is.
758 float c_dot_n = mCentroid.Dot(mNormal);
759 mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
760
761 // Calculate closest point to origin using barycentric coordinates:
762 //
763 // v = y0 + l0 * (y1 - y0) + l1 * (y2 - y0)
764 // v . (y1 - y0) = 0
765 // v . (y2 - y0) = 0
766 //
767 // Written in matrix form:
768 //
769 // | y10.y10 y20.y10 | | l0 | = | -y0.y10 |
770 // | y10.y20 y20.y20 | | l1 | | -y0.y20 |
771 //
772 // (y10 = y1 - y0 etc.)
773 //
774 // Cramers rule to invert matrix:
775 float y10_dot_y10 = y10.LengthSq();
776 float y10_dot_y20 = y10.Dot(y20);
777 float determinant = y10_dot_y10 * y20_dot_y20 - y10_dot_y20 * y10_dot_y20;
778 if (determinant > 0.0f) // If determinant == 0 then the system is linearly dependent and the triangle is degenerate, since y10.10 * y20.y20 > y10.y20^2 it should also be > 0
779 {
780 float y0_dot_y10 = y0.Dot(y10);
781 float y0_dot_y20 = y0.Dot(y20);
782 float l0 = (y10_dot_y20 * y0_dot_y20 - y20_dot_y20 * y0_dot_y10) / determinant;
783 float l1 = (y10_dot_y20 * y0_dot_y10 - y10_dot_y10 * y0_dot_y20) / determinant;
784 mLambda[0] = l0;
785 mLambda[1] = l1;
786 mLambdaRelativeTo0 = true;
787
788 // Check if closest point is interior to the triangle. For a convex hull which contains the origin each face must contain the origin, but because
789 // our faces are triangles, we can have multiple coplanar triangles and only 1 will have the origin as an interior point. We want to use this triangle
790 // to calculate the contact points because it gives the most accurate results, so we will only add these triangles to the priority queue.
791 if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon)
792 mClosestPointInterior = true;
793 }
794 }
795 }
796 else
797 {
798 // We select the edges y10 and y21
799 mNormal = y10.Cross(y21);
800
801 // Check if triangle is degenerate
802 float normal_len_sq = mNormal.LengthSq();
803 if (normal_len_sq > cMinTriangleArea)
804 {
805 // Again calculate distance between triangle and origin
806 float c_dot_n = mCentroid.Dot(mNormal);
807 mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
808
809 // Calculate closest point to origin using barycentric coordinates but this time using y1 as the reference vertex
810 //
811 // v = y1 + l0 * (y0 - y1) + l1 * (y2 - y1)
812 // v . (y0 - y1) = 0
813 // v . (y2 - y1) = 0
814 //
815 // Written in matrix form:
816 //
817 // | y10.y10 -y21.y10 | | l0 | = | y1.y10 |
818 // | -y10.y21 y21.y21 | | l1 | | -y1.y21 |
819 //
820 // Cramers rule to invert matrix:
821 float y10_dot_y10 = y10.LengthSq();
822 float y10_dot_y21 = y10.Dot(y21);
823 float determinant = y10_dot_y10 * y21_dot_y21 - y10_dot_y21 * y10_dot_y21;
824 if (determinant > 0.0f)
825 {
826 float y1_dot_y10 = y1.Dot(y10);
827 float y1_dot_y21 = y1.Dot(y21);
828 float l0 = (y21_dot_y21 * y1_dot_y10 - y10_dot_y21 * y1_dot_y21) / determinant;
829 float l1 = (y10_dot_y21 * y1_dot_y10 - y10_dot_y10 * y1_dot_y21) / determinant;
830 mLambda[0] = l0;
831 mLambda[1] = l1;
832 mLambdaRelativeTo0 = false;
833
834 // Again check if the closest point is inside the triangle
835 if (l0 > -cBarycentricEpsilon && l1 > -cBarycentricEpsilon && l0 + l1 < 1.0f + cBarycentricEpsilon)
836 mClosestPointInterior = true;
837 }
838 }
839 }
840}
841
842JPH_PRECISE_MATH_OFF
843
std::uint8_t uint8
Definition: Core.h:440
#define JPH_NAMESPACE_END
Definition: Core.h:367
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
#define JPH_ASSERT(...)
Definition: IssueReporting.h:33
float Real
Definition: Real.h:27
String ConvertToString(const T &inValue)
Convert type to string.
Definition: StringTools.h:15
Class that holds an RGBA color with 8-bits per component.
Definition: Color.h:16
static const Color sWhite
Definition: Color.h:64
static Color sGetDistinctColor(int inIndex)
Get a visually distinct color.
Definition: Color.cpp:31
static const Color sDarkGreen
Definition: Color.h:53
static const Color sGrey
Definition: Color.h:62
static const Color sYellow
Definition: Color.h:57
virtual void DrawText3D(RVec3Arg inPosition, const string_view &inString, ColorArg inColor=Color::sWhite, float inHeight=0.5f)=0
Draw text.
void DrawMarker(RVec3Arg inPosition, ColorArg inColor, float inSize)
Draw a marker on a position.
Definition: DebugRenderer.cpp:172
void DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor)
Draw wireframe triangle.
Definition: DebugRenderer.cpp:242
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
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.
Class that holds the information of an edge.
Definition: EPAConvexHullBuilder.h:48
int mStartIdx
Vertex index in mPositions that indicates the start vertex of this edge.
Definition: EPAConvexHullBuilder.h:54
Triangle * mNeighbourTriangle
Information about neighbouring triangle.
Definition: EPAConvexHullBuilder.h:51
int mNeighbourEdge
Index in mEdge that specifies edge that this Edge is connected to.
Definition: EPAConvexHullBuilder.h:52
Specialized points list that allows direct access to the size.
Definition: EPAConvexHullBuilder.h:172
size_type & GetSizeRef()
Definition: EPAConvexHullBuilder.h:174
Factory that creates triangles in a fixed size buffer.
Definition: EPAConvexHullBuilder.h:103
Triangle * CreateTriangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
Allocate a new triangle with 3 indexes.
Definition: EPAConvexHullBuilder.h:126
void FreeTriangle(Triangle *inT)
Free a triangle.
Definition: EPAConvexHullBuilder.h:151
void Clear()
Return all triangles to the free pool.
Definition: EPAConvexHullBuilder.h:119
Class that holds the information of one triangle.
Definition: EPAConvexHullBuilder.h:62
Vec3 mCentroid
Center of the triangle.
Definition: EPAConvexHullBuilder.h:89
bool mClosestPointInterior
Flag that indicates that the closest point from this triangle to the origin is an interior point.
Definition: EPAConvexHullBuilder.h:93
bool IsFacing(Vec3Arg inPosition) const
Check if triangle is facing inPosition.
Definition: EPAConvexHullBuilder.h:68
const Edge & GetNextEdge(int inIndex) const
Get the next edge of edge inIndex.
Definition: EPAConvexHullBuilder.h:82
Vec3 mNormal
Normal of this triangle, length is 2 times area of triangle.
Definition: EPAConvexHullBuilder.h:88
Edge mEdge[3]
3 edges of this triangle
Definition: EPAConvexHullBuilder.h:87
bool IsFacingOrigin() const
Check if triangle is facing the origin.
Definition: EPAConvexHullBuilder.h:75
float mClosestLenSq
Closest distance^2 from origin to triangle.
Definition: EPAConvexHullBuilder.h:90
Triangle(int inIdx0, int inIdx1, int inIdx2, const Vec3 *inPositions)
Constructor.
Definition: EPAConvexHullBuilder.h:713
bool mInQueue
Flag that indicates that this triangle was placed in the sorted heap (stays true after it is popped b...
Definition: EPAConvexHullBuilder.h:95
float mLambda[2]
Barycentric coordinates of closest point to origin on triangle.
Definition: EPAConvexHullBuilder.h:91
bool mLambdaRelativeTo0
How to calculate the closest point, true: y0 + l0 * (y1 - y0) + l1 * (y2 - y0), false: y1 + l0 * (y0 ...
Definition: EPAConvexHullBuilder.h:92
bool mRemoved
Flag that indicates that triangle has been removed.
Definition: EPAConvexHullBuilder.h:94
Specialized triangles list that keeps them sorted on closest distance to origin.
Definition: EPAConvexHullBuilder.h:182
void push_back(Triangle *inT)
Add triangle to the list.
Definition: EPAConvexHullBuilder.h:191
Triangle * PeekClosest()
Peek the next closest triangle without removing it.
Definition: EPAConvexHullBuilder.h:204
Triangle * PopClosest()
Get next closest triangle.
Definition: EPAConvexHullBuilder.h:210
static bool sTriangleSorter(const Triangle *inT1, const Triangle *inT2)
Function to sort triangles on closest distance to origin.
Definition: EPAConvexHullBuilder.h:185
A convex hull builder specifically made for the EPA penetration depth calculation....
Definition: EPAConvexHullBuilder.h:24
Triangle * PeekClosestTriangleInQueue()
Access to the next closest triangle to the origin (won't remove it from the queue).
Definition: EPAConvexHullBuilder.h:267
bool HasNextTriangle() const
Check if there's another triangle to process from the queue.
Definition: EPAConvexHullBuilder.h:261
static constexpr int cMaxTriangles
Max triangles in hull.
Definition: EPAConvexHullBuilder.h:35
StaticArray< Edge, cMaxEdgeLength > Edges
Definition: EPAConvexHullBuilder.h:57
static constexpr float cMinTriangleArea
Minimum area of a triangle before, if smaller than this it will not be added to the priority queue.
Definition: EPAConvexHullBuilder.h:40
static constexpr float cBarycentricEpsilon
Epsilon value used to determine if a point is in the interior of a triangle.
Definition: EPAConvexHullBuilder.h:41
static constexpr int cMaxPoints
Max number of points in hull.
Definition: EPAConvexHullBuilder.h:36
Triangle * PopClosestTriangleFromQueue()
Access to the next closest triangle to the origin and remove it from the queue.
Definition: EPAConvexHullBuilder.h:273
static constexpr int cMaxEdgeLength
Max number of edges in FindEdge.
Definition: EPAConvexHullBuilder.h:39
void FreeTriangle(Triangle *inT)
Free a triangle.
Definition: EPAConvexHullBuilder.h:365
Triangle * FindFacingTriangle(Vec3Arg inPosition, float &outBestDistSq)
Definition: EPAConvexHullBuilder.h:280
void Initialize(int inIdx1, int inIdx2, int inIdx3)
Initialize the hull with 3 points.
Definition: EPAConvexHullBuilder.h:233
bool AddPoint(Triangle *inFacingTriangle, int inIdx, float inClosestDistSq, NewTriangles &outTriangles)
Add a new point to the convex hull.
Definition: EPAConvexHullBuilder.h:305
EPAConvexHullBuilder(const Points &inPositions)
Constructor.
Definition: EPAConvexHullBuilder.h:223
StaticArray< Triangle *, cMaxTriangles > Triangles
Definition: EPAConvexHullBuilder.h:168
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 sScale(float inScale)
Get matrix that scales uniformly.
Definition: Mat44.inl:163
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
Simple variable length array backed by a fixed size buffer.
Definition: StaticArray.h:12
void push_back(const T &inElement)
Add element to the back of the array.
Definition: StaticArray.h:59
T * iterator
Definition: StaticArray.h:124
uint size_type
Definition: StaticArray.h:16
size_type size() const
Returns amount of elements in the array.
Definition: StaticArray.h:87
A simple triangle and its material.
Definition: Triangle.h:11
Definition: Vec3.h:16
JPH_INLINE float Dot(Vec3Arg inV2) const
Dot product.
Definition: Vec3.inl:637
JPH_INLINE Vec3 Cross(Vec3Arg inV2) const
Cross product.
Definition: Vec3.inl:582
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
static JPH_INLINE Vec3 sReplicate(float inV)
Replicate inV across all components.
Definition: Vec3.inl:118