Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
InternalEdgeRemovingCollector.h
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
9
10//#define JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
11
12#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
14#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
15
17
24{
25 static constexpr uint cMaxDelayedResults = 16;
26 static constexpr uint cMaxVoidedFeatures = 128;
27
29 inline bool IsVoided(const SubShapeID &inSubShapeID, Vec3 inV) const
30 {
31 for (const Voided &vf : mVoidedFeatures)
32 if (vf.mSubShapeID == inSubShapeID
33 && inV.IsClose(Vec3::sLoadFloat3Unsafe(vf.mFeature), 1.0e-8f))
34 return true;
35 return false;
36 }
37
39 inline void VoidFeatures(const CollideShapeResult &inResult)
40 {
41 if (mVoidedFeatures.size() < cMaxVoidedFeatures)
42 for (const Vec3 &v : inResult.mShape2Face)
43 if (!IsVoided(inResult.mSubShapeID1, v))
44 {
45 Voided vf;
46 v.StoreFloat3(&vf.mFeature);
47 vf.mSubShapeID = inResult.mSubShapeID1;
48 mVoidedFeatures.push_back(vf);
49 if (mVoidedFeatures.size() == cMaxVoidedFeatures)
50 break;
51 }
52 }
53
55 inline void Chain(const CollideShapeResult &inResult)
56 {
57 // Make sure the chained collector has the same context as we do
58 mChainedCollector.SetContext(GetContext());
59
60 // Forward the hit
61 mChainedCollector.AddHit(inResult);
62
63 // If our chained collector updated its early out fraction, we need to follow
65 }
66
68 inline void ChainAndVoid(const CollideShapeResult &inResult)
69 {
70 Chain(inResult);
71 VoidFeatures(inResult);
72
73 #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
76 #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
77 }
78
79public:
82 mChainedCollector(inChainedCollector)
83 {
84 }
85
86 // See: CollideShapeCollector::Reset
87 virtual void Reset() override
88 {
90
91 mChainedCollector.Reset();
92
93 mVoidedFeatures.clear();
94 mDelayedResults.clear();
95 }
96
97 // See: CollideShapeCollector::OnBody
98 virtual void OnBody(const Body &inBody) override
99 {
100 // Just forward the call to our chained collector
101 mChainedCollector.OnBody(inBody);
102 }
103
104 // See: CollideShapeCollector::AddHit
105 virtual void AddHit(const CollideShapeResult &inResult) override
106 {
107 // We only support welding when the shape is a triangle or has more vertices so that we can calculate a normal
108 if (inResult.mShape2Face.size() < 3)
109 return ChainAndVoid(inResult);
110
111 // Get the triangle normal of shape 2 face
112 Vec3 triangle_normal = (inResult.mShape2Face[1] - inResult.mShape2Face[0]).Cross(inResult.mShape2Face[2] - inResult.mShape2Face[0]);
113 float triangle_normal_len = triangle_normal.Length();
114 if (triangle_normal_len < 1e-6f)
115 return ChainAndVoid(inResult);
116
117 // If the triangle normal matches the contact normal within 1 degree, we can process the contact immediately
118 // We make the assumption here that if the contact normal and the triangle normal align that the we're dealing with a 'face contact'
119 Vec3 contact_normal = -inResult.mPenetrationAxis;
120 float contact_normal_len = inResult.mPenetrationAxis.Length();
121 if (triangle_normal.Dot(contact_normal) > 0.999848f * contact_normal_len * triangle_normal_len) // cos(1 degree)
122 return ChainAndVoid(inResult);
123
124 // Delayed processing
125 if (mDelayedResults.size() == cMaxDelayedResults)
126 return ChainAndVoid(inResult);
127 mDelayedResults.push_back(inResult);
128 }
129
131 void Flush()
132 {
133 // Sort on biggest penetration depth first
134 uint sorted_indices[cMaxDelayedResults];
135 for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
136 sorted_indices[i] = i;
137 QuickSort(sorted_indices, sorted_indices + mDelayedResults.size(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; });
138
139 // Loop over all results
140 for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
141 {
142 const CollideShapeResult &r = mDelayedResults[sorted_indices[i]];
143
144 // Determine which vertex or which edge is the closest to the contact point
145 float best_dist_sq = FLT_MAX;
146 uint best_v1_idx = 0;
147 uint best_v2_idx = 0;
148 uint num_v = uint(r.mShape2Face.size());
149 uint v1_idx = num_v - 1;
150 Vec3 v1 = r.mShape2Face[v1_idx] - r.mContactPointOn2;
151 for (uint v2_idx = 0; v2_idx < num_v; ++v2_idx)
152 {
153 Vec3 v2 = r.mShape2Face[v2_idx] - r.mContactPointOn2;
154 Vec3 v1_v2 = v2 - v1;
155 float denominator = v1_v2.LengthSq();
156 if (denominator < Square(FLT_EPSILON))
157 {
158 // Degenerate, assume v1 is closest, v2 will be tested in a later iteration
159 float v1_len_sq = v1.LengthSq();
160 if (v1_len_sq < best_dist_sq)
161 {
162 best_dist_sq = v1_len_sq;
163 best_v1_idx = v1_idx;
164 best_v2_idx = v1_idx;
165 }
166 }
167 else
168 {
169 // Taken from ClosestPoint::GetBaryCentricCoordinates
170 float fraction = -v1.Dot(v1_v2) / denominator;
171 if (fraction < 1.0e-6f)
172 {
173 // Closest lies on v1
174 float v1_len_sq = v1.LengthSq();
175 if (v1_len_sq < best_dist_sq)
176 {
177 best_dist_sq = v1_len_sq;
178 best_v1_idx = v1_idx;
179 best_v2_idx = v1_idx;
180 }
181 }
182 else if (fraction < 1.0f - 1.0e-6f)
183 {
184 // Closest lies on the line segment v1, v2
185 Vec3 closest = v1 + fraction * v1_v2;
186 float closest_len_sq = closest.LengthSq();
187 if (closest_len_sq < best_dist_sq)
188 {
189 best_dist_sq = closest_len_sq;
190 best_v1_idx = v1_idx;
191 best_v2_idx = v2_idx;
192 }
193 }
194 // else closest is v2, but v2 will be tested in a later iteration
195 }
196
197 v1_idx = v2_idx;
198 v1 = v2;
199 }
200
201 // Check if this vertex/edge is voided
202 bool voided = IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])
203 && (best_v1_idx == best_v2_idx || IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx]));
204
205 #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
206 Color color = voided? Color::sRed : Color::sYellow;
210 DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v1_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])? Color::sRed : Color::sYellow, 0.1f);
211 DebugRenderer::sInstance->DrawMarker(RVec3(r.mShape2Face[best_v2_idx]), IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx])? Color::sRed : Color::sYellow, 0.1f);
212 #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
213
214 // No voided features, accept the contact
215 if (!voided)
216 Chain(r);
217
218 // Void the features of this face
219 VoidFeatures(r);
220 }
221
222 // All delayed results have been processed
223 mVoidedFeatures.clear();
224 mDelayedResults.clear();
225 }
226
227 // See: CollideShapeCollector::OnBodyEnd
228 virtual void OnBodyEnd() override
229 {
230 Flush();
231 mChainedCollector.OnBodyEnd();
232 }
233
235 static void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter = { })
236 {
237 JPH_ASSERT(inCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideWithAll); // Won't work without colliding with all edges
238 JPH_ASSERT(inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces); // Won't work without collecting faces
239
240 InternalEdgeRemovingCollector wrapper(ioCollector);
241 CollisionDispatch::sCollideShapeVsShape(inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, wrapper, inShapeFilter);
242 wrapper.Flush();
243 }
244
245private:
246 // This algorithm tests a convex shape (shape 1) against a set of polygons (shape 2).
247 // This assumption doesn't hold if the shape we're testing is a compound shape, so we must also
248 // store the sub shape ID and ignore voided features that belong to another sub shape ID.
249 struct Voided
250 {
251 Float3 mFeature; // Feature that is voided (of shape 2). Read with Vec3::sLoadFloat3Unsafe so must not be the last member.
252 SubShapeID mSubShapeID; // Sub shape ID of the shape that is colliding against the feature (of shape 1).
253 };
254
255 CollideShapeCollector & mChainedCollector;
258};
259
@ CollideWithAll
Collide with all edges. Use this when you're interested in all collisions.
@ CollectFaces
mShape1/2Face is desired
unsigned int uint
Definition Core.h:485
#define JPH_NAMESPACE_END
Definition Core.h:418
#define JPH_NAMESPACE_BEGIN
Definition Core.h:412
#define JPH_ASSERT(...)
Definition IssueReporting.h:33
JPH_INLINE constexpr T Square(T inV)
Square a value.
Definition Math.h:55
void QuickSort(Iterator inBegin, Iterator inEnd, Compare inCompare)
Implementation of the quick sort algorithm. The STL version implementation is not consistent across p...
Definition QuickSort.h:53
Vec3 RVec3
Definition Real.h:29
JPH_SUPPRESS_WARNINGS_STD_BEGIN JPH_SUPPRESS_WARNINGS_STD_END JPH_NAMESPACE_BEGIN String StringFormat(const char *inFMT,...)
Definition StringTools.cpp:15
Definition Body.h:39
ECollectFacesMode mCollectFacesMode
If colliding faces should be collected or only the collision point.
Definition CollideShape.h:80
JPH_OVERRIDE_NEW_DELETE EActiveEdgeMode mActiveEdgeMode
How active edges (edges that a moving object should bump into) are handled.
Definition CollideShape.h:77
Class that contains all information of two colliding shapes.
Definition CollideShape.h:19
SubShapeID mSubShapeID1
Sub shape ID that identifies the face on shape 1.
Definition CollideShape.h:63
Vec3 mContactPointOn2
Contact point on the surface of shape 2 (in world space or relative to base offset)....
Definition CollideShape.h:60
Vec3 mPenetrationAxis
Direction to move shape 2 out of collision along the shortest path (magnitude is meaningless,...
Definition CollideShape.h:61
float mPenetrationDepth
Penetration depth (move shape 2 by this distance to resolve the collision). If CollideShapeSettings::...
Definition CollideShape.h:62
Face mShape2Face
Colliding face on shape 2 (optional result, in world space or relative to base offset)
Definition CollideShape.h:67
Settings to be passed with a collision query.
Definition CollideShape.h:94
Virtual interface that allows collecting multiple collision results.
Definition CollisionCollector.h:45
virtual void AddHit(const ResultType &inResult)=0
This function will be called for every hit found, it's up to the application to decide how to store t...
virtual void Reset()
If you want to reuse this collector, call Reset()
Definition CollisionCollector.h:62
float GetEarlyOutFraction() const
Get the current early out value.
Definition CollisionCollector.h:95
const TransformedShape * GetContext() const
Definition CollisionCollector.h:74
virtual void OnBody(const Body &inBody)
Definition CollisionCollector.h:67
void UpdateEarlyOutFraction(float inFraction)
Update the early out fraction (should be lower than before)
Definition CollisionCollector.h:83
void SetContext(const TransformedShape *inContext)
Set by the collision detection functions to the current TransformedShape that we're colliding against...
Definition CollisionCollector.h:73
virtual void OnBodyEnd()
When running a query through the NarrowPhaseQuery class, this will be called after all AddHit calls h...
Definition CollisionCollector.h:70
static void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter={ })
Definition CollisionDispatch.h:33
Class that holds an RGBA color with 8-bits per component.
Definition Color.h:16
static const Color sGreen
Definition Color.h:57
static const Color sRed
Definition Color.h:55
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
static DebugRenderer * sInstance
Singleton instance.
Definition DebugRenderer.h:179
void DrawWirePolygon(RMat44Arg inTransform, const VERTEX_ARRAY &inVertices, ColorArg inColor, float inArrowSize=0.0f)
Draw a wireframe polygon.
Definition DebugRenderer.h:83
void DrawArrow(RVec3Arg inFrom, RVec3Arg inTo, ColorArg inColor, float inSize)
Draw an arrow.
Definition DebugRenderer.cpp:184
Class that holds 3 floats. Used as a storage class. Convert to Vec3 for calculations.
Definition Float3.h:13
Definition InternalEdgeRemovingCollector.h:24
virtual void OnBody(const Body &inBody) override
Definition InternalEdgeRemovingCollector.h:98
void Flush()
After all hits have been added, call this function to process the delayed results.
Definition InternalEdgeRemovingCollector.h:131
InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector)
Constructor, configures a collector to be called with all the results that do not hit internal edges.
Definition InternalEdgeRemovingCollector.h:81
static void sCollideShapeVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter={ })
Version of CollisionDispatch::sCollideShapeVsShape that removes internal edges.
Definition InternalEdgeRemovingCollector.h:235
virtual void Reset() override
If you want to reuse this collector, call Reset()
Definition InternalEdgeRemovingCollector.h:87
virtual void AddHit(const CollideShapeResult &inResult) override
Definition InternalEdgeRemovingCollector.h:105
virtual void OnBodyEnd() override
When running a query through the NarrowPhaseQuery class, this will be called after all AddHit calls h...
Definition InternalEdgeRemovingCollector.h:228
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 sIdentity()
Identity matrix.
Definition Mat44.inl:35
Filter class.
Definition ShapeFilter.h:17
Base class for all shapes (collision volume of a body). Defines a virtual interface for collision det...
Definition Shape.h:186
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
void clear()
Destruct all elements and set length to zero.
Definition StaticArray.h:52
size_type size() const
Returns amount of elements in the array.
Definition StaticArray.h:89
Definition SubShapeID.h:108
A sub shape id contains a path to an element (usually a triangle or other primitive type) of a compou...
Definition SubShapeID.h:23
Definition Vec3.h:17
JPH_INLINE bool IsClose(Vec3Arg inV2, float inMaxDistSq=1.0e-12f) const
Test if two vectors are close.
Definition Vec3.inl:347
JPH_INLINE float Dot(Vec3Arg inV2) const
Dot product.
Definition Vec3.inl:650
JPH_INLINE float Length() const
Length of vector.
Definition Vec3.inl:682
JPH_INLINE Vec3 NormalizedOr(Vec3Arg inZeroValue) const
Normalize vector or return inZeroValue if the length of the vector is zero.
Definition Vec3.inl:721
JPH_INLINE void StoreFloat3(Float3 *outV) const
Store 3 floats to memory.
Definition Vec3.inl:772
JPH_INLINE float LengthSq() const
Squared length of vector.
Definition Vec3.inl:666
static JPH_INLINE Vec3 sZero()
Vector with all zeros.
Definition Vec3.inl:103
static JPH_INLINE Vec3 sLoadFloat3Unsafe(const Float3 &inV)
Load 3 floats from memory (reads 32 bits extra which it doesn't use)
Definition Vec3.inl:135