16#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
27#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
29 static constexpr Real cDrawScale = 10;
66 inline Triangle(
int inIdx0,
int inIdx1,
int inIdx2,
const Vec3 *inPositions);
85 return mEdge[(inIndex + 1) % 3];
97#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
107 union alignas(Triangle) Block
114 Block mTriangles[cMaxTriangles];
115 Block * mNextFree =
nullptr;
116 int mHighWatermark = 0;
130 if (mNextFree !=
nullptr)
133 t =
reinterpret_cast<Triangle *
>(&mNextFree->mTriangle);
134 mNextFree = mNextFree->mNextFree;
139 if (mHighWatermark >= cMaxTriangles)
141 t =
reinterpret_cast<Triangle *
>(&mTriangles[mHighWatermark].mTriangle);
146 new (t)
Triangle(inIdx0, inIdx1, inIdx2, inPositions);
157 memset(inT, 0xcd,
sizeof(
Triangle));
161 Block *tu =
reinterpret_cast<Block *
>(inT);
162 tu->mNextFree = mNextFree;
195 Triangles::push_back(inT);
225 mPositions(inPositions)
227#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
244 sLinkTriangle(t1, 0, t2, 2);
245 sLinkTriangle(t1, 1, t2, 1);
246 sLinkTriangle(t1, 2, t2, 0);
249 mTriangleQueue.push_back(t1);
250 mTriangleQueue.push_back(t2);
252#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
264 return !mTriangleQueue.empty();
270 return mTriangleQueue.PeekClosest();
276 return mTriangleQueue.PopClosest();
284 float best_dist_sq = 0.0f;
289 float dot = t->
mNormal.
Dot(inPosition - t->mCentroid);
292 float dist_sq = dot * dot / t->mNormal.LengthSq();
293 if (dist_sq > best_dist_sq)
296 best_dist_sq = dist_sq;
301 outBestDistSq = best_dist_sq;
309 Vec3 pos = mPositions[inIdx];
311#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
316#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
323 if (!FindEdge(inFacingTriangle, pos, edges))
327 int num_edges = edges.
size();
328 for (
int i = 0; i < num_edges; ++i)
339 mTriangleQueue.push_back(nt);
343 for (
int i = 0; i < num_edges; ++i)
345 sLinkTriangle(outTriangles[i], 0, edges[i].mNeighbourTriangle, edges[i].mNeighbourEdge);
346 sLinkTriangle(outTriangles[i], 1, outTriangles[(i + 1) % num_edges], 2);
349#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
354#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
368#ifdef JPH_ENABLE_ASSERTS
375#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
382 mFactory.FreeTriangle(inT);
390 Triangle *t = mFactory.CreateTriangle(inIdx1, inIdx2, inIdx3, mPositions.data());
394#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
396 t->mIteration = mIteration;
399#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
401 mTriangles.push_back(t);
408 static void sLinkTriangle(
Triangle *inT1,
int inEdge1,
Triangle *inT2,
int inEdge2)
412 Edge &e1 = inT1->mEdge[inEdge1];
413 Edge &e2 = inT2->mEdge[inEdge2];
420 JPH_ASSERT(e1.mStartIdx == inT2->GetNextEdge(inEdge2).mStartIdx);
421 JPH_ASSERT(e2.mStartIdx == inT1->GetNextEdge(inEdge1).mStartIdx);
424 e1.mNeighbourTriangle = inT2;
425 e1.mNeighbourEdge = inEdge2;
426 e2.mNeighbourTriangle = inT1;
427 e2.mNeighbourEdge = inEdge1;
434 for (
int i = 0; i < 3; ++i)
436 Edge &edge = inT->mEdge[i];
437 if (edge.mNeighbourTriangle !=
nullptr)
439 Edge &neighbour_edge = edge.mNeighbourTriangle->mEdge[edge.mNeighbourEdge];
442 JPH_ASSERT(neighbour_edge.mNeighbourTriangle == inT);
443 JPH_ASSERT(neighbour_edge.mNeighbourEdge == i);
446 neighbour_edge.mNeighbourTriangle =
nullptr;
447 edge.mNeighbourTriangle =
nullptr;
464 JPH_ASSERT(inFacingTriangle->IsFacing(inVertex));
467 inFacingTriangle->mRemoved =
true;
477 int cur_stack_pos = 0;
480 stack[0].mTriangle = inFacingTriangle;
485 int next_expected_start_idx = -1;
489 StackEntry &cur_entry = stack[cur_stack_pos];
492 if (++cur_entry.mIter >= 3)
495 UnlinkTriangle(cur_entry.mTriangle);
498 if (--cur_stack_pos < 0)
504 Edge &e = cur_entry.mTriangle->mEdge[(cur_entry.mEdge + cur_entry.mIter) % 3];
506 if (n !=
nullptr && !n->mRemoved)
509 if (n->IsFacing(inVertex))
517 StackEntry &new_entry = stack[cur_stack_pos];
518 new_entry.mTriangle = n;
519 new_entry.mEdge = e.mNeighbourEdge;
530 if (e.mStartIdx != next_expected_start_idx && next_expected_start_idx != -1)
534 next_expected_start_idx = n->mEdge[e.mNeighbourEdge].mStartIdx;
537 outEdges.push_back(e);
544 JPH_ASSERT(outEdges.empty() || outEdges[0].mStartIdx == next_expected_start_idx);
546#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
548 for (
int i = 0; i < (int)outEdges.size(); ++i)
550 RVec3 edge_start = cDrawScale * (mOffset + mPositions[outEdges[i].mStartIdx]);
562 return outEdges.size() >= 3;
565#ifdef JPH_EPA_CONVEX_BUILDER_VALIDATE
567 void ValidateTriangle(
const Triangle *inT)
const
572 for (
const Edge &my_edge : inT->mEdge)
573 JPH_ASSERT(my_edge.mNeighbourTriangle == nullptr);
577 for (
int i = 0; i < 3; ++i)
579 const Edge &my_edge = inT->mEdge[i];
582 const Triangle *nb = my_edge.mNeighbourTriangle;
588 const Edge &nb_edge = nb->mEdge[my_edge.mNeighbourEdge];
589 JPH_ASSERT(nb_edge.mNeighbourTriangle == inT);
593 const Edge &nb_next_edge = nb->GetNextEdge(my_edge.mNeighbourEdge);
594 JPH_ASSERT(nb_next_edge.mStartIdx == my_edge.mStartIdx);
597 const Edge &my_next_edge = inT->GetNextEdge(i);
598 JPH_ASSERT(my_next_edge.mStartIdx == nb_edge.mStartIdx);
605 void ValidateTriangles()
const
607 for (
const Triangle *t : mTriangles)
612#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
621 for (
const Triangle *t : mTriangles)
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]);
634 RVec3 centroid = cDrawScale * (mOffset + t->mCentroid);
635 float len = t->mNormal.Length();
641 float min_x = FLT_MAX;
642 float max_x = -FLT_MAX;
643 for (
Vec3 p : mPositions)
645 min_x = min(min_x, p.GetX());
646 max_x = max(max_x, p.GetX());
650 mOffset +=
Vec3(max_x - min_x + 0.5f, 0.0f, 0.0f);
654 void DrawLabel(
const string_view &inText)
658 mOffset +=
Vec3(5.0f, 0.0f, 0.0f);
667 mOffset +=
Vec3(inGeometry->mBounds.GetSize().GetX(), 0, 0);
673 RVec3 prev = cDrawScale * (mOffset + mPositions[inTriangle.mEdge[2].mStartIdx]);
674 for (
const Edge &edge : inTriangle.mEdge)
676 RVec3 cur = cDrawScale * (mOffset + mPositions[edge.mStartIdx]);
696 TriangleFactory mFactory;
697 const Points & mPositions;
698 TriangleQueue mTriangleQueue;
700#if defined(JPH_EPA_CONVEX_BUILDER_VALIDATE) || defined(JPH_EPA_CONVEX_BUILDER_DRAW)
704#ifdef JPH_EPA_CONVEX_BUILDER_DRAW
717 JPH_ASSERT(inIdx0 != inIdx1 && inIdx0 != inIdx2 && inIdx1 != inIdx2);
718 mEdge[0].mStartIdx = inIdx0;
719 mEdge[1].mStartIdx = inIdx1;
720 mEdge[2].mStartIdx = inIdx2;
723 mEdge[0].mNeighbourTriangle =
nullptr;
724 mEdge[1].mNeighbourTriangle =
nullptr;
725 mEdge[2].mNeighbourTriangle =
nullptr;
728 Vec3 y0 = inPositions[inIdx0];
729 Vec3 y1 = inPositions[inIdx1];
730 Vec3 y2 = inPositions[inIdx2];
733 mCentroid = (y0 + y1 + y2) / 3.0f;
745 float y20_dot_y20 = y20.
Dot(y20);
746 float y21_dot_y21 = y21.
Dot(y21);
747 if (y20_dot_y20 < y21_dot_y21)
750 mNormal = y10.
Cross(y20);
753 float normal_len_sq = mNormal.
LengthSq();
759 float c_dot_n = mCentroid.Dot(mNormal);
760 mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
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)
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;
787 mLambdaRelativeTo0 =
true;
793 mClosestPointInterior =
true;
800 mNormal = y10.
Cross(y21);
803 float normal_len_sq = mNormal.
LengthSq();
807 float c_dot_n = mCentroid.Dot(mNormal);
808 mClosestLenSq = abs(c_dot_n) * c_dot_n / normal_len_sq;
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)
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;
833 mLambdaRelativeTo0 =
false;
837 mClosestPointInterior =
true;
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
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
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