1 files changed, 301 insertions, 0 deletions
diff --git a/PhysX_3.4/Source/LowLevelDynamics/src/DyContactPrepShared.h b/PhysX_3.4/Source/LowLevelDynamics/src/DyContactPrepShared.h
new file mode 100644
index 00000000..7accabd3
--- /dev/null
+++ b/PhysX_3.4/Source/LowLevelDynamics/src/DyContactPrepShared.h
@@ -0,0 +1,301 @@
+// This code contains NVIDIA Confidential Information and is disclosed to you
+// under a form of NVIDIA software license agreement provided separately to you.
+//
+// Notice
+// NVIDIA Corporation and its licensors retain all intellectual property and
+// proprietary rights in and to this software and related documentation and
+// any modifications thereto. Any use, reproduction, disclosure, or
+// distribution of this software and related documentation without an express
+// license agreement from NVIDIA Corporation is strictly prohibited.
+//
+// ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
+// NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
+// THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
+// MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// Information and code furnished is believed to be accurate and reliable.
+// However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
+// information or for any infringement of patents or other rights of third parties that may
+// result from its use. No license is granted by implication or otherwise under any patent
+// or patent rights of NVIDIA Corporation. Details are subject to change without notice.
+// This code supersedes and replaces all information previously supplied.
+// NVIDIA Corporation products are not authorized for use as critical
+// components in life support devices or systems without express written approval of
+// NVIDIA Corporation.
+//
+// Copyright (c) 2008-2016 NVIDIA Corporation. All rights reserved.
+// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
+// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
+
+#ifndef DY_CONTACT_PREP_SHARED_H
+#define DY_CONTACT_PREP_SHARED_H
+     
+#include "foundation/PxPreprocessor.h"
+#include "PxSceneDesc.h"
+#include "PsVecMath.h"
+#include "PsMathUtils.h"
+#include "DyContactPrep.h"
+#include "DyCorrelationBuffer.h"
+#include "DyArticulationContactPrep.h"
+#include "PxsContactManager.h"
+#include "PxsContactManagerState.h"
+
+namespace physx
+{
+namespace Dy
+{
+
+
+PX_FORCE_INLINE bool pointsAreClose(const PxTransform& body1ToBody0,
+									const PxVec3& localAnchor0, const PxVec3& localAnchor1,
+									const PxVec3& axis, float correlDist)
+{
+	const PxVec3 body0PatchPoint1 = body1ToBody0.transform(localAnchor1);
+
+	return PxAbs((localAnchor0 - body0PatchPoint1).dot(axis))<correlDist;
+}
+
+PX_FORCE_INLINE bool isSeparated(const FrictionPatch& patch, const PxTransform& body1ToBody0, const PxReal correlationDistance)
+{
+	PX_ASSERT(patch.anchorCount <= 2);
+	for(PxU32 a = 0; a < patch.anchorCount; ++a)
+	{
+		if(!pointsAreClose(body1ToBody0, patch.body0Anchors[a], patch.body1Anchors[a], patch.body0Normal, correlationDistance))
+			return true;
+	}
+	return false;
+}
+
+
+inline bool getFrictionPatches(CorrelationBuffer& c,
+						const PxU8* frictionCookie,
+						PxU32 frictionPatchCount,
+						const PxTransform& bodyFrame0,
+						const PxTransform& bodyFrame1,
+						PxReal correlationDistance)
+{
+	PX_UNUSED(correlationDistance);
+	if(frictionCookie == NULL || frictionPatchCount == 0)
+		return true;
+
+	//KS - this is now DMA'd inside the shader so we don't need to immediate DMA it here
+	const FrictionPatch* patches = reinterpret_cast<const FrictionPatch*>(frictionCookie);
+
+	//Try working out relative transforms! TODO - can we compute this lazily for the first friction patch
+	bool evaluated = false;
+	PxTransform body1ToBody0;
+
+	while(frictionPatchCount--)
+	{
+		Ps::prefetchLine(patches,128);
+		const FrictionPatch& patch = *patches++;
+		PX_ASSERT (patch.broken == 0 || patch.broken == 1);
+		if(!patch.broken)
+		{
+			// if the eDISABLE_STRONG_FRICTION flag is there we need to blow away the previous frame's friction correlation, so
+			// that we can associate each friction anchor with a target velocity. So we lose strong friction.
+			if(patch.anchorCount != 0 && !(patch.materialFlags & PxMaterialFlag::eDISABLE_STRONG_FRICTION))
+			{
+				PX_ASSERT(patch.anchorCount <= 2);
+
+				
+				if(!evaluated)
+				{
+					body1ToBody0 = bodyFrame0.transformInv(bodyFrame1);
+					evaluated = true;
+				}
+
+
+				if(patch.body0Normal.dot(body1ToBody0.rotate(patch.body1Normal)) > PXC_SAME_NORMAL)
+				{
+					if(!isSeparated(patch, body1ToBody0, correlationDistance))
+					{
+						if(c.frictionPatchCount == CorrelationBuffer::MAX_FRICTION_PATCHES)
+							return false;
+						{
+							c.contactID[c.frictionPatchCount][0] = 0xffff;
+							c.contactID[c.frictionPatchCount][1] = 0xffff;
+							//Rotate the contact normal into world space
+							c.frictionPatchWorldNormal[c.frictionPatchCount] = bodyFrame0.rotate(patch.body0Normal);
+							c.frictionPatchContactCounts[c.frictionPatchCount] = 0;
+							c.correlationListHeads[c.frictionPatchCount] = CorrelationBuffer::LIST_END;
+							PxMemCopy(&c.frictionPatches[c.frictionPatchCount++], &patch, sizeof(FrictionPatch));
+						}
+					}
+				}
+			}
+		}
+	}
+	return true;
+}
+
+PX_FORCE_INLINE PxU32 extractContacts(Gu::ContactBuffer& buffer, PxsContactManagerOutput& npOutput, bool& hasMaxImpulse, bool& hasTargetVelocity,
+							 PxReal& invMassScale0, PxReal& invMassScale1, PxReal& invInertiaScale0, PxReal& invInertiaScale1, PxReal defaultMaxImpulse)
+{
+	PxContactStreamIterator iter(npOutput.contactPatches, npOutput.contactPoints, npOutput.getInternalFaceIndice(), npOutput.nbPatches, npOutput.nbContacts);	
+
+	PxU32 numContacts = buffer.count, origContactCount = buffer.count;
+	if(!iter.forceNoResponse)
+	{
+		invMassScale0 = iter.getInvMassScale0();
+		invMassScale1 = iter.getInvMassScale1();
+		invInertiaScale0 = iter.getInvInertiaScale0();
+		invInertiaScale1 = iter.getInvInertiaScale1();
+		hasMaxImpulse = (iter.patch->internalFlags & PxContactPatch::eHAS_MAX_IMPULSE) != 0;
+		hasTargetVelocity = (iter.patch->internalFlags & PxContactPatch::eHAS_TARGET_VELOCITY) != 0;
+
+		while(iter.hasNextPatch())
+		{
+			iter.nextPatch();
+			while(iter.hasNextContact())
+			{
+				iter.nextContact();
+				Ps::prefetchLine(iter.contact, 128);
+				Ps::prefetchLine(&buffer.contacts[numContacts], 128);
+				PxReal maxImpulse = hasMaxImpulse ? iter.getMaxImpulse() : defaultMaxImpulse;
+				if(maxImpulse != 0.f)
+				{
+					PX_ASSERT(numContacts < Gu::ContactBuffer::MAX_CONTACTS);
+					buffer.contacts[numContacts].normal = iter.getContactNormal();
+					buffer.contacts[numContacts].point = iter.getContactPoint();
+					buffer.contacts[numContacts].separation = iter.getSeparation();
+					//KS - we use the face indices to cache the material indices and flags - avoids bloating the PxContact structure
+					buffer.contacts[numContacts].materialFlags = PxU8(iter.getMaterialFlags());
+					buffer.contacts[numContacts].maxImpulse = maxImpulse;
+					buffer.contacts[numContacts].staticFriction = iter.getStaticFriction();
+					buffer.contacts[numContacts].dynamicFriction = iter.getDynamicFriction();
+					buffer.contacts[numContacts].restitution = iter.getRestitution();
+					const PxVec3& targetVel = iter.getTargetVel();
+					buffer.contacts[numContacts].targetVel = targetVel;
+					++numContacts;
+				}
+			}
+		}
+	}
+	const PxU32 contactCount = numContacts - origContactCount;
+	buffer.count = numContacts;
+	return contactCount;
+}
+
+struct CorrelationListIterator
+{
+	CorrelationBuffer& buffer;
+	PxU32 currPatch;
+	PxU32 currContact;
+
+	CorrelationListIterator(CorrelationBuffer& correlationBuffer, PxU32 startPatch) : buffer(correlationBuffer)
+	{
+		//We need to force us to advance the correlation buffer to the first available contact (if one exists)
+		PxU32 newPatch = startPatch, newContact = 0;
+
+		while(newPatch != CorrelationBuffer::LIST_END && newContact == buffer.contactPatches[newPatch].count)
+		{
+			newPatch = buffer.contactPatches[newPatch].next;
+			newContact = 0;
+		}
+
+		currPatch = newPatch;
+		currContact = newContact;
+	}
+
+	//Returns true if it has another contact pre-loaded. Returns false otherwise
+	PX_FORCE_INLINE bool hasNextContact()
+	{
+		return (currPatch != CorrelationBuffer::LIST_END && currContact < buffer.contactPatches[currPatch].count);
+	}
+
+	inline void nextContact(PxU32& patch, PxU32& contact)
+	{
+		PX_ASSERT(currPatch != CorrelationBuffer::LIST_END);
+		PX_ASSERT(currContact < buffer.contactPatches[currPatch].count);
+
+		patch = currPatch;
+		contact = currContact;
+		PxU32 newPatch = currPatch, newContact = currContact + 1;
+
+		while(newPatch != CorrelationBuffer::LIST_END && newContact == buffer.contactPatches[newPatch].count)
+		{
+			newPatch = buffer.contactPatches[newPatch].next;
+			newContact = 0;
+		}
+
+		currPatch = newPatch;
+		currContact = newContact;
+	}
+
+private:
+	CorrelationListIterator& operator=(const CorrelationListIterator&);
+
+};
+
+
+	PX_FORCE_INLINE void constructContactConstraint(const Mat33V& invSqrtInertia0, const Mat33V& invSqrtInertia1,  const FloatVArg invMassNorLenSq0, 
+		const FloatVArg invMassNorLenSq1, const FloatVArg angD0, const FloatVArg angD1, const Vec3VArg bodyFrame0p, const Vec3VArg bodyFrame1p,
+		const Vec3VArg normal, const FloatVArg norVel, const VecCrossV& norCross, const Vec3VArg angVel0, const Vec3VArg angVel1,
+		const FloatVArg invDt, const FloatVArg invDtp8, const FloatVArg restDistance, const FloatVArg maxPenBias,  const FloatVArg restitution,
+		const FloatVArg bounceThreshold, const Gu::ContactPoint& contact, SolverContactPoint& solverContact,
+		const FloatVArg ccdMaxSeparation)
+	{
+		const FloatV zero = FZero();
+		const Vec3V point = V3LoadA(contact.point);
+		const FloatV separation = FLoad(contact.separation);
+
+		const FloatV cTargetVel = V3Dot(normal, V3LoadA(contact.targetVel));
+
+		const Vec3V ra = V3Sub(point, bodyFrame0p);
+		const Vec3V rb = V3Sub(point, bodyFrame1p);
+
+		const Vec3V raXn = V3Cross(ra, norCross);
+		const Vec3V rbXn = V3Cross(rb, norCross);
+
+		const Vec3V raXnSqrtInertia = M33MulV3(invSqrtInertia0, raXn);
+		const Vec3V rbXnSqrtInertia = M33MulV3(invSqrtInertia1, rbXn);				
+
+		const FloatV resp0 = FAdd(invMassNorLenSq0, FMul(V3Dot(raXnSqrtInertia, raXnSqrtInertia), angD0));
+		const FloatV resp1 = FSub(FMul(V3Dot(rbXnSqrtInertia, rbXnSqrtInertia), angD1), invMassNorLenSq1);
+
+		const FloatV unitResponse = FAdd(resp0, resp1);
+
+		const FloatV vrel1 = FAdd(norVel, V3Dot(raXn, angVel0));
+		const FloatV vrel2 = V3Dot(rbXn, angVel1);
+		const FloatV vrel = FSub(vrel1, vrel2);
+
+		const FloatV velMultiplier = FSel(FIsGrtr(unitResponse, zero), FRecip(unitResponse), zero);
+
+		const FloatV penetration = FSub(separation, restDistance);
+
+		const FloatV penetrationInvDt = FMul(penetration, invDt);
+
+		const FloatV penetrationInvDtPt8 = FMax(maxPenBias, FMul(penetration, invDtp8));
+
+		FloatV scaledBias = FMul(velMultiplier, penetrationInvDtPt8);
+
+		const BoolV isGreater2 = BAnd(BAnd(FIsGrtr(restitution, zero), FIsGrtr(bounceThreshold, vrel)), FIsGrtr(FNeg(vrel), penetrationInvDt));
+
+		const BoolV ccdSeparationCondition = FIsGrtrOrEq(ccdMaxSeparation, penetration);
+
+		scaledBias = FSel(BAnd(ccdSeparationCondition, isGreater2), zero, scaledBias);
+
+		const FloatV sumVRel(vrel);
+
+		FloatV targetVelocity = FAdd(cTargetVel, FSel(isGreater2, FMul(FNeg(sumVRel), restitution), zero));
+
+		//Note - we add on the initial target velocity
+		targetVelocity = FSub(targetVelocity, vrel);
+
+		const FloatV biasedErr = FScaleAdd(targetVelocity, velMultiplier, FNeg(scaledBias));
+		const FloatV unbiasedErr = FScaleAdd(targetVelocity, velMultiplier, FSel(isGreater2, zero, FNeg(FMax(scaledBias, zero))));
+		//const FloatV unbiasedErr = FScaleAdd(targetVelocity, velMultiplier, FNeg(FMax(scaledBias, zero)));
+
+		FStore(velMultiplier, &solverContact.velMultiplier);
+		FStore(biasedErr, &solverContact.biasedErr);
+		FStore(unbiasedErr, &solverContact.unbiasedErr);
+		solverContact.maxImpulse = contact.maxImpulse;
+
+		solverContact.raXn = raXnSqrtInertia;
+		solverContact.rbXn = rbXnSqrtInertia;
+	}
+}
+}
+
+#endif //DY_CONTACT_PREP_SHARED_H