Initial commit:

PhysX 3.4.0 Update @ 21294896
APEX 1.4.0 Update @ 21275617

[CL 21300167]

1 files changed, 505 insertions, 0 deletions

diff --git a/PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionManager.cpp b/PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionManager.cpp
new file mode 100644
index 00000000..9880f622
--- /dev/null
+++ b/PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionManager.cpp
@@ -0,0 +1,505 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+
+#include "PxcScratchAllocator.h"
+#include "ScConstraintProjectionManager.h"
+#include "ScBodySim.h"
+#include "ScConstraintSim.h"
+#include "ScConstraintInteraction.h"
+
+using namespace physx;
+
+
+namespace physx
+{
+namespace Sc
+{
+
+template<typename T, const PxU32 elementsPerBlock>
+class ScratchAllocatorList
+{
+private:
+	struct ElementBlock
+	{
+		PX_FORCE_INLINE ElementBlock() {}
+		PX_FORCE_INLINE void init(PxU32 countAtStart) { next = NULL; count = countAtStart; }
+
+		ElementBlock* next;
+		PxU32 count;
+		T elements[elementsPerBlock];
+	};
+
+	PX_FORCE_INLINE const ScratchAllocatorList& operator=(const ScratchAllocatorList&) {}
+
+
+public:
+	class Iterator
+	{
+		friend class ScratchAllocatorList;
+
+	public:
+		T const* getNext()
+		{
+			if (mCurrentBlock)
+			{
+				if (mIndex < mCurrentBlock->count)
+				{
+					return &mCurrentBlock->elements[mIndex++];
+				}
+				else
+				{
+					if (mCurrentBlock->next)
+					{
+						PX_ASSERT(mCurrentBlock->count == elementsPerBlock);
+						mCurrentBlock = mCurrentBlock->next;
+						PX_ASSERT(mCurrentBlock->count > 0);
+
+						mIndex = 1;
+						return &mCurrentBlock->elements[0];
+					}
+					else
+						return NULL;
+				}
+			}
+			else
+				return NULL;
+		}
+
+	private:
+		Iterator(const ElementBlock* startBlock) : mCurrentBlock(startBlock), mIndex(0) {}
+
+	private:
+		const ElementBlock* mCurrentBlock;
+		PxU32 mIndex;
+	};
+
+	PX_FORCE_INLINE ScratchAllocatorList(PxcScratchAllocator& scratchAllocator) : mScratchAllocator(scratchAllocator)
+	{
+		mFirstBlock = reinterpret_cast<ElementBlock*>(scratchAllocator.alloc(sizeof(ElementBlock), true));
+		if (mFirstBlock)
+			mFirstBlock->init(0);
+
+		mCurrentBlock = mFirstBlock;
+	}
+
+	PX_FORCE_INLINE ~ScratchAllocatorList()
+	{
+		freeMemory();
+	}
+
+	PX_FORCE_INLINE bool add(const T& element)
+	{
+		if (mCurrentBlock)
+		{
+			if (mCurrentBlock->count < elementsPerBlock)
+			{
+				mCurrentBlock->elements[mCurrentBlock->count] = element;
+				mCurrentBlock->count++;
+				return true;
+			}
+			else
+			{
+				PX_ASSERT(mCurrentBlock->next == NULL);
+				PX_ASSERT(mCurrentBlock->count == elementsPerBlock);
+
+				ElementBlock* newBlock = reinterpret_cast<ElementBlock*>(mScratchAllocator.alloc(sizeof(ElementBlock), true));
+				if (newBlock)
+				{
+					newBlock->init(1);
+					newBlock->elements[0] = element;
+					mCurrentBlock->next = newBlock;
+					mCurrentBlock = newBlock;
+					return true;
+				}
+				else
+					return false;
+			}
+		}
+		else
+			return false;
+	}
+
+	PX_FORCE_INLINE Iterator getIterator() const
+	{
+		return Iterator(mFirstBlock);
+	}
+
+	PX_FORCE_INLINE void freeMemory()
+	{
+		ElementBlock* block = mFirstBlock;
+
+		while(block)
+		{
+			ElementBlock* blockToFree = block;
+			block = block->next;
+
+			mScratchAllocator.free(blockToFree);
+		}
+	}
+
+
+private:
+	PxcScratchAllocator& mScratchAllocator;
+	ElementBlock* mFirstBlock;
+	ElementBlock* mCurrentBlock;
+};
+
+}
+}
+
+
+Sc::ConstraintProjectionManager::ConstraintProjectionManager() : 
+	mNodePool(PX_DEBUG_EXP("projectionNodePool"))
+{
+}
+
+
+void Sc::ConstraintProjectionManager::addToPendingGroupUpdates(Sc::ConstraintSim& s)
+{
+	PX_ASSERT(!s.readFlag(ConstraintSim::ePENDING_GROUP_UPDATE));
+	bool isNew = mPendingGroupUpdates.insert(&s);
+	PX_UNUSED(isNew);
+	PX_ASSERT(isNew);
+
+	s.setFlag(ConstraintSim::ePENDING_GROUP_UPDATE);
+}
+
+
+void Sc::ConstraintProjectionManager::removeFromPendingGroupUpdates(Sc::ConstraintSim& s)
+{
+	PX_ASSERT(s.readFlag(ConstraintSim::ePENDING_GROUP_UPDATE));
+	bool didExist = mPendingGroupUpdates.erase(&s);
+	PX_UNUSED(didExist);
+	PX_ASSERT(didExist);
+
+	s.clearFlag(ConstraintSim::ePENDING_GROUP_UPDATE);
+}
+
+
+void Sc::ConstraintProjectionManager::addToPendingTreeUpdates(ConstraintGroupNode& n)
+{
+	PX_ASSERT(&n == &n.getRoot());
+	PX_ASSERT(!n.readFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE));
+	bool isNew = mPendingTreeUpdates.insert(&n);
+	PX_UNUSED(isNew);
+	PX_ASSERT(isNew);
+
+	n.raiseFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE);
+}
+
+
+void Sc::ConstraintProjectionManager::removeFromPendingTreeUpdates(ConstraintGroupNode& n)
+{
+	PX_ASSERT(&n == &n.getRoot());
+	PX_ASSERT(n.readFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE));
+	bool didExist = mPendingTreeUpdates.erase(&n);
+	PX_UNUSED(didExist);
+	PX_ASSERT(didExist);
+
+	n.clearFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE);
+}
+
+
+PX_INLINE Sc::ConstraintGroupNode* Sc::ConstraintProjectionManager::createGroupNode(BodySim& b)
+{
+	ConstraintGroupNode* n = mNodePool.construct(b);
+	b.setConstraintGroup(n);
+	return n;
+}
+
+
+//
+// Implementation of UNION of 
+// UNION-FIND algo.
+// It also updates the group traversal
+// linked list.
+//
+void Sc::ConstraintProjectionManager::groupUnion(ConstraintGroupNode& root0, ConstraintGroupNode& root1)
+{
+	// Should only get called for the roots
+	PX_ASSERT(&root0 == root0.parent);
+	PX_ASSERT(&root1 == root1.parent);
+
+	if (&root0 != &root1)	//different groups?  If not, its already merged.
+	{
+		//UNION(this, other);	//union-find algo unites groups.
+		ConstraintGroupNode* newRoot;
+		ConstraintGroupNode* otherRoot;
+		if (root0.rank > root1.rank)
+		{
+			//hisGroup appended to mygroup.
+			newRoot = &root0;
+			otherRoot = &root1;
+		}
+		else
+		{
+			//myGroup appended to hisGroup. 
+			newRoot = &root1;
+			otherRoot = &root0;
+			//there is a chance that the two ranks were equal, in which case the tree depth just increased.
+			root1.rank++;
+		}
+
+		PX_ASSERT(newRoot->parent == newRoot);
+		otherRoot->parent = newRoot;
+		
+		//update traversal linked list:
+		newRoot->tail->next = otherRoot;
+		newRoot->tail = otherRoot->tail;
+	}
+}
+
+
+//
+// Add a body to a constraint projection group.
+//
+void Sc::ConstraintProjectionManager::addToGroup(BodySim& b, BodySim* other, ConstraintSim& c)
+{
+	// If both bodies of the constraint are defined, we want to fetch the reference to the group root
+	// from body 0 by default (allows to avoid checking both)
+	PX_ASSERT(&b == c.getBody(0) || (c.getBody(0) == NULL && &b == c.getBody(1)));
+	PX_UNUSED(c);
+
+	ConstraintGroupNode* myRoot;
+	if (!b.getConstraintGroup())
+		myRoot = createGroupNode(b);
+	else
+	{
+		myRoot = &b.getConstraintGroup()->getRoot();
+		if (myRoot->hasProjectionTreeRoot())
+			myRoot->purgeProjectionTrees();  // If a new constraint gets added to a constraint group, projection trees need to be recreated
+	}
+
+	if (other)
+	{
+		ConstraintGroupNode* otherRoot;
+		if (!other->getConstraintGroup())
+			otherRoot = createGroupNode(*other);
+		else
+		{
+			otherRoot = &other->getConstraintGroup()->getRoot();
+			if (otherRoot->hasProjectionTreeRoot())
+				otherRoot->purgeProjectionTrees();  // If a new constraint gets added to a constraint group, projection trees need to be recreated
+		}
+
+		//merge the two groups, if disjoint.
+		groupUnion(*myRoot, *otherRoot);
+	}
+}
+
+
+//
+// Add all projection constraints connected to the specified body to the pending update list but
+// ignore the specified constraint.
+//
+void Sc::ConstraintProjectionManager::markConnectedConstraintsForUpdate(BodySim& b, ConstraintSim* c)
+{
+	PxU32 size = b.getActorInteractionCount();
+	Interaction** interactions = b.getActorInteractions();
+	while(size--)
+	{
+		Interaction* interaction = *interactions++;
+		if (interaction->getType() == InteractionType::eCONSTRAINTSHADER)
+		{
+			ConstraintSim* ct = static_cast<ConstraintInteraction*>(interaction)->getConstraint();
+
+			if ((ct != c) && ct->needsProjection() && (!ct->readFlag(ConstraintSim::ePENDING_GROUP_UPDATE)))
+			{
+				//mark constraint for pending update:
+				addToPendingGroupUpdates(*ct);
+			}
+		}
+	}
+}
+
+
+//
+// Add all constraints connected to the specified body to an array but
+// ignore the specified constraint.
+//
+PX_FORCE_INLINE static void dumpConnectedConstraints(Sc::BodySim& b, Sc::ConstraintSim* c, Sc::ScratchAllocatorList<Sc::ConstraintSim*>& constraintList)
+{
+	PxU32 size = b.getActorInteractionCount();
+	Sc::Interaction** interactions = b.getActorInteractions();
+	while(size--)
+	{
+		Sc::Interaction* interaction = *interactions++;
+		if (interaction->getType() == Sc::InteractionType::eCONSTRAINTSHADER)
+		{
+			Sc::ConstraintSim* ct = static_cast<Sc::ConstraintInteraction*>(interaction)->getConstraint();
+
+			if ((ct != c) && (!ct->readFlag(Sc::ConstraintSim::ePENDING_GROUP_UPDATE)))
+			{
+				bool success = constraintList.add(ct);
+				PX_UNUSED(success);
+				PX_ASSERT(success);
+			}
+		}
+	}
+}
+
+
+PX_FORCE_INLINE void Sc::ConstraintProjectionManager::processConstraintForGroupBuilding(ConstraintSim* c, ScratchAllocatorList<ConstraintSim*>& constraintList)
+{
+	c->clearFlag(ConstraintSim::ePENDING_GROUP_UPDATE);
+
+	// Find all constraints connected to the two bodies of the dirty constraint.
+	// - Constraints to static anchors are ignored (note: kinematics can't be ignored because they might get switched to dynamics any time which
+	//   does trigger a projection tree rebuild but not a constraint tree rebuild
+	// - Already processed bodies are ignored as well
+	BodySim* b0 = c->getBody(0);
+	if (b0 && !b0->getConstraintGroup())
+	{
+		dumpConnectedConstraints(*b0, c, constraintList);
+	}
+	BodySim* b1 = c->getBody(1);
+	if (b1 && !b1->getConstraintGroup())
+	{
+		dumpConnectedConstraints(*b1, c, constraintList);
+	}
+
+	BodySim* b = c->getAnyBody();
+	PX_ASSERT(b);
+
+	addToGroup(*b, c->getOtherBody(b), *c);  //this will eventually merge some body's constraint groups.
+}
+
+
+void Sc::ConstraintProjectionManager::processPendingUpdates(PxcScratchAllocator& scratchAllocator)
+{
+	//
+	// if there are dirty projection trees, then rebuild them
+	//
+	const PxU32 nbProjectionTreesToUpdate = mPendingTreeUpdates.size();
+	if (nbProjectionTreesToUpdate)
+	{
+		ConstraintGroupNode* const* projectionTreesToUpdate = mPendingTreeUpdates.getEntries();
+		for(PxU32 i=0; i < nbProjectionTreesToUpdate; i++)
+		{
+			ConstraintGroupNode* n = projectionTreesToUpdate[i];
+
+			PX_ASSERT(n == &n->getRoot());  // only root nodes should be in that list
+			PX_ASSERT(n->readFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE));
+
+			n->clearFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE);
+
+			// note: it is valid to get here and not have a projection root. This is the case if all nodes of a constraint graph are kinematic
+			//       at some point (hence no projection root) and later some of those get switched to dynamic.
+			if (n->hasProjectionTreeRoot())
+				n->purgeProjectionTrees();
+			n->buildProjectionTrees();
+		}
+
+		mPendingTreeUpdates.clear();
+	}
+
+	//
+	// if there are new/dirty constraints, update groups
+	//
+	const PxU32 nbProjectionConstraintsToUpdate = mPendingGroupUpdates.size();
+
+	if (nbProjectionConstraintsToUpdate)
+	{
+		ScratchAllocatorList<ConstraintSim*> nonProjectionConstraintList(scratchAllocator);
+
+		ConstraintSim* const* projectionConstraintsToUpdate = mPendingGroupUpdates.getEntries();
+
+#if PX_DEBUG
+		// At the beginning the list should only contain constraints with projection.
+		// Further below other constraints, connected to the constraints with projection, will be added too.
+		for(PxU32 i=0; i < nbProjectionConstraintsToUpdate; i++)
+		{
+			PX_ASSERT(projectionConstraintsToUpdate[i]->needsProjection());
+		}
+#endif
+		for(PxU32 i=0; i < nbProjectionConstraintsToUpdate; i++)
+		{
+			processConstraintForGroupBuilding(projectionConstraintsToUpdate[i], nonProjectionConstraintList);
+		}
+
+		ScratchAllocatorList<ConstraintSim*>::Iterator iter = nonProjectionConstraintList.getIterator();
+		ConstraintSim* const* nextConstraint = iter.getNext();
+		while(nextConstraint)
+		{
+			processConstraintForGroupBuilding(*nextConstraint, nonProjectionConstraintList);
+
+			nextConstraint = iter.getNext();
+		}
+
+		// Now find all the newly made groups and build projection trees.
+		// Don't need to iterate over the additionally constraints since the roots are supposed to be
+		// fetchable from any node.
+		for (PxU32 i=0; i < nbProjectionConstraintsToUpdate; i++)
+		{
+			ConstraintSim* c = projectionConstraintsToUpdate[i];
+			BodySim* b = c->getAnyBody();
+			PX_ASSERT(b);
+			PX_ASSERT(b->getConstraintGroup());
+
+			ConstraintGroupNode& root = b->getConstraintGroup()->getRoot();
+			if (!root.hasProjectionTreeRoot())  // Build projection tree only once
+				root.buildProjectionTrees();
+		}
+
+		mPendingGroupUpdates.clear();
+	}
+}
+
+
+//
+// Called if a body or a constraint gets deleted. All projecting constraints of the
+// group (except the deleted one) are moved to the dirty list and all group nodes are destroyed.
+//
+void Sc::ConstraintProjectionManager::invalidateGroup(ConstraintGroupNode& node, ConstraintSim* deletedConstraint)
+{
+	ConstraintGroupNode* n = &node.getRoot();
+
+	if (n->readFlag(ConstraintGroupNode::ePENDING_TREE_UPDATE))
+	{
+		removeFromPendingTreeUpdates(*n);
+	}
+
+	while (n) //go through nodes in constraint group
+	{
+		markConnectedConstraintsForUpdate(*n->body, deletedConstraint);
+
+		//destroy the body's constraint group information
+
+		ConstraintGroupNode* next = n->next;	//save next node ptr before we destroy it!
+
+		BodySim* b = n->body;
+		b->setConstraintGroup(NULL);
+		if (n->hasProjectionTreeRoot())
+			n->purgeProjectionTrees();
+		mNodePool.destroy(n);
+
+		n = next;
+	}
+}