2475 lines
85 KiB
C++
2475 lines
85 KiB
C++
// 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-2025 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 "PxImmediateMode.h"
|
|
#include "PxBroadPhase.h"
|
|
#include "../../lowleveldynamics/src/DyBodyCoreIntegrator.h"
|
|
#include "../../lowleveldynamics/src/DyContactPrep.h"
|
|
#include "../../lowleveldynamics/src/DyCorrelationBuffer.h"
|
|
#include "../../lowleveldynamics/src/DyConstraintPrep.h"
|
|
#include "../../lowleveldynamics/src/DySolverControl.h"
|
|
#include "../../lowleveldynamics/src/DySolverContext.h"
|
|
#include "../../lowlevel/common/include/collision/PxcContactMethodImpl.h"
|
|
#include "../../lowleveldynamics/src/DyTGSContactPrep.h"
|
|
#include "../../lowleveldynamics/src/DyTGS.h"
|
|
#include "../../lowleveldynamics/src/DyConstraintPartition.h"
|
|
#include "../../lowleveldynamics/src/DyPGS.h"
|
|
#include "../../lowleveldynamics/shared/DyCpuGpuArticulation.h"
|
|
#include "GuPersistentContactManifold.h"
|
|
#include "NpConstraint.h"
|
|
#include "common/PxProfileZone.h"
|
|
|
|
#include "../../lowleveldynamics/include/DyFeatherstoneArticulation.h"
|
|
|
|
#include "../../lowlevel/common/include/utils/PxcScratchAllocator.h"
|
|
|
|
using namespace physx;
|
|
using namespace Dy;
|
|
using namespace immediate;
|
|
|
|
void immediate::PxConstructSolverBodies(const PxRigidBodyData* inRigidData, PxSolverBodyData* outSolverBodyData, PxU32 nbBodies, const PxVec3& gravity, PxReal dt, bool gyroscopicForces)
|
|
{
|
|
PX_ASSERT((size_t(inRigidData) & 0xf) == 0);
|
|
PX_ASSERT((size_t(outSolverBodyData) & 0xf) == 0);
|
|
|
|
for(PxU32 a=0; a<nbBodies; a++)
|
|
{
|
|
const PxRigidBodyData& rigidData = inRigidData[a];
|
|
PxVec3 lv = rigidData.linearVelocity, av = rigidData.angularVelocity;
|
|
Dy::bodyCoreComputeUnconstrainedVelocity(gravity, dt, rigidData.linearDamping, rigidData.angularDamping, 1.0f, rigidData.maxLinearVelocitySq, rigidData.maxAngularVelocitySq, lv, av, false);
|
|
Dy::copyToSolverBodyData(lv, av, rigidData.invMass, rigidData.invInertia, rigidData.body2World, -rigidData.maxDepenetrationVelocity, rigidData.maxContactImpulse,
|
|
PX_INVALID_NODE, PX_MAX_F32, outSolverBodyData[a], 0, dt, gyroscopicForces);
|
|
}
|
|
}
|
|
|
|
void immediate::PxConstructStaticSolverBody(const PxTransform& globalPose, PxSolverBodyData& solverBodyData)
|
|
{
|
|
PX_ASSERT((size_t(&solverBodyData) & 0xf) == 0);
|
|
|
|
const PxVec3 zero(0.0f);
|
|
Dy::copyToSolverBodyData(zero, zero, 0.0f, zero, globalPose, -PX_MAX_F32, PX_MAX_F32, PX_INVALID_NODE, PX_MAX_F32, solverBodyData, 0, 0.0f, false);
|
|
}
|
|
|
|
void immediate::PxIntegrateSolverBodies(PxSolverBodyData* solverBodyData, PxSolverBody* solverBody, PxVec3* linearMotionVelocity, PxVec3* angularMotionState, PxU32 nbBodiesToIntegrate, PxReal dt)
|
|
{
|
|
PX_ASSERT((size_t(solverBodyData) & 0xf) == 0);
|
|
PX_ASSERT((size_t(solverBody) & 0xf) == 0);
|
|
|
|
for(PxU32 i=0; i<nbBodiesToIntegrate; ++i)
|
|
Dy::integrateCore(linearMotionVelocity[i], angularMotionState[i], solverBody[i], solverBodyData[i], dt, 0);
|
|
}
|
|
|
|
namespace
|
|
{
|
|
// PT: local structure to provide a ctor for the PxArray below, I don't want to make it visible to the regular PhysX code
|
|
struct immArticulationJointCore : Dy::ArticulationJointCore
|
|
{
|
|
immArticulationJointCore() : Dy::ArticulationJointCore(PxTransform(PxIdentity), PxTransform(PxIdentity))
|
|
{
|
|
}
|
|
};
|
|
|
|
// PT: this class makes it possible to call the FeatherstoneArticulation protected functions from here.
|
|
class immArticulation : public FeatherstoneArticulation
|
|
{
|
|
public:
|
|
immArticulation(const PxArticulationDataRC& data);
|
|
~immArticulation();
|
|
|
|
PX_FORCE_INLINE void immSolveInternalConstraints(PxReal dt, PxReal invDt, PxReal elapsedTime, bool velocityIteration, bool isTGS)
|
|
{
|
|
// PT: TODO: revisit the TGS coeff (PX-4516)
|
|
FeatherstoneArticulation::solveInternalConstraints(dt, dt, invDt, velocityIteration, isTGS, elapsedTime, isTGS ? 0.7f : DY_ARTICULATION_PGS_BIAS_COEFFICIENT, false, false); // pass correct flag value - PX-4744
|
|
}
|
|
|
|
PX_FORCE_INLINE void immComputeUnconstrainedVelocitiesTGS(PxReal dt, PxReal totalDt, PxReal invDt, PxReal /*invTotalDt*/, const PxVec3& gravity, PxReal invLengthScale)
|
|
{
|
|
mArticulationData.setDt(totalDt);
|
|
|
|
FeatherstoneArticulation::computeUnconstrainedVelocitiesInternal(gravity, invLengthScale); // pass perIterationGravity flag here? -> PX-4744
|
|
|
|
setupInternalConstraints(mArticulationData.getLinks(), mArticulationData.getLinkCount(),
|
|
mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE, mArticulationData, dt, totalDt, invDt, true);
|
|
}
|
|
|
|
PX_FORCE_INLINE void immComputeUnconstrainedVelocities(PxReal dt, const PxVec3& gravity, PxReal invLengthScale)
|
|
{
|
|
mArticulationData.setDt(dt);
|
|
|
|
FeatherstoneArticulation::computeUnconstrainedVelocitiesInternal(gravity, invLengthScale);
|
|
const PxReal invDt = 1.0f/dt;
|
|
setupInternalConstraints(mArticulationData.getLinks(), mArticulationData.getLinkCount(),
|
|
mArticulationData.getArticulationFlags() & PxArticulationFlag::eFIX_BASE, mArticulationData, dt, dt, invDt, false);
|
|
}
|
|
|
|
void allocate(const PxU32 nbLinks);
|
|
PxU32 addLink(const PxU32 parent, const PxArticulationLinkDataRC& data);
|
|
|
|
void complete();
|
|
|
|
PxArray<Dy::ArticulationLink> mLinks;
|
|
PxArray<PxsBodyCore> mBodyCores;
|
|
PxArray<immArticulationJointCore> mArticulationJointCores;
|
|
PxArticulationFlags mFlags; // PT: PX-1399. Stored in Dy::ArticulationCore for retained mode.
|
|
|
|
// PT: quick and dirty version, to improve later
|
|
struct immArticulationLinkDataRC : PxArticulationLinkDataRC
|
|
{
|
|
PxArticulationLinkCookie parent;
|
|
PxU32 userID;
|
|
};
|
|
PxArray<immArticulationLinkDataRC> mTemp;
|
|
PxArray<Cm::SpatialVectorF> mTempDeltaV;
|
|
|
|
bool mImmDirty;
|
|
bool mJCalcDirty;
|
|
private:
|
|
void initJointCore(Dy::ArticulationJointCore& core, const PxArticulationJointDataRC& inboundJoint);
|
|
};
|
|
|
|
PxU32 createHeaders(PxU32 nbConstraints, const PxU32* PX_RESTRICT constraintsPerPartition, PxU32 maxBatchPartition,
|
|
PxConstraintBatchHeader* PX_RESTRICT outBatchHeaders, const PxSolverConstraintDesc* PX_RESTRICT outOrderedConstraintDescs)
|
|
{
|
|
PxU32 numHeaders = 0;
|
|
PxU32 currentPartition = 0;
|
|
PxU32 maxJ = nbConstraints == 0 ? 0 : constraintsPerPartition[0];
|
|
|
|
//printf("\nnbConstraints: %d\n", nbConstraints);
|
|
|
|
for (PxU32 a = 0; a < nbConstraints;)
|
|
{
|
|
PxConstraintBatchHeader& header = outBatchHeaders[numHeaders++];
|
|
header.startIndex = a;
|
|
|
|
PxU32 loopMax = PxMin(maxJ - a, 4u);
|
|
PxU16 j = 0;
|
|
if (loopMax > 0)
|
|
{
|
|
j = 1;
|
|
const PxSolverConstraintDesc& desc = outOrderedConstraintDescs[a];
|
|
|
|
if(isArticulationConstraint(desc))
|
|
loopMax = 1;
|
|
|
|
if (currentPartition < maxBatchPartition)
|
|
{
|
|
for (; j < loopMax && desc.constraintType == outOrderedConstraintDescs[a + j].constraintType
|
|
&& !isArticulationConstraint(outOrderedConstraintDescs[a + j]); ++j);
|
|
}
|
|
header.stride = j;
|
|
header.constraintType = desc.constraintType;
|
|
}
|
|
if (maxJ == (a + j) && maxJ != nbConstraints)
|
|
{
|
|
currentPartition++;
|
|
maxJ = constraintsPerPartition[currentPartition];
|
|
}
|
|
a += j;
|
|
}
|
|
return numHeaders;
|
|
}
|
|
}
|
|
|
|
static PxU32 batchConstraints( const PxSolverConstraintDesc* solverConstraintDescs, PxU32 nbConstraints, PxU8* solverBodies, PxU32 nbBodies,
|
|
PxConstraintBatchHeader* outBatchHeaders, PxSolverConstraintDesc* outOrderedConstraintDescs,
|
|
PxArticulationHandle* articulations, PxU32 nbArticulations, PxU32 stride, PxU32 maxPartitions, bool)
|
|
{
|
|
PX_ASSERT((size_t(solverBodies) & 0xf) == 0);
|
|
|
|
if(!nbConstraints)
|
|
return 0;
|
|
|
|
// PT: used to dump data for UTs. Please keep that code around.
|
|
/* if(0)
|
|
{
|
|
FILE* fp = fopen("d:\\tmp\\dump.txt", "w");
|
|
if(fp)
|
|
{
|
|
for(PxU32 i=0;i<nbConstraints;i++)
|
|
{
|
|
const PxU32 indexA = PxU32(PxU64(reinterpret_cast<PxU8*>(solverConstraintDescs[i].bodyA) - solverBodies)/stride);
|
|
const PxU32 indexB = PxU32(PxU64(reinterpret_cast<PxU8*>(solverConstraintDescs[i].bodyB) - solverBodies)/stride);
|
|
fprintf(fp, "%d, %d,\n", indexA, indexB);
|
|
}
|
|
fclose(fp);
|
|
}
|
|
}*/
|
|
|
|
const ConstraintPartitionIn in( solverBodies, nbBodies, stride,
|
|
reinterpret_cast<Dy::FeatherstoneArticulation**>(articulations), nbArticulations,
|
|
solverConstraintDescs, nbConstraints, maxPartitions, true); // PT: crashes with false!
|
|
|
|
// PT: this is really a waste of space for what we use it for. It should be allocated on-the-fly or maybe use indices
|
|
// within the source buffer, which would also be cheaper to copy/reshuffle.
|
|
PxSolverConstraintDesc* overflowConstraintDescriptors = PX_ALLOCATE(PxSolverConstraintDesc, nbConstraints, "overflowConstraintDescriptors");
|
|
|
|
PxArray<PxU32> constraintsPerPartition;
|
|
constraintsPerPartition.reserve(128);
|
|
|
|
ConstraintPartitionOut out(outOrderedConstraintDescs, overflowConstraintDescriptors, &constraintsPerPartition);
|
|
|
|
// PT: maxPartition usually well below 32 but some scenes go far beyond (I've seen a 60....)
|
|
const PxU32 maxPartition = partitionContactConstraints(out, in);
|
|
PX_UNUSED(maxPartition);
|
|
//printf("maxPartition: %d\n", maxPartition);
|
|
|
|
//printf("mNumOverflowConstraints: %d\n", out.mNumOverflowConstraints);
|
|
|
|
PX_FREE(overflowConstraintDescriptors);
|
|
|
|
// PT: we probably cannot reuse the code from the SDK here because it's hardcoded for Dy types in constraintLengthOver16,
|
|
// while imm mode used e.g. PxSolverConstraintDesc::eCONTACT_CONSTRAINT there.
|
|
const PxU32 nbHeaders = createHeaders(nbConstraints, constraintsPerPartition.begin(), maxPartition, outBatchHeaders, outOrderedConstraintDescs);
|
|
|
|
#ifdef MISSING_CALL
|
|
if(tgs)
|
|
{
|
|
::processOverflowConstraints(reinterpret_cast<PxU8*>(solverBodies), sizeof(PxTGSSolverBodyVel), nbBodies,
|
|
//Dy::ArticulationSolverDesc* articulationDescs, PxU32 numArticulations,
|
|
NULL, 0,
|
|
outOrderedConstraintDescs, out.mNumOverflowConstraints/* ? nbConstraints : 0*/);
|
|
|
|
// processOverflowConstraints(reinterpret_cast<PxU8*>(mContext.mSolverBodyVelPool.begin() + mObjects.solverBodyOffset+1), sizeof(PxTGSSolverBodyVel),
|
|
// mCounts.bodies, mThreadContext.getArticulations().begin(), mThreadContext.getArticulations().size(), contactDescBegin,
|
|
// mThreadContext.mHasOverflowPartitions ? mThreadContext.mConstraintsPerPartition[0] : 0);
|
|
}
|
|
#endif
|
|
return nbHeaders;
|
|
}
|
|
|
|
PxU32 immediate::PxBatchConstraints(const PxSolverConstraintDesc* solverConstraintDescs, PxU32 nbConstraints, PxSolverBody* solverBodies, PxU32 nbBodies,
|
|
PxConstraintBatchHeader* outBatchHeaders, PxSolverConstraintDesc* outOrderedConstraintDescs,
|
|
PxArticulationHandle* articulations, PxU32 nbArticulations)
|
|
{
|
|
return batchConstraints(solverConstraintDescs, nbConstraints, reinterpret_cast<PxU8*>(solverBodies), nbBodies,
|
|
outBatchHeaders, outOrderedConstraintDescs, articulations, nbArticulations,
|
|
sizeof(PxSolverBody), PX_MAX_U32, false);
|
|
}
|
|
|
|
PxU32 immediate::PxBatchConstraintsTGS(const PxSolverConstraintDesc* solverConstraintDescs, PxU32 nbConstraints, PxTGSSolverBodyVel* solverBodies, PxU32 nbBodies,
|
|
PxConstraintBatchHeader* outBatchHeaders, PxSolverConstraintDesc* outOrderedConstraintDescs,
|
|
PxArticulationHandle* articulations, PxU32 nbArticulations)
|
|
{
|
|
// PT: SDK code uses 64 for TGS but in immediate mode it makes the caterpillar track scene explode, most likely because mNumOverflowConstraints = 1024.
|
|
// I think the missing bit is the call to processOverflowConstraints, that we do for TGS in the SDK but not here in immediate mode. Using PX_MAX_U32
|
|
// instead, in immediate mode, fixes the explosion.
|
|
//const PxU32 maxPartitions = 64;
|
|
const PxU32 maxPartitions = PX_MAX_U32;
|
|
return batchConstraints(solverConstraintDescs, nbConstraints, reinterpret_cast<PxU8*>(solverBodies), nbBodies,
|
|
outBatchHeaders, outOrderedConstraintDescs, articulations, nbArticulations,
|
|
sizeof(PxTGSSolverBodyVel), maxPartitions, true);
|
|
}
|
|
|
|
bool immediate::PxCreateContactConstraints(PxConstraintBatchHeader* batchHeaders, PxU32 nbHeaders, PxSolverContactDesc* contactDescs,
|
|
PxConstraintAllocator& allocator, PxReal invDt, PxReal bounceThreshold, PxReal frictionOffsetThreshold,
|
|
PxReal correlationDistance, PxSpatialVector* ZV)
|
|
{
|
|
PX_ASSERT(invDt > 0.0f && PxIsFinite(invDt));
|
|
PX_ASSERT(bounceThreshold < 0.0f);
|
|
PX_ASSERT(frictionOffsetThreshold > 0.0f);
|
|
PX_ASSERT(correlationDistance > 0.0f);
|
|
|
|
Dy::CorrelationBuffer cb;
|
|
|
|
PxU32 currentContactDescIdx = 0;
|
|
|
|
const PxReal dt = 1.0f / invDt;
|
|
|
|
for(PxU32 i=0; i<nbHeaders; ++i)
|
|
{
|
|
Dy::SolverConstraintPrepState::Enum state = Dy::SolverConstraintPrepState::eUNBATCHABLE;
|
|
|
|
PxConstraintBatchHeader& batchHeader = batchHeaders[i];
|
|
if (batchHeader.stride == 4)
|
|
{
|
|
const PxSolverContactDesc* ctc = contactDescs + currentContactDescIdx;
|
|
const PxU32 totalContacts = ctc[0].numContacts
|
|
+ ctc[1].numContacts
|
|
+ ctc[2].numContacts
|
|
+ ctc[3].numContacts;
|
|
|
|
if (totalContacts <= 64)
|
|
{
|
|
state = Dy::createFinalizeSolverContacts4(cb,
|
|
contactDescs + currentContactDescIdx,
|
|
invDt,
|
|
dt,
|
|
bounceThreshold,
|
|
frictionOffsetThreshold,
|
|
correlationDistance,
|
|
allocator);
|
|
}
|
|
}
|
|
|
|
if (state == Dy::SolverConstraintPrepState::eUNBATCHABLE)
|
|
{
|
|
Cm::SpatialVectorF* Z = reinterpret_cast<Cm::SpatialVectorF*>(ZV);
|
|
for(PxU32 a=0; a<batchHeader.stride; ++a)
|
|
{
|
|
Dy::createFinalizeSolverContacts(contactDescs[currentContactDescIdx + a], cb, invDt, dt, bounceThreshold,
|
|
frictionOffsetThreshold, correlationDistance, allocator, Z);
|
|
}
|
|
}
|
|
|
|
if(contactDescs[currentContactDescIdx].desc->constraint)
|
|
{
|
|
PxU8 type = *contactDescs[currentContactDescIdx].desc->constraint;
|
|
|
|
if(type == DY_SC_TYPE_STATIC_CONTACT)
|
|
{
|
|
//Check if any block of constraints is classified as type static (single) contact constraint.
|
|
//If they are, iterate over all constraints grouped with it and switch to "dynamic" contact constraint
|
|
//type if there's a dynamic contact constraint in the group.
|
|
for(PxU32 c=1; c<batchHeader.stride; ++c)
|
|
{
|
|
if (*contactDescs[currentContactDescIdx + c].desc->constraint == DY_SC_TYPE_RB_CONTACT)
|
|
{
|
|
type = DY_SC_TYPE_RB_CONTACT;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
batchHeader.constraintType = type;
|
|
}
|
|
|
|
currentContactDescIdx += batchHeader.stride;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool immediate::PxCreateJointConstraints(PxConstraintBatchHeader* batchHeaders, PxU32 nbHeaders, PxSolverConstraintPrepDesc* jointDescs,
|
|
PxConstraintAllocator& allocator, PxSpatialVector* /* ZV */, PxReal dt, PxReal invDt)
|
|
{
|
|
PX_ASSERT(dt > 0.0f);
|
|
PX_ASSERT(invDt > 0.0f && PxIsFinite(invDt));
|
|
|
|
|
|
PxU32 currentDescIdx = 0;
|
|
for(PxU32 i=0; i<nbHeaders; ++i)
|
|
{
|
|
Dy::SolverConstraintPrepState::Enum state = Dy::SolverConstraintPrepState::eUNBATCHABLE;
|
|
|
|
PxConstraintBatchHeader& batchHeader = batchHeaders[i];
|
|
|
|
PxU8 type = DY_SC_TYPE_BLOCK_1D;
|
|
if (batchHeader.stride == 4)
|
|
{
|
|
PxU32 totalRows = 0;
|
|
PxU32 maxRows = 0;
|
|
bool batchable = true;
|
|
for (PxU32 a = 0; a < batchHeader.stride; ++a)
|
|
{
|
|
if (jointDescs[currentDescIdx + a].numRows == 0)
|
|
{
|
|
batchable = false;
|
|
break;
|
|
}
|
|
totalRows += jointDescs[currentDescIdx + a].numRows;
|
|
maxRows = PxMax(maxRows, jointDescs[currentDescIdx + a].numRows);
|
|
}
|
|
|
|
if (batchable)
|
|
{
|
|
state = Dy::setupSolverConstraint4
|
|
(jointDescs + currentDescIdx,
|
|
dt, invDt, totalRows,
|
|
allocator, maxRows, false);
|
|
}
|
|
}
|
|
|
|
if (state == Dy::SolverConstraintPrepState::eUNBATCHABLE)
|
|
{
|
|
type = DY_SC_TYPE_RB_1D;
|
|
for(PxU32 a=0; a<batchHeader.stride; ++a)
|
|
{
|
|
// PT: TODO: And "isExtended" is already computed in Dy::ConstraintHelper::setupSolverConstraint
|
|
PxSolverConstraintDesc& desc = *jointDescs[currentDescIdx + a].desc;
|
|
const bool isExtended = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY || desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY;
|
|
if(isExtended)
|
|
type = DY_SC_TYPE_EXT_1D;
|
|
|
|
Dy::ConstraintHelper::setupSolverConstraint(jointDescs[currentDescIdx + a], allocator, dt, invDt);
|
|
}
|
|
}
|
|
|
|
batchHeader.constraintType = type;
|
|
currentDescIdx += batchHeader.stride;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
template<class LeafTestT, class ParamsT>
|
|
static bool PxCreateJointConstraintsWithShadersT(PxConstraintBatchHeader* batchHeaders, const PxU32 nbHeaders, ParamsT* params, PxSolverConstraintPrepDesc* jointDescs,
|
|
PxConstraintAllocator& allocator, PxSpatialVector* Z, PxReal dt, PxReal invDt)
|
|
{
|
|
Px1DConstraint allRows[Dy::MAX_CONSTRAINT_ROWS * 4];
|
|
|
|
//Runs shaders to fill in rows...
|
|
|
|
PxU32 currentDescIdx = 0;
|
|
|
|
for(PxU32 i=0; i<nbHeaders; i++)
|
|
{
|
|
Px1DConstraint* rows = allRows;
|
|
Px1DConstraint* rows2 = allRows;
|
|
|
|
PxU32 maxRows = 0;
|
|
PxU32 nbToPrep = MAX_CONSTRAINT_ROWS;
|
|
|
|
PxConstraintBatchHeader& batchHeader = batchHeaders[i];
|
|
|
|
for(PxU32 a=0; a<batchHeader.stride; a++)
|
|
{
|
|
PxSolverConstraintPrepDesc& desc = jointDescs[currentDescIdx + a];
|
|
|
|
PxConstraintSolverPrep prep;
|
|
const void* constantBlock;
|
|
const bool useExtendedLimits = LeafTestT::getData(params, currentDescIdx + a, &prep, &constantBlock);
|
|
PX_ASSERT(prep);
|
|
|
|
PX_ASSERT(rows2 + nbToPrep <= allRows + MAX_CONSTRAINT_ROWS*4);
|
|
setupConstraintRows(rows2, nbToPrep);
|
|
rows2 += nbToPrep;
|
|
|
|
desc.invMassScales.linear0 = desc.invMassScales.linear1 = desc.invMassScales.angular0 = desc.invMassScales.angular1 = 1.0f;
|
|
desc.body0WorldOffset = PxVec3(0.0f);
|
|
|
|
PxVec3p unused_cA2w, unused_cB2w;
|
|
//TAG:solverprepcall
|
|
const PxU32 constraintCount = prep(rows,
|
|
desc.body0WorldOffset,
|
|
Dy::MAX_CONSTRAINT_ROWS,
|
|
desc.invMassScales,
|
|
constantBlock,
|
|
desc.bodyFrame0, desc.bodyFrame1,
|
|
useExtendedLimits,
|
|
unused_cA2w, unused_cB2w);
|
|
|
|
nbToPrep = constraintCount;
|
|
maxRows = PxMax(constraintCount, maxRows);
|
|
|
|
desc.rows = rows;
|
|
desc.numRows = constraintCount;
|
|
rows += constraintCount;
|
|
}
|
|
|
|
PxCreateJointConstraints(&batchHeader, 1, jointDescs + currentDescIdx, allocator, Z, dt, invDt);
|
|
|
|
currentDescIdx += batchHeader.stride;
|
|
}
|
|
return true; //KS - TODO - do some error reporting/management...
|
|
}
|
|
|
|
namespace
|
|
{
|
|
class PxConstraintAdapter
|
|
{
|
|
public:
|
|
static PX_FORCE_INLINE bool getData(PxConstraint** constraints, PxU32 i, PxConstraintSolverPrep* prep, const void** constantBlock)
|
|
{
|
|
NpConstraint* npConstraint = static_cast<NpConstraint*>(constraints[i]);
|
|
const Sc::ConstraintCore& core = npConstraint->getCore();
|
|
|
|
if (npConstraint->isDirty())
|
|
{
|
|
core.getPxConnector()->prepareData();
|
|
npConstraint->markClean();
|
|
}
|
|
|
|
*prep = core.getPxConnector()->getPrep();
|
|
*constantBlock = core.getPxConnector()->getConstantBlock();
|
|
return core.getFlags() & PxConstraintFlag::eENABLE_EXTENDED_LIMITS;
|
|
}
|
|
};
|
|
}
|
|
|
|
bool immediate::PxCreateJointConstraintsWithShaders(PxConstraintBatchHeader* batchHeaders, PxU32 nbHeaders, PxConstraint** constraints, PxSolverConstraintPrepDesc* jointDescs,
|
|
PxConstraintAllocator& allocator, PxReal dt, PxReal invDt, PxSpatialVector* Z)
|
|
{
|
|
return PxCreateJointConstraintsWithShadersT<PxConstraintAdapter>(batchHeaders, nbHeaders, constraints, jointDescs, allocator, Z, dt, invDt);
|
|
}
|
|
|
|
bool immediate::PxCreateJointConstraintsWithImmediateShaders(PxConstraintBatchHeader* batchHeaders, PxU32 nbHeaders, PxImmediateConstraint* constraints, PxSolverConstraintPrepDesc* jointDescs,
|
|
PxConstraintAllocator& allocator, PxReal dt, PxReal invDt, PxSpatialVector* Z)
|
|
{
|
|
class immConstraintAdapter
|
|
{
|
|
public:
|
|
static PX_FORCE_INLINE bool getData(PxImmediateConstraint* constraints_, PxU32 i, PxConstraintSolverPrep* prep, const void** constantBlock)
|
|
{
|
|
const PxImmediateConstraint& ic = constraints_[i];
|
|
*prep = ic.prep;
|
|
*constantBlock = ic.constantBlock;
|
|
return false;
|
|
}
|
|
};
|
|
|
|
return PxCreateJointConstraintsWithShadersT<immConstraintAdapter>(batchHeaders, nbHeaders, constraints, jointDescs, allocator, Z, dt, invDt);
|
|
}
|
|
|
|
/*static*/ PX_FORCE_INLINE bool PxIsZero(const PxSolverBody* bodies, PxU32 nbBodies)
|
|
{
|
|
for(PxU32 i=0; i<nbBodies; i++)
|
|
{
|
|
if( !bodies[i].linearVelocity.isZero() ||
|
|
!bodies[i].angularState.isZero())
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void immediate::PxSolveConstraints(const PxConstraintBatchHeader* batchHeaders, PxU32 nbBatchHeaders, const PxSolverConstraintDesc* solverConstraintDescs,
|
|
const PxSolverBody* solverBodies, PxVec3* linearMotionVelocity, PxVec3* angularMotionVelocity, PxU32 nbSolverBodies, PxU32 nbPositionIterations, PxU32 nbVelocityIterations,
|
|
float dt, float invDt, PxU32 nbSolverArticulations, PxArticulationHandle* solverArticulations, PxSpatialVector* /* pxZ */, PxSpatialVector* pxDeltaV)
|
|
{
|
|
#ifdef TO_REVISIT
|
|
if(0)
|
|
{
|
|
// PT:
|
|
// - problem 1: why do we need bodyDataList? => this is actually the same solverBodyArray from the SolverContext so we already encountered that one.
|
|
// Can be null because we don't do "writeBacks" here.
|
|
// - problem 2: the articulation descs
|
|
// - problem 3: spatial vector (same problem as for reusing saveMotionVelocities)
|
|
// Honestly I don't understand:
|
|
// - PxSpatialVector / SpatialVector / SpatialVectorF / UnAlignedSpatialVector / SpatialVectorV
|
|
SolverIslandParams params;
|
|
params.positionIterations = nbPositionIterations;
|
|
params.velocityIterations = nbVelocityIterations;
|
|
params.bodyListStart = solverBodies;
|
|
params.bodyDataList = NULL;
|
|
params.bodyListSize = nbSolverBodies;
|
|
params.articulationListStart = NULL;
|
|
params.articulationListSize = 0;
|
|
params.constraintList = solverConstraintDescs;
|
|
params.constraintBatchHeaders = batchHeaders;
|
|
params.numConstraintHeaders = nbBatchHeaders;
|
|
params.dt = dt;
|
|
params.invDt = invDt;
|
|
params.thresholdStream = NULL;
|
|
params.thresholdStreamLength = 0;
|
|
params.outThresholdPairs = NULL;
|
|
params.Z = NULL;//pxZ;
|
|
params.deltaV = NULL;//pxDeltaV;
|
|
|
|
Cm::SpatialVector* spatialVectors = PX_ALLOCATE(Cm::SpatialVector, nbSolverBodies, "");
|
|
params.motionVelocityArray = spatialVectors;
|
|
for(PxU32 i=0;i<nbSolverBodies;i++)
|
|
{
|
|
spatialVectors[i].linear = linearMotionVelocity[i];
|
|
spatialVectors[i].angular = angularMotionVelocity[i];
|
|
}
|
|
|
|
SolverCoreGeneral solver(false);
|
|
//solver.solveV_Blocks(params);
|
|
|
|
/* params.headersPerPartition = mThreadContext.mConstraintsPerPartition.begin();
|
|
params.nbPartitions = mThreadContext.mConstraintsPerPartition.size();
|
|
const PxU32 unrollSize = 8;
|
|
const PxU32 denom = PxMax(1u, (mThreadContext.mMaxPartitions*unrollSize));
|
|
const PxU32 MaxTasks = getTaskManager()->getCpuDispatcher()->getWorkerCount();
|
|
// PT: small improvement: if there's no contacts, use the number of bodies instead.
|
|
// That way the integration work still benefits from multiple tasks.
|
|
const PxU32 numWorkItems = mThreadContext.numContactConstraintBatches ? mThreadContext.numContactConstraintBatches : mIslandContext.mCounts.bodies;
|
|
const PxU32 idealThreads = (numWorkItems+denom-1)/denom;
|
|
const PxU32 numTasks = PxMax(1u, PxMin(idealThreads, MaxTasks));
|
|
|
|
if(numTasks > 1)
|
|
{
|
|
const PxU32 idealBatchSize = PxMax(unrollSize, idealThreads*unrollSize/(numTasks*2));
|
|
params.batchSize = idealBatchSize; //assigning ideal batch size for the solver to grab work at. Only needed by the multi-threaded island solver.
|
|
*/
|
|
|
|
params.headersPerPartition;
|
|
params.nbPartitions;
|
|
params.batchSize;
|
|
solver.solveVParallelAndWriteBack(params, NULL, NULL);
|
|
|
|
for(PxU32 i=0;i<nbSolverBodies;i++)
|
|
{
|
|
linearMotionVelocity[i] = spatialVectors[i].linear;
|
|
angularMotionVelocity[i] = spatialVectors[i].angular;
|
|
}
|
|
|
|
PX_FREE(spatialVectors);
|
|
|
|
return;
|
|
}
|
|
#endif
|
|
|
|
PX_ASSERT(nbPositionIterations > 0);
|
|
PX_ASSERT(nbVelocityIterations > 0);
|
|
PX_ASSERT(PxIsZero(solverBodies, nbSolverBodies)); //Ensure that solver body velocities have been zeroed before solving
|
|
PX_ASSERT((size_t(solverBodies) & 0xf) == 0);
|
|
|
|
const Dy::SolveBlockMethod* solveTable = Dy::gVTableSolveBlock;
|
|
const Dy::SolveBlockMethod* solveConcludeTable = Dy::gVTableSolveConcludeBlock;
|
|
const Dy::SolveWriteBackBlockMethod* solveWritebackTable = Dy::gVTableSolveWriteBackBlock;
|
|
|
|
Dy::SolverContext cache;
|
|
cache.solverBodyArray = NULL;
|
|
cache.mThresholdStreamIndex = 0;
|
|
cache.mThresholdStreamLength = 0xFFFFFFF;
|
|
|
|
PX_ASSERT(nbPositionIterations > 0);
|
|
PX_ASSERT(nbVelocityIterations > 0);
|
|
|
|
Cm::SpatialVectorF* deltaV = reinterpret_cast<Cm::SpatialVectorF*>(pxDeltaV);
|
|
cache.deltaV = deltaV;
|
|
|
|
Dy::FeatherstoneArticulation** articulations = reinterpret_cast<Dy::FeatherstoneArticulation**>(solverArticulations);
|
|
|
|
struct PGS
|
|
{
|
|
static PX_FORCE_INLINE void solveArticulationInternalConstraints(float dt_, float invDt_, PxU32 nbSolverArticulations_, Dy::FeatherstoneArticulation** solverArticulations_, bool velIter_)
|
|
{
|
|
while(nbSolverArticulations_--)
|
|
{
|
|
immArticulation* immArt = static_cast<immArticulation*>(*solverArticulations_++);
|
|
immArt->immSolveInternalConstraints(dt_, invDt_, 0.0f, velIter_, false);
|
|
}
|
|
}
|
|
|
|
static PX_FORCE_INLINE void runIter(const PxConstraintBatchHeader* batchHeaders_, PxU32 nbBatchHeaders_, const PxSolverConstraintDesc* solverConstraintDescs_,
|
|
PxU32 nbSolverArticulations_, Dy::FeatherstoneArticulation** articulations_,
|
|
const Dy::SolveBlockMethod* solveTable_, Dy::SolverContext& solverCache_, float dt_, float invDt_, bool doFriction, bool velIter_)
|
|
{
|
|
solverCache_.doFriction = doFriction;
|
|
for(PxU32 a=0; a<nbBatchHeaders_; ++a)
|
|
{
|
|
const PxConstraintBatchHeader& batch = batchHeaders_[a];
|
|
solveTable_[batch.constraintType](solverConstraintDescs_ + batch.startIndex, batch.stride, solverCache_);
|
|
}
|
|
|
|
solveArticulationInternalConstraints(dt_, invDt_, nbSolverArticulations_, articulations_, velIter_);
|
|
}
|
|
};
|
|
|
|
cache.isPositionIteration = true;
|
|
for(PxU32 i=nbPositionIterations; i>1; --i)
|
|
PGS::runIter(batchHeaders, nbBatchHeaders, solverConstraintDescs, nbSolverArticulations, articulations, solveTable, cache, dt, invDt, i <= 3, false);
|
|
PGS::runIter(batchHeaders, nbBatchHeaders, solverConstraintDescs, nbSolverArticulations, articulations, solveConcludeTable, cache, dt, invDt, true, false);
|
|
|
|
//Save motion velocities...
|
|
for(PxU32 a=0; a<nbSolverBodies; a++)
|
|
{
|
|
linearMotionVelocity[a] = solverBodies[a].linearVelocity;
|
|
angularMotionVelocity[a] = solverBodies[a].angularState;
|
|
}
|
|
|
|
for(PxU32 a=0; a<nbSolverArticulations; a++)
|
|
FeatherstoneArticulation::saveVelocity(reinterpret_cast<Dy::FeatherstoneArticulation*>(solverArticulations[a]), deltaV);
|
|
|
|
cache.isPositionIteration = false;
|
|
for(PxU32 i=nbVelocityIterations; i>1; --i)
|
|
PGS::runIter(batchHeaders, nbBatchHeaders, solverConstraintDescs, nbSolverArticulations, articulations, solveTable, cache, dt, invDt, true, true);
|
|
PGS::runIter(batchHeaders, nbBatchHeaders, solverConstraintDescs, nbSolverArticulations, articulations, solveWritebackTable, cache, dt, invDt, true, true);
|
|
}
|
|
|
|
static void createCache(Gu::Cache& cache, PxGeometryType::Enum geomType0, PxGeometryType::Enum geomType1, PxCacheAllocator& allocator)
|
|
{
|
|
if(gEnablePCMCaching[geomType0][geomType1])
|
|
{
|
|
if(geomType0 <= PxGeometryType::eCONVEXMESH && geomType1 <= PxGeometryType::eCONVEXMESH)
|
|
{
|
|
if(geomType0 == PxGeometryType::eSPHERE || geomType1 == PxGeometryType::eSPHERE)
|
|
{
|
|
Gu::PersistentContactManifold* manifold = PX_PLACEMENT_NEW(allocator.allocateCacheData(sizeof(Gu::SpherePersistentContactManifold)), Gu::SpherePersistentContactManifold)();
|
|
cache.setManifold(manifold);
|
|
}
|
|
else
|
|
{
|
|
Gu::PersistentContactManifold* manifold = PX_PLACEMENT_NEW(allocator.allocateCacheData(sizeof(Gu::LargePersistentContactManifold)), Gu::LargePersistentContactManifold)();
|
|
cache.setManifold(manifold);
|
|
}
|
|
cache.getManifold().clearManifold();
|
|
}
|
|
else
|
|
{
|
|
//ML: raised 1 to indicate the manifold is multiManifold which is for contact gen in mesh/height field
|
|
//cache.manifold = 1;
|
|
cache.setMultiManifold(NULL);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//cache.manifold = 0;
|
|
cache.mCachedData = NULL;
|
|
cache.mManifoldFlags = 0;
|
|
}
|
|
}
|
|
|
|
bool immediate::PxGenerateContacts( const PxGeometry* const * geom0, const PxGeometry* const * geom1, const PxTransform* pose0, const PxTransform* pose1, PxCache* contactCache, PxU32 nbPairs,
|
|
PxContactRecorder& contactRecorder, PxReal contactDistance, PxReal meshContactMargin, PxReal toleranceLength, PxCacheAllocator& allocator)
|
|
{
|
|
PX_ASSERT(meshContactMargin > 0.0f);
|
|
PX_ASSERT(toleranceLength > 0.0f);
|
|
PX_ASSERT(contactDistance > 0.0f);
|
|
PxContactBuffer contactBuffer;
|
|
|
|
const Gu::NarrowPhaseParams params(contactDistance, meshContactMargin, toleranceLength);
|
|
|
|
PxTransform32 transform0;
|
|
PxTransform32 transform1;
|
|
for(PxU32 i=0; i<nbPairs; i++)
|
|
{
|
|
contactBuffer.count = 0;
|
|
PxGeometryType::Enum type0 = geom0[i]->getType();
|
|
PxGeometryType::Enum type1 = geom1[i]->getType();
|
|
|
|
const PxGeometry* tempGeom0;
|
|
const PxGeometry* tempGeom1;
|
|
|
|
const bool bSwap = type0 > type1;
|
|
if(bSwap)
|
|
{
|
|
PxSwap(type0, type1);
|
|
|
|
tempGeom1 = geom0[i];
|
|
tempGeom0 = geom1[i];
|
|
|
|
transform1 = pose0[i];
|
|
transform0 = pose1[i];
|
|
}
|
|
else
|
|
{
|
|
tempGeom0 = geom0[i];
|
|
tempGeom1 = geom1[i];
|
|
|
|
transform0 = pose0[i];
|
|
transform1 = pose1[i];
|
|
}
|
|
|
|
//Now work out which type of PCM we need...
|
|
|
|
Gu::Cache& cache = static_cast<Gu::Cache&>(contactCache[i]);
|
|
|
|
const bool needsMultiManifold = type1 > PxGeometryType::eCONVEXMESH;
|
|
|
|
if(needsMultiManifold)
|
|
{
|
|
Gu::MultiplePersistentContactManifold multiManifold;
|
|
|
|
if(cache.isMultiManifold())
|
|
{
|
|
multiManifold.fromBuffer(cache.mCachedData);
|
|
}
|
|
else
|
|
{
|
|
multiManifold.initialize();
|
|
}
|
|
cache.setMultiManifold(&multiManifold);
|
|
|
|
//Do collision detection, then write manifold out...
|
|
g_PCMContactMethodTable[type0][type1](*tempGeom0, *tempGeom1, transform0, transform1, params, cache, contactBuffer, NULL);
|
|
|
|
const PxU32 size = (sizeof(Gu::MultiPersistentManifoldHeader) +
|
|
multiManifold.mNumManifolds * sizeof(Gu::SingleManifoldHeader) +
|
|
multiManifold.mNumTotalContacts * sizeof(Gu::CachedMeshPersistentContact));
|
|
|
|
PxU8* buffer = allocator.allocateCacheData(size);
|
|
|
|
multiManifold.toBuffer(buffer);
|
|
|
|
cache.setMultiManifold(buffer);
|
|
}
|
|
else
|
|
{
|
|
//Allocate the type of manifold we need again...
|
|
Gu::PersistentContactManifold* oldManifold = NULL;
|
|
|
|
if(cache.isManifold())
|
|
oldManifold = &cache.getManifold();
|
|
|
|
//Allocates and creates the PCM...
|
|
createCache(cache, type0, type1, allocator);
|
|
|
|
//Copy PCM from old to new manifold...
|
|
if(oldManifold)
|
|
{
|
|
Gu::PersistentContactManifold& manifold = cache.getManifold();
|
|
manifold.mRelativeTransform = oldManifold->mRelativeTransform;
|
|
manifold.mQuatA = oldManifold->mQuatA;
|
|
manifold.mQuatB = oldManifold->mQuatB;
|
|
manifold.mNumContacts = oldManifold->mNumContacts;
|
|
manifold.mNumWarmStartPoints = oldManifold->mNumWarmStartPoints;
|
|
manifold.mAIndice[0] = oldManifold->mAIndice[0]; manifold.mAIndice[1] = oldManifold->mAIndice[1];
|
|
manifold.mAIndice[2] = oldManifold->mAIndice[2]; manifold.mAIndice[3] = oldManifold->mAIndice[3];
|
|
manifold.mBIndice[0] = oldManifold->mBIndice[0]; manifold.mBIndice[1] = oldManifold->mBIndice[1];
|
|
manifold.mBIndice[2] = oldManifold->mBIndice[2]; manifold.mBIndice[3] = oldManifold->mBIndice[3];
|
|
PxMemCopy(manifold.mContactPoints, oldManifold->mContactPoints, sizeof(Gu::PersistentContact)*manifold.mNumContacts);
|
|
}
|
|
|
|
g_PCMContactMethodTable[type0][type1](*tempGeom0, *tempGeom1, transform0, transform1, params, cache, contactBuffer, NULL);
|
|
}
|
|
|
|
if(contactBuffer.count)
|
|
{
|
|
if(bSwap)
|
|
{
|
|
for(PxU32 a=0; a<contactBuffer.count; a++)
|
|
contactBuffer.contacts[a].normal = -contactBuffer.contacts[a].normal;
|
|
}
|
|
|
|
//Record this contact pair...
|
|
contactRecorder.recordContacts(contactBuffer.contacts, contactBuffer.count, i);
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
immArticulation::immArticulation(const PxArticulationDataRC& data) :
|
|
FeatherstoneArticulation(this),
|
|
mFlags (data.flags),
|
|
mImmDirty (true),
|
|
mJCalcDirty (true)
|
|
{
|
|
// PT: TODO: we only need the flags here, maybe drop the solver desc?
|
|
getSolverDesc().initData(NULL, &mFlags);
|
|
}
|
|
|
|
immArticulation::~immArticulation()
|
|
{
|
|
}
|
|
|
|
void immArticulation::initJointCore(Dy::ArticulationJointCore& core, const PxArticulationJointDataRC& inboundJoint)
|
|
{
|
|
core.init(inboundJoint.parentPose, inboundJoint.childPose);
|
|
|
|
core.jCalcUpdateFrames = true;
|
|
|
|
const PxU32* binP = reinterpret_cast<const PxU32*>(inboundJoint.targetPos);
|
|
const PxU32* binV = reinterpret_cast<const PxU32*>(inboundJoint.targetVel);
|
|
|
|
for(PxU32 i=0; i<PxArticulationAxis::eCOUNT; i++)
|
|
{
|
|
core.setLimit(PxArticulationAxis::Enum(i), inboundJoint.limits[i]);
|
|
core.setDrive(PxArticulationAxis::Enum(i), inboundJoint.drives[i]);
|
|
core.setMaxJointVelocity(inboundJoint.maxJointVelocity[i]);
|
|
|
|
// See Sc::ArticulationJointCore::setTargetP and Sc::ArticulationJointCore::setTargetV
|
|
if(binP[i]!=0xffffffff)
|
|
{
|
|
core.targetP[i] = inboundJoint.targetPos[i];
|
|
}
|
|
if(binV[i]!=0xffffffff)
|
|
{
|
|
core.targetV[i] = inboundJoint.targetVel[i];
|
|
}
|
|
core.armature[i] = inboundJoint.armature[i];
|
|
core.jointPos[i] = inboundJoint.jointPos[i];
|
|
core.jointVel[i] = inboundJoint.jointVel[i];
|
|
core.motion[i] = PxU8(inboundJoint.motion[i]);
|
|
}
|
|
|
|
core.setFrictionCoefficient(inboundJoint.frictionCoefficient);
|
|
core.setJointType(inboundJoint.type);
|
|
}
|
|
|
|
void immArticulation::allocate(const PxU32 nbLinks)
|
|
{
|
|
mLinks.reserve(nbLinks);
|
|
mBodyCores.resize(nbLinks);
|
|
mArticulationJointCores.resize(nbLinks);
|
|
}
|
|
|
|
PxU32 immArticulation::addLink(const PxU32 parentIndex, const PxArticulationLinkDataRC& data)
|
|
{
|
|
PX_ASSERT(data.pose.p.isFinite());
|
|
PX_ASSERT(data.pose.q.isFinite());
|
|
|
|
mImmDirty = true;
|
|
mJCalcDirty = true;
|
|
|
|
// Replicate ArticulationSim::addBody
|
|
addBody();
|
|
|
|
const PxU32 index = mLinks.size();
|
|
|
|
const PxTransform& bodyPose = data.pose;
|
|
//
|
|
PxsBodyCore* bodyCore = &mBodyCores[index];
|
|
{
|
|
// PT: this function inits everything but we only need a fraction of the data there for articulations
|
|
bodyCore->init( bodyPose, data.inverseInertia, data.inverseMass, 0.0f, 0.0f,
|
|
data.linearDamping, data.angularDamping,
|
|
data.maxLinearVelocitySq, data.maxAngularVelocitySq, PxActorType::eARTICULATION_LINK);
|
|
|
|
// PT: TODO: consider exposing all used data to immediate mode API (PX-1398)
|
|
// bodyCore->maxPenBias = -1e32f; // <= this one is related to setMaxDepenetrationVelocity
|
|
// bodyCore->linearVelocity = PxVec3(0.0f);
|
|
// bodyCore->angularVelocity = PxVec3(0.0f);
|
|
// bodyCore->linearVelocity = PxVec3(0.0f, 10.0f, 0.0f);
|
|
// bodyCore->angularVelocity = PxVec3(0.0f, 10.0f, 0.0f);
|
|
bodyCore->cfmScale = data.cfmScale;
|
|
}
|
|
|
|
/* PX_ASSERT((((index==0) && (joint == 0)) && (parent == 0)) ||
|
|
(((index!=0) && joint) && (parent && (parent->getArticulation() == this))));*/
|
|
|
|
// PT: TODO: add ctors everywhere
|
|
ArticulationLink& link = mLinks.insert();
|
|
|
|
// void BodySim::postActorFlagChange(PxU32 oldFlags, PxU32 newFlags)
|
|
bodyCore->disableGravity = data.disableGravity;
|
|
link.bodyCore = bodyCore;
|
|
link.mPathToRootStartIndex = 0;
|
|
link.mPathToRootCount = 0;
|
|
link.mChildrenStartIndex = 0xffffffff;
|
|
link.mNumChildren = 0;
|
|
|
|
const bool isRoot = parentIndex==0xffffffff;
|
|
if(!isRoot)
|
|
{
|
|
link.parent = parentIndex;
|
|
link.inboundJoint = &mArticulationJointCores[index];
|
|
|
|
ArticulationLink& parentLink = mLinks[parentIndex];
|
|
|
|
if(parentLink.mChildrenStartIndex == 0xffffffff)
|
|
parentLink.mChildrenStartIndex = index;
|
|
|
|
parentLink.mNumChildren++;
|
|
|
|
initJointCore(*link.inboundJoint, data.inboundJoint);
|
|
}
|
|
else
|
|
{
|
|
link.parent = DY_ARTICULATION_LINK_NONE;
|
|
link.inboundJoint = NULL;
|
|
}
|
|
|
|
return index;
|
|
}
|
|
|
|
void immArticulation::complete()
|
|
{
|
|
// Based on Sc::ArticulationSim::checkResize()
|
|
|
|
if(!mImmDirty)
|
|
return;
|
|
mImmDirty = false;
|
|
|
|
const PxU32 linkSize = mLinks.size();
|
|
setupLinks(linkSize, const_cast<ArticulationLink*>(mLinks.begin()));
|
|
jcalc<true>(mArticulationData);
|
|
mJCalcDirty = false;
|
|
initPathToRoot();
|
|
|
|
mTempDeltaV.resize(linkSize);
|
|
}
|
|
|
|
PxArticulationCookie immediate::PxBeginCreateArticulationRC(const PxArticulationDataRC& data)
|
|
{
|
|
// PT: we create the same class as before under the hood, we just don't tell users yet. Returning a void pointer/cookie
|
|
// means we can prevent them from using the articulation before it's fully completed. We do this because we're going to
|
|
// delay the link creation, so we don't want them to call PxAddArticulationLink and expect the link to be here already.
|
|
void* memory = PxAlignedAllocator<64>().allocate(sizeof(immArticulation), PX_FL);
|
|
PX_PLACEMENT_NEW(memory, immArticulation(data));
|
|
return memory;
|
|
}
|
|
|
|
PxArticulationLinkCookie immediate::PxAddArticulationLink(PxArticulationCookie articulation, const PxArticulationLinkCookie* parent, const PxArticulationLinkDataRC& data)
|
|
{
|
|
if(!articulation)
|
|
return PxCreateArticulationLinkCookie();
|
|
|
|
immArticulation* immArt = reinterpret_cast<immArticulation*>(articulation);
|
|
|
|
const PxU32 id = immArt->mTemp.size();
|
|
|
|
// PT: TODO: this is the quick-and-dirty version, we could try something smarter where we don't just batch everything like barbarians
|
|
immArticulation::immArticulationLinkDataRC tmp;
|
|
static_cast<PxArticulationLinkDataRC&>(tmp) = data;
|
|
tmp.userID = id;
|
|
tmp.parent = parent ? *parent : PxCreateArticulationLinkCookie();
|
|
immArt->mTemp.pushBack(tmp);
|
|
|
|
// WARNING: cannot be null, snippet uses null for regular rigid bodies (non articulation links)
|
|
return PxCreateArticulationLinkCookie(articulation, id);
|
|
}
|
|
|
|
PxArticulationHandle immediate::PxEndCreateArticulationRC(PxArticulationCookie articulation, PxArticulationLinkHandle* handles, PxU32 bufferSize)
|
|
{
|
|
if(!articulation)
|
|
return NULL;
|
|
|
|
immArticulation* immArt = reinterpret_cast<immArticulation*>(articulation);
|
|
|
|
PxU32 nbLinks = immArt->mTemp.size();
|
|
if(nbLinks!=bufferSize)
|
|
return NULL;
|
|
|
|
immArticulation::immArticulationLinkDataRC* linkData = immArt->mTemp.begin();
|
|
|
|
{
|
|
struct _{ bool operator()(const immArticulation::immArticulationLinkDataRC& data1, const immArticulation::immArticulationLinkDataRC& data2) const
|
|
{
|
|
if(!data1.parent.articulation)
|
|
return true;
|
|
if(!data2.parent.articulation)
|
|
return false;
|
|
|
|
return data1.parent.linkId < data2.parent.linkId;
|
|
}};
|
|
PxSort(linkData, nbLinks, _());
|
|
}
|
|
|
|
PxMemSet(handles, 0, sizeof(PxArticulationLinkHandle)*nbLinks);
|
|
|
|
immArt->allocate(nbLinks);
|
|
|
|
while(nbLinks--)
|
|
{
|
|
const PxU32 userID = linkData->userID;
|
|
PxU32 parentID = linkData->parent.linkId;
|
|
|
|
if(parentID != 0xffffffff)
|
|
parentID = handles[parentID].linkId;
|
|
const PxU32 realID = immArt->addLink(parentID, *linkData);
|
|
|
|
handles[userID] = PxArticulationLinkHandle(immArt, realID);
|
|
|
|
linkData++;
|
|
}
|
|
|
|
immArt->complete();
|
|
return immArt;
|
|
}
|
|
|
|
void immediate::PxReleaseArticulation(PxArticulationHandle articulation)
|
|
{
|
|
if(!articulation)
|
|
return;
|
|
|
|
immArticulation* immArt = static_cast<immArticulation*>(articulation);
|
|
immArt->~immArticulation();
|
|
PxAlignedAllocator<64>().deallocate(articulation);
|
|
}
|
|
|
|
PxArticulationCache* immediate::PxCreateArticulationCache(PxArticulationHandle articulation)
|
|
{
|
|
immArticulation* immArt = static_cast<immArticulation*>(articulation);
|
|
immArt->complete();
|
|
|
|
return FeatherstoneArticulation::createCache(immArt->getDofs(), immArt->getBodyCount());
|
|
}
|
|
|
|
void immediate::PxCopyInternalStateToArticulationCache(PxArticulationHandle articulation, PxArticulationCache& cache, PxArticulationCacheFlags flag)
|
|
{
|
|
immArticulation* immArt = static_cast<immArticulation *>(articulation);
|
|
immArt->copyInternalStateToCache(cache, flag, false);
|
|
}
|
|
|
|
void immediate::PxApplyArticulationCache(PxArticulationHandle articulation, PxArticulationCache& cache, PxArticulationCacheFlags flag)
|
|
{
|
|
bool shouldWake = false;
|
|
immArticulation* immArt = static_cast<immArticulation *>(articulation);
|
|
immArt->applyCache(cache, flag, shouldWake);
|
|
}
|
|
|
|
void immediate::PxReleaseArticulationCache(PxArticulationCache& cache)
|
|
{
|
|
PxcScratchAllocator* scratchAlloc = reinterpret_cast<PxcScratchAllocator*>(cache.scratchAllocator);
|
|
PX_DELETE(scratchAlloc);
|
|
cache.scratchAllocator = NULL;
|
|
|
|
PX_FREE(cache.scratchMemory);
|
|
|
|
PxArticulationCache* ptr = &cache;
|
|
PX_FREE(ptr);
|
|
}
|
|
|
|
void immediate::PxComputeUnconstrainedVelocities(PxArticulationHandle articulation, const PxVec3& gravity, PxReal dt, PxReal invLengthScale)
|
|
{
|
|
if(!articulation)
|
|
return;
|
|
|
|
immArticulation* immArt = static_cast<immArticulation*>(articulation);
|
|
immArt->complete();
|
|
if(immArt->mJCalcDirty)
|
|
{
|
|
immArt->mJCalcDirty = false;
|
|
immArt->jcalc<true>(immArt->mArticulationData);
|
|
}
|
|
immArt->immComputeUnconstrainedVelocities(dt, gravity, invLengthScale);
|
|
}
|
|
|
|
void immediate::PxUpdateArticulationBodies(PxArticulationHandle articulation, PxReal dt)
|
|
{
|
|
if(!articulation)
|
|
return;
|
|
|
|
immArticulation* immArt = static_cast<immArticulation*>(articulation);
|
|
|
|
FeatherstoneArticulation::updateBodies(immArt, immArt->mTempDeltaV.begin(), dt, true);
|
|
}
|
|
|
|
void immediate::PxComputeUnconstrainedVelocitiesTGS(PxArticulationHandle articulation, const PxVec3& gravity, PxReal dt,
|
|
PxReal totalDt, PxReal invDt, PxReal invTotalDt, PxReal invLengthScale)
|
|
{
|
|
if (!articulation)
|
|
return;
|
|
|
|
immArticulation* immArt = static_cast<immArticulation*>(articulation);
|
|
immArt->complete();
|
|
if(immArt->mJCalcDirty)
|
|
{
|
|
immArt->mJCalcDirty = false;
|
|
immArt->jcalc<true>(immArt->mArticulationData);
|
|
}
|
|
immArt->immComputeUnconstrainedVelocitiesTGS(dt, totalDt, invDt, invTotalDt, gravity, invLengthScale);
|
|
}
|
|
|
|
void immediate::PxUpdateArticulationBodiesTGS(PxArticulationHandle articulation, PxReal dt)
|
|
{
|
|
if (!articulation)
|
|
return;
|
|
|
|
immArticulation* immArt = static_cast<immArticulation*>(articulation);
|
|
|
|
FeatherstoneArticulation::updateBodies(immArt, immArt->mTempDeltaV.begin(), dt, false);
|
|
}
|
|
|
|
static void copyLinkData(PxArticulationLinkDerivedDataRC& data, const immArticulation* immArt, PxU32 index)
|
|
{
|
|
data.pose = immArt->mBodyCores[index].body2World;
|
|
// data.linearVelocity = immArt->mBodyCores[index].linearVelocity;
|
|
// data.angularVelocity = immArt->mBodyCores[index].angularVelocity;
|
|
const Cm::SpatialVectorF& velocity = immArt->getArticulationData().getMotionVelocity(index);
|
|
data.linearVelocity = velocity.bottom;
|
|
data.angularVelocity = velocity.top;
|
|
}
|
|
|
|
static PX_FORCE_INLINE const immArticulation* getFromLink(const PxArticulationLinkHandle& link, PxU32& index)
|
|
{
|
|
if(!link.articulation)
|
|
return NULL;
|
|
|
|
const immArticulation* immArt = static_cast<const immArticulation*>(link.articulation);
|
|
index = link.linkId;
|
|
|
|
if(index>=immArt->mLinks.size())
|
|
return NULL;
|
|
|
|
return immArt;
|
|
}
|
|
|
|
bool immediate::PxGetLinkData(const PxArticulationLinkHandle& link, PxArticulationLinkDerivedDataRC& data)
|
|
{
|
|
PxU32 index;
|
|
const immArticulation* immArt = getFromLink(link, index);
|
|
if(!immArt)
|
|
return false;
|
|
|
|
copyLinkData(data, immArt, index);
|
|
|
|
return true;
|
|
}
|
|
|
|
PxU32 immediate::PxGetAllLinkData(const PxArticulationHandle articulation, PxArticulationLinkDerivedDataRC* data)
|
|
{
|
|
if(!articulation)
|
|
return 0;
|
|
|
|
const immArticulation* immArt = static_cast<const immArticulation*>(articulation);
|
|
|
|
const PxU32 nb = immArt->mLinks.size();
|
|
if(data)
|
|
{
|
|
for(PxU32 i=0;i<nb;i++)
|
|
copyLinkData(data[i], immArt, i);
|
|
}
|
|
|
|
return nb;
|
|
}
|
|
|
|
bool immediate::PxGetMutableLinkData(const PxArticulationLinkHandle& link , PxArticulationLinkMutableDataRC& data)
|
|
{
|
|
PxU32 index;
|
|
const immArticulation* immArt = getFromLink(link, index);
|
|
if(!immArt)
|
|
return false;
|
|
|
|
data.inverseInertia = immArt->mBodyCores[index].inverseInertia;
|
|
data.inverseMass = immArt->mBodyCores[index].inverseMass;
|
|
data.linearDamping = immArt->mBodyCores[index].linearDamping;
|
|
data.angularDamping = immArt->mBodyCores[index].angularDamping;
|
|
data.maxLinearVelocitySq = immArt->mBodyCores[index].maxLinearVelocitySq;
|
|
data.maxAngularVelocitySq = immArt->mBodyCores[index].maxAngularVelocitySq;
|
|
data.cfmScale = immArt->mBodyCores[index].cfmScale;
|
|
data.disableGravity = immArt->mBodyCores[index].disableGravity!=0;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool immediate::PxSetMutableLinkData(const PxArticulationLinkHandle& link , const PxArticulationLinkMutableDataRC& data)
|
|
{
|
|
PxU32 index;
|
|
immArticulation* immArt = const_cast<immArticulation*>(getFromLink(link, index));
|
|
if(!immArt)
|
|
return false;
|
|
|
|
immArt->mBodyCores[index].inverseInertia = data.inverseInertia; // See Sc::BodyCore::setInverseInertia
|
|
immArt->mBodyCores[index].inverseMass = data.inverseMass; // See Sc::BodyCore::setInverseMass
|
|
immArt->mBodyCores[index].linearDamping = data.linearDamping; // See Sc::BodyCore::setLinearDamping
|
|
immArt->mBodyCores[index].angularDamping = data.angularDamping; // See Sc::BodyCore::setAngularDamping
|
|
immArt->mBodyCores[index].maxLinearVelocitySq = data.maxLinearVelocitySq; // See Sc::BodyCore::setMaxLinVelSq
|
|
immArt->mBodyCores[index].maxAngularVelocitySq = data.maxAngularVelocitySq; // See Sc::BodyCore::setMaxAngVelSq
|
|
immArt->mBodyCores[index].cfmScale = data.cfmScale; // See Sc::BodyCore::setCfmScale
|
|
immArt->mBodyCores[index].disableGravity = data.disableGravity; // See BodySim::postActorFlagChange
|
|
|
|
return true;
|
|
}
|
|
|
|
bool immediate::PxGetJointData(const PxArticulationLinkHandle& link, PxArticulationJointDataRC& data)
|
|
{
|
|
PxU32 index;
|
|
const immArticulation* immArt = getFromLink(link, index);
|
|
if(!immArt)
|
|
return false;
|
|
|
|
const Dy::ArticulationJointCore& core = immArt->mArticulationJointCores[index];
|
|
|
|
data.parentPose = core.parentPose;
|
|
data.childPose = core.childPose;
|
|
data.frictionCoefficient = core.frictionCoefficient;
|
|
data.type = PxArticulationJointType::Enum(core.jointType);
|
|
for(PxU32 i=0;i<PxArticulationAxis::eCOUNT;i++)
|
|
{
|
|
data.motion[i] = PxArticulationMotion::Enum(PxU8(core.motion[i]));
|
|
data.limits[i] = core.limits[i];
|
|
data.drives[i] = core.drives[i];
|
|
data.targetPos[i] = core.targetP[i];
|
|
data.targetVel[i] = core.targetV[i];
|
|
data.armature[i] = core.armature[i];
|
|
data.jointPos[i] = core.jointPos[i];
|
|
data.jointVel[i] = core.jointVel[i];
|
|
data.maxJointVelocity[i] = core.maxJointVelocity[i];
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static bool samePoses(const PxTransform& pose0, const PxTransform& pose1)
|
|
{
|
|
return (pose0.p == pose1.p) && (pose0.q == pose1.q);
|
|
}
|
|
|
|
// PT: this is not super efficient if you only want to change one parameter. We could consider adding individual, atomic accessors (but that would
|
|
// bloat the API) or flags to validate the desired parameters.
|
|
bool immediate::PxSetJointData(const PxArticulationLinkHandle& link, const PxArticulationJointDataRC& data)
|
|
{
|
|
PxU32 index;
|
|
immArticulation* immArt = const_cast<immArticulation*>(getFromLink(link, index));
|
|
if(!immArt)
|
|
return false;
|
|
|
|
Dy::ArticulationJointCore& core = immArt->mArticulationJointCores[index];
|
|
|
|
// PT: poses read by jcalc in ArticulationJointCore::setJointFrame. We need to set ArticulationJointCoreDirtyFlag::eFRAME for this.
|
|
{
|
|
if(!samePoses(core.parentPose, data.parentPose))
|
|
{
|
|
core.setParentPose(data.parentPose); // PT: also sets ArticulationJointCoreDirtyFlag::eFRAME
|
|
immArt->mJCalcDirty = true;
|
|
}
|
|
|
|
if(!samePoses(core.childPose, data.childPose))
|
|
{
|
|
core.setChildPose(data.childPose); // PT: also sets ArticulationJointCoreDirtyFlag::eFRAME
|
|
immArt->mJCalcDirty = true;
|
|
}
|
|
}
|
|
|
|
// PT: joint type read by jcalc in computeMotionMatrix, called from ArticulationJointCore::setJointFrame
|
|
if(core.jointType!=PxU8(data.type))
|
|
{
|
|
core.setJointType(data.type);
|
|
immArt->mJCalcDirty = true;
|
|
}
|
|
|
|
// PT: TODO: do we need to recompute jcalc for these?
|
|
core.frictionCoefficient = data.frictionCoefficient;
|
|
|
|
for(PxU32 i=0;i<PxArticulationAxis::eCOUNT;i++)
|
|
{
|
|
// PT: we don't need to recompute jcalc for these
|
|
core.limits[i] = data.limits[i];
|
|
core.drives[i] = data.drives[i];
|
|
core.maxJointVelocity[i] = data.maxJointVelocity[i];
|
|
|
|
core.jointPos[i] = data.jointPos[i];
|
|
core.jointVel[i] = data.jointVel[i];
|
|
|
|
// PT: joint motion read by jcalc in computeJointDof.
|
|
if(core.motion[i]!=data.motion[i])
|
|
{
|
|
core.setMotion(PxArticulationAxis::Enum(i), data.motion[i]);
|
|
immArt->mJCalcDirty = true;
|
|
}
|
|
|
|
// PT: targetP read by jcalc
|
|
if(core.targetP[i] != data.targetPos[i])
|
|
{
|
|
core.setTargetP(PxArticulationAxis::Enum(i), data.targetPos[i]);
|
|
immArt->mJCalcDirty = true;
|
|
}
|
|
|
|
// PT: targetV read by jcalc
|
|
if(core.targetV[i] != data.targetVel[i])
|
|
{
|
|
core.setTargetV(PxArticulationAxis::Enum(i), data.targetVel[i]);
|
|
immArt->mJCalcDirty = true;
|
|
}
|
|
|
|
|
|
if(core.armature[i] != data.armature[i])
|
|
{
|
|
core.setArmature(PxArticulationAxis::Enum(i), data.armature[i]);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void immediate::PxConstructSolverBodiesTGS(const PxRigidBodyData* inRigidData, PxTGSSolverBodyVel* outSolverBodyVel,
|
|
PxTGSSolverBodyTxInertia* outSolverBodyTxInertia, PxTGSSolverBodyData* outSolverBodyData, PxU32 nbBodies, const PxVec3& gravity,
|
|
PxReal dt, bool gyroscopicForces)
|
|
{
|
|
for (PxU32 a = 0; a<nbBodies; a++)
|
|
{
|
|
const PxRigidBodyData& rigidData = inRigidData[a];
|
|
PxVec3 lv = rigidData.linearVelocity, av = rigidData.angularVelocity;
|
|
Dy::bodyCoreComputeUnconstrainedVelocity(gravity, dt, rigidData.linearDamping, rigidData.angularDamping, 1.0f, rigidData.maxLinearVelocitySq, rigidData.maxAngularVelocitySq, lv, av, false);
|
|
|
|
Dy::copyToSolverBodyDataStep(lv, av, rigidData.invMass, rigidData.invInertia, rigidData.body2World, -rigidData.maxDepenetrationVelocity, rigidData.maxContactImpulse, PX_INVALID_NODE,
|
|
PX_MAX_F32, rigidData.maxAngularVelocitySq, 0, false, outSolverBodyVel[a], outSolverBodyTxInertia[a], outSolverBodyData[a], dt, gyroscopicForces);
|
|
}
|
|
}
|
|
|
|
void immediate::PxConstructStaticSolverBodyTGS(const PxTransform& globalPose, PxTGSSolverBodyVel& solverBodyVel, PxTGSSolverBodyTxInertia& solverBodyTxInertia, PxTGSSolverBodyData& solverBodyData)
|
|
{
|
|
const PxVec3 zero(0.0f);
|
|
Dy::copyToSolverBodyDataStep(zero, zero, 0.0f, zero, globalPose, -PX_MAX_F32, PX_MAX_F32, PX_INVALID_NODE, PX_MAX_F32, PX_MAX_F32, 0, true, solverBodyVel, solverBodyTxInertia, solverBodyData, 0.0f, false);
|
|
}
|
|
|
|
bool immediate::PxCreateContactConstraintsTGS(PxConstraintBatchHeader* batchHeaders, PxU32 nbHeaders, PxTGSSolverContactDesc* contactDescs,
|
|
PxConstraintAllocator& allocator, PxReal invDt, PxReal invTotalDt, PxReal bounceThreshold, PxReal frictionOffsetThreshold, PxReal correlationDistance)
|
|
{
|
|
PX_ASSERT(invDt > 0.0f && PxIsFinite(invDt));
|
|
PX_ASSERT(bounceThreshold < 0.0f);
|
|
PX_ASSERT(frictionOffsetThreshold > 0.0f);
|
|
PX_ASSERT(correlationDistance > 0.0f);
|
|
|
|
Dy::CorrelationBuffer cb;
|
|
|
|
PxU32 currentContactDescIdx = 0;
|
|
|
|
// PT: we call it this way in the snippet:
|
|
// PxCreateContactConstraintsTGS(..., invStepDt, invDt, ...);
|
|
// with:
|
|
// const PxReal dt = 1.f / 60.f;
|
|
// const PxReal invDt = 60.f;
|
|
// const PxReal stepDt = dt/PxReal(nbPositionIterations);
|
|
// const PxReal invStepDt = invDt * PxReal(nbPositionIterations);
|
|
//
|
|
// So this is a bit confusing here because "invDt" is not the same for the calling code and now:
|
|
// invStepDt => invDt
|
|
// invDt => invTotalDt
|
|
//
|
|
// Thus:
|
|
// bias = invTotalDt/invDt (in function) = invDt/invStepDt (calling code) = invDt/(invDt * PxReal(nbPositionIterations)) = 1/nbPositionIterations
|
|
// Which is the same as what we used for bias inside the SDK (non immediate mode)
|
|
|
|
const PxReal biasCoefficient = 2.f*PxSqrt(invTotalDt/invDt);
|
|
const PxReal totalDt = 1.f/invTotalDt;
|
|
const PxReal dt = 1.f / invDt;
|
|
|
|
for (PxU32 i = 0; i < nbHeaders; ++i)
|
|
{
|
|
Dy::SolverConstraintPrepState::Enum state = Dy::SolverConstraintPrepState::eUNBATCHABLE;
|
|
|
|
PxConstraintBatchHeader& batchHeader = batchHeaders[i];
|
|
if (batchHeader.stride == 4)
|
|
{
|
|
PxU32 totalContacts = contactDescs[currentContactDescIdx].numContacts + contactDescs[currentContactDescIdx + 1].numContacts +
|
|
contactDescs[currentContactDescIdx + 2].numContacts + contactDescs[currentContactDescIdx + 3].numContacts;
|
|
|
|
if (totalContacts <= 64)
|
|
{
|
|
state = Dy::createFinalizeSolverContacts4Step(cb,
|
|
contactDescs + currentContactDescIdx,
|
|
invDt,
|
|
totalDt,
|
|
invTotalDt,
|
|
dt,
|
|
bounceThreshold,
|
|
frictionOffsetThreshold,
|
|
correlationDistance,
|
|
biasCoefficient,
|
|
allocator);
|
|
}
|
|
}
|
|
|
|
if (state == Dy::SolverConstraintPrepState::eUNBATCHABLE)
|
|
{
|
|
for (PxU32 a = 0; a < batchHeader.stride; ++a)
|
|
{
|
|
Dy::createFinalizeSolverContactsStep(contactDescs[currentContactDescIdx + a], cb, invDt, invTotalDt, totalDt, dt, bounceThreshold,
|
|
frictionOffsetThreshold, correlationDistance, biasCoefficient, allocator);
|
|
}
|
|
}
|
|
|
|
if(contactDescs[currentContactDescIdx].desc->constraint)
|
|
{
|
|
PxU8 type = *contactDescs[currentContactDescIdx].desc->constraint;
|
|
|
|
if (type == DY_SC_TYPE_STATIC_CONTACT)
|
|
{
|
|
//Check if any block of constraints is classified as type static (single) contact constraint.
|
|
//If they are, iterate over all constraints grouped with it and switch to "dynamic" contact constraint
|
|
//type if there's a dynamic contact constraint in the group.
|
|
for (PxU32 c = 1; c < batchHeader.stride; ++c)
|
|
{
|
|
if (*contactDescs[currentContactDescIdx + c].desc->constraint == DY_SC_TYPE_RB_CONTACT)
|
|
{
|
|
type = DY_SC_TYPE_RB_CONTACT;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
batchHeader.constraintType = type;
|
|
}
|
|
|
|
currentContactDescIdx += batchHeader.stride;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool immediate::PxCreateJointConstraintsTGS(PxConstraintBatchHeader* batchHeaders, PxU32 nbHeaders,
|
|
PxTGSSolverConstraintPrepDesc* jointDescs, PxConstraintAllocator& allocator, PxReal dt, PxReal totalDt, PxReal invDt,
|
|
PxReal invTotalDt, PxReal lengthScale)
|
|
{
|
|
PX_ASSERT(dt > 0.0f);
|
|
PX_ASSERT(invDt > 0.0f && PxIsFinite(invDt));
|
|
|
|
const PxReal biasCoefficient = 2.f*PxSqrt(dt/totalDt);
|
|
|
|
PxU32 currentDescIdx = 0;
|
|
for (PxU32 i = 0; i < nbHeaders; ++i)
|
|
{
|
|
Dy::SolverConstraintPrepState::Enum state = Dy::SolverConstraintPrepState::eUNBATCHABLE;
|
|
|
|
PxConstraintBatchHeader& batchHeader = batchHeaders[i];
|
|
|
|
PxU8 type = DY_SC_TYPE_BLOCK_1D;
|
|
if (batchHeader.stride == 4)
|
|
{
|
|
PxU32 totalRows = 0;
|
|
PxU32 maxRows = 0;
|
|
bool batchable = true;
|
|
for (PxU32 a = 0; a < batchHeader.stride; ++a)
|
|
{
|
|
if (jointDescs[currentDescIdx + a].numRows == 0)
|
|
{
|
|
batchable = false;
|
|
break;
|
|
}
|
|
totalRows += jointDescs[currentDescIdx + a].numRows;
|
|
maxRows = PxMax(maxRows, jointDescs[currentDescIdx + a].numRows);
|
|
}
|
|
|
|
if (batchable)
|
|
{
|
|
state = Dy::setupSolverConstraintStep4
|
|
(jointDescs + currentDescIdx,
|
|
dt, totalDt, invDt, invTotalDt, totalRows,
|
|
allocator, maxRows, lengthScale, biasCoefficient, false);
|
|
}
|
|
}
|
|
|
|
if (state == Dy::SolverConstraintPrepState::eUNBATCHABLE)
|
|
{
|
|
type = DY_SC_TYPE_RB_1D;
|
|
for (PxU32 a = 0; a < batchHeader.stride; ++a)
|
|
{
|
|
// PT: TODO: And "isExtended" is already computed in Dy::ConstraintHelper::setupSolverConstraint
|
|
PxSolverConstraintDesc& desc = *jointDescs[currentDescIdx + a].desc;
|
|
const bool isExtended = desc.linkIndexA != PxSolverConstraintDesc::RIGID_BODY || desc.linkIndexB != PxSolverConstraintDesc::RIGID_BODY;
|
|
if (isExtended)
|
|
type = DY_SC_TYPE_EXT_1D;
|
|
|
|
Dy::setupSolverConstraintStep(jointDescs[currentDescIdx + a], allocator, dt, totalDt, invDt, invTotalDt, lengthScale, biasCoefficient);
|
|
}
|
|
}
|
|
|
|
batchHeader.constraintType = type;
|
|
currentDescIdx += batchHeader.stride;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
template<class LeafTestT, class ParamsT>
|
|
static bool PxCreateJointConstraintsWithShadersTGS_T(PxConstraintBatchHeader* batchHeaders, PxU32 nbHeaders, ParamsT* params, PxTGSSolverConstraintPrepDesc* jointDescs,
|
|
PxConstraintAllocator& allocator, PxReal dt, PxReal totalDt, PxReal invDt, PxReal invTotalDt, PxReal lengthScale)
|
|
{
|
|
Px1DConstraint allRows[Dy::MAX_CONSTRAINT_ROWS * 4];
|
|
|
|
//Runs shaders to fill in rows...
|
|
|
|
PxU32 currentDescIdx = 0;
|
|
|
|
for (PxU32 i = 0; i<nbHeaders; i++)
|
|
{
|
|
Px1DConstraint* rows = allRows;
|
|
Px1DConstraint* rows2 = allRows;
|
|
|
|
PxU32 maxRows = 0;
|
|
PxU32 nbToPrep = MAX_CONSTRAINT_ROWS;
|
|
|
|
PxConstraintBatchHeader& batchHeader = batchHeaders[i];
|
|
|
|
for (PxU32 a = 0; a<batchHeader.stride; a++)
|
|
{
|
|
PxTGSSolverConstraintPrepDesc& desc = jointDescs[currentDescIdx + a];
|
|
|
|
PxConstraintSolverPrep prep;
|
|
const void* constantBlock;
|
|
const bool useExtendedLimits = LeafTestT::getData(params, currentDescIdx + a, &prep, &constantBlock);
|
|
PX_ASSERT(prep);
|
|
|
|
PX_ASSERT(rows2 + nbToPrep <= allRows + MAX_CONSTRAINT_ROWS*4);
|
|
setupConstraintRows(rows2, nbToPrep);
|
|
rows2 += nbToPrep;
|
|
|
|
desc.invMassScales.linear0 = desc.invMassScales.linear1 = desc.invMassScales.angular0 = desc.invMassScales.angular1 = 1.0f;
|
|
desc.body0WorldOffset = PxVec3(0.0f);
|
|
|
|
//TAG:solverprepcall
|
|
const PxU32 constraintCount = prep(rows,
|
|
desc.body0WorldOffset,
|
|
Dy::MAX_CONSTRAINT_ROWS,
|
|
desc.invMassScales,
|
|
constantBlock,
|
|
desc.bodyFrame0, desc.bodyFrame1,
|
|
useExtendedLimits,
|
|
desc.cA2w, desc.cB2w);
|
|
|
|
nbToPrep = constraintCount;
|
|
maxRows = PxMax(constraintCount, maxRows);
|
|
|
|
desc.rows = rows;
|
|
desc.numRows = constraintCount;
|
|
rows += constraintCount;
|
|
}
|
|
|
|
PxCreateJointConstraintsTGS(&batchHeader, 1, jointDescs + currentDescIdx, allocator, dt, totalDt, invDt,
|
|
invTotalDt, lengthScale);
|
|
|
|
currentDescIdx += batchHeader.stride;
|
|
}
|
|
return true; //KS - TODO - do some error reporting/management...
|
|
}
|
|
|
|
bool immediate::PxCreateJointConstraintsWithShadersTGS(PxConstraintBatchHeader* batchHeaders, const PxU32 nbBatchHeaders, PxConstraint** constraints, PxTGSSolverConstraintPrepDesc* jointDescs, PxConstraintAllocator& allocator, const PxReal dt,
|
|
const PxReal totalDt, const PxReal invDt, const PxReal invTotalDt, const PxReal lengthScale)
|
|
{
|
|
return PxCreateJointConstraintsWithShadersTGS_T<PxConstraintAdapter>(batchHeaders, nbBatchHeaders, constraints, jointDescs, allocator, dt, totalDt, invDt, invTotalDt, lengthScale);
|
|
}
|
|
|
|
bool immediate::PxCreateJointConstraintsWithImmediateShadersTGS(PxConstraintBatchHeader* batchHeaders, PxU32 nbHeaders, PxImmediateConstraint* constraints, PxTGSSolverConstraintPrepDesc* jointDescs,
|
|
PxConstraintAllocator& allocator, PxReal dt, PxReal totalDt, PxReal invDt, PxReal invTotalDt, PxReal lengthScale)
|
|
{
|
|
class immConstraintAdapter
|
|
{
|
|
public:
|
|
static PX_FORCE_INLINE bool getData(PxImmediateConstraint* constraints_, PxU32 i, PxConstraintSolverPrep* prep, const void** constantBlock)
|
|
{
|
|
const PxImmediateConstraint& ic = constraints_[i];
|
|
*prep = ic.prep;
|
|
*constantBlock = ic.constantBlock;
|
|
return false;
|
|
}
|
|
};
|
|
|
|
return PxCreateJointConstraintsWithShadersTGS_T<immConstraintAdapter>(batchHeaders, nbHeaders, constraints, jointDescs, allocator, dt, totalDt, invDt, invTotalDt, lengthScale);
|
|
}
|
|
|
|
void immediate::PxSolveConstraintsTGS(const PxConstraintBatchHeader* batchHeaders, PxU32 nbBatchHeaders, const PxSolverConstraintDesc* solverConstraintDescs,
|
|
PxTGSSolverBodyVel* solverBodies, PxTGSSolverBodyTxInertia* txInertias, PxU32 nbSolverBodies, PxU32 nbPositionIterations, PxU32 nbVelocityIterations,
|
|
float dt, float invDt, PxU32 nbSolverArticulations, PxArticulationHandle* solverArticulations, PxSpatialVector* /* pxZ */, PxSpatialVector* pxDeltaV)
|
|
{
|
|
PX_ASSERT(nbPositionIterations > 0);
|
|
PX_ASSERT(nbVelocityIterations > 0);
|
|
|
|
const Dy::TGSSolveBlockMethod* solveTable = Dy::g_SolveTGSMethods;
|
|
const Dy::TGSSolveConcludeMethod* solveConcludeTable = Dy::g_SolveConcludeTGSMethods;
|
|
const Dy::TGSWriteBackMethod* writebackTable = Dy::g_WritebackTGSMethods;
|
|
|
|
Dy::SolverContext cache;
|
|
cache.solverBodyArray = NULL;
|
|
cache.mThresholdStreamIndex = 0;
|
|
cache.mThresholdStreamLength = 0xFFFFFFF;
|
|
|
|
Cm::SpatialVectorF* deltaV = reinterpret_cast<Cm::SpatialVectorF*>(pxDeltaV);
|
|
|
|
cache.deltaV = deltaV;
|
|
cache.doFriction = true;
|
|
|
|
Dy::FeatherstoneArticulation** articulations = reinterpret_cast<Dy::FeatherstoneArticulation**>(solverArticulations);
|
|
|
|
struct TGS
|
|
{
|
|
static PX_FORCE_INLINE void solveArticulationInternalConstraints(float dt_, float invDt_, PxU32 nbSolverArticulations_, Dy::FeatherstoneArticulation** solverArticulations_,
|
|
PxReal elapsedTime, bool velIter_)
|
|
{
|
|
while(nbSolverArticulations_--)
|
|
{
|
|
immArticulation* immArt = static_cast<immArticulation*>(*solverArticulations_++);
|
|
immArt->immSolveInternalConstraints(dt_, invDt_, elapsedTime, velIter_, true);
|
|
}
|
|
}
|
|
};
|
|
|
|
const PxReal invTotalDt = 1.0f/(dt*nbPositionIterations);
|
|
|
|
PxReal elapsedTime = 0.0f;
|
|
|
|
cache.isPositionIteration = true;
|
|
while(nbPositionIterations--)
|
|
{
|
|
TGS::solveArticulationInternalConstraints(dt, invDt, nbSolverArticulations, articulations, elapsedTime, false);
|
|
|
|
for(PxU32 a=0; a<nbBatchHeaders; ++a)
|
|
{
|
|
const PxConstraintBatchHeader& batch = batchHeaders[a];
|
|
if(nbPositionIterations)
|
|
solveTable[batch.constraintType](batch, solverConstraintDescs, txInertias, -PX_MAX_F32, elapsedTime, cache);
|
|
else
|
|
solveConcludeTable[batch.constraintType](batch, solverConstraintDescs, txInertias, elapsedTime, cache);
|
|
}
|
|
|
|
{
|
|
for(PxU32 j=0; j<nbSolverBodies; ++j)
|
|
Dy::integrateCoreStep(solverBodies[j], txInertias[j], dt);
|
|
|
|
for(PxU32 j=0; j<nbSolverArticulations; ++j)
|
|
{
|
|
immArticulation* immArt = static_cast<immArticulation*>(solverArticulations[j]);
|
|
immArt->recordDeltaMotion(immArt->getSolverDesc(), dt, deltaV, invTotalDt);
|
|
}
|
|
}
|
|
|
|
elapsedTime += dt;
|
|
}
|
|
|
|
for (PxU32 a=0; a<nbSolverArticulations; a++)
|
|
{
|
|
immArticulation* immArt = static_cast<immArticulation*>(articulations[a]);
|
|
immArt->saveVelocityTGS(immArt, invTotalDt);
|
|
}
|
|
|
|
cache.isPositionIteration = false;
|
|
while(nbVelocityIterations--)
|
|
{
|
|
TGS::solveArticulationInternalConstraints(dt, invDt, nbSolverArticulations, articulations, elapsedTime, true);
|
|
|
|
for(PxU32 a=0; a<nbBatchHeaders; ++a)
|
|
{
|
|
const PxConstraintBatchHeader& batch = batchHeaders[a];
|
|
solveTable[batch.constraintType](batch, solverConstraintDescs, txInertias, 0.0f, elapsedTime, cache);
|
|
if(!nbVelocityIterations)
|
|
writebackTable[batch.constraintType](batch, solverConstraintDescs, &cache);
|
|
}
|
|
}
|
|
}
|
|
|
|
void immediate::PxIntegrateSolverBodiesTGS(PxTGSSolverBodyVel* solverBody, const PxTGSSolverBodyTxInertia* txInertia, PxTransform* poses, PxU32 nbBodiesToIntegrate, PxReal /*dt*/)
|
|
{
|
|
for(PxU32 i = 0; i < nbBodiesToIntegrate; ++i)
|
|
{
|
|
solverBody[i].angularVelocity = txInertia[i].sqrtInvInertia * solverBody[i].angularVelocity;
|
|
poses[i].p = txInertia[i].body2WorldP;
|
|
poses[i].q = (txInertia[i].deltaBody2WorldQ * poses[i].q).getNormalized();
|
|
}
|
|
}
|
|
|
|
|
|
#include "PxvGlobals.h"
|
|
#include "PxPhysXGpu.h"
|
|
#include "BpBroadPhase.h"
|
|
#include "PxsHeapMemoryAllocator.h"
|
|
#include "PxsKernelWrangler.h"
|
|
#include "PxsMemoryManager.h"
|
|
|
|
PX_COMPILE_TIME_ASSERT(sizeof(Bp::FilterGroup::Enum)==sizeof(PxBpFilterGroup));
|
|
|
|
PX_IMPLEMENT_OUTPUT_ERROR
|
|
|
|
PxBpFilterGroup physx::PxGetBroadPhaseStaticFilterGroup()
|
|
{
|
|
return Bp::getFilterGroup_Statics();
|
|
}
|
|
|
|
PxBpFilterGroup physx::PxGetBroadPhaseDynamicFilterGroup(PxU32 id)
|
|
{
|
|
return Bp::getFilterGroup_Dynamics(id, false);
|
|
}
|
|
|
|
PxBpFilterGroup physx::PxGetBroadPhaseKinematicFilterGroup(PxU32 id)
|
|
{
|
|
return Bp::getFilterGroup_Dynamics(id, true);
|
|
}
|
|
|
|
namespace
|
|
{
|
|
// PT: the Bp::BroadPhase API is quite confusing and the file cannot be included from everywhere
|
|
// so let's have a user-friendly wrapper for now.
|
|
class ImmCPUBP : public PxBroadPhase, public PxBroadPhaseRegions, public PxUserAllocated
|
|
{
|
|
public:
|
|
ImmCPUBP(const PxBroadPhaseDesc& desc);
|
|
virtual ~ImmCPUBP();
|
|
|
|
virtual bool init(const PxBroadPhaseDesc& desc);
|
|
|
|
// PxBroadPhase
|
|
virtual void release() PX_OVERRIDE PX_FINAL;
|
|
virtual PxBroadPhaseType::Enum getType() const PX_OVERRIDE PX_FINAL;
|
|
virtual void getCaps(PxBroadPhaseCaps& caps) const PX_OVERRIDE PX_FINAL;
|
|
virtual PxBroadPhaseRegions* getRegions() PX_OVERRIDE PX_FINAL;
|
|
virtual PxAllocatorCallback* getAllocator() PX_OVERRIDE;
|
|
virtual PxU64 getContextID() const PX_OVERRIDE PX_FINAL;
|
|
virtual void setScratchBlock(void* scratchBlock, PxU32 size) PX_OVERRIDE PX_FINAL;
|
|
virtual void update(const PxBroadPhaseUpdateData& updateData, PxBaseTask* continuation) PX_OVERRIDE PX_FINAL;
|
|
virtual void fetchResults(PxBroadPhaseResults& results) PX_OVERRIDE PX_FINAL;
|
|
//~PxBroadPhase
|
|
|
|
// PxBroadPhaseRegions
|
|
virtual PxU32 getNbRegions() const PX_OVERRIDE PX_FINAL;
|
|
virtual PxU32 getRegions(PxBroadPhaseRegionInfo* userBuffer, PxU32 bufferSize, PxU32 startIndex) const PX_OVERRIDE PX_FINAL;
|
|
virtual PxU32 addRegion(const PxBroadPhaseRegion& region, bool populateRegion, const PxBounds3* boundsArray, const float* contactDistance) PX_OVERRIDE PX_FINAL;
|
|
virtual bool removeRegion(PxU32 handle) PX_OVERRIDE PX_FINAL;
|
|
virtual PxU32 getNbOutOfBoundsObjects() const PX_OVERRIDE PX_FINAL;
|
|
virtual const PxU32* getOutOfBoundsObjects() const PX_OVERRIDE PX_FINAL;
|
|
//~PxBroadPhaseRegions
|
|
|
|
Bp::BroadPhase* mBroadPhase;
|
|
PxcScratchAllocator mScratchAllocator;
|
|
Bp::BpFilter mFilters;
|
|
PxArray<PxBroadPhasePair> mCreatedPairs;
|
|
PxArray<PxBroadPhasePair> mDeletedPairs;
|
|
|
|
const PxU64 mContextID;
|
|
void* mAABBManager;
|
|
|
|
void releaseBP();
|
|
};
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
ImmCPUBP::ImmCPUBP(const PxBroadPhaseDesc& desc) :
|
|
mBroadPhase (NULL),
|
|
mFilters (desc.mDiscardKinematicVsKinematic, desc.mDiscardStaticVsKinematic),
|
|
mContextID (desc.mContextID),
|
|
mAABBManager(NULL)
|
|
{
|
|
}
|
|
|
|
ImmCPUBP::~ImmCPUBP()
|
|
{
|
|
releaseBP();
|
|
}
|
|
|
|
void ImmCPUBP::releaseBP()
|
|
{
|
|
PX_RELEASE(mBroadPhase);
|
|
}
|
|
|
|
bool ImmCPUBP::init(const PxBroadPhaseDesc& desc)
|
|
{
|
|
if(!desc.isValid())
|
|
return outputError<PxErrorCode::eINVALID_PARAMETER>(__LINE__, "PxCreateBroadPhase: invalid broadphase descriptor");
|
|
|
|
const PxU32 maxNbRegions = 0;
|
|
const PxU32 maxNbBroadPhaseOverlaps = 0;
|
|
const PxU32 maxNbStaticShapes = 0;
|
|
const PxU32 maxNbDynamicShapes = 0;
|
|
|
|
// PT: TODO: unify creation of CPU and GPU BPs (PX-2542)
|
|
mBroadPhase = Bp::BroadPhase::create(desc.mType, maxNbRegions, maxNbBroadPhaseOverlaps, maxNbStaticShapes, maxNbDynamicShapes, desc.mContextID);
|
|
return mBroadPhase!=NULL;
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
void ImmCPUBP::release()
|
|
{
|
|
if(mAABBManager)
|
|
{
|
|
outputError<PxErrorCode::eINVALID_OPERATION>(__LINE__, "ImmCPUBP::release: AABB manager is still present, release the AABB manager first");
|
|
return;
|
|
}
|
|
|
|
PX_DELETE_THIS;
|
|
}
|
|
|
|
PxBroadPhaseType::Enum ImmCPUBP::getType() const
|
|
{
|
|
PX_ASSERT(mBroadPhase);
|
|
return mBroadPhase->getType();
|
|
}
|
|
|
|
void ImmCPUBP::getCaps(PxBroadPhaseCaps& caps) const
|
|
{
|
|
PX_ASSERT(mBroadPhase);
|
|
mBroadPhase->getCaps(caps);
|
|
}
|
|
|
|
PxBroadPhaseRegions* ImmCPUBP::getRegions()
|
|
{
|
|
PX_ASSERT(mBroadPhase);
|
|
return mBroadPhase->getType() == PxBroadPhaseType::eMBP ? this : NULL;
|
|
}
|
|
|
|
PxAllocatorCallback* ImmCPUBP::getAllocator()
|
|
{
|
|
return PxGetAllocatorCallback();
|
|
}
|
|
|
|
PxU64 ImmCPUBP::getContextID() const
|
|
{
|
|
return mContextID;
|
|
}
|
|
|
|
void ImmCPUBP::setScratchBlock(void* scratchBlock, PxU32 size)
|
|
{
|
|
if(scratchBlock && size)
|
|
mScratchAllocator.setBlock(scratchBlock, size);
|
|
}
|
|
|
|
void ImmCPUBP::update(const PxBroadPhaseUpdateData& updateData, PxBaseTask* continuation)
|
|
{
|
|
PX_PROFILE_ZONE("ImmCPUBP::update", mContextID);
|
|
|
|
PX_ASSERT(mBroadPhase);
|
|
|
|
// PT: convert PxBroadPhaseUpdateData to Bp::BroadPhaseUpdateData. Main differences is the two undocumented bools
|
|
// added for the GPU version. For now we just set them to true, which may not be the fastest but it should always
|
|
// be correct.
|
|
// TODO: revisit this / get rid of the bools in the low-level API (PX-2835)
|
|
|
|
const Bp::BroadPhaseUpdateData defaultUpdateData(
|
|
updateData.mCreated, updateData.mNbCreated,
|
|
updateData.mUpdated, updateData.mNbUpdated,
|
|
updateData.mRemoved, updateData.mNbRemoved,
|
|
updateData.mBounds,
|
|
reinterpret_cast<const Bp::FilterGroup::Enum*>(updateData.mGroups),
|
|
updateData.mDistances, updateData.mCapacity,
|
|
mFilters,
|
|
true, true);
|
|
|
|
// PT: preBroadPhase & fetchBroadPhaseResults are only needed for the GPU BP.
|
|
// The PxBroadPhase API hides this from users and gives them an easier API that
|
|
// deals with these differences under the hood.
|
|
|
|
mBroadPhase->preBroadPhase(defaultUpdateData); // ### could be skipped for CPU BPs
|
|
|
|
// PT: BP UPDATE CALL
|
|
mBroadPhase->update(&mScratchAllocator, defaultUpdateData, continuation);
|
|
|
|
mBroadPhase->fetchBroadPhaseResults(); // ### could be skipped for CPU BPs
|
|
}
|
|
|
|
void ImmCPUBP::fetchResults(PxBroadPhaseResults& results)
|
|
{
|
|
PX_PROFILE_ZONE("ImmCPUBP::fetchResults", mContextID);
|
|
PX_ASSERT(mBroadPhase);
|
|
|
|
// PT: TODO: flags to skip the copies (PX-2929)
|
|
if(0)
|
|
{
|
|
results.mCreatedPairs = reinterpret_cast<const PxBroadPhasePair*>(mBroadPhase->getCreatedPairs(results.mNbCreatedPairs));
|
|
results.mDeletedPairs = reinterpret_cast<const PxBroadPhasePair*>(mBroadPhase->getDeletedPairs(results.mNbDeletedPairs));
|
|
}
|
|
else
|
|
{
|
|
struct Local
|
|
{
|
|
static void copyPairs(PxArray<PxBroadPhasePair>& pairs, PxU32 nbPairs, const Bp::BroadPhasePair* bpPairs)
|
|
{
|
|
pairs.resetOrClear();
|
|
const PxBroadPhasePair* src = reinterpret_cast<const PxBroadPhasePair*>(bpPairs);
|
|
PxBroadPhasePair* dst = Cm::reserveContainerMemory(pairs, nbPairs);
|
|
PxMemCopy(dst, src, sizeof(PxBroadPhasePair)*nbPairs);
|
|
}
|
|
};
|
|
|
|
{
|
|
PX_PROFILE_ZONE("copyPairs", mContextID);
|
|
{
|
|
PxU32 nbCreatedPairs;
|
|
const Bp::BroadPhasePair* createdPairs = mBroadPhase->getCreatedPairs(nbCreatedPairs);
|
|
Local::copyPairs(mCreatedPairs, nbCreatedPairs, createdPairs);
|
|
}
|
|
{
|
|
PxU32 nbDeletedPairs;
|
|
const Bp::BroadPhasePair* deletedPairs = mBroadPhase->getDeletedPairs(nbDeletedPairs);
|
|
Local::copyPairs(mDeletedPairs, nbDeletedPairs, deletedPairs);
|
|
}
|
|
}
|
|
|
|
results.mNbCreatedPairs = mCreatedPairs.size();
|
|
results.mNbDeletedPairs = mDeletedPairs.size();
|
|
results.mCreatedPairs = mCreatedPairs.begin();
|
|
results.mDeletedPairs = mDeletedPairs.begin();
|
|
}
|
|
|
|
// PT: TODO: this function got introduced in the "GRB merge" (CL 20888255) for the SAP but wasn't necessary before,
|
|
// and isn't necessary for the other BPs (even the GPU one). That makes no sense and should probably be removed.
|
|
//mBroadPhase->deletePairs();
|
|
|
|
// PT: similarly this is only needed for the SAP. This is also called at the exact same time as "deletePairs" so
|
|
// I'm not sure why we used 2 separate functions. It just bloats the API for no reason.
|
|
mBroadPhase->freeBuffers();
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
// PT: the following calls are just re-routed to the LL functions. This should only be available/needed for MBP.
|
|
|
|
PxU32 ImmCPUBP::getNbRegions() const
|
|
{
|
|
PX_ASSERT(mBroadPhase);
|
|
return mBroadPhase->getNbRegions();
|
|
}
|
|
|
|
PxU32 ImmCPUBP::getRegions(PxBroadPhaseRegionInfo* userBuffer, PxU32 bufferSize, PxU32 startIndex) const
|
|
{
|
|
PX_ASSERT(mBroadPhase);
|
|
return mBroadPhase->getRegions(userBuffer, bufferSize, startIndex);
|
|
}
|
|
|
|
PxU32 ImmCPUBP::addRegion(const PxBroadPhaseRegion& region, bool populateRegion, const PxBounds3* boundsArray, const float* contactDistance)
|
|
{
|
|
PX_ASSERT(mBroadPhase);
|
|
return mBroadPhase->addRegion(region, populateRegion, boundsArray, contactDistance);
|
|
}
|
|
|
|
bool ImmCPUBP::removeRegion(PxU32 handle)
|
|
{
|
|
PX_ASSERT(mBroadPhase);
|
|
return mBroadPhase->removeRegion(handle);
|
|
}
|
|
|
|
PxU32 ImmCPUBP::getNbOutOfBoundsObjects() const
|
|
{
|
|
PX_ASSERT(mBroadPhase);
|
|
return mBroadPhase->getNbOutOfBoundsObjects();
|
|
}
|
|
|
|
const PxU32* ImmCPUBP::getOutOfBoundsObjects() const
|
|
{
|
|
PX_ASSERT(mBroadPhase);
|
|
return mBroadPhase->getOutOfBoundsObjects();
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
#if PX_SUPPORT_GPU_PHYSX
|
|
namespace
|
|
{
|
|
class ImmGPUBP : public ImmCPUBP, public PxAllocatorCallback
|
|
{
|
|
public:
|
|
ImmGPUBP(const PxBroadPhaseDesc& desc);
|
|
virtual ~ImmGPUBP();
|
|
|
|
// PxAllocatorCallback
|
|
virtual void* allocate(size_t size, const char* /*typeName*/, const char* filename, int line) PX_OVERRIDE PX_FINAL;
|
|
virtual void deallocate(void* ptr) PX_OVERRIDE PX_FINAL;
|
|
//~PxAllocatorCallback
|
|
|
|
// PxBroadPhase
|
|
virtual PxAllocatorCallback* getAllocator() PX_OVERRIDE PX_FINAL;
|
|
//~PxBroadPhase
|
|
|
|
// ImmCPUBP
|
|
virtual bool init(const PxBroadPhaseDesc& desc) PX_OVERRIDE PX_FINAL;
|
|
//~ImmCPUBP
|
|
|
|
PxPhysXGpu* mPxGpu;
|
|
PxsMemoryManager* mMemoryManager;
|
|
PxsKernelWranglerManager* mGpuWranglerManagers;
|
|
PxsHeapMemoryAllocatorManager* mHeapMemoryAllocationManager;
|
|
};
|
|
}
|
|
#endif
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
#if PX_SUPPORT_GPU_PHYSX
|
|
ImmGPUBP::ImmGPUBP(const PxBroadPhaseDesc& desc) :
|
|
ImmCPUBP (desc),
|
|
mPxGpu (NULL),
|
|
mMemoryManager (NULL),
|
|
mGpuWranglerManagers (NULL),
|
|
mHeapMemoryAllocationManager(NULL)
|
|
{
|
|
}
|
|
|
|
ImmGPUBP::~ImmGPUBP()
|
|
{
|
|
releaseBP(); // PT: must release the BP first, before the base dtor is called
|
|
PX_DELETE(mHeapMemoryAllocationManager);
|
|
PX_DELETE(mMemoryManager);
|
|
//PX_RELEASE(mPxGpu);
|
|
PxvReleasePhysXGpu(mPxGpu);
|
|
mPxGpu = NULL;
|
|
}
|
|
|
|
void* ImmGPUBP::allocate(size_t size, const char* /*typeName*/, const char* filename, int line)
|
|
{
|
|
PX_ASSERT(mMemoryManager);
|
|
PxVirtualAllocatorCallback* cb = mMemoryManager->getHostMemoryAllocator();
|
|
return cb->allocate(size, 0, filename, line);
|
|
}
|
|
|
|
void ImmGPUBP::deallocate(void* ptr)
|
|
{
|
|
PX_ASSERT(mMemoryManager);
|
|
PxVirtualAllocatorCallback* cb = mMemoryManager->getHostMemoryAllocator();
|
|
cb->deallocate(ptr);
|
|
}
|
|
|
|
PxAllocatorCallback* ImmGPUBP::getAllocator()
|
|
{
|
|
return this;
|
|
}
|
|
|
|
bool ImmGPUBP::init(const PxBroadPhaseDesc& desc)
|
|
{
|
|
PX_ASSERT(desc.mType==PxBroadPhaseType::eGPU);
|
|
|
|
if(!desc.isValid())
|
|
return outputError<PxErrorCode::eINVALID_PARAMETER>(__LINE__, "PxCreateBroadPhase: invalid broadphase descriptor");
|
|
|
|
PxCudaContextManager* contextManager = desc.mContextManager;
|
|
|
|
// PT: one issue with PxvGetPhysXGpu is that it creates the whole PxPhysXGpu object, not just the BP. Questionable coupling there.
|
|
|
|
//mPxGpu = PxCreatePhysXGpu();
|
|
|
|
mPxGpu = PxvGetPhysXGpu(true);
|
|
if(!mPxGpu)
|
|
return false;
|
|
|
|
const PxU32 gpuComputeVersion = 0;
|
|
|
|
// PT: what's the difference between the "GPU memory manager" and the "GPU heap memory allocator manager" ?
|
|
mMemoryManager = mPxGpu->createGpuMemoryManager(contextManager);
|
|
if(!mMemoryManager)
|
|
return false;
|
|
|
|
mGpuWranglerManagers = mPxGpu->getGpuKernelWranglerManager(contextManager);
|
|
if(!mGpuWranglerManagers)
|
|
return false;
|
|
|
|
PxGpuDynamicsMemoryConfig gpuDynamicsConfig;
|
|
gpuDynamicsConfig.foundLostPairsCapacity = desc.mFoundLostPairsCapacity;
|
|
|
|
mHeapMemoryAllocationManager = mPxGpu->createGpuHeapMemoryAllocatorManager(gpuDynamicsConfig.heapCapacity, mMemoryManager, gpuComputeVersion);
|
|
if(!mHeapMemoryAllocationManager)
|
|
return false;
|
|
|
|
// PT: we currently do not expose PxGpuBroadPhaseDesc for the standalone BP,
|
|
// as the API does not expose environment IDs there either.
|
|
PxGpuBroadPhaseDesc defaultGpuBPDesc;
|
|
mBroadPhase = mPxGpu->createGpuBroadPhase(defaultGpuBPDesc, mGpuWranglerManagers, contextManager, gpuComputeVersion, gpuDynamicsConfig, mHeapMemoryAllocationManager, desc.mContextID);
|
|
return mBroadPhase!=NULL;
|
|
}
|
|
#endif
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
// PT: TODO: why don't we even have a PxBroadPhaseDesc in the main Px API by now? (PX-2933)
|
|
// The BP parameters are scattered in PxSceneDesc/PxSceneLimits/etc
|
|
// The various BP-related APIs are particularly messy.
|
|
PxBroadPhase* physx::PxCreateBroadPhase(const PxBroadPhaseDesc& desc)
|
|
{
|
|
ImmCPUBP* immBP;
|
|
if(desc.mType == PxBroadPhaseType::eGPU)
|
|
#if PX_SUPPORT_GPU_PHYSX
|
|
immBP = PX_NEW(ImmGPUBP)(desc);
|
|
#else
|
|
return NULL;
|
|
#endif
|
|
else
|
|
immBP = PX_NEW(ImmCPUBP)(desc);
|
|
|
|
if(!immBP->init(desc))
|
|
{
|
|
PX_DELETE(immBP);
|
|
return NULL;
|
|
}
|
|
return immBP;
|
|
}
|
|
|
|
|
|
|
|
|
|
namespace
|
|
{
|
|
// TODO: user-data? (PX-2934)
|
|
// TODO: aggregates? (PX-2935)
|
|
// TODO: do we really need the bitmaps in this wrapper anyway?
|
|
|
|
class HighLevelBroadPhaseAPI : public PxAABBManager, public PxUserAllocated
|
|
{
|
|
public:
|
|
HighLevelBroadPhaseAPI(PxBroadPhase& broadphase);
|
|
virtual ~HighLevelBroadPhaseAPI();
|
|
|
|
// PxAABBManager
|
|
virtual void release() PX_OVERRIDE PX_FINAL { PX_DELETE_THIS; }
|
|
|
|
virtual void addObject(PxU32 index, const PxBounds3& bounds, PxBpFilterGroup group, float distance) PX_OVERRIDE PX_FINAL;
|
|
virtual void removeObject(PxU32 index) PX_OVERRIDE PX_FINAL;
|
|
virtual void updateObject(PxU32 index, const PxBounds3* bounds, const float* distance) PX_OVERRIDE PX_FINAL;
|
|
virtual void update(PxBaseTask* continuation) PX_OVERRIDE PX_FINAL;
|
|
virtual void fetchResults(PxBroadPhaseResults& results) PX_OVERRIDE PX_FINAL;
|
|
|
|
virtual PxBroadPhase& getBroadPhase() PX_OVERRIDE PX_FINAL { return mBroadPhase; }
|
|
virtual const PxBounds3* getBounds() const PX_OVERRIDE PX_FINAL { return mBounds; }
|
|
virtual const float* getDistances() const PX_OVERRIDE PX_FINAL { return mDistances; }
|
|
virtual const PxBpFilterGroup* getGroups() const PX_OVERRIDE PX_FINAL { return mGroups; }
|
|
virtual PxU32 getCapacity() const PX_OVERRIDE PX_FINAL { return mCapacity; }
|
|
//~PxAABBManager
|
|
|
|
void reserveSpace(PxU32 nbTotalBounds);
|
|
|
|
PxBroadPhase& mBroadPhase;
|
|
PxBounds3* mBounds;
|
|
float* mDistances;
|
|
PxBpFilterGroup* mGroups;
|
|
PxU32 mCapacity; // PT: same capacity for all the above buffers
|
|
|
|
// PT: TODO: pinned? (PX-2936)
|
|
PxBitMap mAddedHandleMap; // PT: indexed by BoundsIndex
|
|
PxBitMap mRemovedHandleMap; // PT: indexed by BoundsIndex
|
|
PxBitMap mUpdatedHandleMap; // PT: indexed by BoundsIndex
|
|
|
|
// PT: TODO: pinned? (PX-2936)
|
|
PxArray<PxU32> mAddedHandles;
|
|
PxArray<PxU32> mUpdatedHandles;
|
|
PxArray<PxU32> mRemovedHandles;
|
|
|
|
const PxU64 mContextID;
|
|
};
|
|
}
|
|
|
|
HighLevelBroadPhaseAPI::HighLevelBroadPhaseAPI(PxBroadPhase& broadphase) :
|
|
mBroadPhase (broadphase),
|
|
mBounds (NULL),
|
|
mDistances (NULL),
|
|
mGroups (NULL),
|
|
mCapacity (0),
|
|
mContextID (broadphase.getContextID())
|
|
{
|
|
ImmCPUBP& baseBP = static_cast<ImmCPUBP&>(broadphase);
|
|
PX_ASSERT(!baseBP.mAABBManager);
|
|
baseBP.mAABBManager = this;
|
|
}
|
|
|
|
HighLevelBroadPhaseAPI::~HighLevelBroadPhaseAPI()
|
|
{
|
|
PxAllocatorCallback* allocator = mBroadPhase.getAllocator();
|
|
if(mDistances)
|
|
{
|
|
allocator->deallocate(mDistances);
|
|
mDistances = NULL;
|
|
}
|
|
if(mGroups)
|
|
{
|
|
allocator->deallocate(mGroups);
|
|
mGroups = NULL;
|
|
}
|
|
if(mBounds)
|
|
{
|
|
allocator->deallocate(mBounds);
|
|
mBounds = NULL;
|
|
}
|
|
|
|
ImmCPUBP& baseBP = static_cast<ImmCPUBP&>(mBroadPhase);
|
|
baseBP.mAABBManager = NULL;
|
|
}
|
|
|
|
void HighLevelBroadPhaseAPI::reserveSpace(PxU32 nbEntriesNeeded)
|
|
{
|
|
PX_PROFILE_ZONE("HighLevelBroadPhaseAPI::reserveSpace", mContextID);
|
|
PX_ASSERT(mCapacity<nbEntriesNeeded); // PT: otherwise don't call this function
|
|
|
|
// PT: allocate more than necessary to minimize the amount of reallocations
|
|
nbEntriesNeeded = PxNextPowerOfTwo(nbEntriesNeeded);
|
|
|
|
// PT: use the allocator provided by the BP, in case we need CUDA-friendly buffers
|
|
PxAllocatorCallback* allocator = mBroadPhase.getAllocator();
|
|
|
|
{
|
|
// PT: for bounds we always allocate one more entry to ensure safe SIMD loads
|
|
PxBounds3* newBounds = reinterpret_cast<PxBounds3*>(allocator->allocate(sizeof(PxBounds3)*(nbEntriesNeeded+1), "HighLevelBroadPhaseAPI::mBounds", PX_FL));
|
|
if(mCapacity && mBounds)
|
|
PxMemCopy(newBounds, mBounds, sizeof(PxBounds3)*mCapacity);
|
|
for(PxU32 i=mCapacity;i<nbEntriesNeeded;i++)
|
|
newBounds[i].setEmpty(); // PT: maybe we could skip this for perf
|
|
if(mBounds)
|
|
allocator->deallocate(mBounds);
|
|
mBounds = newBounds;
|
|
}
|
|
|
|
{
|
|
PxBpFilterGroup* newGroups = reinterpret_cast<PxBpFilterGroup*>(allocator->allocate(sizeof(PxBpFilterGroup)*nbEntriesNeeded, "HighLevelBroadPhaseAPI::mGroups", PX_FL));
|
|
if(mCapacity && mGroups)
|
|
PxMemCopy(newGroups, mGroups, sizeof(PxBpFilterGroup)*mCapacity);
|
|
for(PxU32 i=mCapacity;i<nbEntriesNeeded;i++)
|
|
newGroups[i] = PX_INVALID_BP_FILTER_GROUP; // PT: maybe we could skip this for perf
|
|
if(mGroups)
|
|
allocator->deallocate(mGroups);
|
|
mGroups = newGroups;
|
|
}
|
|
|
|
{
|
|
float* newDistances = reinterpret_cast<float*>(allocator->allocate(sizeof(float)*nbEntriesNeeded, "HighLevelBroadPhaseAPI::mDistances", PX_FL));
|
|
if(mCapacity && mDistances)
|
|
PxMemCopy(newDistances, mDistances, sizeof(float)*mCapacity);
|
|
for(PxU32 i=mCapacity;i<nbEntriesNeeded;i++)
|
|
newDistances[i] = 0.0f; // PT: maybe we could skip this for perf
|
|
if(mDistances)
|
|
allocator->deallocate(mDistances);
|
|
mDistances = newDistances;
|
|
}
|
|
|
|
mAddedHandleMap.resize(nbEntriesNeeded);
|
|
mRemovedHandleMap.resize(nbEntriesNeeded);
|
|
|
|
mCapacity = nbEntriesNeeded;
|
|
}
|
|
|
|
// PT: TODO: version with internal index management? (PX-2942)
|
|
|
|
// PT: TODO: batched version?
|
|
void HighLevelBroadPhaseAPI::addObject(PxBpIndex index, const PxBounds3& bounds, PxBpFilterGroup group, float distance)
|
|
{
|
|
PX_ASSERT(group != PX_INVALID_BP_FILTER_GROUP); // PT: we use group == PX_INVALID_BP_FILTER_GROUP to mark removed/invalid entries
|
|
|
|
const PxU32 nbEntriesNeeded = index + 1;
|
|
if(mCapacity<nbEntriesNeeded)
|
|
reserveSpace(nbEntriesNeeded);
|
|
|
|
mBounds[index] = bounds;
|
|
mGroups[index] = group;
|
|
mDistances[index] = distance;
|
|
|
|
if(mRemovedHandleMap.test(index))
|
|
mRemovedHandleMap.reset(index);
|
|
else // PT: for case where an object in the BP gets removed and then we re-add same frame (we don't want to set the add bit in this case)
|
|
mAddedHandleMap.set(index);
|
|
}
|
|
|
|
// PT: TODO: batched version?
|
|
void HighLevelBroadPhaseAPI::removeObject(PxBpIndex index)
|
|
{
|
|
PX_ASSERT(index < mCapacity);
|
|
PX_ASSERT(mGroups[index] != PX_INVALID_BP_FILTER_GROUP);
|
|
|
|
if(mAddedHandleMap.test(index)) // PT: if object had been added this frame...
|
|
mAddedHandleMap.reset(index); // PT: ...then simply revert the previous operation locally (it hasn't been passed to the BP yet).
|
|
else
|
|
mRemovedHandleMap.set(index); // PT: else we need to remove it from the BP
|
|
|
|
mBounds[index].setEmpty();
|
|
mGroups[index] = PX_INVALID_BP_FILTER_GROUP;
|
|
mDistances[index] = 0.0f;
|
|
}
|
|
|
|
// PT: TODO: batched version?
|
|
void HighLevelBroadPhaseAPI::updateObject(PxBpIndex index, const PxBounds3* bounds, const float* distance)
|
|
{
|
|
PX_ASSERT(index < mCapacity);
|
|
|
|
mUpdatedHandleMap.growAndSet(index);
|
|
|
|
if(bounds)
|
|
mBounds[index] = *bounds;
|
|
if(distance)
|
|
mDistances[index] = *distance;
|
|
}
|
|
|
|
namespace
|
|
{
|
|
struct HandleTest_Add
|
|
{
|
|
static PX_FORCE_INLINE void processEntry(HighLevelBroadPhaseAPI& bp, PxU32 handle)
|
|
{
|
|
PX_ASSERT(bp.mGroups[handle] != PX_INVALID_BP_FILTER_GROUP);
|
|
bp.mAddedHandles.pushBack(handle);
|
|
}
|
|
};
|
|
|
|
struct HandleTest_Update
|
|
{
|
|
static PX_FORCE_INLINE void processEntry(HighLevelBroadPhaseAPI& bp, PxU32 handle)
|
|
{
|
|
// PT: TODO: revisit the logic here (PX-2937)
|
|
|
|
PX_ASSERT(!bp.mRemovedHandleMap.test(handle)); // a handle may only be updated and deleted if it was just added.
|
|
|
|
if(bp.mAddedHandleMap.test(handle)) // just-inserted handles may also be marked updated, so skip them
|
|
return;
|
|
|
|
PX_ASSERT(bp.mGroups[handle] != PX_INVALID_BP_FILTER_GROUP);
|
|
|
|
bp.mUpdatedHandles.pushBack(handle);
|
|
}
|
|
};
|
|
|
|
struct HandleTest_Remove
|
|
{
|
|
static PX_FORCE_INLINE void processEntry(HighLevelBroadPhaseAPI& bp, PxU32 handle)
|
|
{
|
|
PX_ASSERT(bp.mGroups[handle] == PX_INVALID_BP_FILTER_GROUP);
|
|
bp.mRemovedHandles.pushBack(handle);
|
|
}
|
|
};
|
|
}
|
|
|
|
template<class FunctionT, class ParamsT>
|
|
static void iterateBitmap(const PxBitMap& bitmap, ParamsT& params)
|
|
{
|
|
const PxU32* bits = bitmap.getWords();
|
|
if(bits)
|
|
{
|
|
const PxU32 lastSetBit = bitmap.findLast();
|
|
for(PxU32 w = 0; w <= lastSetBit >> 5; ++w)
|
|
{
|
|
for(PxU32 b = bits[w]; b; b &= b-1)
|
|
{
|
|
const PxU32 index = PxU32(w<<5|PxLowestSetBit(b));
|
|
|
|
FunctionT::processEntry(params, index);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*static void shuffle(PxArray<PxU32>& handles)
|
|
{
|
|
PxU32 nb = handles.size();
|
|
PxU32* data = handles.begin();
|
|
for(PxU32 i=0;i<nb*10;i++)
|
|
{
|
|
PxU32 id0 = rand() % nb;
|
|
PxU32 id1 = rand() % nb;
|
|
PxSwap(data[id0], data[id1]);
|
|
}
|
|
}*/
|
|
|
|
void HighLevelBroadPhaseAPI::update(PxBaseTask* continuation)
|
|
{
|
|
PX_PROFILE_ZONE("HighLevelBroadPhaseAPI::update", mContextID);
|
|
|
|
{
|
|
PX_PROFILE_ZONE("resetOrClear", mContextID);
|
|
mAddedHandles.resetOrClear();
|
|
mUpdatedHandles.resetOrClear();
|
|
mRemovedHandles.resetOrClear();
|
|
}
|
|
|
|
{
|
|
{
|
|
PX_PROFILE_ZONE("iterateBitmap added", mContextID);
|
|
iterateBitmap<HandleTest_Add>(mAddedHandleMap, *this);
|
|
}
|
|
{
|
|
PX_PROFILE_ZONE("iterateBitmap updated", mContextID);
|
|
iterateBitmap<HandleTest_Update>(mUpdatedHandleMap, *this);
|
|
}
|
|
{
|
|
PX_PROFILE_ZONE("iterateBitmap removed", mContextID);
|
|
iterateBitmap<HandleTest_Remove>(mRemovedHandleMap, *this);
|
|
}
|
|
}
|
|
|
|
// PT: call the low-level BP API
|
|
{
|
|
PX_PROFILE_ZONE("BP update", mContextID);
|
|
/* if(1) // Suffle test
|
|
{
|
|
shuffle(mAddedHandles);
|
|
shuffle(mUpdatedHandles);
|
|
shuffle(mRemovedHandles);
|
|
}*/
|
|
|
|
const PxBroadPhaseUpdateData updateData(
|
|
mAddedHandles.begin(), mAddedHandles.size(),
|
|
mUpdatedHandles.begin(), mUpdatedHandles.size(),
|
|
mRemovedHandles.begin(), mRemovedHandles.size(),
|
|
mBounds, mGroups, mDistances,
|
|
mCapacity);
|
|
|
|
mBroadPhase.update(updateData, continuation);
|
|
}
|
|
|
|
{
|
|
PX_PROFILE_ZONE("clear bitmaps", mContextID);
|
|
mAddedHandleMap.clear();
|
|
mRemovedHandleMap.clear();
|
|
mUpdatedHandleMap.clear();
|
|
}
|
|
}
|
|
|
|
void HighLevelBroadPhaseAPI::fetchResults(PxBroadPhaseResults& results)
|
|
{
|
|
PX_PROFILE_ZONE("HighLevelBroadPhaseAPI::fetchResults", mContextID);
|
|
mBroadPhase.fetchResults(results);
|
|
}
|
|
|
|
PxAABBManager* physx::PxCreateAABBManager(PxBroadPhase& bp)
|
|
{
|
|
// PT: make sure we cannot link a bp to multiple managers
|
|
ImmCPUBP& baseBP = static_cast<ImmCPUBP&>(bp);
|
|
if(baseBP.mAABBManager)
|
|
return NULL;
|
|
|
|
return PX_NEW(HighLevelBroadPhaseAPI)(bp);
|
|
}
|