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); }