From 3dfe2108cfab31ba3ee5527e217d0d8e99a51162 Mon Sep 17 00:00:00 2001 From: git perforce import user Date: Tue, 25 Oct 2016 12:29:14 -0600 Subject: Initial commit: PhysX 3.4.0 Update @ 21294896 APEX 1.4.0 Update @ 21275617 [CL 21300167] --- .../src/ScConstraintProjectionManager.cpp | 505 +++++++++++++++++++++ 1 file changed, 505 insertions(+) create mode 100644 PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionManager.cpp (limited to 'PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionManager.cpp') 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 +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(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(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(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& 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(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& 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 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::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; + } +} -- cgit v1.2.3