float GetSpeed(FVector projdirection, FVector PawnVelocity)
{
FVector projvelocity = Scale(Normal(projdirection), 3920.0);
projvelocity.Z += 0;
float ForwardPct = FMin(Dot(Normal(PawnVelocity), Normal(projdirection)), 0.5);
float InheritPct = FMax(0.5, ForwardPct);
projvelocity.X += InheritPct * PawnVelocity.X;
projvelocity.Y += InheritPct * PawnVelocity.Y;
projvelocity.Z += 0.5 * PawnVelocity.Z;
return VSize(projvelocity);
}
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);
}
bool pcmContactCapsuleConvex(GU_CONTACT_METHOD_ARGS)
{
PX_UNUSED(renderOutput);
const PxConvexMeshGeometryLL& shapeConvex = shape1.get<const PxConvexMeshGeometryLL>();
const PxCapsuleGeometry& shapeCapsule = shape0.get<const PxCapsuleGeometry>();
PersistentContactManifold& manifold = cache.getManifold();
Ps::prefetchLine(shapeConvex.hullData);
PX_ASSERT(transform1.q.isSane());
PX_ASSERT(transform0.q.isSane());
const Vec3V zeroV = V3Zero();
const Vec3V vScale = V3LoadU_SafeReadW(shapeConvex.scale.scale); // PT: safe because 'rotation' follows 'scale' in PxMeshScale
const FloatV contactDist = FLoad(params.mContactDistance);
const FloatV capsuleHalfHeight = FLoad(shapeCapsule.halfHeight);
const FloatV capsuleRadius = FLoad(shapeCapsule.radius);
const ConvexHullData* hullData =shapeConvex.hullData;
//Transfer A into the local space of B
const PsTransformV transf0 = loadTransformA(transform0);
const PsTransformV transf1 = loadTransformA(transform1);
const PsTransformV curRTrans(transf1.transformInv(transf0));
const PsMatTransformV aToB(curRTrans);
const FloatV convexMargin = Gu::CalculatePCMConvexMargin(hullData, vScale);
const FloatV capsuleMinMargin = Gu::CalculateCapsuleMinMargin(capsuleRadius);
const FloatV minMargin = FMin(convexMargin, capsuleMinMargin);
const PxU32 initialContacts = manifold.mNumContacts;
const FloatV projectBreakingThreshold = FMul(minMargin, FLoad(1.25f));
const FloatV refreshDist = FAdd(contactDist, capsuleRadius);
manifold.refreshContactPoints(aToB, projectBreakingThreshold, refreshDist);
//ML: after refreshContactPoints, we might lose some contacts
const bool bLostContacts = (manifold.mNumContacts != initialContacts);
GjkStatus status = manifold.mNumContacts > 0 ? GJK_UNDEFINED : GJK_NON_INTERSECT;
Vec3V closestA(zeroV), closestB(zeroV), normal(zeroV); // from a to b
const FloatV zero = FZero();
FloatV penDep = zero;
PX_UNUSED(bLostContacts);
if(bLostContacts || manifold.invalidate_SphereCapsule(curRTrans, minMargin))
{
const bool idtScale = shapeConvex.scale.isIdentity();
manifold.setRelativeTransform(curRTrans);
const QuatV vQuat = QuatVLoadU(&shapeConvex.scale.rotation.x);
ConvexHullV convexHull(hullData, zeroV, vScale, vQuat, idtScale);
convexHull.setMargin(zero);
//transform capsule(a) into the local space of convexHull(b)
CapsuleV capsule(aToB.p, aToB.rotate(V3Scale(V3UnitX(), capsuleHalfHeight)), capsuleRadius);
LocalConvex<CapsuleV> convexA(capsule);
const Vec3V initialSearchDir = V3Sub(capsule.getCenter(), convexHull.getCenter());
if(idtScale)
{
LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullNoScaleV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep,
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
}
else
{
LocalConvex<ConvexHullV> convexB(convexHull);
status = gjkPenetration<LocalConvex<CapsuleV>, LocalConvex<ConvexHullV> >(convexA, convexB, initialSearchDir, contactDist, closestA, closestB, normal, penDep,
manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints, true);
}
Gu::PersistentContact* manifoldContacts = PX_CP_TO_PCP(contactBuffer.contacts);
bool doOverlapTest = false;
if(status == GJK_NON_INTERSECT)
{
return false;
}
else if(status == GJK_DEGENERATE)
{
return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal,
closestB, convexHull.getMargin(), contactDist, true, renderOutput, FLoad(params.mToleranceLength));
}
else
{
const FloatV replaceBreakingThreshold = FMul(minMargin, FLoad(0.05f));
if(status == GJK_CONTACT)
{
const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
//Add contact to contact stream
manifoldContacts[0].mLocalPointA = localPointA;
manifoldContacts[0].mLocalPointB = closestB;
manifoldContacts[0].mLocalNormalPen = localNormalPen;
//Add contact to manifold
manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
}
else
{
PX_ASSERT(status == EPA_CONTACT);
if(idtScale)
{
LocalConvex<ConvexHullNoScaleV> convexB(*PX_CONVEX_TO_NOSCALECONVEX(&convexHull));
status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
closestA, closestB, normal, penDep, true);
}
else
{
LocalConvex<ConvexHullV> convexB(convexHull);
status= Gu::epaPenetration(convexA, convexB, manifold.mAIndice, manifold.mBIndice, manifold.mNumWarmStartPoints,
closestA, closestB, normal, penDep, true);
}
if(status == EPA_CONTACT)
{
const Vec3V localPointA = aToB.transformInv(closestA);//curRTrans.transformInv(closestA);
const Vec4V localNormalPen = V4SetW(Vec4V_From_Vec3V(normal), penDep);
//Add contact to contact stream
manifoldContacts[0].mLocalPointA = localPointA;
manifoldContacts[0].mLocalPointB = closestB;
manifoldContacts[0].mLocalNormalPen = localNormalPen;
//Add contact to manifold
manifold.addManifoldPoint2(localPointA, closestB, localNormalPen, replaceBreakingThreshold);
}
else
{
doOverlapTest = true;
}
}
if(initialContacts == 0 || bLostContacts || doOverlapTest)
{
return fullContactsGenerationCapsuleConvex(capsule, convexHull, aToB, transf0, transf1, manifoldContacts, contactBuffer, idtScale, manifold, normal,
closestB, convexHull.getMargin(), contactDist, doOverlapTest, renderOutput, FLoad(params.mToleranceLength));
}
else
{
//This contact is either come from GJK or EPA
normal = transf1.rotate(normal);
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
#if PCM_LOW_LEVEL_DEBUG
manifold.drawManifold(*renderOutput, transf0, transf1);
#endif
return true;
}
}
}
else if (manifold.getNumContacts() > 0)
{
normal = manifold.getWorldNormal(transf1);
manifold.addManifoldContactsToContactBuffer(contactBuffer, normal, transf0, capsuleRadius, contactDist);
#if PCM_LOW_LEVEL_DEBUG
manifold.drawManifold(*renderOutput, transf0, transf1);
#endif
return true;
}
return false;
}