bool BigInt::FMulPow5(long c5) { AssertBi(this); Assert(c5 >= 0); const ulong k5to13 = 1220703125; long clu = (c5 + 12) / 13; ulong luT; if (0 == m_clu || 0 == c5) return true; if (m_clu + clu > m_cluMax && !FResize(m_clu + clu)) return false; for (; c5 >= 13; c5 -= 13) AssertVerify(FMulAdd(k5to13, 0)); if (c5 > 0) { for (luT = 5; --c5 > 0; ) luT *= 5; AssertVerify(FMulAdd(luT, 0)); } AssertBi(this); return true; }
bool BigInt::FInitFromDigits(const EncodedChar *prgch, long cch, long *pcchDig) { AssertBi(this); Assert(cch >= 0); Assert(prgch != 0); Assert(pcchDig != 0); ulong luAdd; ulong luMul; long clu = (cch + 8) / 9; const EncodedChar *pchLim = prgch + cch; if (clu > m_cluMax && !FResize(clu)) return false; m_clu = 0; luAdd = 0; luMul = 1; for (*pcchDig = cch; prgch < pchLim; prgch++) { if (*prgch == '.') { (*pcchDig)--; continue; } Assert(NumberUtilities::IsDigit(*prgch)); if (luMul == 1000000000) { AssertVerify(FMulAdd(luMul, luAdd)); luMul = 1; luAdd = 0; } luMul *= 10; luAdd = luAdd * 10 + *prgch - '0'; } Assert(1 < luMul); AssertVerify(FMulAdd(luMul, luAdd)); AssertBi(this); return true; }
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 solveContactCoulomb_BStatic(const PxcSolverConstraintDesc& desc, PxcSolverContext& /*cache*/) { PxcSolverBody& b0 = *desc.bodyA; Vec3V linVel0 = V3LoadA(b0.linearVelocity); Vec3V angVel0 = V3LoadA(b0.angularVelocity); PxcSolverContactCoulombHeader* firstHeader = (PxcSolverContactCoulombHeader*)desc.constraint; const PxU8* PX_RESTRICT last = desc.constraint + firstHeader->frictionOffset;//getConstraintLength(desc); //hopefully pointer aliasing doesn't bite. const PxU8* PX_RESTRICT currPtr = desc.constraint; const FloatV zero = FZero(); while(currPtr < last) { PxcSolverContactCoulombHeader* PX_RESTRICT hdr = (PxcSolverContactCoulombHeader*)currPtr; currPtr += sizeof(PxcSolverContactCoulombHeader); const PxU32 numNormalConstr = hdr->numNormalConstr; PxcSolverContact* PX_RESTRICT contacts = (PxcSolverContact*)currPtr; Ps::prefetchLine(contacts); currPtr += numNormalConstr * sizeof(PxcSolverContact); PxF32* appliedImpulse = (PxF32*) (((PxU8*)hdr) + hdr->frictionOffset + sizeof(PxcSolverFrictionHeader)); Ps::prefetchLine(appliedImpulse); const Vec3V normal = hdr->getNormal(); const FloatV invMassDom0 = FLoad(hdr->dominance0); FloatV normalVel1 = V3Dot(normal, linVel0); const Vec3V delLinVel0 = V3Scale(normal, invMassDom0); FloatV accumDeltaF = zero; //FloatV accumImpulse = zero; for(PxU32 i=0;i<numNormalConstr;i++) { PxcSolverContact& c = contacts[i]; Ps::prefetchLine(&contacts[i+1]); //const Vec4V normalXYZ_velMultiplierW = c.normalXYZ_velMultiplierW; const Vec4V raXnXYZ_appliedForceW = c.raXnXYZ_appliedForceW; const Vec4V rbXnXYZ_velMultiplierW = c.rbXnXYZ_velMultiplierW; //const Vec3V normal = c.normal; //const Vec3V normal = Vec3V_From_Vec4V(normalXYZ_velMultiplierW); const Vec3V raXn = Vec3V_From_Vec4V(raXnXYZ_appliedForceW); const FloatV appliedForce = V4GetW(raXnXYZ_appliedForceW); const FloatV velMultiplier = V4GetW(rbXnXYZ_velMultiplierW); //const FloatV velMultiplier = V4GetW(normalXYZ_velMultiplierW); const Vec3V delAngVel0 = Vec3V_From_Vec4V(c.delAngVel0_InvMassADom); const FloatV targetVel = c.getTargetVelocity(); const FloatV nScaledBias = FNeg(c.getScaledBias()); const FloatV maxImpulse = c.getMaxImpulse(); //Compute the normal velocity of the constraint. //const FloatV normalVel1 = V3Dot(normal, linVel0); const FloatV normalVel2 = V3Dot(raXn, angVel0); const FloatV normalVel = FAdd(normalVel1, normalVel2); //const FloatV unbiasedErr = FMul(targetVel, velMultiplier); const FloatV biasedErr = FMulAdd(targetVel, velMultiplier, nScaledBias); // 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 _deltaF = FMax(FNegMulSub(normalVel, velMultiplier, biasedErr), FNeg(appliedForce)); const FloatV _newForce = FAdd(appliedForce, _deltaF); const FloatV newForce = FMin(_newForce, maxImpulse); const FloatV deltaF = FSub(newForce, appliedForce); //linVel0 = V3MulAdd(delLinVel0, deltaF, linVel0); normalVel1 = FScaleAdd(invMassDom0, deltaF, normalVel1); angVel0 = V3ScaleAdd(delAngVel0, deltaF, angVel0); accumDeltaF = FAdd(accumDeltaF, deltaF); c.setAppliedForce(newForce); Ps::aos::FStore(newForce, &appliedImpulse[i]); Ps::prefetchLine(&appliedImpulse[i], 128); //accumImpulse = FAdd(accumImpulse, newAppliedForce); } linVel0 = V3ScaleAdd(delLinVel0, accumDeltaF, linVel0); //hdr->setAccumlatedForce(accumImpulse); } // Write back V3StoreU(linVel0, b0.linearVelocity); V3StoreU(angVel0, b0.angularVelocity); PX_ASSERT(currPtr == last); }