1//
2// Redistribution and use in source and binary forms, with or without
3// modification, are permitted provided that the following conditions
4// are met:
5// * Redistributions of source code must retain the above copyright
6// notice, this list of conditions and the following disclaimer.
7// * Redistributions in binary form must reproduce the above copyright
8// notice, this list of conditions and the following disclaimer in the
9// documentation and/or other materials provided with the distribution.
10// * Neither the name of NVIDIA CORPORATION nor the names of its
11// contributors may be used to endorse or promote products derived
12// from this software without specific prior written permission.
13//
14// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
15// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
17// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
18// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
19// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
20// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
21// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
22// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25//
26// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
27// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
28// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
29
30#include "geometry/PxGeometry.h"
31#include "Ps.h"
32#include "GuCCDSweepConvexMesh.h"
33#include "PsHashMap.h"
34#include "PxsIslandSim.h"
35
36#ifndef PXS_CCD_H
37#define PXS_CCD_H
38
39#define CCD_DEBUG_PRINTS 0
40#define CCD_POST_DEPENETRATE_DIST 0.001f
41#define CCD_ROTATION_LOCKING 0
42#define CCD_MIN_TIME_LEFT 0.01f
43#define CCD_ANGULAR_IMPULSE 0
44
45#define DEBUG_RENDER_CCD 0
46
47#if CCD_DEBUG_PRINTS
48namespace physx {
49 extern void printCCDDebug(const char* msg, const PxsRigidBody* atom0, PxGeometryType::Enum g0, bool printPtr = true);
50 extern void printShape(PxsRigidBody* atom0, PxGeometryType::Enum g0, const char* annotation, PxReal dt, PxU32 pass, bool printPtr = true);
51}
52#define PRINTCCDSHAPE(x) printShape x
53#define PRINTCCDDEBUG(x) printCCDDebug x
54#else
55#define PRINTCCDSHAPE(x)
56#define PRINTCCDDEBUG(x)
57#endif
58
59namespace physx
60{
61
62// ------------------------------------------------------------------------------------------------------------
63// a fraction of objects will be CCD active so this is dynamic, not a member of PsxRigidBody
64// CCD code builds a temporary array of PxsCCDPair objects (allocated in blocks)
65// this is done to gather scattered data from memory and also to reduce PxsRidigBody permanent memory footprint
66// we have to do it every pass since new CMs can become fast moving after each pass (and sometimes cease to be)
67//
68struct PxsCCDBody;
69class PxsRigidBody;
70struct PxsShapeCore;
71struct PxsRigidCore;
72class PxsContactManager;
73class PxsContext;
74class PxCCDContactModifyCallback;
75class PxcNpThreadContext;
76
77class PxvNphaseImplementationContext;
78
79namespace Dy
80{
81 class ThresholdStream;
82}
83
84
85/**
86\brief structure to represent interactions between a given body and another body.
87*/
88struct PxsCCDOverlap
89{
90 //The body the interaction relates to
91 PxsCCDBody* mBody;
92 //The next interaction in the list
93 PxsCCDOverlap* mNext;
94};
95
96/**
97\brief Temporary CCD representation for a shape.
98
99Stores data about a shape that may be frequently used in CCD. It also stores update counters per-shape that can be compared with the body's update
100counter to determine if the shape needs its transforms re-calculated. This avoids us needing to store a list of shapes in a CCD body.
101*/
102struct PxsCCDShape : public Gu::CCDShape
103{
104public:
105 const PxsShapeCore* mShapeCore; //Shape core (can be shared)
106 const PxsRigidCore* mRigidCore; //Rigid body core
107 IG::NodeIndex mNodeIndex;
108
109 /**
110 \brief Returns the world-space pose for this shape
111 \param[in] atom The rigid body that this CCD shape is associated with
112 */
113 PxTransform getAbsPose(const PxsRigidBody* atom) const;
114 /**
115 \brief Returns the world-space previous pose for this shape
116 \param[in] atom The rigid body that this CCD shape is associated with
117 */
118 PxTransform getLastCCDAbsPose(const PxsRigidBody* atom) const;
119};
120
121/**
122\brief Structure to represent a body in the CCD system.
123*/
124struct PxsCCDBody
125{
126 Cm::SpatialVector mPreSolverVelocity;
127 PxU16 mIndex; //The CCD body's index
128 bool mPassDone; //Whether it has been processed in the current CCD pass
129 bool mHasAnyPassDone; //Whether this body was influenced by any passes
130 PxReal mTimeLeft; //CCD time left to elapse (normalized in range 0-1)
131 PxsRigidBody* mBody; //The rigid body
132 PxsCCDOverlap* mOverlappingObjects; //A list of overlapping bodies for island update
133 PxU32 mUpdateCount; //How many times this body has eben updated in the CCD. This is correlated with CCD shapes' update counts.
134 PxU32 mNbInteractionsThisPass; //How many interactions this pass
135
136
137
138 /**
139 \brief Returns the CCD body's index.
140 \return The CCD body's index.
141 */
142 PX_FORCE_INLINE PxU32 getIndex() const { return mIndex; }
143
144 /**
145 \brief Tests whether this body has already registered an overlap with a given body.
146 \param[in] body The body to test against.
147 \return Whether this body has already registered an overlap with a given body.
148 */
149 bool overlaps(PxsCCDBody* body) const
150 {
151 PxsCCDOverlap* overlaps = mOverlappingObjects;
152
153 while(overlaps)
154 {
155 if(overlaps->mBody == body)
156 return true;
157 overlaps = overlaps->mNext;
158 }
159 return false;
160 }
161
162 /**
163 \brief Registers an overlap with a given body
164 \param[in] overlap The CCD overlap to register.
165 */
166 void addOverlap(PxsCCDOverlap* overlap)
167 {
168 overlap->mNext = mOverlappingObjects;
169 mOverlappingObjects = overlap;
170 }
171
172};
173
174/**
175\brief a container class used in the CCD that minimizes frequency of hitting the allocator.
176
177This class stores a set of blocks of memory. It is effectively an array that resizes more efficiently because it doesn't need to
178reallocate an entire buffer and copy data.
179*/
180template<typename T, int BLOCK_SIZE>
181struct PxsCCDBlockArray
182{
183 /**
184 \brief A block of data
185 */
186 struct Block : Ps::UserAllocated { T items[BLOCK_SIZE]; };
187 /**
188 \brief A header for a block of data.
189 */
190 struct BlockInfo
191 {
192 Block* block;
193 PxU32 count; // number of elements in this block
194 BlockInfo(Block* aBlock, PxU32 aCount) : block(aBlock), count(aCount) {}
195 };
196 /*
197 \brief An array of block headers
198 */
199 Ps::Array<BlockInfo> blocks;
200 /**
201 \brief The current block.
202 */
203 PxU32 currentBlock;
204
205 /**
206 \brief Constructor
207 */
208 PxsCCDBlockArray() : currentBlock(0)
209 {
210 blocks.pushBack(BlockInfo(PX_NEW(Block), 0));
211 }
212
213 /**
214 \brief Destructor
215 */
216 ~PxsCCDBlockArray()
217 {
218 for (PxU32 i = 0; i < blocks.size(); i++)
219 {
220 PX_DELETE(blocks[i].block);
221 }
222 currentBlock = 0;
223 }
224
225 /**
226 \brief Clears this block array.
227 \note This clear function also deletes all additional blocks
228 */
229 void clear()
230 {
231 for (PxU32 i = 0; i < blocks.size(); i++)
232 {
233 PX_DELETE(blocks[i].block);
234 }
235 blocks.clear();
236 blocks.pushBack(BlockInfo(PX_NEW(Block), 0)); // at least one block is expected to always be present in the array
237 currentBlock = 0;
238 }
239
240 /**
241 \brief Clears this block array but does not release the memory.
242 */
243 void clear_NoDelete()
244 {
245 currentBlock = 0;
246 blocks[0].count = 0;
247 }
248
249 /**
250 \brief Push a new element onto the back of the block array
251 \return The new element
252 */
253 T& pushBack()
254 {
255 PxU32 numBlocks = blocks.size();
256 if (blocks[currentBlock].count == BLOCK_SIZE)
257 {
258 if((currentBlock + 1) == numBlocks)
259 {
260 blocks.pushBack(BlockInfo(PX_NEW(Block), 0));
261 numBlocks ++;
262 }
263 currentBlock++;
264 blocks[currentBlock].count = 0;
265 }
266 const PxU32 count = blocks[currentBlock].count ++;
267
268 return blocks[currentBlock].block->items[count];
269 }
270
271 /**
272 \brief Pushes a new element onto the back of this array, intitializing it to match the data
273 \param data The data to initialize the new element to
274 \return The new element
275 */
276 T& pushBack(T& data)
277 {
278 PxU32 numBlocks = blocks.size();
279 if (blocks[currentBlock].count == BLOCK_SIZE)
280 {
281 if((currentBlock + 1) == numBlocks)
282 {
283 blocks.pushBack(BlockInfo(PX_NEW(Block), 0));
284 numBlocks ++;
285 }
286 currentBlock++;
287 blocks[currentBlock].count = 0;
288 }
289 const PxU32 count = blocks[currentBlock].count ++;
290 blocks[currentBlock].block->items[count] = data;
291 return blocks[currentBlock].block->items[count];
292 }
293
294 /**
295 \brief Pops the last element from the list.
296 */
297 void popBack()
298 {
299 PX_ASSERT(blocks[currentBlock].count > 0);
300 if (blocks[currentBlock].count > 1)
301 blocks[currentBlock].count --;
302 else
303 {
304 PX_DELETE(blocks[currentBlock].block);
305 blocks.popBack();
306 currentBlock--;
307 }
308 }
309
310 /**
311 \brief Returns the current size of the array.
312 \return The current size of the array.
313 */
314 PxU32 size() const
315 {
316 return (currentBlock)*BLOCK_SIZE + blocks[currentBlock].count;
317 }
318
319 /**
320 \brief Returns the element at a given index in the array
321 \param[in] index The index of the element in the array
322 \return The element at a given index in the array.
323 */
324 T& operator[] (PxU32 index) const
325 {
326 PX_ASSERT(index/BLOCK_SIZE < blocks.size());
327 PX_ASSERT(index%BLOCK_SIZE < blocks[index/BLOCK_SIZE].count);
328 return blocks[index/BLOCK_SIZE].block->items[index%BLOCK_SIZE];
329 }
330};
331
332/**
333\brief A structure to represent a potential CCD interaction between a pair of shapes
334*/
335struct PxsCCDPair
336{
337 /**
338 \brief Defines whether this is an estimated TOI or an accurate TOI.
339
340 We store pairs in a priority queue based on the TOIs. We use cheap estimates to cull away work and lazily evaluate TOIs. This means that an element in the
341 priority queue may either be an estimate or a precise result.
342 */
343 enum E_TOIType
344 {
345 eEstimate,
346 ePrecise
347 };
348 PxsRigidBody* mBa0; // Body A. Can be NULL for statics
349 PxsRigidBody* mBa1; // Body B. Can be NULL for statics
350 PxsCCDShape* mCCDShape0; // Shape A
351 PxsCCDShape* mCCDShape1; // Shape B
352 PxVec3 mMinToiNormal; // The contact normal. Only valid for precise results. On the surface of body/shape A
353 PxReal mMinToi; // Min TOI. Valid for both precise and estimated results but estimates may be too early (i.e. conservative).
354 PxReal mPenetrationPostStep; // Valid only for precise sweeps. Only used for initial intersections (i.e. at TOI = 0).
355 PxVec3 mMinToiPoint; // The contact point. Only valid for precise sweep results.
356 PxReal mPenetration; // The penetration. Only valid for precise sweep results.
357 PxsContactManager* mCm; // The contact manager.
358 PxU32 mIslandId; // The index of the island this pair is in
359 PxGeometryType::Enum mG0, mG1; // The geometry types for shapes 0 and 1
360 bool mIsEarliestToiHit; // Indicates this was the earliest hit for one of the bodies in the pair
361 bool mIsModifiable; // Indicates whether this contact is modifiable
362 PxU32 mFaceIndex; // The face index. Only valid for precise sweeps involving meshes or heightfields.
363 PxU16 mMaterialIndex0; // The material index for shape 0
364 PxU16 mMaterialIndex1; // The material index for shape 1
365 PxReal mDynamicFriction; // The dynamic friction coefficient
366 PxReal mStaticFriction; // The static friction coefficient
367 PxReal mRestitution; // The restitution coefficient
368 PxU32 mEstimatePass; // The current estimation pass. Used after a sweep hit was found to determine if the pair needs re-estimating.
369 PxReal mAppliedForce; // The applied force for this pair. Only valid if the pair has been responded to.
370 PxReal mMaxImpulse; // The maximum impulse to be applied
371
372 E_TOIType mToiType; // The TOI type (estimate, precise).
373 bool mHasFriction; // Whether we want to simulate CCD friction for this pair
374
375 /**
376 \brief Perform a precise sweep for this pair
377 \param[in] threadContext The per-thread context
378 \param[in] dt The time-step
379 \param[in] pass The current CCD pass
380 \return The normalized TOI. <=1.0 indicates a hit. Otherwise PX_MAX_REAL.
381 */
382 PxReal sweepFindToi(PxcNpThreadContext& threadContext, PxReal dt, PxU32 pass, PxReal ccdThreshold);
383 /**
384 \brief Performs a sweep estimation for this pair
385 \return The normalized TOI. <= 1.0 indicates a potential hit, otherwise PX_MAX_REAL.
386 */
387 PxReal sweepEstimateToi(PxReal ccdThreshold);
388 /**
389 \brief Advances this pair to the TOI
390 \param[in] dt The time-step
391 \param[in] clipTrajectoryToToi Indicates whether we clip the body's trajectory to the end pose. Only done in the final pass
392 \return Whether the advance was successful. An advance will be unsuccessful if body bodies were already updated.
393 */
394 bool sweepAdvanceToToi(PxReal dt, bool clipTrajectoryToToi);
395 /**
396 \brief Updates the transforms of the shapes involved in this pair.
397 */
398 void updateShapes();
399
400};
401
402/**
403\brief Block array of CCD bodies
404*/
405typedef PxsCCDBlockArray<PxsCCDBody, 128> PxsCCDBodyArray;
406/**
407\brief Block array of CCD pairs
408*/
409typedef PxsCCDBlockArray<PxsCCDPair, 128> PxsCCDPairArray;
410/**
411\brief Block array of CCD overlaps
412*/
413typedef PxsCCDBlockArray<PxsCCDOverlap, 128> PxsCCDOverlapArray;
414/**
415\brief Block array of CCD shapes
416*/
417typedef PxsCCDBlockArray<PxsCCDShape, 128> PxsCCDShapeArray;
418
419/**
420\brief Pair structure to be able to look-up a rigid body-shape pair in a map
421*/
422typedef Ps::Pair<const PxsRigidCore*, const PxsShapeCore*> PxsRigidShapePair;
423
424
425/**
426\brief CCD context object.
427*/
428class PxsCCDContext
429{
430public:
431
432 /**
433 \brief Creates this PxsCCDContext
434 */
435 static PxsCCDContext* create(PxsContext* context, Dy::ThresholdStream& dynamicsContext, PxvNphaseImplementationContext& nPhaseContext,
436 PxReal ccdThreshold);
437
438 /**
439 \brief Destroys this PxsCCDContext
440 */
441 void destroy();
442
443 /**
444 \brief Returns the CCD contact modification callback
445 \return The CCD contact modification callback
446 */
447 PX_FORCE_INLINE PxCCDContactModifyCallback* getCCDContactModifyCallback() const { return mCCDContactModifyCallback; }
448 /**
449 \brief Sets the CCD contact modification callback
450 \param[in] c The CCD contact modification callback
451 */
452 PX_FORCE_INLINE void setCCDContactModifyCallback(PxCCDContactModifyCallback* c) { mCCDContactModifyCallback = c; }
453 /**
454 \brief Returns the maximum number of CCD passes
455 \return The maximum number of CCD passes
456 */
457 PX_FORCE_INLINE PxU32 getCCDMaxPasses() const { return mCCDMaxPasses; }
458 /**
459 \brief Sets the maximum number of CCD passes
460 \param[in] ccdMaxPasses The maximum number of CCD passes
461 */
462 PX_FORCE_INLINE void setCCDMaxPasses(PxU32 ccdMaxPasses) { mCCDMaxPasses = ccdMaxPasses; }
463 /**
464 \brief Returns the current CCD pass
465 \return The current CCD pass
466 */
467 PX_FORCE_INLINE PxU32 getCurrentCCDPass() const { return miCCDPass; }
468 /**
469 \brief Returns The number of swept hits reported
470 \return The number of swept hits reported
471 */
472 PX_FORCE_INLINE PxI32 getNumSweepHits() const { return mSweepTotalHits; }
473 /**
474 \brief Returns The number of updated bodies
475 \return The number of updated bodies in this CCD pass
476 */
477 PX_FORCE_INLINE PxU32 getNumUpdatedBodies() const { return mUpdatedCCDBodies.size(); }
478 /**
479 \brief Returns The update bodies array
480 \return The updated bodies array from this CCD pass
481 */
482 PX_FORCE_INLINE PxsRigidBody*const* getUpdatedBodies() const { return mUpdatedCCDBodies.begin(); }
483
484 /**
485 \brief Returns Clears the updated bodies array
486 */
487 PX_FORCE_INLINE void clearUpdatedBodies() { mUpdatedCCDBodies.forceSize_Unsafe(size: 0); }
488
489 PX_FORCE_INLINE PxReal getCCDThreshold() const { return mCCDThreshold;}
490
491 /**
492 \brief Runs the CCD contact modification.
493 \param[in] contacts The list of modifiable contacts
494 \param[in] contactCount The number of contacts
495 \param[in] shapeCore0 The first shape core
496 \param[in] shapeCore1 The second shape core
497 \param[in] rigidCore0 The first rigid core
498 \param[in] rigidCore1 The second rigid core
499 \param[in] rigid0 The first rigid body
500 \param[in] rigid1 The second rigid body
501 */
502 void runCCDModifiableContact(PxModifiableContact* PX_RESTRICT contacts, PxU32 contactCount, const PxsShapeCore* PX_RESTRICT shapeCore0,
503 const PxsShapeCore* PX_RESTRICT shapeCore1, const PxsRigidCore* PX_RESTRICT rigidCore0, const PxsRigidCore* PX_RESTRICT rigidCore1,
504 const PxsRigidBody* PX_RESTRICT rigid0, const PxsRigidBody* PX_RESTRICT rigid1);
505
506 /**
507 \brief Performs a single CCD update
508 This occurs after broad phase and is responsible for creating islands, finding the TOI of collisions, filtering contacts, issuing modification callbacks and responding to
509 collisions. At the end of this phase all bodies will have stepper to their first TOI if they were involved in a CCD collision this frame.
510 \param[in] dt The timestep to simulate
511 \param[in] continuation The continuation task
512 \param[in] islandSim The island manager
513 \param[in] disableResweep If this is true, we perform a reduced-fidelity CCD approach
514 */
515 void updateCCD(PxReal dt, PxBaseTask* continuation, IG::IslandSim& islandSim, bool disableResweep, PxI32 numFastMovingShapes);
516
517 /**
518 \brief Signals the beginning of a CCD multi-pass update
519 */
520 void updateCCDBegin();
521
522 /**
523 \brief Resets the CCD contact state in any contact managers that previously had a reported CCD touch. This must be called if CCD update is bypassed for a frame
524 */
525 void resetContactManagers();
526
527
528
529
530protected:
531
532 /**
533 \brief Constructor for PxsCCDContext
534 \param[in] context The PxsContext that is associated with this PxsCCDContext.
535 */
536 PxsCCDContext(PxsContext* context, Dy::ThresholdStream& thresholdStream, PxvNphaseImplementationContext& nPhaseContext,
537 PxReal ccdThreshold);
538 /**
539 \brief Destructor for PxsCCDContext
540 */
541 ~PxsCCDContext();
542
543private:
544
545
546 /**
547 \brief Verifies the consistency of the CCD context at the beginning
548 */
549 void verifyCCDBegin();
550
551 /**
552 \brief Cleans up after the CCD update has completed
553 */
554 void updateCCDEnd();
555
556 /**
557 \brief Spawns the update island tasks after the initial sweep estimates have been performed
558 \param[in] continuation The continuation task
559 */
560 void postCCDSweep(PxBaseTask* continuation);
561 /**
562 \brief Creates contact buffers for CCD contacts. These will be sent to the user in the contact notification.
563 \param[in] continuation The continuation task
564 */
565 void postCCDAdvance(PxBaseTask* continuation);
566 /**
567 \brief The final phase of the CCD task chain. Cleans up after the parallel update/postCCDAdvance stages.
568 \param[in] continuation The continuation task
569 */
570 void postCCDDepenetrate(PxBaseTask* continuation);
571
572 typedef Cm::DelegateTask<PxsCCDContext, &PxsCCDContext::postCCDSweep> PostCCDSweepTask;
573 typedef Cm::DelegateTask<PxsCCDContext, &PxsCCDContext::postCCDAdvance> PostCCDAdvanceTask;
574 typedef Cm::DelegateTask<PxsCCDContext, &PxsCCDContext::postCCDDepenetrate> PostCCDDepenetrateTask;
575
576 PostCCDSweepTask mPostCCDSweepTask;
577 PostCCDAdvanceTask mPostCCDAdvanceTask;
578 PostCCDDepenetrateTask mPostCCDDepenetrateTask;
579
580 PxCCDContactModifyCallback* mCCDContactModifyCallback;
581
582 // CCD global data
583 bool mDisableCCDResweep;
584 PxU32 miCCDPass;
585 PxI32 mSweepTotalHits;
586
587 // a fraction of objects will be CCD active so PxsCCDBody is dynamic, not a member of PxsRigidBody
588 PxsCCDBodyArray mCCDBodies;
589 PxsCCDOverlapArray mCCDOverlaps;
590 PxsCCDShapeArray mCCDShapes;
591 Ps::Array<PxsCCDBody*> mIslandBodies;
592 Ps::Array<PxU16> mIslandSizes;
593 Ps::Array<PxsRigidBody*> mUpdatedCCDBodies;
594 Ps::HashMap<PxsRigidShapePair, PxsCCDShape*> mMap;
595
596 // temporary array updated during CCD update
597 //Array<PxsCCDPair> mCCDPairs;
598 PxsCCDPairArray mCCDPairs;
599 Ps::Array<PxsCCDPair*> mCCDPtrPairs;
600 // number of pairs per island
601 Ps::Array<PxU32> mCCDIslandHistogram;
602 // thread context valid during CCD update
603 PxcNpThreadContext* mCCDThreadContext;
604 // number of pairs to process per thread
605 PxU32 mCCDPairsPerBatch;
606 PxU32 mCCDMaxPasses;
607
608 PxsContext* mContext;
609 Dy::ThresholdStream& mThresholdStream;
610
611 PxvNphaseImplementationContext& mNphaseContext;
612
613 Ps::Mutex mMutex;
614
615 PxReal mCCDThreshold;
616
617private:
618
619 PX_NOCOPY(PxsCCDContext)
620};
621
622
623}
624
625
626
627#endif
628
629

source code of qtquick3dphysics/src/3rdparty/PhysX/source/lowlevel/software/include/PxsCCD.h