Пример #1
0
PxVehicleNoDrive* PxVehicleNoDrive::allocate(const PxU32 numWheels)
{
	PX_CHECK_AND_RETURN_NULL(numWheels>0, "Cars with zero wheels are illegal");

	//Compute the bytes needed.
	const PxU32 numWheels4 = (((numWheels + 3) & ~3) >> 2);
	const PxU32 inputByteSize16 = sizeof(PxReal)*numWheels4*4;
	const PxU32 byteSize = sizeof(PxVehicleNoDrive) + 3*inputByteSize16 + PxVehicleWheels::computeByteSize(numWheels4);

	//Allocate the memory.
	PxVehicleNoDrive* veh = (PxVehicleNoDrive*)PX_ALLOC(byteSize, PX_DEBUG_EXP("PxVehicleNoDrive"));
	Cm::markSerializedMem(veh, byteSize);
	new(veh) PxVehicleNoDrive();

	//Patch up the pointers.
	PxU8* ptr = (PxU8*)veh + sizeof(PxVehicleNoDrive);
	ptr=PxVehicleWheels::patchupPointers(veh,ptr,numWheels4,numWheels);
	veh->mSteerAngles = (PxReal*)ptr;
	ptr+=inputByteSize16;
	veh->mDriveTorques = (PxReal*)ptr;
	ptr+=inputByteSize16;
	veh->mBrakeTorques = (PxReal*)ptr;
	ptr+=inputByteSize16;

	PxMemZero(veh->mSteerAngles, inputByteSize16);
	PxMemZero(veh->mDriveTorques, inputByteSize16);
	PxMemZero(veh->mBrakeTorques, inputByteSize16);

	//Set the vehicle type.
	veh->mType = PxVehicleTypes::eNODRIVE;

	return veh;
}
void PxcFsApplyJointDrives(PxcFsData& matrix,
						   const Vec3V* Q)
{				
	typedef PxcArticulationFnsSimd<PxcArticulationFnsSimdBase> Fns;

	PX_ASSERT(matrix.linkCount<=PXC_ARTICULATION_MAX_SIZE);

	const PxcFsRow* rows = getFsRows(matrix);
	const PxcFsRowAux* aux = getAux(matrix);
	const PxcFsJointVectors* jointVectors = getJointVectors(matrix);

	PxcSIMDSpatial Z[PXC_ARTICULATION_MAX_SIZE];
	PxcSIMDSpatial dV[PXC_ARTICULATION_MAX_SIZE];
	Vec3V SZminusQ[PXC_ARTICULATION_MAX_SIZE];

	PxMemZero(Z, matrix.linkCount*sizeof(PxcSIMDSpatial));

	for(PxU32 i=matrix.linkCount;i-->1;)
		Z[matrix.parent[i]] += propagateDrivenImpulse(rows[i], jointVectors[i], SZminusQ[i], Z[i], Q[i]);

	
	dV[0] = Fns::multiply(getRootInverseInertia(matrix), -Z[0]);

	for(PxU32 i=1;i<matrix.linkCount;i++)
		dV[i] = Fns::propagateVelocity(rows[i], jointVectors[i], SZminusQ[i], dV[matrix.parent[i]], aux[i]);

	PxcSIMDSpatial* V = getVelocity(matrix);
	for(PxU32 i=0;i<matrix.linkCount;i++)
		V[i] += dV[i];
}
Пример #3
0
void PxVehicleNoDrive::setToRestState()
{
	const PxU32 numWheels4 = (((mWheelsSimData.getNbWheels() + 3) & ~3) >> 2);
	const PxU32 inputByteSize = sizeof(PxReal)*numWheels4*4;
	const PxU32 inputByteSize16 = (inputByteSize + 15) & ~15;
	PxMemZero(mSteerAngles, 3*inputByteSize16);

	//Set core to rest state.
	PxVehicleWheels::setToRestState();
}
bool setupFinalizeExtSolverContactsCoulomb(
						    const ContactBuffer& buffer,
							const CorrelationBuffer& c,
							const PxTransform& bodyFrame0,
							const PxTransform& bodyFrame1,
							PxU8* workspace,
							PxReal invDt,
							PxReal bounceThresholdF32,
							const SolverExtBody& b0,
							const SolverExtBody& b1,
							PxU32 frictionCountPerPoint,
							PxReal invMassScale0, PxReal invInertiaScale0, 
							PxReal invMassScale1, PxReal invInertiaScale1,
							PxReal restDist,
							PxReal ccdMaxDistance)	
{
	// NOTE II: the friction patches are sparse (some of them have no contact patches, and
	// therefore did not get written back to the cache) but the patch addresses are dense,
	// corresponding to valid patches

	const FloatV ccdMaxSeparation = FLoad(ccdMaxDistance);

	PxU8* PX_RESTRICT ptr = workspace;

	//KS - TODO - this should all be done in SIMD to avoid LHS
	const PxF32 maxPenBias0 = b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
	const PxF32 maxPenBias1 = b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b1.mLinkIndex];

	const FloatV maxPenBias = FLoad(PxMax(maxPenBias0, maxPenBias1)/invDt);

	const FloatV restDistance = FLoad(restDist); 
	const FloatV bounceThreshold = FLoad(bounceThresholdF32);

	const FloatV invDtV = FLoad(invDt);
	const FloatV pt8 = FLoad(0.8f);

	const FloatV invDtp8 = FMul(invDtV, pt8);

	Ps::prefetchLine(c.contactID);
	Ps::prefetchLine(c.contactID, 128);

	const PxU32 frictionPatchCount = c.frictionPatchCount;

	const PxU32 pointStride = sizeof(SolverContactPointExt);
	const PxU32 frictionStride = sizeof(SolverContactFrictionExt);
	const PxU8 pointHeaderType = DY_SC_TYPE_EXT_CONTACT;
	const PxU8 frictionHeaderType = DY_SC_TYPE_EXT_FRICTION;

	PxReal d0 = invMassScale0;
	PxReal d1 = invMassScale1;
	PxReal angD0 = invInertiaScale0;
	PxReal angD1 = invInertiaScale1;

	PxU8 flags = 0;

	for(PxU32 i=0;i< frictionPatchCount;i++)
	{
		const PxU32 contactCount = c.frictionPatchContactCounts[i];
		if(contactCount == 0)
			continue;

		const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;

		const Vec3V normalV = Ps::aos::V3LoadA(contactBase0->normal);
		const Vec3V normal = V3LoadA(contactBase0->normal);

		const PxReal combinedRestitution = contactBase0->restitution;
	
		
		SolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<SolverContactCoulombHeader*>(ptr);
		ptr += sizeof(SolverContactCoulombHeader);

		Ps::prefetchLine(ptr, 128);
		Ps::prefetchLine(ptr, 256);
		Ps::prefetchLine(ptr, 384);

		const FloatV restitution = FLoad(combinedRestitution);


		header->numNormalConstr		= PxU8(contactCount);
		header->type				= pointHeaderType;
		//header->setRestitution(combinedRestitution);

		header->setDominance0(d0);
		header->setDominance1(d1);
		header->angDom0 = angD0;
		header->angDom1 = angD1;
		header->flags = flags;
		
		header->setNormal(normalV);
		
		for(PxU32 patch=c.correlationListHeads[i]; 
			patch!=CorrelationBuffer::LIST_END; 
			patch = c.contactPatches[patch].next)
		{
			const PxU32 count = c.contactPatches[patch].count;
			const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
				
			PxU8* p = ptr;
			for(PxU32 j=0;j<count;j++)
			{
				const Gu::ContactPoint& contact = contactBase[j];

				SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
				p += pointStride;

				setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0, bodyFrame1, normal, invDtV, invDtp8, restDistance, maxPenBias, restitution,
					bounceThreshold, contact, *solverContact, ccdMaxSeparation);
			}			
			ptr = p;
		}
	}

	//construct all the frictions

	PxU8* PX_RESTRICT ptr2 = workspace;

	const PxF32 orthoThreshold = 0.70710678f;
	const PxF32 eps = 0.00001f;
	bool hasFriction = false;

	for(PxU32 i=0;i< frictionPatchCount;i++)
	{
		const PxU32 contactCount = c.frictionPatchContactCounts[i];
		if(contactCount == 0)
			continue;

		SolverContactCoulombHeader* header = reinterpret_cast<SolverContactCoulombHeader*>(ptr2); 
		header->frictionOffset = PxU16(ptr - ptr2);
		ptr2 += sizeof(SolverContactCoulombHeader) + header->numNormalConstr * pointStride;

		const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;

		PxVec3 normal = contactBase0->normal;

		const PxReal staticFriction = contactBase0->staticFriction;
		const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
		const bool haveFriction = (disableStrongFriction == 0);
	
		SolverFrictionHeader* frictionHeader = reinterpret_cast<SolverFrictionHeader*>(ptr);
		frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
		frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatchContactCounts[i] * frictionCountPerPoint : 0);
		frictionHeader->flags = flags;
		ptr += sizeof(SolverFrictionHeader);
		PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
		ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
		PxMemZero(forceBuffer, sizeof(PxF32) * c.frictionPatchContactCounts[i]);
		Ps::prefetchLine(ptr, 128);
		Ps::prefetchLine(ptr, 256);
		Ps::prefetchLine(ptr, 384);


		const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
		const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
		const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
		const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
		const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
		const PxReal sqDist = t0_.dot(t0_);
		const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
		const PxVec3 tDir1 = tDir0.cross(normal);
		PxVec3 tFallback[2] = {tDir0, tDir1};

		PxU32 ind = 0;

		if(haveFriction)
		{
			hasFriction = true;
			frictionHeader->setStaticFriction(staticFriction);
			frictionHeader->invMass0D0 = d0;
			frictionHeader->invMass1D1 = d1;
			frictionHeader->angDom0 = angD0;
			frictionHeader->angDom1 = angD1;
			frictionHeader->type			= frictionHeaderType;
			
			PxU32 totalPatchContactCount = 0;
		
			for(PxU32 patch=c.correlationListHeads[i]; 
				patch!=CorrelationBuffer::LIST_END; 
				patch = c.contactPatches[patch].next)
			{
				const PxU32 count = c.contactPatches[patch].count;
				const PxU32 start = c.contactPatches[patch].start;
				const Gu::ContactPoint* contactBase = buffer.contacts + start;
					
				PxU8* p = ptr;

				for(PxU32 j =0; j < count; j++)
				{
					const Gu::ContactPoint& contact = contactBase[j];
					const PxVec3 ra = contact.point - bodyFrame0.p;
					const PxVec3 rb = contact.point - bodyFrame1.p;
						
					const PxVec3 targetVel = contact.targetVel;
					const PxVec3 pVRa = b0.getLinVel() + b0.getAngVel().cross(ra);
					const PxVec3 pVRb = b1.getLinVel() + b1.getAngVel().cross(rb);
					//const PxVec3 vrel = pVRa - pVRb;

					for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
					{
						SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(p);
						p += frictionStride;

						PxVec3 t0 = tFallback[ind];
						ind = 1 - ind;
						PxVec3 raXn = ra.cross(t0); 
						PxVec3 rbXn = rb.cross(t0); 
						Cm::SpatialVector deltaV0, deltaV1;

						const Cm::SpatialVector resp0 = createImpulseResponseVector(t0, raXn, b0);
						const Cm::SpatialVector resp1 = createImpulseResponseVector(-t0, -rbXn, b1);

						PxReal unitResponse = getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
																 b1, resp1, deltaV1, d1, angD1);

						PxReal tv = targetVel.dot(t0);
						if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
							tv += pVRa.dot(t0);
						else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
							tv -= pVRb.dot(t0);


						f0->setVelMultiplier(FLoad(unitResponse>0.0f ? 1.f/unitResponse : 0.0f));
						f0->setRaXn(resp0.angular);
						f0->setRbXn(-resp1.angular);
						f0->targetVel = tv;
						f0->setNormal(t0);
						f0->setAppliedForce(0.0f);
						f0->linDeltaVA = V3LoadA(deltaV0.linear);
						f0->angDeltaVA = V3LoadA(deltaV0.angular);
						f0->linDeltaVB = V3LoadA(deltaV1.linear);
						f0->angDeltaVB = V3LoadA(deltaV1.angular);
					}					
				}

				totalPatchContactCount += c.contactPatches[patch].count;
				
				ptr = p;	
			}
		}
	}
	//PX_ASSERT(ptr - workspace == n.solverConstraintSize);
	return hasFriction;
}
	void setupFinalizeExtSolverContacts(
						    const ContactPoint* buffer,
							const CorrelationBuffer& c,
							const PxTransform& bodyFrame0,
							const PxTransform& bodyFrame1,
							PxU8* workspace,
							const SolverExtBody& b0,
							const SolverExtBody& b1,
							const PxReal invDtF32,
							PxReal bounceThresholdF32,
							PxReal invMassScale0, PxReal invInertiaScale0, 
							PxReal invMassScale1, PxReal invInertiaScale1,
							const PxReal restDist,
							PxU8* frictionDataPtr,
							PxReal ccdMaxContactDist)	
{
	// NOTE II: the friction patches are sparse (some of them have no contact patches, and
	// therefore did not get written back to the cache) but the patch addresses are dense,
	// corresponding to valid patches

	/*const bool haveFriction = PX_IR(n.staticFriction) > 0 || PX_IR(n.dynamicFriction) > 0;*/

	const FloatV ccdMaxSeparation = FLoad(ccdMaxContactDist);

	PxU8* PX_RESTRICT ptr = workspace;

	const FloatV zero=FZero();

	//KS - TODO - this should all be done in SIMD to avoid LHS
	const PxF32 maxPenBias0 = b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
	const PxF32 maxPenBias1 = b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b1.mLinkIndex];

	const FloatV maxPenBias = FLoad(PxMax(maxPenBias0, maxPenBias1));


	const PxReal d0 = invMassScale0;
	const PxReal d1 = invMassScale1;

	const PxReal angD0 = invInertiaScale0;
	const PxReal angD1 = invInertiaScale1;

	Vec4V staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W = V4Zero();
	staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetZ(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(d0));
	staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetW(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(d1));

	const FloatV restDistance = FLoad(restDist); 

	PxU32 frictionPatchWritebackAddrIndex = 0;
	PxU32 contactWritebackCount = 0;

	Ps::prefetchLine(c.contactID);
	Ps::prefetchLine(c.contactID, 128);

	const FloatV invDt = FLoad(invDtF32);
	const FloatV p8 = FLoad(0.8f);
	const FloatV bounceThreshold = FLoad(bounceThresholdF32);

	const FloatV invDtp8 = FMul(invDt, p8);

	PxU8 flags = 0;

	for(PxU32 i=0;i<c.frictionPatchCount;i++)
	{
		PxU32 contactCount = c.frictionPatchContactCounts[i];
		if(contactCount == 0)
			continue;

		const FrictionPatch& frictionPatch = c.frictionPatches[i];
		PX_ASSERT(frictionPatch.anchorCount <= 2);  //0==anchorCount is allowed if all the contacts in the manifold have a large offset. 

		const Gu::ContactPoint* contactBase0 = buffer + c.contactPatches[c.correlationListHeads[i]].start;
		const PxReal combinedRestitution = contactBase0->restitution;

		const PxReal staticFriction = contactBase0->staticFriction;
		const PxReal dynamicFriction = contactBase0->dynamicFriction;
		const bool disableStrongFriction = !!(contactBase0->materialFlags & PxMaterialFlag::eDISABLE_FRICTION);
		staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetX(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(staticFriction));
		staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W=V4SetY(staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W, FLoad(dynamicFriction));
	
		SolverContactHeader* PX_RESTRICT header = reinterpret_cast<SolverContactHeader*>(ptr);
		ptr += sizeof(SolverContactHeader);		


		Ps::prefetchLine(ptr + 128);
		Ps::prefetchLine(ptr + 256);
		Ps::prefetchLine(ptr + 384);
		
		const bool haveFriction = (disableStrongFriction == 0) ;//PX_IR(n.staticFriction) > 0 || PX_IR(n.dynamicFriction) > 0;
		header->numNormalConstr		= Ps::to8(contactCount);
		header->numFrictionConstr	= Ps::to8(haveFriction ? frictionPatch.anchorCount*2 : 0);
	
		header->type				= Ps::to8(DY_SC_TYPE_EXT_CONTACT);

		header->flags = flags;

		const FloatV restitution = FLoad(combinedRestitution);
	
		header->staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W = staticFrictionX_dynamicFrictionY_dominance0Z_dominance1W;

		header->angDom0 = angD0;
		header->angDom1 = angD1;
	
		const PxU32 pointStride = sizeof(SolverContactPointExt);
		const PxU32 frictionStride = sizeof(SolverContactFrictionExt);

		const Vec3V normal = V3LoadU(buffer[c.contactPatches[c.correlationListHeads[i]].start].normal);

		header->normal = normal;
		
		for(PxU32 patch=c.correlationListHeads[i]; 
			patch!=CorrelationBuffer::LIST_END; 
			patch = c.contactPatches[patch].next)
		{
			const PxU32 count = c.contactPatches[patch].count;
			const Gu::ContactPoint* contactBase = buffer + c.contactPatches[patch].start;
				
			PxU8* p = ptr;
			for(PxU32 j=0;j<count;j++)
			{
				const Gu::ContactPoint& contact = contactBase[j];

				SolverContactPointExt* PX_RESTRICT solverContact = reinterpret_cast<SolverContactPointExt*>(p);
				p += pointStride;

				setupExtSolverContact(b0, b1, d0, d1, angD0, angD1, bodyFrame0, bodyFrame1, normal, invDt, invDtp8, restDistance, maxPenBias, restitution,
					bounceThreshold, contact, *solverContact, ccdMaxSeparation);
			
			}

			ptr = p;
		}
		contactWritebackCount += contactCount;

		PxF32* forceBuffer = reinterpret_cast<PxF32*>(ptr);
		PxMemZero(forceBuffer, sizeof(PxF32) * contactCount);
		ptr += sizeof(PxF32) * ((contactCount + 3) & (~3));

		header->broken = 0;

		if(haveFriction)
		{
			//const Vec3V normal = Vec3V_From_PxVec3(buffer.contacts[c.contactPatches[c.correlationListHeads[i]].start].normal);
			PxVec3 normalS = buffer[c.contactPatches[c.correlationListHeads[i]].start].normal;

			PxVec3 t0, t1;
			computeFrictionTangents(b0.getLinVel() - b1.getLinVel(), normalS, t0, t1);

			Vec3V vT0 = V3LoadU(t0);
			Vec3V vT1 = V3LoadU(t1);
			
			//We want to set the writeBack ptr to point to the broken flag of the friction patch.
			//On spu we have a slight problem here because the friction patch array is 
			//in local store rather than in main memory. The good news is that the address of the friction 
			//patch array in main memory is stored in the work unit. These two addresses will be equal 
			//except on spu where one is local store memory and the other is the effective address in main memory.
			//Using the value stored in the work unit guarantees that the main memory address is used on all platforms.
			PxU8* PX_RESTRICT writeback = frictionDataPtr + frictionPatchWritebackAddrIndex*sizeof(FrictionPatch);

			header->frictionBrokenWritebackByte = writeback;			

			for(PxU32 j = 0; j < frictionPatch.anchorCount; j++)
			{
				SolverContactFrictionExt* PX_RESTRICT f0 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
				ptr += frictionStride;
				SolverContactFrictionExt* PX_RESTRICT f1 = reinterpret_cast<SolverContactFrictionExt*>(ptr);
				ptr += frictionStride;

				PxVec3 ra = bodyFrame0.q.rotate(frictionPatch.body0Anchors[j]);
				PxVec3 rb = bodyFrame1.q.rotate(frictionPatch.body1Anchors[j]);
				PxVec3 error = (ra + bodyFrame0.p) - (rb + bodyFrame1.p);

				{
					const PxVec3 raXn = ra.cross(t0);
					const PxVec3 rbXn = rb.cross(t0);

					Cm::SpatialVector deltaV0, deltaV1;

					const Cm::SpatialVector resp0 = createImpulseResponseVector(t0, raXn, b0);
					const Cm::SpatialVector resp1 = createImpulseResponseVector(-t1, -rbXn, b1);
					FloatV resp = FLoad(getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
															 b1, resp1, deltaV1, d1, angD1));

					const FloatV velMultiplier = FSel(FIsGrtr(resp, zero), FMul(p8, FRecip(resp)), zero);

					PxU32 index = c.contactPatches[c.correlationListHeads[i]].start;
					PxF32 targetVel = buffer[index].targetVel.dot(t0);

					if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
						targetVel -= b0.projectVelocity(t0, raXn);
					else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
						targetVel += b1.projectVelocity(t0, rbXn);

					f0->normalXYZ_appliedForceW = V4SetW(vT0, zero);
					f0->raXnXYZ_velMultiplierW = V4SetW(V4LoadA(&resp0.angular.x), velMultiplier);
					f0->rbXnXYZ_biasW = V4SetW(V4Neg(V4LoadA(&resp1.angular.x)), FLoad(t0.dot(error) * invDtF32));
					f0->linDeltaVA = V3LoadA(deltaV0.linear);
					f0->angDeltaVA = V3LoadA(deltaV0.angular);
					f0->linDeltaVB = V3LoadA(deltaV1.linear);
					f0->angDeltaVB = V3LoadA(deltaV1.angular);
					f0->targetVel = targetVel;
				}

				{

					const PxVec3 raXn = ra.cross(t1);
					const PxVec3 rbXn = rb.cross(t1);

					Cm::SpatialVector deltaV0, deltaV1;


					const Cm::SpatialVector resp0 = createImpulseResponseVector(t1, raXn, b0);
					const Cm::SpatialVector resp1 = createImpulseResponseVector(-t1, -rbXn, b1);

					FloatV resp = FLoad(getImpulseResponse(b0, resp0, deltaV0, d0, angD0,
														   b1, resp1, deltaV1, d1, angD1));

					const FloatV velMultiplier = FSel(FIsGrtr(resp, zero), FMul(p8, FRecip(resp)), zero);

					PxU32 index = c.contactPatches[c.correlationListHeads[i]].start;
					PxF32 targetVel = buffer[index].targetVel.dot(t0);

					if(b0.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
						targetVel -= b0.projectVelocity(t1, raXn);
					else if(b1.mLinkIndex == PxSolverConstraintDesc::NO_LINK)
						targetVel += b1.projectVelocity(t1, rbXn);

					f1->normalXYZ_appliedForceW = V4SetW(vT1, zero);
					f1->raXnXYZ_velMultiplierW = V4SetW(V4LoadA(&resp0.angular.x), velMultiplier);
					f1->rbXnXYZ_biasW = V4SetW(V4Neg(V4LoadA(&resp1.angular.x)), FLoad(t1.dot(error) * invDtF32));
					f1->linDeltaVA = V3LoadA(deltaV0.linear);
					f1->angDeltaVA = V3LoadA(deltaV0.angular);
					f1->linDeltaVB = V3LoadA(deltaV1.linear);
					f1->angDeltaVB = V3LoadA(deltaV1.angular);
					f1->targetVel = targetVel;
				}
			}
		}

		frictionPatchWritebackAddrIndex++;
	}
}
void PxVehicleCopyDynamicsData(const PxVehicleCopyDynamicsMap& wheelMap, const PxVehicleWheels& src, PxVehicleWheels* trg)
{
	PX_CHECK_AND_RETURN(trg, "PxVehicleCopyDynamicsData requires that trg is a valid vehicle pointer");

	PX_CHECK_AND_RETURN(src.getVehicleType() == trg->getVehicleType(), "PxVehicleCopyDynamicsData requires that both src and trg are the same type of vehicle");

#ifdef PX_CHECKED
	{
		const PxU32 numWheelsSrc = src.mWheelsSimData.getNbWheels();
		const PxU32 numWheelsTrg = trg->mWheelsSimData.getNbWheels();
		PxU8 copiedWheelsSrc[PX_MAX_NB_WHEELS];
		PxMemZero(copiedWheelsSrc, sizeof(PxU8) * PX_MAX_NB_WHEELS);
		PxU8 setWheelsTrg[PX_MAX_NB_WHEELS];
		PxMemZero(setWheelsTrg, sizeof(PxU8) * PX_MAX_NB_WHEELS);
		for(PxU32 i = 0; i < PxMin(numWheelsSrc, numWheelsTrg); i++)
		{
			const PxU32 srcWheelId = wheelMap.sourceWheelIds[i];
			PX_CHECK_AND_RETURN(srcWheelId < numWheelsSrc, "PxVehicleCopyDynamicsData - wheelMap contains illegal source wheel id");
			PX_CHECK_AND_RETURN(0 == copiedWheelsSrc[srcWheelId], "PxVehicleCopyDynamicsData - wheelMap contains illegal source wheel id");
			copiedWheelsSrc[srcWheelId] = 1;

			const PxU32 trgWheelId = wheelMap.targetWheelIds[i];
			PX_CHECK_AND_RETURN(trgWheelId < numWheelsTrg, "PxVehicleCopyDynamicsData - wheelMap contains illegal target wheel id");
			PX_CHECK_AND_RETURN(0 == setWheelsTrg[trgWheelId], "PxVehicleCopyDynamicsData - wheelMap contains illegal target wheel id");
			setWheelsTrg[trgWheelId]=1;
		}
	}
#endif


	const PxU32 numWheelsSrc = src.mWheelsSimData.getNbWheels();
	const PxU32 numWheelsTrg = trg->mWheelsSimData.getNbWheels();

	//Set all dynamics data on the target to zero.
	//Be aware that setToRestState sets the rigid body to 
	//rest so set the momentum back after calling setToRestState.
	PxRigidDynamic* actorTrg = trg->getRigidDynamicActor();
	PxVec3 linVel = actorTrg->getLinearVelocity();
	PxVec3 angVel = actorTrg->getAngularVelocity();
	switch(src.getVehicleType())
	{
	case PxVehicleTypes::eDRIVE4W:
		((PxVehicleDrive4W*)trg)->setToRestState();
		break;
	case PxVehicleTypes::eDRIVENW:
		((PxVehicleDriveNW*)trg)->setToRestState();
		break;
	case PxVehicleTypes::eDRIVETANK:
		((PxVehicleDriveTank*)trg)->setToRestState();
		break;
	case PxVehicleTypes::eNODRIVE:
		((PxVehicleNoDrive*)trg)->setToRestState();
		break;
	default:
		break;
	}
	actorTrg->setLinearVelocity(linVel);
	actorTrg->setAngularVelocity(angVel);


	//Keep a track of the wheels on trg that have their dynamics data set as a copy from src.
	PxU8 setWheelsTrg[PX_MAX_NB_WHEELS];
	PxMemZero(setWheelsTrg, sizeof(PxU8) * PX_MAX_NB_WHEELS);

	//Keep a track of the average wheel rotation speed of all enabled wheels on src.
	PxU32 numEnabledWheelsSrc = 0;
	PxF32 accumulatedWheelRotationSpeedSrc = 0.0f;

	//Copy wheel dynamics data from src wheels to trg wheels.
	//Track the target wheels that have been given dynamics data from src wheels.
	//Compute the accumulated wheel rotation speed of all enabled src wheels.
	const PxU32 numMappedWheels = PxMin(numWheelsSrc, numWheelsTrg);
	for(PxU32 i = 0; i < numMappedWheels; i++)
	{
		const PxU32 srcWheelId = wheelMap.sourceWheelIds[i];
		const PxU32 trgWheelId = wheelMap.targetWheelIds[i];

		trg->mWheelsDynData.copy(src.mWheelsDynData, srcWheelId, trgWheelId);

		setWheelsTrg[trgWheelId] = 1;

		if(!src.mWheelsSimData.getIsWheelDisabled(srcWheelId))
		{
			numEnabledWheelsSrc++;
			accumulatedWheelRotationSpeedSrc += src.mWheelsDynData.getWheelRotationSpeed(srcWheelId);
		}
	}

	//Compute the average wheel rotation speed of src.
	PxF32 averageWheelRotationSpeedSrc = 0;
	if(numEnabledWheelsSrc > 0)
	{
		averageWheelRotationSpeedSrc = (accumulatedWheelRotationSpeedSrc/ (1.0f * numEnabledWheelsSrc));
	}

	//For wheels on trg that have not had their dynamics data copied from src just set
	//their wheel rotation speed to the average wheel rotation speed.
	for(PxU32 i = 0; i < numWheelsTrg; i++)
	{
		if(0 == setWheelsTrg[i] && !trg->mWheelsSimData.getIsWheelDisabled(i))
		{
			trg->mWheelsDynData.setWheelRotationSpeed(i, averageWheelRotationSpeedSrc);
		}
	}

	//Copy the engine rotation speed/gear states/autobox states/etc.
	switch(src.getVehicleType())
	{
	case PxVehicleTypes::eDRIVE4W:
	case PxVehicleTypes::eDRIVENW:
	case PxVehicleTypes::eDRIVETANK:
		{
			const PxVehicleDriveDynData& driveDynDataSrc = ((const PxVehicleDrive&)src).mDriveDynData;
			PxVehicleDriveDynData* driveDynDataTrg = &((PxVehicleDrive*)trg)->mDriveDynData;
			*driveDynDataTrg = driveDynDataSrc;
		}
		break;
	default:
		break;
	}
}