void PxScaleRigidActor(PxRigidActor& actor, PxReal scale, bool scaleMassProps)
{
PX_CHECK_AND_RETURN(scale > 0,
"PxScaleRigidActor requires that the scale parameter is greater than zero");
Ps::InlineArray<PxShape*, 64> shapes;
shapes.resize(actor.getNbShapes());
actor.getShapes(shapes.begin(), shapes.size());
for(PxU32 i=0;i<shapes.size();i++)
{
shapes[i]->setLocalPose(scalePosition(shapes[i]->getLocalPose(), scale));
PxGeometryHolder h = shapes[i]->getGeometry();
switch(h.getType())
{
case PxGeometryType::eSPHERE:
h.sphere().radius *= scale;
break;
case PxGeometryType::ePLANE:
break;
case PxGeometryType::eCAPSULE:
h.capsule().halfHeight *= scale;
h.capsule().radius *= scale;
break;
case PxGeometryType::eBOX:
h.box().halfExtents *= scale;
break;
case PxGeometryType::eCONVEXMESH:
h.convexMesh().scale.scale *= scale;
break;
case PxGeometryType::eTRIANGLEMESH:
h.triangleMesh().scale.scale *= scale;
break;
case PxGeometryType::eHEIGHTFIELD:
h.heightField().heightScale *= scale;
h.heightField().rowScale *= scale;
h.heightField().columnScale *= scale;
break;
case PxGeometryType::eINVALID:
case PxGeometryType::eGEOMETRY_COUNT:
default:
PX_ASSERT(0);
}
shapes[i]->setGeometry(h.any());
}
if(!scaleMassProps)
return;
PxRigidDynamic* dynamic = (&actor)->is<PxRigidDynamic>();
if(!dynamic)
return;
PxReal scale3 = scale*scale*scale;
dynamic->setMass(dynamic->getMass()*scale3);
dynamic->setMassSpaceInertiaTensor(dynamic->getMassSpaceInertiaTensor()*scale3*scale*scale);
dynamic->setCMassLocalPose(scalePosition(dynamic->getCMassLocalPose(), scale));
}
/*
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!");
}
}