Init
This commit is contained in:
926
physx/source/lowleveldynamics/src/DyConstraintPartition.cpp
Normal file
926
physx/source/lowleveldynamics/src/DyConstraintPartition.cpp
Normal file
@ -0,0 +1,926 @@
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions
|
||||
// are met:
|
||||
// * Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
||||
// contributors may be used to endorse or promote products derived
|
||||
// from this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
||||
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
||||
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
||||
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
||||
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
//
|
||||
// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
|
||||
|
||||
#include "DyConstraintPartition.h"
|
||||
#include "DyArticulationUtils.h"
|
||||
|
||||
#define INTERLEAVE_SELF_CONSTRAINTS 1
|
||||
|
||||
|
||||
namespace physx
|
||||
{
|
||||
namespace Dy
|
||||
{
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
PX_FORCE_INLINE PxU32 getArticulationIndex(const uintptr_t eaArticulation, const uintptr_t* eaArticulations, const PxU32 numEas)
|
||||
{
|
||||
PxU32 index=0xffffffff;
|
||||
for(PxU32 i=0;i<numEas;i++)
|
||||
{
|
||||
if(eaArticulations[i]== eaArticulation)
|
||||
{
|
||||
index=i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
PX_ASSERT(index!=0xffffffff);
|
||||
return index;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#define MAX_NUM_PARTITIONS 32
|
||||
|
||||
|
||||
class RigidBodyClassification
|
||||
{
|
||||
PxU8* PX_RESTRICT mBodies;
|
||||
PxU32 mBodySize;
|
||||
PxU32 mBodyStride;
|
||||
PxU32 mBodyCount;
|
||||
|
||||
public:
|
||||
RigidBodyClassification(PxU8* PX_RESTRICT bodies, PxU32 bodyCount, PxU32 bodyStride) : mBodies(bodies), mBodySize(bodyCount*bodyStride), mBodyStride(bodyStride),
|
||||
mBodyCount(bodyCount)
|
||||
{
|
||||
}
|
||||
|
||||
//Returns true if it is a dynamic-dynamic constriant; false if it is a dynamic-static or dynamic-kinematic constraint
|
||||
PX_FORCE_INLINE bool classifyConstraint(const PxSolverConstraintDesc& desc, uintptr_t& indexA, uintptr_t& indexB,
|
||||
bool& activeA, bool& activeB, PxU32& bodyAProgress, PxU32& bodyBProgress) const
|
||||
{
|
||||
indexA=uintptr_t(reinterpret_cast<PxU8*>(desc.bodyA) - mBodies)/mBodyStride;
|
||||
indexB=uintptr_t(reinterpret_cast<PxU8*>(desc.bodyB) - mBodies)/mBodyStride;
|
||||
activeA = indexA < mBodyCount;
|
||||
activeB = indexB < mBodyCount;
|
||||
bodyAProgress = desc.bodyA->solverProgress;
|
||||
bodyBProgress = desc.bodyB->solverProgress;
|
||||
return activeA && activeB;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void getProgress(const PxSolverConstraintDesc& desc,
|
||||
PxU32& bodyAProgress, PxU32& bodyBProgress)
|
||||
{
|
||||
bodyAProgress = desc.bodyA->solverProgress;
|
||||
bodyBProgress = desc.bodyB->solverProgress;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void storeProgress(const PxSolverConstraintDesc& desc, const PxU32 bodyAProgress, const PxU32 bodyBProgress,
|
||||
const PxU16 availablePartition)
|
||||
{
|
||||
desc.bodyA->solverProgress = bodyAProgress;
|
||||
desc.bodyA->maxSolverNormalProgress = PxMax(desc.bodyA->maxSolverNormalProgress, availablePartition);
|
||||
desc.bodyB->solverProgress = bodyBProgress;
|
||||
desc.bodyB->maxSolverNormalProgress = PxMax(desc.bodyB->maxSolverNormalProgress, availablePartition);
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void storeProgress(const PxSolverConstraintDesc& desc, const PxU32 bodyAProgress, const PxU32 bodyBProgress)
|
||||
{
|
||||
desc.bodyA->solverProgress = bodyAProgress;
|
||||
desc.bodyB->solverProgress = bodyBProgress;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxU32 getStaticContactWriteIndex(const PxSolverConstraintDesc& desc,
|
||||
bool activeA, bool activeB)
|
||||
{
|
||||
if (activeA)
|
||||
return PxU32(desc.bodyA->maxSolverNormalProgress + desc.bodyA->maxSolverFrictionProgress++);
|
||||
else if (activeB)
|
||||
return PxU32(desc.bodyB->maxSolverNormalProgress + desc.bodyB->maxSolverFrictionProgress++);
|
||||
|
||||
return 0xffffffff;
|
||||
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void recordStaticConstraint(const PxSolverConstraintDesc& desc, bool& activeA, bool& activeB) const
|
||||
{
|
||||
if (activeA)
|
||||
{
|
||||
desc.bodyA->maxSolverFrictionProgress++;
|
||||
}
|
||||
|
||||
if (activeB)
|
||||
{
|
||||
desc.bodyB->maxSolverFrictionProgress++;
|
||||
}
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void clearState()
|
||||
{
|
||||
for(PxU32 a = 0; a < mBodySize; a+= mBodyStride)
|
||||
reinterpret_cast<PxSolverBody*>(mBodies+a)->solverProgress = 0;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void reserveSpaceForStaticConstraints(Ps::Array<PxU32>& numConstraintsPerPartition)
|
||||
{
|
||||
for(PxU32 a = 0; a < mBodySize; a += mBodyStride)
|
||||
{
|
||||
PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(mBodies+a);
|
||||
body.solverProgress = 0;
|
||||
|
||||
PxU32 requiredSize = PxU32(body.maxSolverNormalProgress + body.maxSolverFrictionProgress);
|
||||
if(requiredSize > numConstraintsPerPartition.size())
|
||||
{
|
||||
numConstraintsPerPartition.resize(requiredSize);
|
||||
}
|
||||
|
||||
for(PxU32 b = 0; b < body.maxSolverFrictionProgress; ++b)
|
||||
{
|
||||
numConstraintsPerPartition[body.maxSolverNormalProgress + b]++;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class ExtendedRigidBodyClassification
|
||||
{
|
||||
PxU8* PX_RESTRICT mBodies;
|
||||
PxU32 mBodyCount;
|
||||
PxU32 mBodySize;
|
||||
PxU32 mStride;
|
||||
//uintptr_t* PX_RESTRICT mFsDatas;
|
||||
uintptr_t* PX_RESTRICT mArticulations;
|
||||
PxU32 mNumArticulations;
|
||||
|
||||
public:
|
||||
|
||||
ExtendedRigidBodyClassification(PxU8* PX_RESTRICT bodies, PxU32 numBodies, PxU32 stride, uintptr_t* articulations, PxU32 numArticulations)
|
||||
: mBodies(bodies), mBodyCount(numBodies), mBodySize(numBodies*stride), mStride(stride), mArticulations(articulations), mNumArticulations(numArticulations)
|
||||
{
|
||||
}
|
||||
|
||||
//Returns true if it is a dynamic-dynamic constriant; false if it is a dynamic-static or dynamic-kinematic constraint
|
||||
PX_FORCE_INLINE bool classifyConstraint(const PxSolverConstraintDesc& desc, uintptr_t& indexA, uintptr_t& indexB,
|
||||
bool& activeA, bool& activeB, PxU32& bodyAProgress, PxU32& bodyBProgress) const
|
||||
{
|
||||
bool hasStatic = false;
|
||||
if(PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
|
||||
{
|
||||
indexA=uintptr_t(reinterpret_cast<PxU8*>(desc.bodyA) - mBodies)/mStride;
|
||||
activeA = indexA < mBodyCount;
|
||||
hasStatic = !activeA;//desc.bodyADataIndex == 0;
|
||||
bodyAProgress = activeA ? desc.bodyA->solverProgress: 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
ArticulationV* articulationA = desc.articulationA;
|
||||
indexA=mBodyCount+getArticulationIndex(uintptr_t(articulationA), mArticulations ,mNumArticulations);
|
||||
//bodyAProgress = articulationA->getFsDataPtr()->solverProgress;
|
||||
bodyAProgress = articulationA->solverProgress;
|
||||
activeA = true;
|
||||
}
|
||||
if(PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
|
||||
{
|
||||
indexB=uintptr_t(reinterpret_cast<PxU8*>(desc.bodyB) - mBodies)/mStride;
|
||||
activeB = indexB < mBodyCount;
|
||||
hasStatic = hasStatic || !activeB;
|
||||
bodyBProgress = activeB ? desc.bodyB->solverProgress : 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationB = desc.articulationB;
|
||||
indexB=mBodyCount+getArticulationIndex(uintptr_t(articulationB), mArticulations, mNumArticulations);
|
||||
activeB = true;
|
||||
bodyBProgress = articulationB->solverProgress;
|
||||
}
|
||||
return !hasStatic;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void getProgress(const PxSolverConstraintDesc& desc,
|
||||
PxU32& bodyAProgress, PxU32& bodyBProgress) const
|
||||
{
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
|
||||
{
|
||||
bodyAProgress = desc.bodyA->solverProgress;
|
||||
}
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationA = desc.articulationA;
|
||||
bodyAProgress = articulationA->solverProgress;
|
||||
}
|
||||
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
|
||||
{
|
||||
|
||||
bodyBProgress = desc.bodyB->solverProgress;
|
||||
}
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationB = desc.articulationB;
|
||||
bodyBProgress = articulationB->solverProgress;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
PX_FORCE_INLINE void storeProgress(const PxSolverConstraintDesc& desc, const PxU32 bodyAProgress, const PxU32 bodyBProgress,
|
||||
const PxU16 availablePartition)
|
||||
{
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
|
||||
{
|
||||
desc.bodyA->solverProgress = bodyAProgress;
|
||||
desc.bodyA->maxSolverNormalProgress = PxMax(desc.bodyA->maxSolverNormalProgress, availablePartition);
|
||||
}
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationA = desc.articulationA;
|
||||
articulationA->solverProgress = bodyAProgress;
|
||||
articulationA->maxSolverNormalProgress = PxMax(articulationA->maxSolverNormalProgress, availablePartition);
|
||||
}
|
||||
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
|
||||
{
|
||||
desc.bodyB->solverProgress = bodyBProgress;
|
||||
desc.bodyB->maxSolverNormalProgress = PxMax(desc.bodyB->maxSolverNormalProgress, availablePartition);
|
||||
}
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationB = desc.articulationB;
|
||||
articulationB->solverProgress = bodyBProgress;
|
||||
articulationB->maxSolverNormalProgress = PxMax(articulationB->maxSolverNormalProgress, availablePartition);
|
||||
}
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void storeProgress(const PxSolverConstraintDesc& desc, const PxU32 bodyAProgress,
|
||||
const PxU32 bodyBProgress)
|
||||
{
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
|
||||
{
|
||||
desc.bodyA->solverProgress = bodyAProgress;
|
||||
}
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationA = desc.articulationA;
|
||||
articulationA->solverProgress = bodyAProgress;
|
||||
}
|
||||
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
|
||||
{
|
||||
desc.bodyB->solverProgress = bodyBProgress;
|
||||
}
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationB = desc.articulationB;
|
||||
articulationB->solverProgress = bodyBProgress;
|
||||
}
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void recordStaticConstraint(const PxSolverConstraintDesc& desc, bool& activeA, bool& activeB)
|
||||
{
|
||||
if (activeA)
|
||||
{
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
|
||||
desc.bodyA->maxSolverFrictionProgress++;
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationA = desc.articulationA;
|
||||
if(!articulationA->willStoreStaticConstraint())
|
||||
articulationA->maxSolverFrictionProgress++;
|
||||
}
|
||||
}
|
||||
|
||||
if (activeB)
|
||||
{
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
|
||||
desc.bodyB->maxSolverFrictionProgress++;
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationB = desc.articulationB;
|
||||
if (!articulationB->willStoreStaticConstraint())
|
||||
articulationB->maxSolverFrictionProgress++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxU32 getStaticContactWriteIndex(const PxSolverConstraintDesc& desc,
|
||||
bool activeA, bool activeB)
|
||||
{
|
||||
if (activeA)
|
||||
{
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexA)
|
||||
{
|
||||
return PxU32(desc.bodyA->maxSolverNormalProgress + desc.bodyA->maxSolverFrictionProgress++);
|
||||
}
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationA = desc.articulationA;
|
||||
//Attempt to store static constraints on the articulation (only supported with the reduced coordinate articulations).
|
||||
//This acts as an optimization
|
||||
if(articulationA->storeStaticConstraint(desc))
|
||||
return 0xffffffff;
|
||||
return PxU32(articulationA->maxSolverNormalProgress + articulationA->maxSolverFrictionProgress++);
|
||||
}
|
||||
}
|
||||
else if (activeB)
|
||||
{
|
||||
if (PxSolverConstraintDesc::NO_LINK == desc.linkIndexB)
|
||||
{
|
||||
return PxU32(desc.bodyB->maxSolverNormalProgress + desc.bodyB->maxSolverFrictionProgress++);
|
||||
}
|
||||
else
|
||||
{
|
||||
ArticulationV* articulationB = desc.articulationB;
|
||||
//Attempt to store static constraints on the articulation (only supported with the reduced coordinate articulations).
|
||||
//This acts as an optimization
|
||||
if (articulationB->storeStaticConstraint(desc))
|
||||
return 0xffffffff;
|
||||
return PxU32(articulationB->maxSolverNormalProgress + articulationB->maxSolverFrictionProgress++);
|
||||
}
|
||||
}
|
||||
|
||||
return 0xffffffff;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void clearState()
|
||||
{
|
||||
for(PxU32 a = 0; a < mBodySize; a+= mStride)
|
||||
reinterpret_cast<PxSolverBody*>(mBodies+a)->solverProgress = 0;
|
||||
|
||||
for(PxU32 a = 0; a < mNumArticulations; ++a)
|
||||
(reinterpret_cast<ArticulationV*>(mArticulations[a]))->solverProgress = 0;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void reserveSpaceForStaticConstraints(Ps::Array<PxU32>& numConstraintsPerPartition)
|
||||
{
|
||||
for(PxU32 a = 0; a < mBodySize; a+= mStride)
|
||||
{
|
||||
PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(mBodies+a);
|
||||
body.solverProgress = 0;
|
||||
|
||||
PxU32 requiredSize = PxU32(body.maxSolverNormalProgress + body.maxSolverFrictionProgress);
|
||||
if(requiredSize > numConstraintsPerPartition.size())
|
||||
{
|
||||
numConstraintsPerPartition.resize(requiredSize);
|
||||
}
|
||||
|
||||
for(PxU32 b = 0; b < body.maxSolverFrictionProgress; ++b)
|
||||
{
|
||||
numConstraintsPerPartition[body.maxSolverNormalProgress + b]++;
|
||||
}
|
||||
}
|
||||
|
||||
for(PxU32 a = 0; a < mNumArticulations; ++a)
|
||||
{
|
||||
ArticulationV* articulation = reinterpret_cast<ArticulationV*>(mArticulations[a]);
|
||||
articulation->solverProgress = 0;
|
||||
|
||||
PxU32 requiredSize = PxU32(articulation->maxSolverNormalProgress + articulation->maxSolverFrictionProgress);
|
||||
if(requiredSize > numConstraintsPerPartition.size())
|
||||
{
|
||||
numConstraintsPerPartition.resize(requiredSize);
|
||||
}
|
||||
|
||||
for(PxU32 b = 0; b < articulation->maxSolverFrictionProgress; ++b)
|
||||
{
|
||||
numConstraintsPerPartition[articulation->maxSolverNormalProgress + b]++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template <typename Classification>
|
||||
void classifyConstraintDesc(const PxSolverConstraintDesc* PX_RESTRICT descs, const PxU32 numConstraints, Classification& classification,
|
||||
Ps::Array<PxU32>& numConstraintsPerPartition, PxSolverConstraintDesc* PX_RESTRICT eaTempConstraintDescriptors)
|
||||
{
|
||||
const PxSolverConstraintDesc* _desc = descs;
|
||||
const PxU32 numConstraintsMin1 = numConstraints - 1;
|
||||
|
||||
PxU32 numUnpartitionedConstraints = 0;
|
||||
|
||||
numConstraintsPerPartition.forceSize_Unsafe(32);
|
||||
|
||||
PxMemZero(numConstraintsPerPartition.begin(), sizeof(PxU32) * 32);
|
||||
|
||||
for(PxU32 i = 0; i < numConstraints; ++i, _desc++)
|
||||
{
|
||||
const PxU32 prefetchOffset = PxMin(numConstraintsMin1 - i, 4u);
|
||||
Ps::prefetchLine(_desc[prefetchOffset].constraint);
|
||||
Ps::prefetchLine(_desc[prefetchOffset].bodyA);
|
||||
Ps::prefetchLine(_desc[prefetchOffset].bodyB);
|
||||
Ps::prefetchLine(_desc + 8);
|
||||
|
||||
uintptr_t indexA, indexB;
|
||||
bool activeA, activeB;
|
||||
|
||||
PxU32 partitionsA, partitionsB;
|
||||
const bool notContainsStatic = classification.classifyConstraint(*_desc, indexA, indexB, activeA, activeB,
|
||||
partitionsA, partitionsB);
|
||||
|
||||
if(notContainsStatic)
|
||||
{
|
||||
PxU32 availablePartition;
|
||||
{
|
||||
const PxU32 combinedMask = (~partitionsA & ~partitionsB);
|
||||
availablePartition = combinedMask == 0 ? MAX_NUM_PARTITIONS : Ps::lowestSetBit(combinedMask);
|
||||
if(availablePartition == MAX_NUM_PARTITIONS)
|
||||
{
|
||||
eaTempConstraintDescriptors[numUnpartitionedConstraints++] = *_desc;
|
||||
continue;
|
||||
}
|
||||
|
||||
const PxU32 partitionBit = (1u << availablePartition);
|
||||
if (activeA)
|
||||
partitionsA |= partitionBit;
|
||||
if(activeB)
|
||||
partitionsB |= partitionBit;
|
||||
}
|
||||
|
||||
numConstraintsPerPartition[availablePartition]++;
|
||||
availablePartition++;
|
||||
|
||||
classification.storeProgress(*_desc, partitionsA, partitionsB, PxU16(availablePartition));
|
||||
}
|
||||
else
|
||||
{
|
||||
classification.recordStaticConstraint(*_desc, activeA, activeB);
|
||||
}
|
||||
}
|
||||
|
||||
PxU32 partitionStartIndex = 0;
|
||||
|
||||
while(numUnpartitionedConstraints > 0)
|
||||
{
|
||||
classification.clearState();
|
||||
|
||||
partitionStartIndex += 32;
|
||||
//Keep partitioning the un-partitioned constraints and blat the whole thing to 0!
|
||||
numConstraintsPerPartition.resize(32 + numConstraintsPerPartition.size());
|
||||
PxMemZero(numConstraintsPerPartition.begin() + partitionStartIndex, sizeof(PxU32) * 32);
|
||||
|
||||
PxU32 newNumUnpartitionedConstraints = 0;
|
||||
PxU32 partitionsA, partitionsB;
|
||||
bool activeA, activeB;
|
||||
uintptr_t indexA, indexB;
|
||||
for(PxU32 i = 0; i < numUnpartitionedConstraints; ++i)
|
||||
{
|
||||
const PxSolverConstraintDesc& desc = eaTempConstraintDescriptors[i];
|
||||
|
||||
classification.classifyConstraint(desc, indexA, indexB, activeA, activeB,
|
||||
partitionsA, partitionsB);
|
||||
|
||||
PxU32 availablePartition;
|
||||
{
|
||||
const PxU32 combinedMask = (~partitionsA & ~partitionsB);
|
||||
availablePartition = combinedMask == 0 ? MAX_NUM_PARTITIONS : Ps::lowestSetBit(combinedMask);
|
||||
if(availablePartition == MAX_NUM_PARTITIONS)
|
||||
{
|
||||
//Need to shuffle around unpartitioned constraints...
|
||||
eaTempConstraintDescriptors[newNumUnpartitionedConstraints++] = desc;
|
||||
continue;
|
||||
}
|
||||
|
||||
const PxU32 partitionBit = (1u << availablePartition);
|
||||
if(activeA)
|
||||
partitionsA |= partitionBit;
|
||||
if(activeB)
|
||||
partitionsB |= partitionBit;
|
||||
}
|
||||
|
||||
|
||||
/*desc.bodyA->solverProgress = partitionsA;
|
||||
desc.bodyB->solverProgress = partitionsB;*/
|
||||
availablePartition += partitionStartIndex;
|
||||
numConstraintsPerPartition[availablePartition]++;
|
||||
availablePartition++;
|
||||
|
||||
classification.storeProgress(desc, partitionsA, partitionsB, PxU16(availablePartition) );
|
||||
|
||||
/* desc.bodyA->maxSolverNormalProgress = PxMax(desc.bodyA->maxSolverNormalProgress, PxU16(availablePartition));
|
||||
desc.bodyB->maxSolverNormalProgress = PxMax(desc.bodyB->maxSolverNormalProgress, PxU16(availablePartition));*/
|
||||
}
|
||||
|
||||
numUnpartitionedConstraints = newNumUnpartitionedConstraints;
|
||||
}
|
||||
|
||||
classification.reserveSpaceForStaticConstraints(numConstraintsPerPartition);
|
||||
|
||||
}
|
||||
|
||||
template <typename Classification>
|
||||
PxU32 writeConstraintDesc(const PxSolverConstraintDesc* PX_RESTRICT descs, const PxU32 numConstraints, Classification& classification,
|
||||
Ps::Array<PxU32>& accumulatedConstraintsPerPartition, PxSolverConstraintDesc* eaTempConstraintDescriptors,
|
||||
PxSolverConstraintDesc* PX_RESTRICT eaOrderedConstraintDesc)
|
||||
{
|
||||
PX_UNUSED(eaTempConstraintDescriptors);
|
||||
const PxSolverConstraintDesc* _desc = descs;
|
||||
const PxU32 numConstraintsMin1 = numConstraints - 1;
|
||||
|
||||
PxU32 numUnpartitionedConstraints = 0;
|
||||
PxU32 numStaticConstraints = 0;
|
||||
|
||||
for(PxU32 i = 0; i < numConstraints; ++i, _desc++)
|
||||
{
|
||||
const PxU32 prefetchOffset = PxMin(numConstraintsMin1 - i, 4u);
|
||||
Ps::prefetchLine(_desc[prefetchOffset].constraint);
|
||||
Ps::prefetchLine(_desc[prefetchOffset].bodyA);
|
||||
Ps::prefetchLine(_desc[prefetchOffset].bodyB);
|
||||
Ps::prefetchLine(_desc + 8);
|
||||
|
||||
uintptr_t indexA, indexB;
|
||||
bool activeA, activeB;
|
||||
PxU32 partitionsA, partitionsB;
|
||||
const bool notContainsStatic = classification.classifyConstraint(*_desc, indexA, indexB, activeA, activeB, partitionsA, partitionsB);
|
||||
|
||||
if(notContainsStatic)
|
||||
{
|
||||
PxU32 availablePartition;
|
||||
{
|
||||
const PxU32 combinedMask = (~partitionsA & ~partitionsB);
|
||||
availablePartition = combinedMask == 0 ? MAX_NUM_PARTITIONS : Ps::lowestSetBit(combinedMask);
|
||||
if(availablePartition == MAX_NUM_PARTITIONS)
|
||||
{
|
||||
eaTempConstraintDescriptors[numUnpartitionedConstraints++] = *_desc;
|
||||
continue;
|
||||
}
|
||||
|
||||
const PxU32 partitionBit = (1u << availablePartition);
|
||||
if(activeA)
|
||||
partitionsA |= partitionBit;
|
||||
if(activeB)
|
||||
partitionsB |= partitionBit;
|
||||
}
|
||||
|
||||
classification.storeProgress(*_desc, partitionsA, partitionsB, PxU16(availablePartition + 1));
|
||||
|
||||
eaOrderedConstraintDesc[accumulatedConstraintsPerPartition[availablePartition]++] = *_desc;
|
||||
}
|
||||
else
|
||||
{
|
||||
//Just count the number of static constraints and store in maxSolverFrictionProgress...
|
||||
PxU32 index = classification.getStaticContactWriteIndex(*_desc, activeA, activeB);
|
||||
if (index != 0xffffffff)
|
||||
{
|
||||
eaOrderedConstraintDesc[accumulatedConstraintsPerPartition[index]++] = *_desc;
|
||||
}
|
||||
else
|
||||
numStaticConstraints++;
|
||||
}
|
||||
}
|
||||
|
||||
PxU32 partitionStartIndex = 0;
|
||||
|
||||
while(numUnpartitionedConstraints > 0)
|
||||
{
|
||||
classification.clearState();
|
||||
|
||||
partitionStartIndex += 32;
|
||||
PxU32 newNumUnpartitionedConstraints = 0;
|
||||
|
||||
PxU32 partitionsA, partitionsB;
|
||||
bool activeA, activeB;
|
||||
uintptr_t indexA, indexB;
|
||||
for(PxU32 i = 0; i < numUnpartitionedConstraints; ++i)
|
||||
{
|
||||
const PxSolverConstraintDesc& desc = eaTempConstraintDescriptors[i];
|
||||
|
||||
/* PxU32 partitionsA=desc.bodyA->solverProgress;
|
||||
PxU32 partitionsB=desc.bodyB->solverProgress;*/
|
||||
|
||||
classification.classifyConstraint(desc, indexA, indexB,
|
||||
activeA, activeB, partitionsA, partitionsB);
|
||||
|
||||
PxU32 availablePartition;
|
||||
{
|
||||
const PxU32 combinedMask = (~partitionsA & ~partitionsB);
|
||||
availablePartition = combinedMask == 0 ? MAX_NUM_PARTITIONS : Ps::lowestSetBit(combinedMask);
|
||||
if(availablePartition == MAX_NUM_PARTITIONS)
|
||||
{
|
||||
//Need to shuffle around unpartitioned constraints...
|
||||
eaTempConstraintDescriptors[newNumUnpartitionedConstraints++] = desc;
|
||||
continue;
|
||||
}
|
||||
|
||||
const PxU32 partitionBit = (1u << availablePartition);
|
||||
|
||||
if(activeA)
|
||||
partitionsA |= partitionBit;
|
||||
if(activeB)
|
||||
partitionsB |= partitionBit;
|
||||
}
|
||||
|
||||
/*desc.bodyA->solverProgress = partitionsA;
|
||||
desc.bodyB->solverProgress = partitionsB;
|
||||
*/
|
||||
classification.storeProgress(desc, partitionsA, partitionsB);
|
||||
availablePartition += partitionStartIndex;
|
||||
eaOrderedConstraintDesc[accumulatedConstraintsPerPartition[availablePartition]++] = desc;
|
||||
}
|
||||
|
||||
numUnpartitionedConstraints = newNumUnpartitionedConstraints;
|
||||
}
|
||||
|
||||
return numStaticConstraints;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#define PX_NORMALIZE_PARTITIONS 1
|
||||
|
||||
#if PX_NORMALIZE_PARTITIONS
|
||||
|
||||
template<typename Classification>
|
||||
PxU32 normalizePartitions(Ps::Array<PxU32>& accumulatedConstraintsPerPartition, PxSolverConstraintDesc* PX_RESTRICT eaOrderedConstraintDescriptors,
|
||||
const PxU32 numConstraintDescriptors, Ps::Array<PxU32>& bitField, const Classification& classification, const PxU32 numBodies, const PxU32 numArticulations)
|
||||
{
|
||||
PxU32 numPartitions = 0;
|
||||
|
||||
PxU32 prevAccumulation = 0;
|
||||
for(; numPartitions < accumulatedConstraintsPerPartition.size() && accumulatedConstraintsPerPartition[numPartitions] > prevAccumulation;
|
||||
prevAccumulation = accumulatedConstraintsPerPartition[numPartitions++]);
|
||||
|
||||
PxU32 targetSize = (numPartitions == 0 ? 0 : (numConstraintDescriptors)/numPartitions);
|
||||
|
||||
bitField.reserve((numBodies + numArticulations + 31)/32);
|
||||
bitField.forceSize_Unsafe((numBodies + numArticulations + 31)/32);
|
||||
|
||||
for(PxU32 i = numPartitions; i > 0; i--)
|
||||
{
|
||||
PxU32 partitionIndex = i-1;
|
||||
|
||||
//Build the partition mask...
|
||||
|
||||
PxU32 startIndex = partitionIndex == 0 ? 0 : accumulatedConstraintsPerPartition[partitionIndex-1];
|
||||
PxU32 endIndex = accumulatedConstraintsPerPartition[partitionIndex];
|
||||
|
||||
//If its greater than target size, there's nothing that will be pulled into it from earlier partitions
|
||||
if((endIndex - startIndex) >= targetSize)
|
||||
continue;
|
||||
|
||||
|
||||
PxMemZero(bitField.begin(), sizeof(PxU32)*bitField.size());
|
||||
|
||||
for(PxU32 a = startIndex; a < endIndex; ++a)
|
||||
{
|
||||
PxSolverConstraintDesc& desc = eaOrderedConstraintDescriptors[a];
|
||||
|
||||
uintptr_t indexA, indexB;
|
||||
bool activeA, activeB;
|
||||
PxU32 partitionsA, partitionsB;
|
||||
|
||||
classification.classifyConstraint(desc, indexA, indexB, activeA, activeB, partitionsA, partitionsB);
|
||||
|
||||
if (activeA)
|
||||
bitField[PxU32(indexA) / 32] |= (1u << (indexA & 31));
|
||||
if(activeB)
|
||||
bitField[PxU32(indexB)/32] |= (1u << (indexB & 31));
|
||||
}
|
||||
|
||||
bool bTerm = false;
|
||||
for(PxU32 a = partitionIndex; a > 0 && !bTerm; --a)
|
||||
{
|
||||
PxU32 pInd = a-1;
|
||||
|
||||
PxU32 si = pInd == 0 ? 0 : accumulatedConstraintsPerPartition[pInd-1];
|
||||
PxU32 ei = accumulatedConstraintsPerPartition[pInd];
|
||||
|
||||
for(PxU32 b = ei; b > si && !bTerm; --b)
|
||||
{
|
||||
PxU32 ind = b-1;
|
||||
PxSolverConstraintDesc& desc = eaOrderedConstraintDescriptors[ind];
|
||||
|
||||
uintptr_t indexA, indexB;
|
||||
bool activeA, activeB;
|
||||
PxU32 partitionsA, partitionsB;
|
||||
|
||||
classification.classifyConstraint(desc, indexA, indexB, activeA, activeB, partitionsA, partitionsB);
|
||||
|
||||
bool canAdd = true;
|
||||
|
||||
if(activeA && (bitField[PxU32(indexA)/32] & (1u << (indexA & 31))))
|
||||
canAdd = false;
|
||||
if(activeB && (bitField[PxU32(indexB)/32] & (1u << (indexB & 31))))
|
||||
canAdd = false;
|
||||
|
||||
if(canAdd)
|
||||
{
|
||||
PxSolverConstraintDesc tmp = eaOrderedConstraintDescriptors[ind];
|
||||
|
||||
if(activeA)
|
||||
bitField[PxU32(indexA)/32] |= (1u << (indexA & 31));
|
||||
if(activeB)
|
||||
bitField[PxU32(indexB)/32] |= (1u << (indexB & 31));
|
||||
|
||||
PxU32 index = ind;
|
||||
for(PxU32 c = pInd; c < partitionIndex; ++c)
|
||||
{
|
||||
PxU32 newIndex = --accumulatedConstraintsPerPartition[c];
|
||||
if(index != newIndex)
|
||||
eaOrderedConstraintDescriptors[index] = eaOrderedConstraintDescriptors[newIndex];
|
||||
index = newIndex;
|
||||
}
|
||||
|
||||
if(index != ind)
|
||||
eaOrderedConstraintDescriptors[index] = tmp;
|
||||
|
||||
if((accumulatedConstraintsPerPartition[partitionIndex] - accumulatedConstraintsPerPartition[partitionIndex-1]) >= targetSize)
|
||||
{
|
||||
bTerm = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
PxU32 partitionCount = 0;
|
||||
PxU32 lastPartitionCount = 0;
|
||||
for (PxU32 a = 0; a < numPartitions; ++a)
|
||||
{
|
||||
const PxU32 constraintCount = accumulatedConstraintsPerPartition[a];
|
||||
accumulatedConstraintsPerPartition[partitionCount] = constraintCount;
|
||||
if (constraintCount != lastPartitionCount)
|
||||
{
|
||||
lastPartitionCount = constraintCount;
|
||||
partitionCount++;
|
||||
}
|
||||
}
|
||||
|
||||
accumulatedConstraintsPerPartition.forceSize_Unsafe(partitionCount);
|
||||
|
||||
return partitionCount;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
PxU32 partitionContactConstraints(ConstraintPartitionArgs& args)
|
||||
{
|
||||
PxU32 maxPartition = 0;
|
||||
//Unpack the input data.
|
||||
const PxU32 numBodies=args.mNumBodies;
|
||||
const PxU32 numArticulations=args.mNumArticulationPtrs;
|
||||
|
||||
const PxU32 numConstraintDescriptors=args.mNumContactConstraintDescriptors;
|
||||
|
||||
PxSolverConstraintDesc* PX_RESTRICT eaConstraintDescriptors=args.mContactConstraintDescriptors;
|
||||
PxSolverConstraintDesc* PX_RESTRICT eaOrderedConstraintDescriptors=args.mOrderedContactConstraintDescriptors;
|
||||
PxSolverConstraintDesc* PX_RESTRICT eaTempConstraintDescriptors=args.mTempContactConstraintDescriptors;
|
||||
|
||||
Ps::Array<PxU32>& constraintsPerPartition = *args.mConstraintsPerPartition;
|
||||
constraintsPerPartition.forceSize_Unsafe(0);
|
||||
|
||||
const PxU32 stride = args.mStride;
|
||||
|
||||
for(PxU32 a = 0, offset = 0; a < numBodies; ++a, offset += stride)
|
||||
{
|
||||
PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(args.mBodies + offset);
|
||||
body.solverProgress = 0;
|
||||
//We re-use maxSolverFrictionProgress and maxSolverNormalProgress to record the
|
||||
//maximum partition used by dynamic constraints and the number of static constraints affecting
|
||||
//a body. We use this to make partitioning much cheaper and be able to support
|
||||
body.maxSolverFrictionProgress = 0;
|
||||
body.maxSolverNormalProgress = 0;
|
||||
}
|
||||
|
||||
PxU32 numOrderedConstraints=0;
|
||||
PxU32 numStaticConstraints = 0;
|
||||
|
||||
if(numArticulations == 0)
|
||||
{
|
||||
RigidBodyClassification classification(args.mBodies, numBodies, stride);
|
||||
classifyConstraintDesc(eaConstraintDescriptors, numConstraintDescriptors, classification, constraintsPerPartition,
|
||||
eaTempConstraintDescriptors);
|
||||
|
||||
PxU32 accumulation = 0;
|
||||
for(PxU32 a = 0; a < constraintsPerPartition.size(); ++a)
|
||||
{
|
||||
PxU32 count = constraintsPerPartition[a];
|
||||
constraintsPerPartition[a] = accumulation;
|
||||
accumulation += count;
|
||||
}
|
||||
|
||||
for(PxU32 a = 0, offset = 0; a < numBodies; ++a, offset += stride)
|
||||
{
|
||||
PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(args.mBodies + offset);
|
||||
Ps::prefetchLine(&args.mBodies[a], 256);
|
||||
body.solverProgress = 0;
|
||||
//Keep the dynamic constraint count but bump the static constraint count back to 0.
|
||||
//This allows us to place the static constraints in the appropriate place when we see them
|
||||
//because we know the maximum index for the dynamic constraints...
|
||||
body.maxSolverFrictionProgress = 0;
|
||||
}
|
||||
|
||||
writeConstraintDesc(eaConstraintDescriptors, numConstraintDescriptors, classification, constraintsPerPartition,
|
||||
eaTempConstraintDescriptors, eaOrderedConstraintDescriptors);
|
||||
|
||||
numOrderedConstraints = numConstraintDescriptors;
|
||||
|
||||
/*if(!args.enhancedDeterminism)
|
||||
maxPartition = normalizePartitions(constraintsPerPartition, eaOrderedConstraintDescriptors, numConstraintDescriptors, *args.mBitField,
|
||||
classification, numBodies, 0);*/
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
const ArticulationSolverDesc* articulationDescs=args.mArticulationPtrs;
|
||||
PX_ALLOCA(_eaArticulations, uintptr_t, numArticulations);
|
||||
uintptr_t* eaArticulations = _eaArticulations;
|
||||
for(PxU32 i=0;i<numArticulations;i++)
|
||||
{
|
||||
ArticulationV* articulation = articulationDescs[i].articulation;
|
||||
eaArticulations[i]=uintptr_t(articulation);
|
||||
articulation->solverProgress = 0;
|
||||
articulation->maxSolverFrictionProgress = 0;
|
||||
articulation->maxSolverNormalProgress = 0;
|
||||
}
|
||||
ExtendedRigidBodyClassification classification(args.mBodies, numBodies, stride, eaArticulations, numArticulations);
|
||||
|
||||
classifyConstraintDesc(eaConstraintDescriptors, numConstraintDescriptors, classification,
|
||||
constraintsPerPartition, eaTempConstraintDescriptors);
|
||||
|
||||
PxU32 accumulation = 0;
|
||||
for(PxU32 a = 0; a < constraintsPerPartition.size(); ++a)
|
||||
{
|
||||
PxU32 count = constraintsPerPartition[a];
|
||||
constraintsPerPartition[a] = accumulation;
|
||||
accumulation += count;
|
||||
}
|
||||
|
||||
for(PxU32 a = 0, offset = 0; a < numBodies; ++a, offset += stride)
|
||||
{
|
||||
PxSolverBody& body = *reinterpret_cast<PxSolverBody*>(args.mBodies+offset);
|
||||
body.solverProgress = 0;
|
||||
//Keep the dynamic constraint count but bump the static constraint count back to 0.
|
||||
//This allows us to place the static constraints in the appropriate place when we see them
|
||||
//because we know the maximum index for the dynamic constraints...
|
||||
body.maxSolverFrictionProgress = 0;
|
||||
}
|
||||
|
||||
for(PxU32 a = 0; a < numArticulations; ++a)
|
||||
{
|
||||
ArticulationV* articulation = reinterpret_cast<ArticulationV*>(eaArticulations[a]);
|
||||
articulation->solverProgress = 0;
|
||||
articulation->maxSolverFrictionProgress = 0;
|
||||
}
|
||||
|
||||
numStaticConstraints = writeConstraintDesc(eaConstraintDescriptors, numConstraintDescriptors, classification, constraintsPerPartition,
|
||||
eaTempConstraintDescriptors, eaOrderedConstraintDescriptors);
|
||||
|
||||
numOrderedConstraints = numConstraintDescriptors - numStaticConstraints;
|
||||
|
||||
/*if (!args.enhancedDeterminism)
|
||||
maxPartition = normalizePartitions(constraintsPerPartition, eaOrderedConstraintDescriptors,
|
||||
numOrderedConstraints, *args.mBitField, classification, numBodies, numArticulations);*/
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
const PxU32 numConstraintsDifferentBodies=numOrderedConstraints;
|
||||
|
||||
//PX_ASSERT(numConstraintsDifferentBodies == numConstraintDescriptors);
|
||||
|
||||
//Now handle the articulated self-constraints.
|
||||
PxU32 totalConstraintCount = numConstraintsDifferentBodies;
|
||||
|
||||
args.mNumDifferentBodyConstraints=numConstraintsDifferentBodies;
|
||||
args.mNumSelfConstraints=totalConstraintCount-numConstraintsDifferentBodies;
|
||||
|
||||
args.mNumStaticConstraints = numStaticConstraints;
|
||||
|
||||
//if (args.enhancedDeterminism)
|
||||
{
|
||||
PxU32 prevPartitionSize = 0;
|
||||
maxPartition = 0;
|
||||
for (PxU32 a = 0; a < constraintsPerPartition.size(); ++a, maxPartition++)
|
||||
{
|
||||
if (constraintsPerPartition[a] == prevPartitionSize)
|
||||
break;
|
||||
prevPartitionSize = constraintsPerPartition[a];
|
||||
}
|
||||
}
|
||||
|
||||
return maxPartition;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user