PhysX 3.4 / APEX 1.4 release candidate @21506124

7 files changed, 187 insertions, 226 deletions

diff --git a/PhysX_3.4/Source/LowLevel/common/include/pipeline/PxcNpBatch.h b/PhysX_3.4/Source/LowLevel/common/include/pipeline/PxcNpBatch.h
index fe243aed..f5fcd071 100644
--- a/PhysX_3.4/Source/LowLevel/common/include/pipeline/PxcNpBatch.h
+++ b/PhysX_3.4/Source/LowLevel/common/include/pipeline/PxcNpBatch.h
@@ -58,8 +58,8 @@ namespace Gu
 	class PxgGpuNarrowphaseCoreInterface;
 }
 
-void PxcDiscreteNarrowPhase(PxcNpThreadContext& context, PxcNpWorkUnit& cmInput, Gu::Cache& cache, PxsContactManagerOutput& output);
-void PxcDiscreteNarrowPhasePCM(PxcNpThreadContext& context, PxcNpWorkUnit& cmInput, Gu::Cache& cache, PxsContactManagerOutput& output);
+void PxcDiscreteNarrowPhase(PxcNpThreadContext& context, const PxcNpWorkUnit& cmInput, Gu::Cache& cache, PxsContactManagerOutput& output);
+void PxcDiscreteNarrowPhasePCM(PxcNpThreadContext& context, const PxcNpWorkUnit& cmInput, Gu::Cache& cache, PxsContactManagerOutput& output);
 }
 
 #endif
diff --git a/PhysX_3.4/Source/LowLevel/common/include/pipeline/PxcNpContactPrepShared.h b/PhysX_3.4/Source/LowLevel/common/include/pipeline/PxcNpContactPrepShared.h
index 4fc9c122..bdaabd7e 100644
--- a/PhysX_3.4/Source/LowLevel/common/include/pipeline/PxcNpContactPrepShared.h
+++ b/PhysX_3.4/Source/LowLevel/common/include/pipeline/PxcNpContactPrepShared.h
@@ -46,8 +46,6 @@ namespace Gu
 
 static const PxReal PXC_SAME_NORMAL = 0.999f; //Around 6 degrees
 
