예제 #1
0
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;
	}

}
예제 #2
0
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);
}
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;
}
예제 #4
0
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);
}
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);
}