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