-bool finishContacts(PxcNpWorkUnit& input, PxsContactManagerOutput& npOutput, PxcNpThreadContext& threadContext, PxsMaterialInfo* pMaterialInfo, const bool isMeshType);
-
 PxU32 writeCompressedContact(const Gu::ContactPoint* const PX_RESTRICT contactPoints, const PxU32 numContactPoints, PxcNpThreadContext* threadContext,
 									PxU8& writtenContactCount, PxU8*& outContactPatches, PxU8*& outContactPoints, PxU16& compressedContactSize, PxReal*& contactForces, PxU32 contactForceByteSize,
 									const PxsMaterialManager* materialManager, bool hasModifiableContacts, bool forceNoResponse, PxsMaterialInfo* PX_RESTRICT pMaterial, PxU8& numPatches,
diff --git a/PhysX_3.4/Source/LowLevel/common/src/pipeline/PxcNpBatch.cpp b/PhysX_3.4/Source/LowLevel/common/src/pipeline/PxcNpBatch.cpp
index 7ed87b05..4a1e5a6e 100644
--- a/PhysX_3.4/Source/LowLevel/common/src/pipeline/PxcNpBatch.cpp
+++ b/PhysX_3.4/Source/LowLevel/common/src/pipeline/PxcNpBatch.cpp
@@ -167,9 +167,7 @@ static bool copyBuffers(PxsContactManagerOutput& cmOutput, Gu::Cache& cache, Pxc
 		}
 
 		if(forceSize)
-		{
 			PxMemZero(forceBuffer, forceSize);
-		}
 		
 		cmOutput.contactPatches= contactPatches;
 		cmOutput.contactPoints = contactPoints;
@@ -198,19 +196,81 @@ static bool copyBuffers(PxsContactManagerOutput& cmOutput, Gu::Cache& cache, Pxc
 	return ret;
 }
 
-void physx::PxcDiscreteNarrowPhase(PxcNpThreadContext& context, PxcNpWorkUnit& input, Gu::Cache& cache, PxsContactManagerOutput& output)
+//ML: isMeshType is used in the GPU codepath. If the collision pair is mesh/heightfield vs primitives, we need to allocate enough memory for the mForceAndIndiceStreamPool in the threadContext. 
+static bool finishContacts(const PxcNpWorkUnit& input, PxsContactManagerOutput& npOutput, PxcNpThreadContext& threadContext, PxsMaterialInfo* PX_RESTRICT pMaterials, const bool isMeshType)
 {
-	//ML : if user doesn't raise the eDETECT_DISCRETE_CONTACT, we should not generate contacts
-	if(!(input.flags & PxcNpWorkUnitFlag::eDETECT_DISCRETE_CONTACT))
-		return;
+	ContactBuffer& buffer = threadContext.mContactBuffer;
 
-	PxGeometryType::Enum type0 = static_cast<PxGeometryType::Enum>(input.geomType0);
-	PxGeometryType::Enum type1 = static_cast<PxGeometryType::Enum>(input.geomType1);
+	PX_ASSERT((npOutput.statusFlag & PxsContactManagerStatusFlag::eTOUCH_KNOWN) != PxsContactManagerStatusFlag::eTOUCH_KNOWN);
+	PxU8 statusFlags = PxU16(npOutput.statusFlag & (~PxsContactManagerStatusFlag::eTOUCH_KNOWN));
+	if (buffer.count != 0)
+		statusFlags |= PxsContactManagerStatusFlag::eHAS_TOUCH;
+	else
+		statusFlags |= PxsContactManagerStatusFlag::eHAS_NO_TOUCH;
 
-	const bool flip = (type1<type0);
+	npOutput.nbContacts = Ps::to8(buffer.count);
 
-	const PxsCachedTransform* cachedTransform0 = &context.mTransformCache->getTransformCache(input.mTransformCache0);
-	const PxsCachedTransform* cachedTransform1 = &context.mTransformCache->getTransformCache(input.mTransformCache1);
+	if(buffer.count==0)
+	{
+		npOutput.statusFlag = statusFlags;
+		npOutput.nbContacts = 0;
+		npOutput.nbPatches = 0;
+		return true;
+	}
+
+#if PX_ENABLE_SIM_STATS
+	if(buffer.count)
+		threadContext.mNbDiscreteContactPairsWithContacts++;
+#endif
+
+	npOutput.statusFlag = statusFlags;
+
+	PxU32 contactForceByteSize = buffer.count * sizeof(PxReal);
+
+	//Regardless of the flags, we need to now record the compressed contact stream
+
+	PxU16 compressedContactSize;
+
+	const bool createReports =
+		input.flags & PxcNpWorkUnitFlag::eOUTPUT_CONTACTS
+		|| threadContext.mCreateContactStream
+		|| (input.flags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD);
+
+	if(!buffer.count || (!isMeshType && !createReports))
+		contactForceByteSize = 0;
+
+	bool res = (writeCompressedContact(buffer.contacts, buffer.count, &threadContext, npOutput.nbContacts, npOutput.contactPatches, npOutput.contactPoints, compressedContactSize,
+		reinterpret_cast<PxReal*&>(npOutput.contactForces), contactForceByteSize, threadContext.mMaterialManager, ((input.flags & PxcNpWorkUnitFlag::eMODIFIABLE_CONTACT) != 0), 
+		false, pMaterials, npOutput.nbPatches, 0, NULL, NULL, threadContext.mCreateAveragePoint, threadContext.mContactStreamPool, 
+		threadContext.mPatchStreamPool, threadContext.mForceAndIndiceStreamPool, isMeshType) != 0) || (buffer.count == 0);
+
+	//handle buffer overflow
+	if (buffer.count && !npOutput.nbContacts)
+	{
+		PxU8 thisStatusFlags = PxU16(npOutput.statusFlag & (~PxsContactManagerStatusFlag::eTOUCH_KNOWN));
+		thisStatusFlags |= PxsContactManagerStatusFlag::eHAS_NO_TOUCH;
+
+		npOutput.statusFlag = thisStatusFlags;
+		npOutput.nbContacts = 0;
+		npOutput.nbPatches = 0;
+#if PX_ENABLE_SIM_STATS
+		if(buffer.count)
+			threadContext.mNbDiscreteContactPairsWithContacts--;
+#endif
+	}
+	return res;
+}
+
+template<bool useContactCacheT>
+static PX_FORCE_INLINE bool checkContactsMustBeGenerated(PxcNpThreadContext& context, const PxcNpWorkUnit& input, Gu::Cache& cache, PxsContactManagerOutput& output,
+										 const PxsCachedTransform* cachedTransform0, const PxsCachedTransform* cachedTransform1,
+										 const bool flip, PxGeometryType::Enum type0, PxGeometryType::Enum type1)
+{
+	PX_ASSERT(cachedTransform0->transform.isSane() && cachedTransform1->transform.isSane());
+
+	//ML : if user doesn't raise the eDETECT_DISCRETE_CONTACT, we should not generate contacts
+	if(!(input.flags & PxcNpWorkUnitFlag::eDETECT_DISCRETE_CONTACT))
+		return false;
 
 	if(!(output.statusFlag & PxcNpWorkUnitStatusFlag::eDIRTY_MANAGER) && !(input.flags & PxcNpWorkUnitFlag::eMODIFIABLE_CONTACT))
 	{
@@ -225,7 +285,7 @@ void physx::PxcDiscreteNarrowPhase(PxcNpThreadContext& context, PxcNpWorkUnit& i
 			if(flip)
 				Ps::swap(type0, type1);
 
-			const bool useContactCache = context.mContactCache && g_CanUseContactCache[type0][type1];
+			const bool useContactCache = useContactCacheT ? context.mContactCache && g_CanUseContactCache[type0][type1] : false;
 			
 #if PX_ENABLE_SIM_STATS
 			if(output.nbContacts)
@@ -233,12 +293,34 @@ void physx::PxcDiscreteNarrowPhase(PxcNpThreadContext& context, PxcNpWorkUnit& i
 #endif
 			const bool isMeshType = type1 > PxGeometryType::eCONVEXMESH;
 			copyBuffers(output, cache, context, useContactCache, isMeshType);
-			return;
+			return false;
 		}
 	}
 
 	output.statusFlag &= (~PxcNpWorkUnitStatusFlag::eDIRTY_MANAGER);
 
+	const PxReal contactDist0 = context.mContactDistance[input.mTransformCache0];
+	const PxReal contactDist1 = context.mContactDistance[input.mTransformCache1];
+	//context.mNarrowPhaseParams.mContactDistance = shape0->contactOffset + shape1->contactOffset;
+	context.mNarrowPhaseParams.mContactDistance = contactDist0 + contactDist1;
+
+	return true;
+}
+
+template<bool useLegacyCodepath>
+static PX_FORCE_INLINE void discreteNarrowPhase(PxcNpThreadContext& context, const PxcNpWorkUnit& input, Gu::Cache& cache, PxsContactManagerOutput& output)
+{
+	PxGeometryType::Enum type0 = static_cast<PxGeometryType::Enum>(input.geomType0);
+	PxGeometryType::Enum type1 = static_cast<PxGeometryType::Enum>(input.geomType1);
+
+	const bool flip = (type1<type0);
+
+	const PxsCachedTransform* cachedTransform0 = &context.mTransformCache->getTransformCache(input.mTransformCache0);
+	const PxsCachedTransform* cachedTransform1 = &context.mTransformCache->getTransformCache(input.mTransformCache1);
+
+	if(!checkContactsMustBeGenerated<useLegacyCodepath>(context, input, cache, output, cachedTransform0, cachedTransform1, flip, type0, type1))
+		return;
+
 	PxsShapeCore* shape0 = const_cast<PxsShapeCore*>(input.shapeCore0);
 	PxsShapeCore* shape1 = const_cast<PxsShapeCore*>(input.shapeCore1);
 
@@ -249,142 +331,70 @@ void physx::PxcDiscreteNarrowPhase(PxcNpThreadContext& context, PxcNpWorkUnit& i
 		Ps::swap(cachedTransform0, cachedTransform1);
 	}
 
-	// PT: many cache misses here...
-	// PT: TODO: refactor this change with runNpBatchPPU
-	
-	Ps::prefetchLine(shape1, 0);	// PT: at least get rid of L2s for shape1
-
-	const PxTransform* tm0 = &cachedTransform0->transform;
-	const PxTransform* tm1 = &cachedTransform1->transform;
-	PX_ASSERT(tm0->isSane() && tm1->isSane());
-
-	updateDiscreteContactStats(context, type0, type1);
-
-	const PxReal contactDist0 = context.mContactDistance[input.mTransformCache0];
-	const PxReal contactDist1 = context.mContactDistance[input.mTransformCache1];
-	//context.mNarrowPhaseParams.mContactDistance = shape0->contactOffset + shape1->contactOffset;
-	context.mNarrowPhaseParams.mContactDistance = contactDist0 + contactDist1;
-
-	startContacts(output, context);
+	PxsMaterialInfo materialInfo[ContactBuffer::MAX_CONTACTS];
 
-	const PxcContactMethod conMethod = g_ContactMethodTable[type0][type1];
-	PX_ASSERT(conMethod);
+	Gu::MultiplePersistentContactManifold& manifold = context.mTempManifold;
+	bool isMultiManifold = false;
 
-	const bool useContactCache = context.mContactCache && g_CanUseContactCache[type0][type1];
-	if(useContactCache)
-	{
-#if PX_ENABLE_SIM_STATS
-		if(PxcCacheLocalContacts(context, cache, *tm0, *tm1, conMethod, shape0->geometry, shape1->geometry))
-			context.mNbDiscreteContactPairsWithCacheHits++;
-#else
-		PxcCacheLocalContacts(context, n.pairCache, *tm0, *tm1, conMethod, shape0->geometry, shape1->geometry);
-#endif
-	}
-	else
+	if(!useLegacyCodepath)
 	{
-		conMethod(shape0->geometry, shape1->geometry, *tm0, *tm1, context.mNarrowPhaseParams, cache, context.mContactBuffer, &context.mRenderOutput);
+		if(cache.isMultiManifold())
+		{
+			//We are using a multi-manifold. This is cached in a reduced npCache...
+			isMultiManifold = true;
+			uintptr_t address = uintptr_t(&cache.getMultipleManifold());
+			manifold.fromBuffer(reinterpret_cast<PxU8*>(address));
+			cache.setMultiManifold(&manifold);
+		}
+		else if(cache.isManifold())
+		{
+			void* address = reinterpret_cast<void*>(&cache.getManifold());
+			Ps::prefetch(address);
+			Ps::prefetch(address, 128);
+			Ps::prefetch(address, 256);
+		}
 	}
 
-	PxsMaterialInfo materialInfo[ContactBuffer::MAX_CONTACTS];
-
-	const PxcGetMaterialMethod materialMethod = g_GetMaterialMethodTable[type0][type1];
-	PX_ASSERT(materialMethod);
-
-	materialMethod(shape0, shape1, context, materialInfo);
-
-	if(flip)
-		flipContacts(context, materialInfo);
-
-	const bool isMeshType = type1 > PxGeometryType::eCONVEXMESH; 
-	finishContacts(input, output, context, materialInfo, isMeshType);
-}
-
-void physx::PxcDiscreteNarrowPhasePCM(PxcNpThreadContext& context, PxcNpWorkUnit& cmInput, Gu::Cache& cache, PxsContactManagerOutput& output)
-{
-	//ML : if user doesn't raise the eDETECT_DISCRETE_CONTACT, we should not generate contacts
-	if(!(cmInput.flags & PxcNpWorkUnitFlag::eDETECT_DISCRETE_CONTACT))
-		return;
-	
-	PxGeometryType::Enum type0 = static_cast<PxGeometryType::Enum>(cmInput.geomType0);
-	PxGeometryType::Enum type1 = static_cast<PxGeometryType::Enum>(cmInput.geomType1);
+	updateDiscreteContactStats(context, type0, type1);
 
-	const bool flip = type1<type0;
+	startContacts(output, context);
 
-	const PxsCachedTransform* tm0 = &context.mTransformCache->getTransformCache(cmInput.mTransformCache0);
-	const PxsCachedTransform* tm1 = &context.mTransformCache->getTransformCache(cmInput.mTransformCache1);
+	const PxTransform* tm0 = &cachedTransform0->transform;
+	const PxTransform* tm1 = &cachedTransform1->transform;
+	PX_ASSERT(tm0->isSane() && tm1->isSane());
 
-	if(!(output.statusFlag & PxcNpWorkUnitStatusFlag::eDIRTY_MANAGER) && !(cmInput.flags & PxcNpWorkUnitFlag::eMODIFIABLE_CONTACT))
+	if(useLegacyCodepath)
 	{
-		const PxU32 body0Dynamic = PxU32(cmInput.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY0);
-		const PxU32 body1Dynamic = PxU32(cmInput.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1);
+		// PT: many cache misses here...
+	
+		Ps::prefetchLine(shape1, 0);	// PT: at least get rid of L2s for shape1
 
-		const PxU32 active0 = PxU32(body0Dynamic && !(tm0->isFrozen()));
-		const PxU32 active1 = PxU32(body1Dynamic && !(tm1->isFrozen()));
+		const PxcContactMethod conMethod = g_ContactMethodTable[type0][type1];
+		PX_ASSERT(conMethod);
 
-		if(!(active0 || active1))
+		const bool useContactCache = context.mContactCache && g_CanUseContactCache[type0][type1];
+		if(useContactCache)
 		{
-			if(flip)
-				Ps::swap(type0, type1);
-
 #if PX_ENABLE_SIM_STATS
-			if(output.nbContacts)
-				context.mNbDiscreteContactPairsWithContacts++;
+			if(PxcCacheLocalContacts(context, cache, *tm0, *tm1, conMethod, shape0->geometry, shape1->geometry))
+				context.mNbDiscreteContactPairsWithCacheHits++;
+#else
+			PxcCacheLocalContacts(context, n.pairCache, *tm0, *tm1, conMethod, shape0->geometry, shape1->geometry);
 #endif
-			const bool isMeshType = type1 > PxGeometryType::eCONVEXMESH; 
-			copyBuffers(output, cache, context, false, isMeshType);
-			return;
+		}
+		else
+		{
+			conMethod(shape0->geometry, shape1->geometry, *tm0, *tm1, context.mNarrowPhaseParams, cache, context.mContactBuffer, &context.mRenderOutput);
 		}
 	}
-
-	output.statusFlag &= (~PxcNpWorkUnitStatusFlag::eDIRTY_MANAGER);
-
-	Gu::MultiplePersistentContactManifold& manifold = context.mTempManifold;
-	bool isMultiManifold = false;
-
-	if(cache.isMultiManifold())
-	{
-		//We are using a multi-manifold. This is cached in a reduced npCache...
-		isMultiManifold = true;
-		uintptr_t address = uintptr_t(&cache.getMultipleManifold());
-		manifold.fromBuffer(reinterpret_cast<PxU8*>(address));
-		cache.setMultiManifold(&manifold);
-	}
-	else if(cache.isManifold())
+	else
 	{
-		void* address = reinterpret_cast<void*>(&cache.getManifold());
-		Ps::prefetch(address);
-		Ps::prefetch(address, 128);
-		Ps::prefetch(address, 256);
-	}
-
-	PxsShapeCore* shape0 = const_cast<PxsShapeCore*>(cmInput.shapeCore0);
-	PxsShapeCore* shape1 = const_cast<PxsShapeCore*>(cmInput.shapeCore1);
+		const PxcContactMethod conMethod = g_PCMContactMethodTable[type0][type1];
+		PX_ASSERT(conMethod);
 
-	if(flip)
-	{
-		Ps::swap(tm0, tm1);
-		Ps::swap(shape0, shape1);
-		Ps::swap(type0, type1);
+		conMethod(shape0->geometry, shape1->geometry, *tm0, *tm1, context.mNarrowPhaseParams, cache, context.mContactBuffer, &context.mRenderOutput);
 	}
 
-	const PxReal contactDist0 = context.mContactDistance[cmInput.mTransformCache0];
-	const PxReal contactDist1 = context.mContactDistance[cmInput.mTransformCache1];
-//	context.mNarrowPhaseParams.mContactDistance = shape0->contactOffset + shape1->contactOffset;
-	context.mNarrowPhaseParams.mContactDistance = contactDist0 + contactDist1;
-
-	PX_ASSERT(tm0->transform.isSane() && tm1->transform.isSane());
-
-	updateDiscreteContactStats(context, type0, type1);
-
-	const PxcContactMethod conMethod = g_PCMContactMethodTable[type0][type1];
-	PX_ASSERT(conMethod);
-
-	startContacts(output, context);
-
-	conMethod(shape0->geometry, shape1->geometry, tm0->transform, tm1->transform, context.mNarrowPhaseParams, cache, context.mContactBuffer, &context.mRenderOutput);
-	
-	PxsMaterialInfo materialInfo[ContactBuffer::MAX_CONTACTS];
-
 	const PxcGetMaterialMethod materialMethod = g_GetMaterialMethodTable[type0][type1];
 	PX_ASSERT(materialMethod);
 
@@ -393,21 +403,34 @@ void physx::PxcDiscreteNarrowPhasePCM(PxcNpThreadContext& context, PxcNpWorkUnit
 	if(flip)
 		flipContacts(context, materialInfo);
 
-	if(isMultiManifold)
+	if(!useLegacyCodepath)
 	{
-		//Store the manifold back...
-		const PxU32 size = (sizeof(MultiPersistentManifoldHeader) +
-			manifold.mNumManifolds * sizeof(SingleManifoldHeader) +
-			manifold.mNumTotalContacts * sizeof(Gu::CachedMeshPersistentContact));
+		if(isMultiManifold)
+		{
+			//Store the manifold back...
+			const PxU32 size = (sizeof(MultiPersistentManifoldHeader) +
+				manifold.mNumManifolds * sizeof(SingleManifoldHeader) +
+				manifold.mNumTotalContacts * sizeof(Gu::CachedMeshPersistentContact));
 
-		PxU8* buffer = context.mNpCacheStreamPair.reserve(size);
+			PxU8* buffer = context.mNpCacheStreamPair.reserve(size);
 
-		PX_ASSERT((reinterpret_cast<uintptr_t>(buffer)& 0xf) == 0);
-		manifold.toBuffer(buffer);
-		cache.setMultiManifold(buffer);
-		cache.mCachedSize = Ps::to16(size);
+			PX_ASSERT((reinterpret_cast<uintptr_t>(buffer)& 0xf) == 0);
+			manifold.toBuffer(buffer);
+			cache.setMultiManifold(buffer);
+			cache.mCachedSize = Ps::to16(size);
+		}
 	}
 
-	const bool isMeshType = type1 > PxGeometryType::eCONVEXMESH;
-	finishContacts(cmInput, output, context, materialInfo, isMeshType);
+	const bool isMeshType = type1 > PxGeometryType::eCONVEXMESH; 
+	finishContacts(input, output, context, materialInfo, isMeshType);
+}
+
+void physx::PxcDiscreteNarrowPhase(PxcNpThreadContext& context, const PxcNpWorkUnit& input, Gu::Cache& cache, PxsContactManagerOutput& output)
+{
+	discreteNarrowPhase<true>(context, input, cache, output);
+}
+
+void physx::PxcDiscreteNarrowPhasePCM(PxcNpThreadContext& context, const PxcNpWorkUnit& input, Gu::Cache& cache, PxsContactManagerOutput& output)
+{
+	discreteNarrowPhase<false>(context, input, cache, output);
 }
diff --git a/PhysX_3.4/Source/LowLevel/common/src/pipeline/PxcNpContactPrepShared.cpp b/PhysX_3.4/Source/LowLevel/common/src/pipeline/PxcNpContactPrepShared.cpp
index e0fa6262..466a2d45 100644
--- a/PhysX_3.4/Source/LowLevel/common/src/pipeline/PxcNpContactPrepShared.cpp
+++ b/PhysX_3.4/Source/LowLevel/common/src/pipeline/PxcNpContactPrepShared.cpp
@@ -552,72 +552,3 @@ PxU32 physx::writeCompressedContact(const Gu::ContactPoint* const PX_RESTRICT co
 
 	return totalRequiredSize;
 }
-
-//ML: isMeshType is used in the GPU codepath. If the collision pair is mesh/heightfield vs primitives, we need to allocate enough memory for the mForceAndIndiceStreamPool in the threadContext. 
-bool physx::finishContacts(PxcNpWorkUnit& input, PxsContactManagerOutput& npOutput, PxcNpThreadContext& threadContext, PxsMaterialInfo* PX_RESTRICT pMaterials, const bool isMeshType)
-{
-	ContactBuffer& buffer = threadContext.mContactBuffer;
-
-	PX_ASSERT((npOutput.statusFlag & PxsContactManagerStatusFlag::eTOUCH_KNOWN) != PxsContactManagerStatusFlag::eTOUCH_KNOWN);
-	PxU8 statusFlags = PxU16(npOutput.statusFlag & (~PxsContactManagerStatusFlag::eTOUCH_KNOWN));
-	if (buffer.count != 0)
-		statusFlags |= PxsContactManagerStatusFlag::eHAS_TOUCH;
-	else
-		statusFlags |= PxsContactManagerStatusFlag::eHAS_NO_TOUCH;
-
-	npOutput.nbContacts = Ps::to8(buffer.count);
-
-	if(buffer.count==0)
-	{
-		npOutput.statusFlag = statusFlags;
-		npOutput.nbContacts = 0;
-		npOutput.nbPatches = 0;
-		return true;
-	}
-
-	
-#if PX_ENABLE_SIM_STATS
-	if(buffer.count)
-		threadContext.mNbDiscreteContactPairsWithContacts++;
-#endif
-
-	npOutput.statusFlag = statusFlags;
-
-	PxU32 contactForceByteSize = buffer.count * sizeof(PxReal);
-
-	//Regardless of the flags, we need to now record the compressed contact stream
-
-	PxU16 compressedContactSize;
-
-	const bool createReports =
-		input.flags & PxcNpWorkUnitFlag::eOUTPUT_CONTACTS
-		|| threadContext.mCreateContactStream
-		|| (input.flags & PxcNpWorkUnitFlag::eFORCE_THRESHOLD);
-
-	if (!buffer.count || (!isMeshType && !createReports))
-	{
-		contactForceByteSize = 0;
-	}
-
-	bool res = (writeCompressedContact(buffer.contacts, buffer.count, &threadContext, npOutput.nbContacts, npOutput.contactPatches, npOutput.contactPoints, compressedContactSize,
-		reinterpret_cast<PxReal*&>(npOutput.contactForces), contactForceByteSize, threadContext.mMaterialManager, ((input.flags & PxcNpWorkUnitFlag::eMODIFIABLE_CONTACT) != 0), 
-		false, pMaterials, npOutput.nbPatches, 0, NULL, NULL, threadContext.mCreateAveragePoint, threadContext.mContactStreamPool, 
-		threadContext.mPatchStreamPool, threadContext.mForceAndIndiceStreamPool, isMeshType) != 0) || (buffer.count == 0);
-
-	//handle buffer overflow
-	if (buffer.count && !npOutput.nbContacts)
-	{
-		PxU8 thisStatusFlags = PxU16(npOutput.statusFlag & (~PxsContactManagerStatusFlag::eTOUCH_KNOWN));
-		thisStatusFlags |= PxsContactManagerStatusFlag::eHAS_NO_TOUCH;
-
-		npOutput.statusFlag = thisStatusFlags;
-		npOutput.nbContacts = 0;
-		npOutput.nbPatches = 0;
-#if PX_ENABLE_SIM_STATS
-		if(buffer.count)
-			threadContext.mNbDiscreteContactPairsWithContacts--;
-#endif
-	}
-
-	return res;
-}
diff --git a/PhysX_3.4/Source/LowLevel/software/include/PxsRigidBody.h b/PhysX_3.4/Source/LowLevel/software/include/PxsRigidBody.h
index 0bbfc8ec..c63b33a1 100644
--- a/PhysX_3.4/Source/LowLevel/software/include/PxsRigidBody.h
+++ b/PhysX_3.4/Source/LowLevel/software/include/PxsRigidBody.h
@@ -95,16 +95,16 @@ class PxsRigidBody : public PxcRigidBody
 															return axis * angle * 1.0f/dt;
 													}
 
-	PX_FORCE_INLINE		PxTransform					getLastCCDTransform()					const	{ return mLastTransform; }
+	PX_FORCE_INLINE		const PxTransform&			getLastCCDTransform()					const	{ return mLastTransform;}
 	PX_FORCE_INLINE		void						saveLastCCDTransform()							{ mLastTransform = mCore->body2World; }
 
 	PX_FORCE_INLINE		bool						isKinematic()							const	{ return (mCore->inverseMass == 0.0f); }
 						
 	PX_FORCE_INLINE		void						setPose(const PxTransform& pose)				{ mCore->body2World = pose; }
 	PX_FORCE_INLINE		void						setPosition(const PxVec3& position)				{ mCore->body2World.p = position; }
-	PX_FORCE_INLINE		PxReal						getInvMass()							const	{ return mCore->inverseMass; }	
+	PX_FORCE_INLINE		PxReal						getInvMass()							const	{ return mCore->inverseMass; }
 	PX_FORCE_INLINE		PxVec3						getInvInertia()							const	{ return mCore->inverseInertia; }
-	PX_FORCE_INLINE		PxReal						getMass()								const	{ return 1.0f/mCore->inverseMass; }	
+	PX_FORCE_INLINE		PxReal						getMass()								const	{ return 1.0f/mCore->inverseMass; }
 	PX_FORCE_INLINE		PxVec3						getInertia()							const	{ return PxVec3(1.0f/mCore->inverseInertia.x,
 																													1.0f/mCore->inverseInertia.y,
 																													1.0f/mCore->inverseInertia.z); }
diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsCCD.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsCCD.cpp
index 4dd7d182..22069680 100644
--- a/PhysX_3.4/Source/LowLevel/software/src/PxsCCD.cpp
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsCCD.cpp
@@ -623,7 +623,7 @@ bool PxsCCDPair::sweepAdvanceToToi(PxReal dt, bool clipTrajectoryToToi)
 			physx::Cm::transformInertiaTensor(atom0->mCore->inverseInertia, PxMat33(trA.q),invInertia0);
 			invInertia0 *= dom0;
 #else
-			v0 = atom0->mCore->linearVelocity + atom0->mCore->angularVelocity.cross(ccds0->mShapeCore->transform.p);
+			v0 = atom0->mCore->linearVelocity + atom0->mCore->angularVelocity.cross(ccds0->mCurrentTransform.p - atom0->mCore->body2World.p);
 #endif
 			invMass0 = atom0->getInvMass() * dom0;
 			
@@ -632,14 +632,16 @@ bool PxsCCDPair::sweepAdvanceToToi(PxReal dt, bool clipTrajectoryToToi)
 		//Work out velocity and invMass for body 1
 		if(atom1)
 		{
+			
 			//Put contact point in local space, then find how much point is moving using point velocity...
 #if CCD_ANGULAR_IMPULSE
+			
 			localPoint1 = mMinToiPoint - trB.p;
 			v1 = atom1->mCore->linearVelocity + atom1->mCore->angularVelocity.cross(localPoint1);
 			physx::Cm::transformInertiaTensor(atom1->mCore->inverseInertia, PxMat33(trB.q),invInertia1);
 			invInertia1 *= dom1;
 #else
-			v1 = atom1->mCore->linearVelocity + atom1->mCore->angularVelocity.cross(ccds1->mShapeCore->transform.p);
+			v1 = atom1->mCore->linearVelocity + atom1->mCore->angularVelocity.cross(ccds1->mCurrentTransform.p - atom1->mCore->body2World.p);
 #endif
 			invMass1 = atom1->getInvMass() * dom1;
 		}
@@ -943,6 +945,7 @@ public:
 			// --------------------------------------------------------------------------------------
 			// sort all pairs within current island by toi
 			PxU32 islandEnd = islandStart+1;
+			PX_ASSERT(mCCDPairs[islandStart]->mIslandId == iIsland);
 			while (islandEnd < mNumPairs && mCCDPairs[islandEnd]->mIslandId == iIsland) // find first index past the current island id
 				islandEnd++;
 
@@ -1003,6 +1006,9 @@ public:
 						}
 					}
 
