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
10
11//#define JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
12
13#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
15#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
16
18
25{
26 static constexpr uint cMaxLocalDelayedResults = 32;
27 static constexpr uint cMaxLocalVoidedFeatures = 128;
28
30 inline bool IsVoided(const SubShapeID &inSubShapeID, Vec3 inV) const
31 {
32 for (const Voided &vf : mVoidedFeatures)
33 if (vf.mSubShapeID == inSubShapeID
34 && inV.IsClose(Vec3::sLoadFloat3Unsafe(vf.mFeature), 1.0e-8f))
35 return true;
36 return false;
37 }
38
40 inline void VoidFeatures(const CollideShapeResult &inResult)
41 {
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 }
50 }
51
53 inline void Chain(const CollideShapeResult &inResult)
54 {
55 // Make sure the chained collector has the same context as we do
56 mChainedCollector.SetContext(GetContext());
57
58 // Forward the hit
59 mChainedCollector.AddHit(inResult);
60
61 // If our chained collector updated its early out fraction, we need to follow
63 }
64
66 inline void ChainAndVoid(const CollideShapeResult &inResult)
67 {
68 Chain(inResult);
69 VoidFeatures(inResult);
70
71 #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
73 DebugRenderer::sInstance->DrawArrow(mBaseOffset + inResult.mContactPointOn2, mBaseOffset + inResult.mContactPointOn2 + inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero()), Color::sGreen, 0.1f);
74 #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
75 }
76
77public:
80 #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
81 , RVec3Arg inBaseOffset
82 #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
83 ) :
84 CollideShapeCollector(inChainedCollector),
85 mChainedCollector(inChainedCollector)
86 #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
87 , mBaseOffset(inBaseOffset)
88 #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
89 {
90 // Initialize arrays to full capacity to avoid needless reallocation calls
91 mVoidedFeatures.reserve(cMaxLocalVoidedFeatures);
92 mDelayedResults.reserve(cMaxLocalDelayedResults);
93 }
94
95 // See: CollideShapeCollector::Reset
96 virtual void Reset() override
97 {
99
100 mChainedCollector.Reset();
101
102 mVoidedFeatures.clear();
103 mDelayedResults.clear();
104 }
105
106 // See: CollideShapeCollector::OnBody
107 virtual void OnBody(const Body &inBody) override
108 {
109 // Just forward the call to our chained collector
110 mChainedCollector.OnBody(inBody);
111 }
112
113 // See: CollideShapeCollector::AddHit
114 virtual void AddHit(const CollideShapeResult &inResult) override
115 {
116 // We only support welding when the shape is a triangle or has more vertices so that we can calculate a normal
117 if (inResult.mShape2Face.size() < 3)
118 return ChainAndVoid(inResult);
119
120 // Get the triangle normal of shape 2 face
121 Vec3 triangle_normal = (inResult.mShape2Face[1] - inResult.mShape2Face[0]).Cross(inResult.mShape2Face[2] - inResult.mShape2Face[0]);
122 float triangle_normal_len = triangle_normal.Length();
123 if (triangle_normal_len < 1e-6f)
124 return ChainAndVoid(inResult);
125
126 // If the triangle normal matches the contact normal within 1 degree, we can process the contact immediately
127 // We make the assumption here that if the contact normal and the triangle normal align that the we're dealing with a 'face contact'
128 Vec3 contact_normal = -inResult.mPenetrationAxis;
129 float contact_normal_len = inResult.mPenetrationAxis.Length();
130 if (triangle_normal.Dot(contact_normal) > 0.999848f * contact_normal_len * triangle_normal_len) // cos(1 degree)
131 return ChainAndVoid(inResult);
132
133 // Delayed processing
134 mDelayedResults.push_back(inResult);
135 }
136
138 void Flush()
139 {
140 // Sort on biggest penetration depth first
142 sorted_indices.resize(mDelayedResults.size());
143 for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
144 sorted_indices[i] = i;
145 QuickSort(sorted_indices.begin(), sorted_indices.end(), [this](uint inLHS, uint inRHS) { return mDelayedResults[inLHS].mPenetrationDepth > mDelayedResults[inRHS].mPenetrationDepth; });
146
147 // Loop over all results
148 for (uint i = 0; i < uint(mDelayedResults.size()); ++i)
149 {
150 const CollideShapeResult &r = mDelayedResults[sorted_indices[i]];
151
152 // Determine which vertex or which edge is the closest to the contact point
153 float best_dist_sq = FLT_MAX;
154 uint best_v1_idx = 0;
155 uint best_v2_idx = 0;
156 uint num_v = uint(r.mShape2Face.size());
157 uint v1_idx = num_v - 1;
158 Vec3 v1 = r.mShape2Face[v1_idx] - r.mContactPointOn2;
159 for (uint v2_idx = 0; v2_idx < num_v; ++v2_idx)
160 {
161 Vec3 v2 = r.mShape2Face[v2_idx] - r.mContactPointOn2;
162 Vec3 v1_v2 = v2 - v1;
163 float denominator = v1_v2.LengthSq();
164 if (denominator < Square(FLT_EPSILON))
165 {
166 // Degenerate, assume v1 is closest, v2 will be tested in a later iteration
167 float v1_len_sq = v1.LengthSq();
168 if (v1_len_sq < best_dist_sq)
169 {
170 best_dist_sq = v1_len_sq;
171 best_v1_idx = v1_idx;
172 best_v2_idx = v1_idx;
173 }
174 }
175 else
176 {
177 // Taken from ClosestPoint::GetBaryCentricCoordinates
178 float fraction = -v1.Dot(v1_v2) / denominator;
179 if (fraction < 1.0e-6f)
180 {
181 // Closest lies on v1
182 float v1_len_sq = v1.LengthSq();
183 if (v1_len_sq < best_dist_sq)
184 {
185 best_dist_sq = v1_len_sq;
186 best_v1_idx = v1_idx;
187 best_v2_idx = v1_idx;
188 }
189 }
190 else if (fraction < 1.0f - 1.0e-6f)
191 {
192 // Closest lies on the line segment v1, v2
193 Vec3 closest = v1 + fraction * v1_v2;
194 float closest_len_sq = closest.LengthSq();
195 if (closest_len_sq < best_dist_sq)
196 {
197 best_dist_sq = closest_len_sq;
198 best_v1_idx = v1_idx;
199 best_v2_idx = v2_idx;
200 }
201 }
202 // else closest is v2, but v2 will be tested in a later iteration
203 }
204
205 v1_idx = v2_idx;
206 v1 = v2;
207 }
208
209 // Check if this vertex/edge is voided
210 bool voided = IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])
211 && (best_v1_idx == best_v2_idx || IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx]));
212
213 #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
214 Color color = voided? Color::sRed : Color::sYellow;
215 DebugRenderer::sInstance->DrawText3D(mBaseOffset + r.mContactPointOn2, StringFormat("%d: %g", i, r.mPenetrationDepth), color, 0.1f);
218 DebugRenderer::sInstance->DrawMarker(mBaseOffset + r.mShape2Face[best_v1_idx], IsVoided(r.mSubShapeID1, r.mShape2Face[best_v1_idx])? Color::sRed : Color::sYellow, 0.1f);
219 DebugRenderer::sInstance->DrawMarker(mBaseOffset + r.mShape2Face[best_v2_idx], IsVoided(r.mSubShapeID1, r.mShape2Face[best_v2_idx])? Color::sRed : Color::sYellow, 0.1f);
220 #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
221
222 // No voided features, accept the contact
223 if (!voided)
224 Chain(r);
225
226 // Void the features of this face
227 VoidFeatures(r);
228 }
229
230 // All delayed results have been processed
231 mVoidedFeatures.clear();
232 mDelayedResults.clear();
233 }
234
235 // See: CollideShapeCollector::OnBodyEnd
236 virtual void OnBodyEnd() override
237 {
238 Flush();
239 mChainedCollector.OnBodyEnd();
240 }
241
243 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 = { }
244#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
245 , RVec3Arg inBaseOffset = RVec3::sZero()
246#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
247 )
248 {
249 JPH_ASSERT(inCollideShapeSettings.mActiveEdgeMode == EActiveEdgeMode::CollideWithAll); // Won't work without colliding with all edges
250 JPH_ASSERT(inCollideShapeSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces); // Won't work without collecting faces
251
252 InternalEdgeRemovingCollector wrapper(ioCollector
253 #ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
254 , inBaseOffset
255 #endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
256 );
257 CollisionDispatch::sCollideShapeVsShape(inShape1, inShape2, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, wrapper, inShapeFilter);
258 wrapper.Flush();
259 }
260
261private:
262 // This algorithm tests a convex shape (shape 1) against a set of polygons (shape 2).
263 // This assumption doesn't hold if the shape we're testing is a compound shape, so we must also
264 // store the sub shape ID and ignore voided features that belong to another sub shape ID.
265 struct Voided
266 {
267 Float3 mFeature; // Feature that is voided (of shape 2). Read with Vec3::sLoadFloat3Unsafe so must not be the last member.
268 SubShapeID mSubShapeID; // Sub shape ID of the shape that is colliding against the feature (of shape 1).
269 };
270
271 CollideShapeCollector & mChainedCollector;
274#ifdef JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
275 RVec3 mBaseOffset; // Base offset for the query, used to draw the results in the right place
276#endif // JPH_INTERNAL_EDGE_REMOVING_COLLECTOR_DEBUG
277};
278
@ 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:492
#define JPH_NAMESPACE_END
Definition Core.h:419
#define JPH_NAMESPACE_BEGIN
Definition Core.h:413
#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
JPH_SUPPRESS_WARNINGS_STD_BEGIN JPH_SUPPRESS_WARNINGS_STD_END JPH_NAMESPACE_BEGIN String StringFormat(const char *inFMT,...)
Definition StringTools.cpp:15
Definition Array.h:36
void resize(size_type inNewSize)
Resize array to new length.
Definition Array.h:197
const_iterator begin() const
Iterators.
Definition Array.h:493
size_type size() const
Returns amount of elements in the array.
Definition Array.h:397
void clear()
Destruct all elements and set length to zero.
Definition Array.h:222
void push_back(const T &inValue)
Add element to the back of the array.
Definition Array.h:354
void reserve(size_type inNewSize)
Reserve array space.
Definition Array.h:190
const_iterator end() const
Definition Array.h:498
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:96
const TransformedShape * GetContext() const
Definition CollisionCollector.h:75
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:84
void SetContext(const TransformedShape *inContext)
Definition CollisionCollector.h:74
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:25
virtual void OnBody(const Body &inBody) override
Definition InternalEdgeRemovingCollector.h:107
void Flush()
After all hits have been added, call this function to process the delayed results.
Definition InternalEdgeRemovingCollector.h:138
InternalEdgeRemovingCollector(CollideShapeCollector &inChainedCollector)
Constructor, configures a collector to be called with all the results that do not hit internal edges.
Definition InternalEdgeRemovingCollector.h:79
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:243
virtual void Reset() override
If you want to reuse this collector, call Reset()
Definition InternalEdgeRemovingCollector.h:96
virtual void AddHit(const CollideShapeResult &inResult) override
Definition InternalEdgeRemovingCollector.h:114
virtual void OnBodyEnd() override
When running a query through the NarrowPhaseQuery class, this will be called after all AddHit calls h...
Definition InternalEdgeRemovingCollector.h:236
Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
Definition Mat44.h:13
static JPH_INLINE Mat44 sTranslation(Vec3Arg inV)
Get matrix that translates.
Definition Mat44.inl:144
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
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