diff options
Diffstat (limited to 'PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionTree.cpp')
| -rw-r--r-- | PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionTree.cpp | 567 |
1 files changed, 567 insertions, 0 deletions
diff --git a/PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionTree.cpp b/PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionTree.cpp new file mode 100644 index 00000000..5cf0a13f --- /dev/null +++ b/PhysX_3.4/Source/SimulationController/src/ScConstraintProjectionTree.cpp @@ -0,0 +1,567 @@ +// 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 "ScConstraintProjectionTree.h" +#include "ScScene.h" +#include "ScBodySim.h" +#include "ScConstraintCore.h" +#include "ScConstraintSim.h" +#include "ScConstraintInteraction.h" + +#include "PsFoundation.h" +#include "PsBasicTemplates.h" +#include "PsSort.h" +#include "PsArray.h" + +using namespace physx; + +//------------------------------------------------------------------------------------------ +// +// The projection tree related code +// +// Projection trees are built out of a constraint group/graph. The constraint group just tracks +// the constraint connectivity while the projection trees define the projection root and +// the projection order. +// A constraint group can contain multiple projection trees. +// +//------------------------------------------------------------------------------------------ + +class Sc::BodyRank +{ +public: + PX_INLINE bool operator>(const BodyRank & b) const + { + return rank > b.rank; + } + + Sc::ConstraintGroupNode* startingNode; + Sc::ConstraintSim* constraintToFixedAnchor; + PxU32 rank; + + // + // The following weights are defined to fulfill the projection priorities described further below + // + static const PxU32 sOneWayProjection = PxU32(1 << 31); + static const PxU32 sAttachedToStatic = (1 << 30); + static const PxU32 sAttachedToKinematic = (1 << 29); + static const PxU32 sAllDominantDynamic = (1 << 28); // if for a dynamic body all connections with projection, are one-way towards the body + static const PxU32 sDominantDynamic = (1 << 27); // almost the same as above but there is at least one two-way projection + static const PxU32 sAttachedToDynamic = 1; + static const PxU32 sPrimaryTreeRootMinRank = sOneWayProjection | sAllDominantDynamic; +}; + + +PX_INLINE bool isFixedBody(const Sc::BodySim* b) +{ + return (!b || (b->isKinematic())); +} + + +void Sc::ConstraintProjectionTree::getConstraintStatus(const ConstraintSim& c, const BodySim* b, BodySim*& otherBody, PxU32& projectToBody, PxU32& projectToOtherBody) +{ + PxU32 isBroken = c.isBroken() ? 0 : 0xffffffff; + PxU32 projFlags = c.getCore().getFlags() & PxConstraintFlag::ePROJECTION; + + if (b == c.getBody(0)) + { + projectToBody = isBroken & (projFlags & PxConstraintFlag::ePROJECT_TO_ACTOR0); + projectToOtherBody = isBroken & (projFlags & PxConstraintFlag::ePROJECT_TO_ACTOR1); + + otherBody = c.getBody(1); + } + else + { + projectToBody = isBroken & (projFlags & PxConstraintFlag::ePROJECT_TO_ACTOR1); + projectToOtherBody = isBroken & (projFlags & PxConstraintFlag::ePROJECT_TO_ACTOR0); + + otherBody = c.getBody(0); + } +} + + +void Sc::ConstraintProjectionTree::rankConstraint(ConstraintSim& c, BodyRank& br, PxU32& dominanceTracking, PxU32& constraintsToProjectCount) +{ + PxU32 projectToBody, projectToOtherBody; + BodySim* otherB; + getConstraintStatus(c, br.startingNode->body, otherB, projectToBody, projectToOtherBody); + + if (isFixedBody(otherB)) // joint to fixed anchor + { + PxU32 rank; + if (projectToOtherBody) + { + dominanceTracking = 0; // makes sure that the flags below will never get raised again for the body + br.rank &= ~(BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic); + rank = BodyRank::sOneWayProjection; //we should prefer picking projected constraints as the root over non-projected ones. + constraintsToProjectCount++; + } + else + rank = 0; + + if (!otherB) + rank |= BodyRank::sAttachedToStatic; + else + { + PX_ASSERT(otherB->isKinematic()); + rank |= BodyRank::sAttachedToKinematic; + } + + // the highest ranked fixed anchor constraint should get tracked + if ((!br.constraintToFixedAnchor) || (rank > br.rank)) + br.constraintToFixedAnchor = &c; + + br.rank |= rank; + } + else + { + if (projectToBody && projectToOtherBody) + { + dominanceTracking &= ~BodyRank::sAllDominantDynamic; // makes sure that from now on this will never get raised again for the body + br.rank &= ~BodyRank::sAllDominantDynamic; + constraintsToProjectCount++; + } + else if (projectToOtherBody) + { + dominanceTracking &= ~(BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic); // makes sure that from now on these will never get raised again for the body + br.rank &= ~(BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic); + constraintsToProjectCount++; + } + else if (projectToBody) + { + br.rank |= BodyRank::sOneWayProjection | (dominanceTracking & (BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic)); + constraintsToProjectCount++; + } + + br.rank += BodyRank::sAttachedToDynamic; + } +} + + +/* +the goal here is to take the constraint group whose root is passed, and create one or more projection trees. + +At the moment, the group has to be acyclic and have at most 1 constraint with the ground to be accepted +without being broken up into multiple trees. + +We 'flood fill' the constraint graph several times, starting at bodies where projection trees can be rooted. +Projection tree roots are always dynamic bodies which either need to get projected to a fixed anchor directly +or have projecting constraints between dynamics some way along the tree branches. Static and kinematic actors +are never roots and will not be explicitly part of any tree (but a tree root can project to at most one such fixed node). + +The algo looks like this: + +for all bodies +mark body as undiscovered +rank this body + +The rank of a body depends on the constraints it's connected to. It defines the projection priority which +should be (highest first): +- dynamic attached to static/world with projection +- dynamic attached to kinematic with projection +- all dominant dynamic (has projecting constraints and all of them are one-way towards this dynamic) +---- all the ones above are guaranteed tree roots +- dominant dynamic (same as above but there is at least one projecting two-way constraint as well) +- partially dominant dynamic (has at least one projecting one-way constraints towards this dynamic and at least one projecting one-way constraints towards an other body) +- dynamic attached to static/world without projection +- dynamic attached to kinematic without projection +- dynamic with or without two-way projecting constraints to other dynamics (among these, the one with the highest connectivity count wins) + +for the first three priority types sorted according to rank: +create a projection tree root and grow the tree one connectivity layer at a time + +do the same for dominant dynamic bodies that have not been visited/discovered yet + +for all remaining bodies sorted according to rank: +if the body still hasn't been visited/discovered start a projection tree there and build the whole tree in one go +before moving to the next potential root. +*/ +void Sc::ConstraintProjectionTree::buildProjectionTrees(ConstraintGroupNode& root) +{ + PX_ASSERT(&root == root.parent); + PX_ASSERT(!root.hasProjectionTreeRoot()); + + Ps::InlineArray<BodyRank, 64> bodyRankArray PX_DEBUG_EXP("bodyRankArray"); + BodyRank br; + PxU32 constraintsToProjectCount = 0; + ConstraintGroupNode* node0 = &root; + while (node0) //for all nodes in group + { + PX_ASSERT(node0->body); + if (!node0->body->isKinematic()) + { + node0->clearFlag(ConstraintGroupNode::eDISCOVERED); + + //rank + br.startingNode = node0; + br.rank = 0; + br.constraintToFixedAnchor = 0; + PxU32 dominanceTracking = BodyRank::sAllDominantDynamic | BodyRank::sDominantDynamic; + + //go through all constraints connected to body + PxU32 size = node0->body->getActorInteractionCount(); + Sc::Interaction** interactions = node0->body->getActorInteractions(); + while(size--) + { + Interaction* interaction = *interactions++; + if (interaction->getType() == InteractionType::eCONSTRAINTSHADER) + { + ConstraintSim* c = static_cast<ConstraintInteraction*>(interaction)->getConstraint(); + rankConstraint(*c, br, dominanceTracking, constraintsToProjectCount); + } + } + + PX_ASSERT(br.rank); //if it has no constraints then why is it in the constraint group? + + if (br.rank >= BodyRank::sPrimaryTreeRootMinRank) + node0->raiseFlag(ConstraintGroupNode::eDISCOVERED); // we create a tree for each node attached to a fixed anchor, or a node which is an all dominating dynamic + // -> make sure they do not include each other + + bodyRankArray.pushBack(br); + } + else + node0->raiseFlag(ConstraintGroupNode::eDISCOVERED); // a kinematic does not get projected, it might only get projected to and it is never part of a tree. + + node0 = node0->next; + } + + root.setProjectionCountHint(constraintsToProjectCount); + + if (bodyRankArray.size()) // all of the bodies might have been switched to kinematic in which case there will be no ranked body + { + //sort bodyRankArray + + Ps::sort(&bodyRankArray.front(), bodyRankArray.size(), Ps::Greater<BodyRank>()); + + ConstraintGroupNode** nodeQueue = reinterpret_cast<ConstraintGroupNode**>(PX_ALLOC_TEMP(sizeof(ConstraintGroupNode*)*bodyRankArray.size(), "ProjectionNodeQueue")); + if (nodeQueue) + { + //build the projectionTree + + ConstraintGroupNode* firstProjectionTreeRoot = NULL; + + //go through it in sorted order + + // + // bodies attached to fixed anchors with projecting constraints or all dominant rigid dynamics should get processed first. + // For each of those we create a projection tree for sure, by extending one connectivity level from the root at a time. + // This way we make sure that scenarios like a bridge that is attached to fixed anchors at both ends breaks in the middle + // and not at one of the fixed anchors. + // + // this gets repeated for dominant dynamics. The reason for this is to cover cases where a dominant dynamic is connected to + // a higher ranked node by a chain of two-way constraints. In such a case the two-way constraint should project the dominant + // dynamic towards the higher ranked node and not start a tree on its own. + // + PxU32 brIdx = 0; + PxU32 stopIdx = bodyRankArray.size(); + PxU32 skipCount = 0; + PxU32 ranksToProcess = BodyRank::sPrimaryTreeRootMinRank; + ConstraintGroupNode** nodeQueueEnd; + ConstraintGroupNode** nodeQueueCurrent; + for(PxU32 i=0; i < 2; i++) + { + nodeQueueEnd = nodeQueue; + while((brIdx < stopIdx) && (bodyRankArray[brIdx].rank >= ranksToProcess)) + { + BodyRank& bRank = bodyRankArray[brIdx]; + PX_ASSERT((brIdx == 0) || (bRank.rank <= bodyRankArray[brIdx-1].rank)); + + ConstraintGroupNode& node = *bRank.startingNode; + PX_ASSERT(node.readFlag(ConstraintGroupNode::eDISCOVERED)); + + node.initProjectionData(NULL, bRank.constraintToFixedAnchor); + + if (bRank.rank & (BodyRank::sAttachedToStatic | BodyRank::sAttachedToKinematic)) + { + // for static/kinematic attached, the current node is already a child, so we must not traverse the neighborhood yet + // but rather add the current node to the queue. + PX_ASSERT(bRank.constraintToFixedAnchor); + *nodeQueueEnd = &node; + nodeQueueEnd++; + } + else + { + PX_ASSERT(!bRank.constraintToFixedAnchor); + PxU32 addedNodeCount = projectionTreeBuildStep(node, bRank.constraintToFixedAnchor, nodeQueueEnd); + nodeQueueEnd += addedNodeCount; + } + + node.projectionNextRoot = firstProjectionTreeRoot; + firstProjectionTreeRoot = &node; + + brIdx++; + } + + // first neighbor connectivity level has been pushed to a queue for all chosen tree roots. Now extend the trees one level at a time. + nodeQueueCurrent = nodeQueue; + while(nodeQueueCurrent != nodeQueueEnd) + { + ConstraintGroupNode* node = *nodeQueueCurrent; + PX_ASSERT(node->readFlag(ConstraintGroupNode::eDISCOVERED)); + nodeQueueCurrent++; + + PxU32 addedNodeCount = projectionTreeBuildStep(*node, node->projectionConstraint, nodeQueueEnd); + nodeQueueEnd += addedNodeCount; + } + + brIdx += skipCount; + skipCount = 0; + + // find dominant dynamics that have not been discovered yet and arrange them in a consecutive block + ranksToProcess = BodyRank::sOneWayProjection | BodyRank::sDominantDynamic; + stopIdx = brIdx; + PxU32 k = brIdx; + while((k < bodyRankArray.size()) && (bodyRankArray[k].rank >= ranksToProcess)) + { + ConstraintGroupNode* node = bodyRankArray[k].startingNode; + if (!node->readFlag(ConstraintGroupNode::eDISCOVERED)) + { + node->raiseFlag(ConstraintGroupNode::eDISCOVERED); + bodyRankArray[stopIdx] = bodyRankArray[k]; + stopIdx++; + } + else + skipCount++; + + k++; + } + } + + // + // for every body that has not been discovered yet, we build a tree. Here we do not advance one connectivity level + // at a time because there should be no fight over the nodes among equal roots anymore (or rather no fight that could + // break one-way projection in an unfair way). + // + PX_ASSERT((brIdx == 0) || (brIdx == bodyRankArray.size()) || (bodyRankArray[brIdx].rank < bodyRankArray[brIdx-1].rank)); + for(PxU32 i=brIdx; i < bodyRankArray.size(); i++) + { + nodeQueueEnd = nodeQueue; + + BodyRank& bRank = bodyRankArray[i]; + PX_ASSERT((i == brIdx) || (bRank.rank <= bodyRankArray[i-1].rank)); +#ifdef _DEBUG + if (bRank.rank & (BodyRank::sAttachedToStatic | BodyRank::sAttachedToKinematic)) + { PX_ASSERT(bRank.constraintToFixedAnchor); } + else + { PX_ASSERT(!bRank.constraintToFixedAnchor); } +#endif + + ConstraintGroupNode& node = *bRank.startingNode; + if (!node.readFlag(ConstraintGroupNode::eDISCOVERED)) + { + node.raiseFlag(ConstraintGroupNode::eDISCOVERED); + + PxU32 addedNodeCount = projectionTreeBuildStep(node, bRank.constraintToFixedAnchor, nodeQueueEnd); + nodeQueueEnd += addedNodeCount; + + nodeQueueCurrent = nodeQueue; + while(nodeQueueCurrent != nodeQueueEnd) + { + ConstraintGroupNode* n = *nodeQueueCurrent; + PX_ASSERT(n->readFlag(ConstraintGroupNode::eDISCOVERED)); + PX_ASSERT(n->projectionConstraint); + nodeQueueCurrent++; + + PxU32 nodeCount = projectionTreeBuildStep(*n, n->projectionConstraint, nodeQueueEnd); + nodeQueueEnd += nodeCount; + } + + node.projectionNextRoot = firstProjectionTreeRoot; + firstProjectionTreeRoot = &node; + } + } + + root.setProjectionTreeRoot(firstProjectionTreeRoot); + + PX_FREE(nodeQueue); + } + else + Ps::getFoundation().error(PxErrorCode::eOUT_OF_MEMORY, __FILE__, __LINE__, "Allocating projection node queue failed!"); + } +} + + +PxU32 Sc::ConstraintProjectionTree::projectionTreeBuildStep(ConstraintGroupNode& node, ConstraintSim* cToParent, ConstraintGroupNode** nodeQueue) +{ + PX_ASSERT(node.readFlag(ConstraintGroupNode::eDISCOVERED)); + + PxU32 nodeQueueFillCount = 0; + + //go through all constraints attached to the body. + BodySim* body = node.body; + PxU32 size = body->getActorInteractionCount(); + Sc::Interaction** interactions = body->getActorInteractions(); + while(size--) + { + Interaction* interaction = *interactions++; + if (interaction->getType() == InteractionType::eCONSTRAINTSHADER) + { + ConstraintSim* c = static_cast<ConstraintInteraction*>(interaction)->getConstraint(); + + if (c != cToParent) //don't go back along the edge we came from (not really necessary I guess since the ConstraintGroupNode::eDISCOVERED marker should solve this) + { + PxU32 projectToBody, projectToOtherBody; + BodySim* neighbor; + getConstraintStatus(*c, body, neighbor, projectToBody, projectToOtherBody); + + if(!isFixedBody(neighbor) && (!projectToOtherBody || projectToBody)) //just ignore the eventual constraint with environment over here. Body might be attached to multiple fixed anchors. + //Also make sure to ignore one-way projection that goes the opposite way. + { + ConstraintGroupNode* neighborNode = neighbor->getConstraintGroup(); + PX_ASSERT(neighborNode); + + if (!neighborNode->readFlag(ConstraintGroupNode::eDISCOVERED)) + { + *nodeQueue = neighborNode; + + neighborNode->initProjectionData(&node, c); + neighborNode->raiseFlag(ConstraintGroupNode::eDISCOVERED); //flag body nodes that we process so we can detect loops + + nodeQueueFillCount++; + nodeQueue++; + } + } + } + } + } + + return nodeQueueFillCount; +} + + +void Sc::ConstraintProjectionTree::purgeProjectionTrees(ConstraintGroupNode& root) +{ + PX_ASSERT(&root == root.parent); + PX_ASSERT(root.hasProjectionTreeRoot()); + + // CA: New code (non recursive: recursive calls can cause stack overflow with huge trees) + ConstraintGroupNode* projRoot = root.projectionFirstRoot; + do + { + ConstraintGroupNode* currentNode = projRoot; + projRoot = projRoot->projectionNextRoot; // need to do it here because the info might get cleared below + + do + { + // Go down the tree until we find a leaf + if (currentNode->projectionFirstChild) + { + currentNode = currentNode->projectionFirstChild; + continue; + } + + // Delete current node and go to next sibling or parent + ConstraintGroupNode* nodeToDelete = currentNode; + ConstraintGroupNode* parent = currentNode->projectionParent; + currentNode = currentNode->projectionNextSibling; + + // Mark parent as leaf + if (nodeToDelete->projectionParent) + nodeToDelete->projectionParent->projectionFirstChild = NULL; + + // Clear projection info + nodeToDelete->clearProjectionData(); + + if (currentNode != NULL) + continue; + + // No more siblings jump back to parent + currentNode = parent; + + } while (currentNode != NULL); + + } while (projRoot != NULL); + + root.projectionFirstRoot = NULL; // it can happen that the constraint graph root is not part of a projection tree (if it is a kinematic, for example) but it still points to the + // first projection tree root and that needs to get cleaned up as well. + PX_ASSERT(!root.projectionNextRoot); + PX_ASSERT(!root.projectionParent); + PX_ASSERT(!root.projectionFirstChild); + PX_ASSERT(!root.projectionNextSibling); + PX_ASSERT(!root.projectionConstraint); +} + + +void Sc::ConstraintProjectionTree::projectPoseForTree(ConstraintGroupNode& node, Ps::Array<BodySim*>& projectedBodies) +{ + // create a dummy node to keep the loops compact while covering the special case of the first node + PX_ASSERT(node.body); + ConstraintGroupNode dummyNode(*node.body); + dummyNode.projectionNextSibling = &node; + ConstraintGroupNode* currentNode = &dummyNode; + + // non recursive: recursive calls can cause stack overflow with huge trees + do + { + ConstraintGroupNode* nextSiblingNode = currentNode->projectionNextSibling; + + while (nextSiblingNode) + { + currentNode = nextSiblingNode; + ConstraintGroupNode* nextChildNode = currentNode; + + do + { + currentNode = nextChildNode; + + //----------------------------------------------------------------------------- + ConstraintSim* c = currentNode->projectionConstraint; + + if (c && c->hasDynamicBody() && c->needsProjection()) + { + c->projectPose(currentNode->body, projectedBodies); + } + //----------------------------------------------------------------------------- + + nextChildNode = currentNode->projectionFirstChild; + + } while (nextChildNode); + + nextSiblingNode = currentNode->projectionNextSibling; + } + + currentNode = currentNode->projectionParent; + + } while (currentNode != NULL); +} + + +void Sc::ConstraintProjectionTree::projectPose(ConstraintGroupNode& root, Ps::Array<BodySim*>& projectedBodies) +{ + PX_ASSERT(&root == root.parent); + PX_ASSERT(root.hasProjectionTreeRoot()); + + ConstraintGroupNode* projRoot = root.projectionFirstRoot; + do + { + projectPoseForTree(*projRoot, projectedBodies); + projRoot = projRoot->projectionNextRoot; + + } while (projRoot != NULL); +} |