+					if (pair.mMinToi > 1.f)
+						break;
+
 					//We now have the earliest contact pair for this island and one/both of the bodies have not been updated. We now perform
 					//contact modification to find out if the user still wants to respond to the collision
 					if(pair.mMinToi <= islandMinToi && 
@@ -1584,8 +1590,8 @@ void PxsCCDContext::updateCCD(PxReal dt, PxBaseTask* continuation, bool disableR
 	
 	for (PxU32 j = 0; j < ccdBodyCount; j++)
 	{
-		//If the body has already been labelled, continue
-		if (islandLabels[j] != noLabelYet)
+		//If the body has already been labelled or if it is kinematic, continue
+		if (islandLabels[j] != noLabelYet || mCCDBodies[j].mBody->isKinematic())
 			continue;
 
 		top = &mCCDBodies[j];
@@ -1657,9 +1663,12 @@ void PxsCCDContext::updateCCD(PxReal dt, PxBaseTask* continuation, bool disableR
 	for(PxU32 a = 0; a < mCCDBodies.size(); ++a)
 	{
 		const PxU32 island = islandLabels[mCCDBodies[a].mIndex];
-		PxU16 writeIndex = mIslandSizes[island];
-		mIslandSizes[island] = PxU16(writeIndex + 1);
-		mIslandBodies[writeIndex] = &mCCDBodies[a];
+		if (island != 0xFFFF)
+		{
+			PxU16 writeIndex = mIslandSizes[island];
+			mIslandSizes[island] = PxU16(writeIndex + 1);
+			mIslandBodies[writeIndex] = &mCCDBodies[a];
+		}
 	}
 
 	// --------------------------------------------------------------------------------------
diff --git a/PhysX_3.4/Source/LowLevel/software/src/PxsNphaseImplementationContext.cpp b/PhysX_3.4/Source/LowLevel/software/src/PxsNphaseImplementationContext.cpp
index e7bcd737..024e87b3 100644
--- a/PhysX_3.4/Source/LowLevel/software/src/PxsNphaseImplementationContext.cpp
+++ b/PhysX_3.4/Source/LowLevel/software/src/PxsNphaseImplementationContext.cpp
@@ -343,7 +343,7 @@ public:
 	}
 
 
-	template < void (*NarrowPhase)(PxcNpThreadContext&, PxcNpWorkUnit&, Gu::Cache&, PxsContactManagerOutput&)>
+	template < void (*NarrowPhase)(PxcNpThreadContext&, const PxcNpWorkUnit&, Gu::Cache&, PxsContactManagerOutput&)>
 	void processCms(PxcNpThreadContext* threadContext)
 	{
 		// PT: use local variables to avoid reading class members N times, if possible
@@ -790,7 +790,7 @@ void PxsNphaseImplementationContext::appendContactManagers()
 void PxsNphaseImplementationContext::appendContactManagersFallback(PxsContactManagerOutput* cmOutputs)
 {
 	PX_PROFILE_ZONE("PxsNphaseImplementationContext.appendContactManagersFallback", mContext.mContextID);
-	//Copy new pairs to end of old pairs. Clear new flag, update npIndex on CM and clear the new pair buffer
+
 	//Copy new pairs to end of old pairs. Clear new flag, update npIndex on CM and clear the new pair buffer
 	const PxU32 existingSize = mNarrowPhasePairs.mContactManagerMapping.size();
 	const PxU32 nbToAdd = mNewNarrowPhasePairs.mContactManagerMapping.size();