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#ifndef PXS_ISLAND_SIM_H
31#define PXS_ISLAND_SIM_H
32
33#include "CmPhysXCommon.h"
34#include "foundation/PxAssert.h"
35#include "PsArray.h"
36#include "CmBitMap.h"
37#include "CmPriorityQueue.h"
38#include "CmBlockArray.h"
39#include "PxsIslandNodeIndex.h"
40
41namespace physx
42{
43
44namespace Dy
45{
46 struct Constraint;
47 class ArticulationV;
48}
49
50namespace Sc
51{
52 class ArticulationSim;
53}
54
55class PxsContactManager;
56class PxsRigidBody;
57
58struct PartitionEdge;
59
60namespace IG
61{
62
63//This index is
64#define IG_INVALID_ISLAND 0xFFFFFFFFu
65#define IG_INVALID_EDGE 0xFFFFFFFFu
66#define IG_INVALID_LINK 0xFFu
67
68typedef PxU32 IslandId;
69typedef PxU32 EdgeIndex;
70typedef PxU32 EdgeInstanceIndex;
71
72class IslandSim;
73
74struct Edge
75{
76 //Edge instances can be implicitly calculated based on this edge index, which is an offset into the array of edges.
77 //From that, the child edge index is simply the
78 //The constraint or contact referenced by this edge
79
80 enum EdgeType
81 {
82 eCONTACT_MANAGER,
83 eCONSTRAINT,
84 eEDGE_TYPE_COUNT
85 };
86
87
88 enum EdgeState
89 {
90 eINSERTED =1<<0,
91 ePENDING_DESTROYED =1<<1,
92 eACTIVE =1<<2,
93 eIN_DIRTY_LIST =1<<3,
94 eDESTROYED =1<<4,
95 eREPORT_ONLY_DESTROY=1<<5,
96 eACTIVATING =1<<6
97 };
98
99
100 //NodeIndex mNode1, mNode2;
101 EdgeType mEdgeType;
102 PxU16 mEdgeState;
103
104 EdgeIndex mNextIslandEdge, mPrevIslandEdge;
105
106
107
108 PX_FORCE_INLINE void setInserted() { mEdgeState |= (eINSERTED); }
109
110 PX_FORCE_INLINE void clearInserted() { mEdgeState &= (~eINSERTED); }
111
112 PX_FORCE_INLINE void clearDestroyed() { mEdgeState &=(~eDESTROYED);}
113 PX_FORCE_INLINE void setPendingDestroyed() { mEdgeState |= ePENDING_DESTROYED; }
114 PX_FORCE_INLINE void clearPendingDestroyed() { mEdgeState &= (~ePENDING_DESTROYED); }
115 PX_FORCE_INLINE void activateEdge() { mEdgeState |= eACTIVE; }
116 PX_FORCE_INLINE void deactivateEdge() { mEdgeState &= (~eACTIVE); }
117 PX_FORCE_INLINE void markInDirtyList() { mEdgeState |= (eIN_DIRTY_LIST); }
118 PX_FORCE_INLINE void clearInDirtyList() { mEdgeState &= (~eIN_DIRTY_LIST); }
119 PX_FORCE_INLINE void setReportOnlyDestroy() { mEdgeState |= (eREPORT_ONLY_DESTROY); }
120 PX_FORCE_INLINE void clearReportOnlyDestroy() { mEdgeState &= (~eREPORT_ONLY_DESTROY); }
121public:
122 Edge() : mEdgeType(Edge::eCONTACT_MANAGER), mEdgeState(eDESTROYED),
123 mNextIslandEdge(IG_INVALID_EDGE), mPrevIslandEdge(IG_INVALID_EDGE)
124 {
125 }
126 PX_FORCE_INLINE bool isInserted() const { return !!(mEdgeState & eINSERTED);}
127 PX_FORCE_INLINE bool isDestroyed() const { return !!(mEdgeState & eDESTROYED); }
128 PX_FORCE_INLINE bool isPendingDestroyed() const { return !!(mEdgeState & ePENDING_DESTROYED); }
129 PX_FORCE_INLINE bool isActive() const { return !!(mEdgeState & eACTIVE); }
130 PX_FORCE_INLINE bool isInDirtyList() const { return !!(mEdgeState & eIN_DIRTY_LIST); }
131 PX_FORCE_INLINE EdgeType getEdgeType() const { return mEdgeType; }
132 //PX_FORCE_INLINE const NodeIndex getIndex1() const { return mNode1; }
133 //PX_FORCE_INLINE const NodeIndex getIndex2() const { return mNode2; }
134 PX_FORCE_INLINE bool isReportOnlyDestroy() { return !!(mEdgeState & eREPORT_ONLY_DESTROY); }
135};
136
137struct EdgeInstance
138{
139 EdgeInstanceIndex mNextEdge, mPrevEdge; //The next edge instance in this node's list of edge instances
140
141 EdgeInstance() : mNextEdge(IG_INVALID_EDGE), mPrevEdge(IG_INVALID_EDGE)
142 {
143 }
144};
145
146template<typename Handle>
147class HandleManager
148{
149 Ps::Array<Handle> mFreeHandles;
150 Handle mCurrentHandle;
151
152public:
153
154 HandleManager() : mFreeHandles(PX_DEBUG_EXP("FreeHandles")), mCurrentHandle(0)
155 {
156 }
157
158 ~HandleManager(){}
159
160 Handle getHandle()
161 {
162 if(mFreeHandles.size())
163 {
164 Handle handle = mFreeHandles.popBack();
165 PX_ASSERT(isValidHandle(handle));
166 return handle;
167 }
168 return mCurrentHandle++;
169 }
170
171 bool isNotFreeHandle(Handle handle)
172 {
173 for(PxU32 a = 0; a < mFreeHandles.size(); ++a)
174 {
175 if(mFreeHandles[a] == handle)
176 return false;
177 }
178 return true;
179 }
180
181 void freeHandle(Handle handle)
182 {
183 PX_ASSERT(isValidHandle(handle));
184 PX_ASSERT(isNotFreeHandle(handle));
185 if(handle == mCurrentHandle)
186 mCurrentHandle--;
187 else
188 mFreeHandles.pushBack(handle);
189 }
190
191 bool isValidHandle(Handle handle)
192 {
193 return handle < mCurrentHandle;
194 }
195
196 PX_FORCE_INLINE PxU32 getTotalHandles() const { return mCurrentHandle; }
197};
198
199class Node
200{
201
202public:
203 enum NodeType
204 {
205 eRIGID_BODY_TYPE,
206 eARTICULATION_TYPE,
207 eTYPE_COUNT
208 };
209 enum State
210 {
211 eREADY_FOR_SLEEPING = 1u << 0, //! Ready to go to sleep
212 eACTIVE = 1u << 1, //! Active
213 eKINEMATIC = 1u << 2, //! Kinematic
214 eDELETED = 1u << 3, //! Is pending deletion
215 eDIRTY = 1u << 4, //! Is dirty (i.e. lost a connection)
216 eACTIVATING = 1u << 5, //! Is in the activating list
217 eDEACTIVATING = 1u << 6 //! It is being forced to deactivate this frame
218 };
219 EdgeInstanceIndex mFirstEdgeIndex;
220
221 PxU8 mFlags;
222 PxU8 mType;
223 PxU16 mStaticTouchCount;
224 //PxU32 mActiveNodeIndex; //! Look-up for this node in the active nodes list, activating list or deactivating list...
225
226 NodeIndex mNextNode, mPrevNode;
227
228 //A counter for the number of active references to this body. Whenever an edge is activated, this is incremented.
229 //Whenver an edge is deactivated, this is decremented. This is used for kinematic bodies to determine if they need
230 //to be in the active kinematics list
231 PxU32 mActiveRefCount;
232
233
234 //A node can correspond with either a rigid body or an articulation or softBody
235 union
236 {
237 PxsRigidBody* mRigidBody;
238 Dy::ArticulationV* mLLArticulation;
239 };
240
241
242
243 PX_FORCE_INLINE Node() : mFirstEdgeIndex(IG_INVALID_EDGE), mFlags(eDELETED), mType(eRIGID_BODY_TYPE),
244 mStaticTouchCount(0), mActiveRefCount(0), mRigidBody(NULL)
245 {
246 }
247
248 PX_FORCE_INLINE ~Node() {}
249
250 PX_FORCE_INLINE void reset()
251 {
252 mFirstEdgeIndex = IG_INVALID_EDGE;
253 mFlags = eDELETED;
254 mRigidBody = NULL;
255 mActiveRefCount = 0;
256 mStaticTouchCount = 0;
257 }
258
259 PX_FORCE_INLINE void setRigidBody(PxsRigidBody* body) { mRigidBody = body; }
260
261 PX_FORCE_INLINE PxsRigidBody* getRigidBody() const { return mRigidBody; }
262
263 PX_FORCE_INLINE Dy::ArticulationV* getArticulation() const { return mLLArticulation; }
264
265
266 PX_FORCE_INLINE void setActive() { mFlags |= eACTIVE; }
267 PX_FORCE_INLINE void clearActive() { mFlags &= ~eACTIVE; }
268
269 PX_FORCE_INLINE void setActivating() { mFlags |= eACTIVATING; }
270 PX_FORCE_INLINE void clearActivating() { mFlags &= ~eACTIVATING; }
271
272 PX_FORCE_INLINE void setDeactivating() { mFlags |= eDEACTIVATING; }
273 PX_FORCE_INLINE void clearDeactivating() { mFlags &= (~eDEACTIVATING); }
274
275
276 //Activates a body/node.
277 PX_FORCE_INLINE void setIsReadyForSleeping() { mFlags |= eREADY_FOR_SLEEPING; }
278
279 PX_FORCE_INLINE void clearIsReadyForSleeping(){ mFlags &= (~eREADY_FOR_SLEEPING);}
280
281 PX_FORCE_INLINE void setIsDeleted(){mFlags |= eDELETED; }
282
283 PX_FORCE_INLINE void setKinematicFlag() {PX_ASSERT(!isKinematic()); mFlags |= eKINEMATIC;}
284
285 PX_FORCE_INLINE void clearKinematicFlag(){ PX_ASSERT(isKinematic()); mFlags &= (~eKINEMATIC);}
286
287 PX_FORCE_INLINE void markDirty(){mFlags |= eDIRTY;}
288
289 PX_FORCE_INLINE void clearDirty(){mFlags &= (~eDIRTY);}
290
291public:
292
293 PX_FORCE_INLINE bool isActive() const { return !!(mFlags & eACTIVE); }
294
295 PX_FORCE_INLINE bool isActiveOrActivating() const { return !!(mFlags & (eACTIVE | eACTIVATING)); }
296
297 PX_FORCE_INLINE bool isActivating() const { return !!(mFlags & eACTIVATING); }
298
299 PX_FORCE_INLINE bool isDeactivating() const { return !!(mFlags & eDEACTIVATING); }
300
301 PX_FORCE_INLINE bool isKinematic() const { return !!(mFlags & eKINEMATIC); }
302
303 PX_FORCE_INLINE bool isDeleted() const { return !!(mFlags & eDELETED); }
304
305 PX_FORCE_INLINE bool isDirty() const { return !!(mFlags & eDIRTY); }
306
307 PX_FORCE_INLINE bool isReadyForSleeping() const { return !!(mFlags & eREADY_FOR_SLEEPING); }
308
309 PX_FORCE_INLINE NodeType getNodeType() const { return NodeType(mType); }
310
311 friend class SimpleIslandManager;
312
313};
314
315struct Island
316{
317 NodeIndex mRootNode;
318 NodeIndex mLastNode;
319 PxU32 mSize[Node::eTYPE_COUNT];
320 PxU32 mActiveIndex;
321
322 EdgeIndex mFirstEdge[Edge::eEDGE_TYPE_COUNT], mLastEdge[Edge::eEDGE_TYPE_COUNT];
323 PxU32 mEdgeCount[Edge::eEDGE_TYPE_COUNT];
324
325 Island() : mActiveIndex(IG_INVALID_ISLAND)
326 {
327 for(PxU32 a = 0; a < Edge::eEDGE_TYPE_COUNT; ++a)
328 {
329 mFirstEdge[a] = IG_INVALID_EDGE;
330 mLastEdge[a] = IG_INVALID_EDGE;
331 mEdgeCount[a] = 0;
332 }
333
334 for(PxU32 a = 0; a < Node::eTYPE_COUNT; ++a)
335 {
336 mSize[a] = 0;
337 }
338 }
339};
340
341struct TraversalState
342{
343 NodeIndex mNodeIndex;
344 PxU32 mCurrentIndex;
345 PxU32 mPrevIndex;
346 PxU32 mDepth;
347
348 TraversalState()
349 {
350 }
351
352 TraversalState(NodeIndex nodeIndex, PxU32 currentIndex, PxU32 prevIndex, PxU32 depth) :
353 mNodeIndex(nodeIndex), mCurrentIndex(currentIndex), mPrevIndex(prevIndex), mDepth(depth)
354 {
355 }
356};
357
358struct QueueElement
359{
360 TraversalState* mState;
361 PxU32 mHopCount;
362
363 QueueElement()
364 {
365 }
366
367 QueueElement(TraversalState* state, PxU32 hopCount) : mState(state), mHopCount(hopCount)
368 {
369 }
370};
371
372struct NodeComparator
373{
374 NodeComparator()
375 {
376 }
377
378 bool operator() (const QueueElement& node0, const QueueElement& node1) const
379 {
380 return node0.mHopCount < node1.mHopCount;
381 }
382private:
383 NodeComparator& operator = (const NodeComparator&);
384};
385
386
387class IslandSim
388{
389 HandleManager<IslandId> mIslandHandles; //! Handle manager for islands
390
391 Ps::Array<Node> mNodes; //! The nodes used in the constraint graph
392 Ps::Array<PxU32> mActiveNodeIndex; //! The active node index for each node
393 Cm::BlockArray<Edge> mEdges;
394 Cm::BlockArray<EdgeInstance> mEdgeInstances; //! Edges used to connect nodes in the constraint graph
395 Ps::Array<Island> mIslands; //! The array of islands
396 Ps::Array<PxU32> mIslandStaticTouchCount; //! Array of static touch counts per-island
397
398
399 Ps::Array<NodeIndex> mActiveNodes[Node::eTYPE_COUNT]; //! An array of active nodes
400 Ps::Array<NodeIndex> mActiveKinematicNodes; //! An array of active or referenced kinematic nodes
401 Ps::Array<EdgeIndex> mActivatedEdges[Edge::eEDGE_TYPE_COUNT]; //! An array of active edges
402
403 PxU32 mActiveEdgeCount[Edge::eEDGE_TYPE_COUNT];
404
405 Ps::Array<PxU32> mHopCounts; //! The observed number of "hops" from a given node to its root node. May be inaccurate but used to accelerate searches.
406 Ps::Array<NodeIndex> mFastRoute; //! The observed last route from a given node to the root node. We try the fast route (unless its broken) before trying others.
407
408 Ps::Array<IslandId> mIslandIds; //! The array of per-node island ids
409
410 Cm::BitMap mIslandAwake; //! Indicates whether an island is awake or not
411
412 Cm::BitMap mActiveContactEdges;
413
414 //An array of active islands
415 Ps::Array<IslandId> mActiveIslands;
416
417 PxU32 mInitialActiveNodeCount[Edge::eEDGE_TYPE_COUNT];
418
419 Ps::Array<NodeIndex> mNodesToPutToSleep[Node::eTYPE_COUNT];
420
421 //Input to this frame's island management (changed nodes/edges)
422
423 //Input list of changes observed this frame. If there no changes, no work to be done.
424 Ps::Array<EdgeIndex> mDirtyEdges[Edge::eEDGE_TYPE_COUNT];
425 //Dirty nodes. These nodes lost at least one connection so we need to recompute islands from these nodes
426 //Ps::Array<NodeIndex> mDirtyNodes;
427 Cm::BitMap mDirtyMap;
428 PxU32 mLastMapIndex;
429
430 //An array of nodes to activate
431 Ps::Array<NodeIndex> mActivatingNodes;
432 Ps::Array<EdgeIndex> mDestroyedEdges;
433 Ps::Array<IslandId> mTempIslandIds;
434
435
436 //Temporary, transient data used for traversals. TODO - move to PxsSimpleIslandManager. Or if we keep it here, we can
437 //process multiple island simulations in parallel
438 Cm::PriorityQueue<QueueElement, NodeComparator>
439 mPriorityQueue; //! Priority queue used for graph traversal
440 Ps::Array<TraversalState> mVisitedNodes; //! The list of nodes visited in the current traversal
441 Cm::BitMap mVisitedState; //! Indicates whether a node has been visited
442 Ps::Array<EdgeIndex> mIslandSplitEdges[Edge::eEDGE_TYPE_COUNT];
443
444 Ps::Array<EdgeIndex> mDeactivatingEdges[Edge::eEDGE_TYPE_COUNT];
445
446 Ps::Array<PartitionEdge*>* mFirstPartitionEdges;
447 Cm::BlockArray<NodeIndex>& mEdgeNodeIndices;
448 Ps::Array<physx::PartitionEdge*>* mDestroyedPartitionEdges;
449
450 PxU32* mNpIndexPtr;
451
452 PxU64 mContextId;
453
454public:
455
456 IslandSim(Ps::Array<PartitionEdge*>* firstPartitionEdges, Cm::BlockArray<NodeIndex>& edgeNodeIndices, Ps::Array<PartitionEdge*>* destroyedPartitionEdges, PxU64 contextID);
457 ~IslandSim() {}
458
459 void resize(const PxU32 nbNodes, const PxU32 nbContactManagers, const PxU32 nbConstraints);
460
461 void addRigidBody(PxsRigidBody* body, bool isKinematic, bool isActive, NodeIndex nodeIndex);
462
463 void addArticulation(Sc::ArticulationSim* articulation, Dy::ArticulationV* llArtic, bool isActive, NodeIndex nodeIndex);
464
465 void addContactManager(PxsContactManager* manager, NodeIndex nodeHandle1, NodeIndex nodeHandle2, EdgeIndex handle);
466
467 void addConstraint(Dy::Constraint* constraint, NodeIndex nodeHandle1, NodeIndex nodeHandle2, EdgeIndex handle);
468
469 void activateNode(NodeIndex index);
470 void deactivateNode(NodeIndex index);
471 void putNodeToSleep(NodeIndex index);
472
473 void removeConnection(EdgeIndex edgeIndex);
474
475 PX_FORCE_INLINE PxU32 getNbNodes() const { return mNodes.size(); }
476
477 PX_FORCE_INLINE PxU32 getNbActiveNodes(Node::NodeType type) const { return mActiveNodes[type].size(); }
478
479 PX_FORCE_INLINE const NodeIndex* getActiveNodes(Node::NodeType type) const { return mActiveNodes[type].begin(); }
480
481 PX_FORCE_INLINE PxU32 getNbActiveKinematics() const { return mActiveKinematicNodes.size(); }
482
483 PX_FORCE_INLINE const NodeIndex* getActiveKinematics() const { return mActiveKinematicNodes.begin(); }
484
485 PX_FORCE_INLINE PxU32 getNbNodesToActivate(Node::NodeType type) const { return mActiveNodes[type].size() - mInitialActiveNodeCount[type]; }
486
487 PX_FORCE_INLINE const NodeIndex* getNodesToActivate(Node::NodeType type) const { return mActiveNodes[type].begin() + mInitialActiveNodeCount[type]; }
488
489 PX_FORCE_INLINE PxU32 getNbNodesToDeactivate(Node::NodeType type) const { return mNodesToPutToSleep[type].size(); }
490
491 PX_FORCE_INLINE const NodeIndex* getNodesToDeactivate(Node::NodeType type) const { return mNodesToPutToSleep[type].begin(); }
492
493 PX_FORCE_INLINE PxU32 getNbActivatedEdges(Edge::EdgeType type) const { return mActivatedEdges[type].size(); }
494
495 PX_FORCE_INLINE const EdgeIndex* getActivatedEdges(Edge::EdgeType type) const { return mActivatedEdges[type].begin(); }
496
497 PX_FORCE_INLINE PxU32 getNbActiveEdges(Edge::EdgeType type) const { return mActiveEdgeCount[type]; }
498
499 PX_FORCE_INLINE PartitionEdge* getFirstPartitionEdge(IG::EdgeIndex edgeIndex) const { return (*mFirstPartitionEdges)[edgeIndex]; }
500 PX_FORCE_INLINE void setFirstPartitionEdge(IG::EdgeIndex edgeIndex, PartitionEdge* partitionEdge) { (*mFirstPartitionEdges)[edgeIndex] = partitionEdge; }
501
502 //PX_FORCE_INLINE const EdgeIndex* getActiveEdges(Edge::EdgeType type) const { return mActiveEdges[type].begin(); }
503
504 PX_FORCE_INLINE PxsRigidBody* getRigidBody(NodeIndex nodeIndex) const
505 {
506 const Node& node = mNodes[nodeIndex.index()];
507 PX_ASSERT(node.mType == Node::eRIGID_BODY_TYPE);
508 return node.mRigidBody;
509 }
510
511 PX_FORCE_INLINE Dy::ArticulationV* getLLArticulation(NodeIndex nodeIndex) const
512 {
513 const Node& node = mNodes[nodeIndex.index()];
514 PX_ASSERT(node.mType == Node::eARTICULATION_TYPE);
515 return node.mLLArticulation;
516 }
517
518 PX_FORCE_INLINE void clearDeactivations()
519 {
520 mNodesToPutToSleep[0].forceSize_Unsafe(size: 0);
521 mNodesToPutToSleep[1].forceSize_Unsafe(size: 0);
522
523 mDeactivatingEdges[0].forceSize_Unsafe(size: 0);
524 mDeactivatingEdges[1].forceSize_Unsafe(size: 0);
525 }
526
527 PX_FORCE_INLINE const Island& getIsland(IG::IslandId islandIndex) const { return mIslands[islandIndex]; }
528
529 PX_FORCE_INLINE PxU32 getNbActiveIslands() const { return mActiveIslands.size(); }
530 PX_FORCE_INLINE const IslandId* getActiveIslands() const { return mActiveIslands.begin(); }
531
532 PX_FORCE_INLINE PxU32 getNbDeactivatingEdges(const IG::Edge::EdgeType edgeType) const { return mDeactivatingEdges[edgeType].size(); }
533 PX_FORCE_INLINE const EdgeIndex* getDeactivatingEdges(const IG::Edge::EdgeType edgeType) const { return mDeactivatingEdges[edgeType].begin(); }
534
535 PX_FORCE_INLINE PxU32 getNbDestroyedEdges() const { return mDestroyedEdges.size(); }
536 PX_FORCE_INLINE const EdgeIndex* getDestroyedEdges() const { return mDestroyedEdges.begin(); }
537
538 PX_FORCE_INLINE PxU32 getNbDestroyedPartitionEdges() const { return mDestroyedPartitionEdges->size(); }
539 PX_FORCE_INLINE const PartitionEdge*const * getDestroyedPartitionEdges() const { return mDestroyedPartitionEdges->begin(); }
540 PX_FORCE_INLINE PartitionEdge** getDestroyedPartitionEdges() { return mDestroyedPartitionEdges->begin(); }
541
542 PX_FORCE_INLINE PxU32 getNbDirtyEdges(IG::Edge::EdgeType type) const { return mDirtyEdges[type].size(); }
543 PX_FORCE_INLINE const EdgeIndex* getDirtyEdges(IG::Edge::EdgeType type) const { return mDirtyEdges[type].begin(); }
544
545 PX_FORCE_INLINE const Edge& getEdge(const EdgeIndex edgeIndex) const { return mEdges[edgeIndex]; }
546
547 PX_FORCE_INLINE Edge& getEdge(const EdgeIndex edgeIndex) { return mEdges[edgeIndex]; }
548
549 PX_FORCE_INLINE const Node& getNode(const NodeIndex& nodeIndex) const { return mNodes[nodeIndex.index()]; }
550
551 PX_FORCE_INLINE const Island& getIsland(const NodeIndex& nodeIndex) const { PX_ASSERT(mIslandIds[nodeIndex.index()] != IG_INVALID_ISLAND); return mIslands[mIslandIds[nodeIndex.index()]]; }
552
553 PX_FORCE_INLINE PxU32 getIslandStaticTouchCount(const NodeIndex& nodeIndex) const { PX_ASSERT(mIslandIds[nodeIndex.index()] != IG_INVALID_ISLAND); return mIslandStaticTouchCount[mIslandIds[nodeIndex.index()]]; }
554
555 PX_FORCE_INLINE const Cm::BitMap& getActiveContactManagerBitmap() const { return mActiveContactEdges; }
556
557 PX_FORCE_INLINE PxU32 getActiveNodeIndex(const NodeIndex& nodeIndex) const { PxU32 activeNodeIndex = mActiveNodeIndex[nodeIndex.index()]; return activeNodeIndex;}
558
559 PX_FORCE_INLINE const PxU32* getActiveNodeIndex() const { return mActiveNodeIndex.begin(); }
560
561 PX_FORCE_INLINE PxU32 getNbActiveNodeIndex() const { return mActiveNodeIndex.size(); }
562
563 void setKinematic(IG::NodeIndex nodeIndex);
564
565 void setDynamic(IG::NodeIndex nodeIndex);
566
567 PX_FORCE_INLINE void setEdgeNodeIndexPtr(PxU32* ptr) { mNpIndexPtr = ptr; }
568
569 PX_FORCE_INLINE NodeIndex getNodeIndex1(IG::EdgeIndex index) const { return mEdgeNodeIndices[2 * index]; }
570 PX_FORCE_INLINE NodeIndex getNodeIndex2(IG::EdgeIndex index) const { return mEdgeNodeIndices[2 * index + 1]; }
571
572 PX_FORCE_INLINE PxU32* getEdgeNodeIndexPtr() const { return mNpIndexPtr; }
573 PX_FORCE_INLINE PxU64 getContextId() const { return mContextId; }
574
575 PxU32 getNbIslands() const { return mIslandStaticTouchCount.size(); }
576
577 const PxU32* getIslandStaticTouchCount() const { return mIslandStaticTouchCount.begin(); }
578
579 const PxU32* getIslandIds() const { return mIslandIds.begin(); }
580
581 bool checkInternalConsistency();
582
583private:
584
585 void insertNewEdges();
586 void removeDestroyedEdges();
587 void wakeIslands();
588 void wakeIslands2();
589 void processNewEdges();
590 void processLostEdges(Ps::Array<NodeIndex>& destroyedNodes, bool allowDeactivation, bool permitKinematicDeactivation, PxU32 dirtyNodeLimit);
591
592 void removeConnectionInternal(EdgeIndex edgeIndex);
593
594 void addConnection(NodeIndex nodeHandle1, NodeIndex nodeHandle2, Edge::EdgeType edgeType, EdgeIndex handle);
595
596 void addConnectionToGraph(EdgeIndex index);
597 void removeConnectionFromGraph(EdgeIndex edgeIndex);
598 void connectEdge(EdgeInstance& instance, EdgeInstanceIndex edgeIndex, Node& source, NodeIndex destination);
599 void disconnectEdge(EdgeInstance& instance, EdgeInstanceIndex edgeIndex, Node& node);
600
601 //Merges 2 islands together. The returned id is the id of the merged island
602 IslandId mergeIslands(IslandId island0, IslandId island1, NodeIndex node0, NodeIndex node1);
603
604 void mergeIslandsInternal(Island& island0, Island& island1, IslandId islandId0, IslandId islandId1, NodeIndex node0, NodeIndex node1);
605
606
607 IslandSim& operator = (const IslandSim&);
608 IslandSim(const IslandSim&);
609
610 void unwindRoute(PxU32 traversalIndex, NodeIndex lastNode, PxU32 hopCount, IslandId id);
611
612 void activateIsland(IslandId island);
613
614 void deactivateIsland(IslandId island);
615
616 bool canFindRoot(NodeIndex startNode, NodeIndex targetNode, Ps::Array<NodeIndex>* visitedNodes);
617
618 bool tryFastPath(NodeIndex startNode, NodeIndex targetNode, IslandId islandId);
619
620 bool findRoute(NodeIndex startNode, NodeIndex targetNode, IslandId islandId);
621
622 bool isPathTo(NodeIndex startNode, NodeIndex targetNode);
623
624 void addNode(bool isActive, bool isKinematic, Node::NodeType type, NodeIndex nodeIndex);
625
626 void activateNodeInternal(NodeIndex index);
627 void deactivateNodeInternal(NodeIndex index);
628
629 PX_FORCE_INLINE void notifyReadyForSleeping(const NodeIndex nodeIndex)
630 {
631 Node& node = mNodes[nodeIndex.index()];
632 //PX_ASSERT(node.isActive());
633 node.setIsReadyForSleeping();
634 }
635
636 PX_FORCE_INLINE void notifyNotReadyForSleeping(const NodeIndex nodeIndex)
637 {
638 Node& node = mNodes[nodeIndex.index()];
639 PX_ASSERT(node.isActive() || node.isActivating());
640 node.clearIsReadyForSleeping();
641 }
642
643 PX_FORCE_INLINE void markIslandActive(IslandId islandId)
644 {
645 Island& island = mIslands[islandId];
646 PX_ASSERT(!mIslandAwake.test(islandId));
647 PX_ASSERT(island.mActiveIndex == IG_INVALID_ISLAND);
648
649 mIslandAwake.set(islandId);
650 island.mActiveIndex = mActiveIslands.size();
651 mActiveIslands.pushBack(a: islandId);
652 }
653
654 PX_FORCE_INLINE void markIslandInactive(IslandId islandId)
655 {
656 Island& island = mIslands[islandId];
657 PX_ASSERT(mIslandAwake.test(islandId));
658 PX_ASSERT(island.mActiveIndex != IG_INVALID_ISLAND);
659 PX_ASSERT(mActiveIslands[island.mActiveIndex] == islandId);
660 IslandId replaceId = mActiveIslands[mActiveIslands.size()-1];
661 PX_ASSERT(mIslandAwake.test(replaceId));
662 Island& replaceIsland = mIslands[replaceId];
663 replaceIsland.mActiveIndex = island.mActiveIndex;
664 mActiveIslands[island.mActiveIndex] = replaceId;
665 mActiveIslands.forceSize_Unsafe(size: mActiveIslands.size()-1);
666 island.mActiveIndex = IG_INVALID_ISLAND;
667 mIslandAwake.reset(index: islandId);
668 }
669
670 PX_FORCE_INLINE void markKinematicActive(NodeIndex index)
671 {
672 Node& node = mNodes[index.index()];
673 PX_ASSERT(node.isKinematic());
674 if(node.mActiveRefCount == 0 && mActiveNodeIndex[index.index()] == IG_INVALID_NODE)
675 {
676 //PX_ASSERT(mActiveNodeIndex[index.index()] == IG_INVALID_NODE);
677 //node.mActiveNodeIndex = mActiveKinematicNodes.size();
678 mActiveNodeIndex[index.index()] = mActiveKinematicNodes.size();
679 mActiveKinematicNodes.pushBack(a: index);
680 }
681 }
682
683 PX_FORCE_INLINE void markKinematicInactive(NodeIndex index)
684 {
685 Node& node = mNodes[index.index()];
686 PX_ASSERT(node.isKinematic());
687 PX_ASSERT(mActiveNodeIndex[index.index()] != IG_INVALID_NODE);
688 PX_ASSERT(mActiveKinematicNodes[mActiveNodeIndex[index.index()]].index() == index.index());
689
690 if(node.mActiveRefCount == 0)
691 {
692 //Only remove from active kinematic list if it has no active contacts referencing it *and* it is asleep
693 if(mActiveNodeIndex[index.index()] != IG_INVALID_NODE)
694 {
695 //Need to verify active node index because there is an edge case where a node could be woken, then put to
696 //sleep in the same frame. This would mean that it would not have an active index at this stage.
697 NodeIndex replaceIndex = mActiveKinematicNodes.back();
698 PX_ASSERT(mActiveNodeIndex[replaceIndex.index()] == mActiveKinematicNodes.size()-1);
699 mActiveNodeIndex[replaceIndex.index()] = mActiveNodeIndex[index.index()];
700 mActiveKinematicNodes[mActiveNodeIndex[index.index()]] = replaceIndex;
701 mActiveKinematicNodes.forceSize_Unsafe(size: mActiveKinematicNodes.size()-1);
702 mActiveNodeIndex[index.index()] = IG_INVALID_NODE;
703 }
704 }
705 }
706
707 PX_FORCE_INLINE void markActive(NodeIndex index)
708 {
709 Node& node = mNodes[index.index()];
710 PX_ASSERT(!node.isKinematic());
711 PX_ASSERT(mActiveNodeIndex[index.index()] == IG_INVALID_NODE);
712 mActiveNodeIndex[index.index()] = mActiveNodes[node.mType].size();
713 mActiveNodes[node.mType].pushBack(a: index);
714 }
715
716 PX_FORCE_INLINE void markInactive(NodeIndex index)
717 {
718 Node& node = mNodes[index.index()];
719
720 PX_ASSERT(!node.isKinematic());
721 PX_ASSERT(mActiveNodeIndex[index.index()] != IG_INVALID_NODE);
722
723 Ps::Array<NodeIndex>& activeNodes = mActiveNodes[node.mType];
724
725 PX_ASSERT(activeNodes[mActiveNodeIndex[index.index()]].index() == index.index());
726 const PxU32 initialActiveNodeCount = mInitialActiveNodeCount[node.mType];
727
728 if(mActiveNodeIndex[index.index()] < initialActiveNodeCount)
729 {
730 //It's in the initial active node set. We retain a list of active nodes, where the existing active nodes
731 //are at the beginning of the array and the newly activated nodes are at the end of the array...
732 //The solution is to move the node to the end of the initial active node list in this case
733 PxU32 activeNodeIndex = mActiveNodeIndex[index.index()];
734 NodeIndex replaceIndex = activeNodes[initialActiveNodeCount-1];
735 PX_ASSERT(mActiveNodeIndex[replaceIndex.index()] == initialActiveNodeCount-1);
736 mActiveNodeIndex[index.index()] = mActiveNodeIndex[replaceIndex.index()];
737 mActiveNodeIndex[replaceIndex.index()] = activeNodeIndex;
738 activeNodes[activeNodeIndex] = replaceIndex;
739 activeNodes[mActiveNodeIndex[index.index()]] = index;
740 mInitialActiveNodeCount[node.mType]--;
741 }
742
743 PX_ASSERT(!node.isKinematic());
744 PX_ASSERT(mActiveNodeIndex[index.index()] != IG_INVALID_NODE);
745 PX_ASSERT(activeNodes[mActiveNodeIndex[index.index()]].index() == index.index());
746
747 NodeIndex replaceIndex = activeNodes.back();
748 PX_ASSERT(mActiveNodeIndex[replaceIndex.index()] == activeNodes.size()-1);
749 mActiveNodeIndex[replaceIndex.index()] = mActiveNodeIndex[index.index()];
750 activeNodes[mActiveNodeIndex[index.index()]] = replaceIndex;
751 activeNodes.forceSize_Unsafe(size: activeNodes.size()-1);
752 mActiveNodeIndex[index.index()] = IG_INVALID_NODE;
753 }
754
755 PX_FORCE_INLINE void markEdgeActive(EdgeIndex index)
756 {
757 Edge& edge = mEdges[index];
758
759 PX_ASSERT((edge.mEdgeState & Edge::eACTIVATING) == 0);
760
761 edge.mEdgeState |= Edge::eACTIVATING;
762
763 mActivatedEdges[edge.mEdgeType].pushBack(a: index);
764
765 mActiveEdgeCount[edge.mEdgeType]++;
766
767 //Set the active bit...
768 if(edge.mEdgeType == Edge::eCONTACT_MANAGER)
769 mActiveContactEdges.set(index);
770
771 NodeIndex nodeIndex1 = mEdgeNodeIndices[2 * index];
772 NodeIndex nodeIndex2 = mEdgeNodeIndices[2 * index + 1];
773
774 if (nodeIndex1.index() != IG_INVALID_NODE && nodeIndex2.index() != IG_INVALID_NODE)
775 {
776 PX_ASSERT((!mNodes[nodeIndex1.index()].isKinematic()) || (!mNodes[nodeIndex2.index()].isKinematic()) || edge.getEdgeType() == IG::Edge::eCONTACT_MANAGER);
777 {
778 Node& node = mNodes[nodeIndex1.index()];
779
780 if(node.mActiveRefCount == 0 && node.isKinematic() && !(node.isActive() || node.isActivating()))
781 {
782 //Add to active kinematic list
783 markKinematicActive(index: nodeIndex1);
784 }
785 node.mActiveRefCount++;
786 }
787
788 {
789 Node& node = mNodes[nodeIndex2.index()];
790 if(node.mActiveRefCount == 0 && node.isKinematic() && !(node.isActive() || node.isActivating()))
791 {
792 //Add to active kinematic list
793 markKinematicActive(index: nodeIndex2);
794 }
795 node.mActiveRefCount++;
796 }
797 }
798
799 }
800
801 void removeEdgeFromActivatingList(EdgeIndex index);
802
803 PX_FORCE_INLINE void removeEdgeFromIsland(Island& island, EdgeIndex edgeIndex)
804 {
805 Edge& edge = mEdges[edgeIndex];
806 if(edge.mNextIslandEdge != IG_INVALID_EDGE)
807 {
808 PX_ASSERT(mEdges[edge.mNextIslandEdge].mPrevIslandEdge == edgeIndex);
809 mEdges[edge.mNextIslandEdge].mPrevIslandEdge = edge.mPrevIslandEdge;
810 }
811 else
812 {
813 PX_ASSERT(island.mLastEdge[edge.mEdgeType] == edgeIndex);
814 island.mLastEdge[edge.mEdgeType] = edge.mPrevIslandEdge;
815 }
816
817 if(edge.mPrevIslandEdge != IG_INVALID_EDGE)
818 {
819 PX_ASSERT(mEdges[edge.mPrevIslandEdge].mNextIslandEdge == edgeIndex);
820 mEdges[edge.mPrevIslandEdge].mNextIslandEdge = edge.mNextIslandEdge;
821 }
822 else
823 {
824 PX_ASSERT(island.mFirstEdge[edge.mEdgeType] == edgeIndex);
825 island.mFirstEdge[edge.mEdgeType] = edge.mNextIslandEdge;
826 }
827
828 island.mEdgeCount[edge.mEdgeType]--;
829 edge.mNextIslandEdge = edge.mPrevIslandEdge = IG_INVALID_EDGE;
830 }
831
832 PX_FORCE_INLINE void addEdgeToIsland(Island& island, EdgeIndex edgeIndex)
833 {
834 Edge& edge = mEdges[edgeIndex];
835 PX_ASSERT(edge.mNextIslandEdge == IG_INVALID_EDGE && edge.mPrevIslandEdge == IG_INVALID_EDGE);
836
837 if(island.mLastEdge[edge.mEdgeType] != IG_INVALID_EDGE)
838 {
839 PX_ASSERT(mEdges[island.mLastEdge[edge.mEdgeType]].mNextIslandEdge == IG_INVALID_EDGE);
840 mEdges[island.mLastEdge[edge.mEdgeType]].mNextIslandEdge = edgeIndex;
841 }
842 else
843 {
844 PX_ASSERT(island.mFirstEdge[edge.mEdgeType] == IG_INVALID_EDGE);
845 island.mFirstEdge[edge.mEdgeType] = edgeIndex;
846 }
847
848 edge.mPrevIslandEdge = island.mLastEdge[edge.mEdgeType];
849 island.mLastEdge[edge.mEdgeType] = edgeIndex;
850 island.mEdgeCount[edge.mEdgeType]++;
851 }
852
853 PX_FORCE_INLINE void removeNodeFromIsland(Island& island, NodeIndex nodeIndex)
854 {
855 Node& node = mNodes[nodeIndex.index()];
856 if(node.mNextNode.isValid())
857 {
858 PX_ASSERT(mNodes[node.mNextNode.index()].mPrevNode.index() == nodeIndex.index());
859 mNodes[node.mNextNode.index()].mPrevNode = node.mPrevNode;
860 }
861 else
862 {
863 PX_ASSERT(island.mLastNode.index() == nodeIndex.index());
864 island.mLastNode = node.mPrevNode;
865 }
866
867 if(node.mPrevNode.isValid())
868 {
869 PX_ASSERT(mNodes[node.mPrevNode.index()].mNextNode.index() == nodeIndex.index());
870 mNodes[node.mPrevNode.index()].mNextNode = node.mNextNode;
871 }
872 else
873 {
874 PX_ASSERT(island.mRootNode.index() == nodeIndex.index());
875 island.mRootNode = node.mNextNode;
876 }
877
878 island.mSize[node.mType]--;
879
880 node.mNextNode = NodeIndex(); node.mPrevNode = NodeIndex();
881 }
882
883 //void setEdgeConnectedInternal(EdgeIndex edgeIndex);
884
885 //void setEdgeDisconnectedInternal(EdgeIndex edgeIndex);
886
887 friend class SimpleIslandManager;
888 friend class ThirdPassTask;
889
890};
891
892
893}
894
895
896struct PartitionIndexData
897{
898 PxU16 mPartitionIndex; //! The current partition this edge is in. Used to find the edge efficiently. PxU8 is probably too small (256 partitions max) but PxU16 should be more than enough
899 PxU8 mPatchIndex; //! The patch index for this partition edge. There may be multiple entries for a given edge if there are multiple patches.
900 PxU8 mCType; //! The type of constraint this is
901 PxU32 mPartitionEntryIndex; //! index of partition edges for this partition
902};
903
904struct PartitionNodeData
905{
906 IG::NodeIndex mNodeIndex0;
907 IG::NodeIndex mNodeIndex1;
908 PxU32 mNextIndex0;
909 PxU32 mNextIndex1;
910};
911
912
913#define INVALID_PARTITION_INDEX 0xFFFF
914
915struct PartitionEdge
916{
917 IG::EdgeIndex mEdgeIndex; //! The edge index into the island manager. Used to identify the contact manager/constraint
918 IG::NodeIndex mNode0; //! The node index for node 0. Can be obtained from the edge index alternatively
919 IG::NodeIndex mNode1; //! The node idnex for node 1. Can be obtained from the edge index alternatively
920 bool mInfiniteMass0; //! Whether body 0 is kinematic
921 bool mArticulation0; //! Whether body 0 is an articulation link
922 bool mInfiniteMass1; //! Whether body 1 is kinematic
923 bool mArticulation1; //! Whether body 1 is an articulation link
924
925 PartitionEdge* mNextPatch; //! for the contact manager has more than 1 patch, we have next patch's edge and previous patch's edge to connect to this edge
926
927 PxU32 mUniqueIndex; //! a unique ID for this edge
928
929
930 //KS - This constructor explicitly does not set mUniqueIndex. It is filled in by the pool allocator and this constructor
931 //is called afterwards. We do not want to stomp the uniqueIndex value
932 PartitionEdge() : mEdgeIndex(IG_INVALID_EDGE), mInfiniteMass0(false), mArticulation0(false),
933 mInfiniteMass1(false), mArticulation1(false), mNextPatch(NULL)//, mUniqueIndex(IG_INVALID_EDGE)
934 {
935 }
936};
937
938}
939
940#endif
941

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