void writeBackContactCoulomb(const PxcSolverConstraintDesc& desc, PxcSolverContext& cache,
PxcSolverBodyData& bd0, PxcSolverBodyData& bd1)
{
PxReal normalForce = 0;
FloatV normalForceV = FZero();
PxU8* PX_RESTRICT cPtr = desc.constraint;
PxReal* PX_RESTRICT vForceWriteback = reinterpret_cast<PxReal*>(desc.writeBack);
const PxcSolverContactCoulombHeader* PX_RESTRICT firstHeader = (const PxcSolverContactCoulombHeader*)cPtr;
PxU8* PX_RESTRICT last = desc.constraint + firstHeader->frictionOffset;
const PxU32 pointStride = firstHeader->type == PXS_SC_TYPE_EXT_CONTACT ? sizeof(PxcSolverContactExt)
: sizeof(PxcSolverContact);
bool hasForceThresholds = false;
while(cPtr < last)
{
const PxcSolverContactCoulombHeader* PX_RESTRICT hdr = (const PxcSolverContactCoulombHeader*)cPtr;
cPtr += sizeof(PxcSolverContactCoulombHeader);
hasForceThresholds = hdr->flags & PxcSolverContactHeader::eHAS_FORCE_THRESHOLDS;
const PxU32 numNormalConstr = hdr->numNormalConstr;
Ps::prefetchLine(cPtr, 256);
Ps::prefetchLine(cPtr, 384);
if(vForceWriteback!=NULL)
{
for(PxU32 i=0; i<numNormalConstr; i++)
{
PxcSolverContact* c = reinterpret_cast<PxcSolverContact*>(cPtr);
cPtr += pointStride;
const FloatV appliedForce = c->getAppliedForce();
FStore(appliedForce, vForceWriteback);
vForceWriteback++;
normalForceV = FAdd(normalForceV, appliedForce);
}
}
else
cPtr += numNormalConstr * pointStride;
}
PX_ASSERT(cPtr == last);
if(hasForceThresholds && desc.linkIndexA == PxcSolverConstraintDesc::NO_LINK && desc.linkIndexB == PxcSolverConstraintDesc::NO_LINK &&
normalForce !=0 && (bd0.reportThreshold < PX_MAX_REAL || bd1.reportThreshold < PX_MAX_REAL))
{
PxcThresholdStreamElement elt;
FStore(normalForceV, &elt.normalForce);
elt.threshold = PxMin<float>(bd0.reportThreshold, bd1.reportThreshold);
elt.body0 = bd0.originalBody;
elt.body1 = bd1.originalBody;
Ps::order(elt.body0,elt.body1);
PX_ASSERT(elt.body0 < elt.body1);
PX_ASSERT(cache.mThresholdStreamIndex<cache.mThresholdStreamLength);
cache.mThresholdStream[cache.mThresholdStreamIndex++] = elt;
}
}
PxReal SolverExtBody::projectVelocity(const PxVec3& linear, const PxVec3& angular) const
{
if(mLinkIndex == PxSolverConstraintDesc::NO_LINK)
{
return mBodyData->projectVelocity(linear, angular);
}
else
{
PxF32 f;
FStore(getVelocity(*mFsData)[mLinkIndex].dot(Cm::SpatialVector(linear, angular)), &f);
return f;
}
}
