void concludeContact(const PxcSolverConstraintDesc& desc, PxcSolverContext& cache)
{
PxU8* cPtr = desc.constraint;
while(cPtr < desc.constraint + getConstraintLength(desc))
{
const PxcSolverContactHeader* PX_RESTRICT hdr = (PxcSolverContactHeader*)cPtr;
cPtr += sizeof(PxcSolverContactHeader);
PxcSolverContactPoint* PX_RESTRICT contacts = (PxcSolverContactPoint*)cPtr;
cPtr += hdr->numNormalConstr * sizeof(PxcSolverContactPoint);
PxcSolverContactFriction* PX_RESTRICT frictions = (PxcSolverContactFriction*)cPtr;
cPtr += hdr->numFrictionConstr * sizeof(PxcSolverContactFriction);
for(PxU32 i=0; i<hdr->numNormalConstr; i++)
//contacts[i].scaledBias = PxMax(contacts[i].scaledBias, 0.0f);
//V3WriteX(contacts[i].scaledBiasX_targetVelocityY_restitutionZ, PxMax(V3ReadX(contacts[i].scaledBiasX_targetVelocityY_restitutionZ), 0.0f));
contacts[i].setScaledBias(PxMax(V3ReadX(
contacts[i].getScaledBias()), 0.0f));
for(PxU32 i=0; i<hdr->numFrictionConstr; i++)
//frictions[i].bias = 0.0f;
//V4WriteW(frictions[i].rbXnXYZ_biasW, 0.0f);
frictions[i].setBias(0.0f);
}
PX_ASSERT(cPtr == desc.constraint + getConstraintLength(desc));
}
void writeBackContact(const PxcSolverConstraintDesc& desc, PxcSolverContext& cache)
{
PxcSolverBody* b0 = desc.bodyA, *b1 = desc.bodyB;
PxReal normalForce = 0;
PxU8* cPtr = desc.constraint;
PxReal* vForceWriteback = reinterpret_cast<PxReal*>(desc.writeBack);
while(cPtr < desc.constraint + getConstraintLength(desc))
{
const PxcSolverContactHeader* PX_RESTRICT hdr = (PxcSolverContactHeader*)cPtr;
cPtr += sizeof(PxcSolverContactHeader);
PxcSolverContactPoint* PX_RESTRICT contacts = (PxcSolverContactPoint*)cPtr;
cPtr += hdr->numNormalConstr * sizeof(PxcSolverContactPoint);
PxcSolverContactFriction* PX_RESTRICT frictions = (PxcSolverContactFriction*)cPtr;
cPtr += hdr->numFrictionConstr * sizeof(PxcSolverContactFriction);
if(vForceWriteback!=NULL)
{
for(PxU32 i=0; i<hdr->numNormalConstr; i++)
{
//*vForceWriteback++ = contacts[i].appliedVForce;
*vForceWriteback++ = V4ReadW(contacts[i].rbXnXYZ_appliedVForceW);
//normalForce += contacts[i].appliedVForce;
normalForce += V4ReadW(contacts[i].rbXnXYZ_appliedVForceW);
}
}
for(PxU32 i=0; i<hdr->numFrictionConstr; i++)
{
if((frictions[i].frictionBrokenWritebackByte != NULL) && frictions[i].broken)
*(frictions[i].frictionBrokenWritebackByte) = 1;
}
}
PX_ASSERT(cPtr == desc.constraint + desc.constraintLength);
if(desc.linkIndexA == PxcSolverConstraintDesc::NO_LINK && desc.linkIndexB == PxcSolverConstraintDesc::NO_LINK &&
normalForce !=0 && (b0->reportThreshold < PX_MAX_REAL || b1->reportThreshold < PX_MAX_REAL))
{
PxcThresholdStreamElement elt;
elt.normalForce = normalForce;
elt.threshold = PxMin<float>(b0->reportThreshold, b1->reportThreshold);
elt.body0 = b0->originalBody;
elt.body1 = b1->originalBody;
Ps::order(elt.body0,elt.body1);
PX_ASSERT(cache.mThresholdStreamIndex<cache.mThresholdStreamLength);
cache.mThresholdStream[cache.mThresholdStreamIndex++] = elt;
}
}
void solveFriction_BStatic(const PxcSolverConstraintDesc& desc, PxcSolverContext& /*cache*/)
{
PxcSolverBody& b0 = *desc.bodyA;
Vec3V linVel0 = V3LoadA(b0.linearVelocity);
Vec3V angVel0 = V3LoadA(b0.angularVelocity);
const PxU8* PX_RESTRICT currPtr = desc.constraint;
const PxU8* PX_RESTRICT last = currPtr + getConstraintLength(desc);
//hopefully pointer aliasing doesn't bite.
//PxVec3 l0, a0;
//PxVec3_From_Vec3V(linVel0, l0);
//PxVec3_From_Vec3V(angVel0, a0);
//PX_ASSERT(l0.isFinite());
//PX_ASSERT(a0.isFinite());
while(currPtr < last)
{
const PxcSolverFrictionHeader* PX_RESTRICT frictionHeader = (PxcSolverFrictionHeader*)currPtr;
const PxU32 numFrictionConstr = frictionHeader->numFrictionConstr;
currPtr +=sizeof(PxcSolverFrictionHeader);
PxF32* appliedImpulse = (PxF32*)currPtr;
currPtr +=frictionHeader->getAppliedForcePaddingSize();
PxcSolverFriction* PX_RESTRICT frictions = (PxcSolverFriction*)currPtr;
currPtr += numFrictionConstr * sizeof(PxcSolverFriction);
const FloatV staticFriction = frictionHeader->getStaticFriction();
for(PxU32 i=0;i<numFrictionConstr;i++)
{
PxcSolverFriction& f = frictions[i];
Ps::prefetchLine(&frictions[i+1]);
const Vec3V t0 = Vec3V_From_Vec4V(f.normalXYZ_appliedForceW);
const Vec3V raXt0 = Vec3V_From_Vec4V(f.raXnXYZ_velMultiplierW);
const FloatV appliedForce = V4GetW(f.normalXYZ_appliedForceW);
const FloatV velMultiplier = V4GetW(f.raXnXYZ_velMultiplierW);
const FloatV targetVel = V4GetW(f.rbXnXYZ_targetVelocityW);
//const FloatV normalImpulse = contacts[f.contactIndex].getAppliedForce();
const FloatV normalImpulse = FLoad(appliedImpulse[f.contactIndex]);
const FloatV maxFriction = FMul(staticFriction, normalImpulse);
const FloatV nMaxFriction = FNeg(maxFriction);
//Compute the normal velocity of the constraint.
const FloatV t0Vel1 = V3Dot(t0, linVel0);
const FloatV t0Vel2 = V3Dot(raXt0, angVel0);
//const FloatV unbiasedErr = FMul(targetVel, velMultiplier);
//const FloatV biasedErr = FMulAdd(targetVel, velMultiplier, nScaledBias);
const FloatV t0Vel = FAdd(t0Vel1, t0Vel2);
const Vec3V delAngVel0 = Vec3V_From_Vec4V(f.delAngVel0_InvMassADom);
const Vec3V delLinVel0 = V3Scale(t0, V4GetW(f.delAngVel0_InvMassADom));
// still lots to do here: using loop pipelining we can interweave this code with the
// above - the code here has a lot of stalls that we would thereby eliminate
//FloatV deltaF = FSub(scaledBias, FMul(t0Vel, velMultiplier));//FNeg(FMul(t0Vel, velMultiplier));
//FloatV deltaF = FMul(t0Vel, velMultiplier);
//FloatV newForce = FMulAdd(t0Vel, velMultiplier, appliedForce);
const FloatV tmp = FNegMulSub(targetVel,velMultiplier,appliedForce);
FloatV newForce = FMulAdd(t0Vel, velMultiplier, tmp);
newForce = FClamp(newForce, nMaxFriction, maxFriction);
const FloatV deltaF = FSub(newForce, appliedForce);
linVel0 = V3ScaleAdd(delLinVel0, deltaF, linVel0);
angVel0 = V3ScaleAdd(delAngVel0, deltaF, angVel0);
f.setAppliedForce(newForce);
}
}
//PxVec3_From_Vec3V(linVel0, l0);
//PxVec3_From_Vec3V(angVel0, a0);
//PX_ASSERT(l0.isFinite());
//PX_ASSERT(a0.isFinite());
// Write back
V3StoreU(linVel0, b0.linearVelocity);
V3StoreU(angVel0, b0.angularVelocity);
PX_ASSERT(currPtr == last);
}
void solveFriction_BStatic(const PxSolverConstraintDesc& desc, SolverContext& /*cache*/)
{
PxSolverBody& b0 = *desc.bodyA;
Vec3V linVel0 = V3LoadA(b0.linearVelocity);
Vec3V angState0 = V3LoadA(b0.angularState);
PxU8* PX_RESTRICT currPtr = desc.constraint;
const PxU8* PX_RESTRICT last = currPtr + getConstraintLength(desc);
while(currPtr < last)
{
const SolverFrictionHeader* PX_RESTRICT frictionHeader = reinterpret_cast<SolverFrictionHeader*>(currPtr);
const PxU32 numFrictionConstr = frictionHeader->numFrictionConstr;
const PxU32 numNormalConstr = frictionHeader->numNormalConstr;
const PxU32 numFrictionPerPoint = numFrictionConstr/numNormalConstr;
currPtr +=sizeof(SolverFrictionHeader);
PxF32* appliedImpulse = reinterpret_cast<PxF32*>(currPtr);
currPtr +=frictionHeader->getAppliedForcePaddingSize();
SolverContactFriction* PX_RESTRICT frictions = reinterpret_cast<SolverContactFriction*>(currPtr);
currPtr += numFrictionConstr * sizeof(SolverContactFriction);
const FloatV invMass0 = FLoad(frictionHeader->invMass0D0);
const FloatV angD0 = FLoad(frictionHeader->angDom0);
//const FloatV angD1 = FLoad(frictionHeader->angDom1);
const FloatV staticFriction = frictionHeader->getStaticFriction();
for(PxU32 i=0, j = 0;i<numFrictionConstr;j++)
{
for(PxU32 p = 0; p < numFrictionPerPoint; p++, i++)
{
SolverContactFriction& f = frictions[i];
Ps::prefetchLine(&frictions[i+1]);
const Vec3V t0 = Vec3V_From_Vec4V(f.normalXYZ_appliedForceW);
const Vec3V raXt0 = Vec3V_From_Vec4V(f.raXnXYZ_velMultiplierW);
const FloatV appliedForce = V4GetW(f.normalXYZ_appliedForceW);
const FloatV velMultiplier = V4GetW(f.raXnXYZ_velMultiplierW);
const FloatV targetVel = FLoad(f.targetVel);
//const FloatV normalImpulse = contacts[f.contactIndex].getAppliedForce();
const FloatV normalImpulse = FLoad(appliedImpulse[j]);
const FloatV maxFriction = FMul(staticFriction, normalImpulse);
const FloatV nMaxFriction = FNeg(maxFriction);
//Compute the normal velocity of the constraint.
const FloatV t0Vel1 = V3Dot(t0, linVel0);
const FloatV t0Vel2 = V3Dot(raXt0, angState0);
const FloatV t0Vel = FAdd(t0Vel1, t0Vel2);
const Vec3V delangState0 = V3Scale(raXt0, angD0);
const Vec3V delLinVel0 = V3Scale(t0, invMass0);
// still lots to do here: using loop pipelining we can interweave this code with the
// above - the code here has a lot of stalls that we would thereby eliminate
const FloatV tmp = FNegScaleSub(targetVel,velMultiplier,appliedForce);
FloatV newForce = FScaleAdd(t0Vel, velMultiplier, tmp);
newForce = FClamp(newForce, nMaxFriction, maxFriction);
const FloatV deltaF = FSub(newForce, appliedForce);
linVel0 = V3ScaleAdd(delLinVel0, deltaF, linVel0);
angState0 = V3ScaleAdd(delangState0, deltaF, angState0);
f.setAppliedForce(newForce);
}
}
}
// Write back
V3StoreA(linVel0, b0.linearVelocity);
V3StoreA(angState0, b0.angularState);
PX_ASSERT(currPtr == last);
}
