text: // 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-2018 NVIDIA Corporation. All rights reserved.
  // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
  // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
  
  
  #include "foundation/PxPreprocessor.h"
  #include "PsMathUtils.h"
  #include "PxcNpWorkUnit.h"
  #include "PxvDynamics.h"
  
  using namespace physx;
  using namespace Gu;
  
  #include "PxsMaterialManager.h"
  #include "PxsMaterialCombiner.h"
  
  #include "PxcNpContactPrepShared.h"
  #include "PsAtomic.h"
  #include "PxsContactManagerState.h"
  
  #include "PsVecMath.h"
  using namespace physx;
  using namespace Ps::aos;
  
  static PX_FORCE_INLINE void copyContactPoint(PxContact* PX_RESTRICT point, const Gu::ContactPoint* PX_RESTRICT cp)
  {
  	// PT: TODO: consider moving "separation" right after "point" in both structures, to copy both at the same time.
  //	point->contact = cp->point;
  	const Vec4V contactV = V4LoadA(&cp->point.x);	// PT: V4LoadA safe because 'point' is aligned.
  	V4StoreU(contactV, &point->contact.x);
  
  	point->separation = cp->separation;
  }
  
  struct StridePatch
  {
  	PxU8 startIndex;
  	PxU8 endIndex;
  	PxU8 nextIndex;
  	PxU8 totalCount;
  	bool isRoot;
  };
  
  PxU32 physx::writeCompressedContact(const Gu::ContactPoint* const PX_RESTRICT contactPoints, const PxU32 numContactPoints, PxcNpThreadContext* threadContext,
  									PxU8& writtenContactCount, PxU8*& outContactPatches, PxU8*& outContactPoints, PxU16& compressedContactSize, PxReal*& outContactForces, PxU32 contactForceByteSize,
  									const PxsMaterialManager* materialManager, bool hasModifiableContacts, bool forceNoResponse, PxsMaterialInfo* PX_RESTRICT pMaterial, PxU8& numPatches,
  									PxU32 additionalHeaderSize,  PxsConstraintBlockManager* manager, PxcConstraintBlockStream* blockStream, bool insertAveragePoint,
  									PxcDataStreamPool* contactStreamPool, PxcDataStreamPool* patchStreamPool, PxcDataStreamPool* forceStreamPool, const bool isMeshType)
  {
  	if(numContactPoints == 0)
  	{
  		writtenContactCount = 0;
  		outContactPatches = NULL;
  		outContactPoints = NULL;
  		outContactForces = NULL;
  		compressedContactSize = 0;
  		numPatches = 0;
  		return 0;
  	}
  
  	//Calculate the size of the contact buffer...
  	PX_ALLOCA(strPatches, StridePatch, numContactPoints);
  
  	StridePatch* stridePatches = &strPatches[0];
  	
  	PxU32 numStrideHeaders = 1;
  
  	/*const bool hasInternalFaceIndex = contactPoints[0].internalFaceIndex0 != PXC_CONTACT_NO_FACE_INDEX ||
  										contactPoints[0].internalFaceIndex1 != PXC_CONTACT_NO_FACE_INDEX;*/
  	const bool isModifiable = !forceNoResponse && hasModifiableContacts;
  
  	PxU32 totalUniquePatches = 1;
  
  	PxU32 totalContactPoints = numContactPoints;
  
  	PxU32 strideStart = 0;
  	bool root = true;
  	StridePatch* parentRootPatch = NULL;
  	{
  		const PxReal closeNormalThresh = PXC_SAME_NORMAL;
  		//Go through and tag how many patches we have...
  		PxVec3 normal = contactPoints[0].normal;
  		PxU16 mat0 = pMaterial[0].mMaterialIndex0;
  		PxU16 mat1 = pMaterial[0].mMaterialIndex1;
  
  		for(PxU32 a = 1; a < numContactPoints; ++a)
  		{
  			if(normal.dot(contactPoints[a].normal) < closeNormalThresh || 
  				pMaterial[a].mMaterialIndex0 != mat0 || pMaterial[a].mMaterialIndex1 != mat1)
  			{
  				StridePatch& patch = stridePatches[numStrideHeaders-1];
  
  				patch.startIndex = PxU8(strideStart);
  				patch.endIndex = PxU8(a);
  				patch.nextIndex = 0xFF;
  				patch.totalCount = PxU8(a - strideStart);
  				patch.isRoot = root;
  				if(parentRootPatch)
  					parentRootPatch->totalCount += PxU8(a - strideStart);
  
  				root = true;
  				parentRootPatch = NULL;
  				for(PxU32 b = 1; b < numStrideHeaders; ++b)
  				{
  					StridePatch& thisPatch = stridePatches[b-1];
  					if(thisPatch.isRoot)
  					{
  						PxU32 ind = thisPatch.startIndex;
  						PxReal dp2 = contactPoints[a].normal.dot(contactPoints[ind].normal);
  						if(dp2 >= closeNormalThresh && pMaterial[a].mMaterialIndex0 == pMaterial[ind].mMaterialIndex0 && 
  							pMaterial[a].mMaterialIndex1 == pMaterial[ind].mMaterialIndex1)
  						{
  							PxU32 nextInd = b-1;
  							while(stridePatches[nextInd].nextIndex != 0xFF)
  								nextInd = stridePatches[nextInd].nextIndex;
  							stridePatches[nextInd].nextIndex = PxU8(numStrideHeaders);
  							root = false;
  							parentRootPatch = &stridePatches[b-1];
  							break;
  						}
  					}
  				}
  
  				normal = contactPoints[a].normal;
  
  				mat0 = pMaterial[a].mMaterialIndex0;
  				mat1 = pMaterial[a].mMaterialIndex1;
  				totalContactPoints = insertAveragePoint && (a - strideStart) > 1 ? totalContactPoints + 1 : totalContactPoints;
  				strideStart = a;
  				numStrideHeaders++;
  				if(root)
  					totalUniquePatches++;
  			}
  		}
  		totalContactPoints = insertAveragePoint &&(numContactPoints - strideStart) > 1 ? totalContactPoints + 1 : totalContactPoints;
  		contactForceByteSize = insertAveragePoint && contactForceByteSize != 0 ? contactForceByteSize + sizeof(PxF32) * (totalContactPoints - numContactPoints) : contactForceByteSize;
  	}
  	{
  		StridePatch& patch = stridePatches[numStrideHeaders-1];
  		patch.startIndex = PxU8(strideStart);
  		patch.endIndex = PxU8(numContactPoints);
  		patch.nextIndex = 0xFF;
  		patch.totalCount = PxU8(numContactPoints - strideStart);
  		patch.isRoot = root;
  		if(parentRootPatch)
  			parentRootPatch->totalCount += PxU8(numContactPoints - strideStart);
  	}
  
  	numPatches = PxU8(totalUniquePatches);
  
  	//Calculate the number of patches/points required
  
  	const PxU32 patchHeaderSize = sizeof(PxContactPatch) * (isModifiable ? totalContactPoints : totalUniquePatches) + additionalHeaderSize;
  	const PxU32 pointSize = totalContactPoints * (isModifiable ? sizeof(PxModifiableContact) : sizeof(PxContact));
  
  	PxU32 requiredContactSize = pointSize;
  	PxU32 requiredPatchSize = patchHeaderSize;
  	PxU32 totalRequiredSize;
  
  	PxU8* PX_RESTRICT contactData = NULL;
  	PxU8* PX_RESTRICT patchData = NULL;
  	PxReal* PX_RESTRICT forceData = NULL;
  	PxU32* PX_RESTRICT triangleIndice = NULL;
  
  	if(contactStreamPool && !isModifiable && additionalHeaderSize == 0) //If the contacts are modifiable, we **DON'T** allocate them in GPU pinned memory. This will be handled later when they're modified
  	{
  		bool isOverflown = false;
  
  		PxU32 contactIndex = PxU32(Ps::atomicAdd(&contactStreamPool->mSharedDataIndex, PxI32(requiredContactSize)));
  		
  		if (contactStreamPool->isOverflown())
  		{
  			PX_WARN_ONCE("Contact buffer overflow detected, please increase its size in the scene desc!\n");
  			isOverflown = true;
  		}
  		
  		contactData = contactStreamPool->mDataStream + contactStreamPool->mDataStreamSize - contactIndex;
  	
  		PxU32 patchIndex = PxU32(Ps::atomicAdd(&patchStreamPool->mSharedDataIndex, PxI32(requiredPatchSize)));
  		
  		if (patchStreamPool->isOverflown())
  		{
  			PX_WARN_ONCE("Patch buffer overflow detected, please increase its size in the scene desc!\n");
  			isOverflown = true;
  		}
  
  		patchData = patchStreamPool->mDataStream + patchStreamPool->mDataStreamSize - patchIndex;
  	
  		if(contactForceByteSize)
  		{
  			contactForceByteSize = isMeshType ? contactForceByteSize * 2 : contactForceByteSize;
  			contactIndex = PxU32(Ps::atomicAdd(&forceStreamPool->mSharedDataIndex, PxI32(contactForceByteSize)));
  			
  			if (forceStreamPool->isOverflown())
  			{
  				PX_WARN_ONCE("Force buffer overflow detected, please increase its size in the scene desc!\n");
  				isOverflown = true;
  			}
  			forceData = reinterpret_cast<PxReal*>(forceStreamPool->mDataStream + forceStreamPool->mDataStreamSize - contactIndex);
  			if (isMeshType)
  				triangleIndice = reinterpret_cast<PxU32*>(forceData + numContactPoints);
  		}
  
  		totalRequiredSize = requiredContactSize + requiredPatchSize;
  
  		if (isOverflown)
  		{
  			patchData = NULL;
  			contactData = NULL;
  			forceData = NULL;
  			triangleIndice = NULL;
  		}
  	}
  	else
  	{
  		PxU32 alignedRequiredSize = (requiredContactSize + requiredPatchSize + 0xf) & 0xfffffff0;
  		contactForceByteSize = (isMeshType ? contactForceByteSize * 2 : contactForceByteSize);
  		PxU32 totalSize = alignedRequiredSize + contactForceByteSize;
  		PxU8* data = manager ? blockStream->reserve(totalSize, *manager) : threadContext->mContactBlockStream.reserve(totalSize);
  
  		patchData = data;
  		contactData = patchData + requiredPatchSize;
  
  		if(contactForceByteSize)
  		{
  			forceData = reinterpret_cast<PxReal*>((data + alignedRequiredSize));
  
  			if (isMeshType)
  				triangleIndice = reinterpret_cast<PxU32*>(forceData + numContactPoints);
  
  			if(data)
  			{
  				PxMemZero(forceData, contactForceByteSize);
  			}
  		}
  
  		totalRequiredSize = alignedRequiredSize;
  
  	}
  	
  	Ps::prefetchLine(patchData);
  	Ps::prefetchLine(contactData);
  
  	if(patchData == NULL)
  	{
  		writtenContactCount = 0;
  		outContactPatches = NULL;
  		outContactPoints = NULL;
  		outContactForces = NULL;
  		compressedContactSize = 0;
  		numPatches = 0;
  		return 0;
  	}
  
  #if PX_ENABLE_SIM_STATS
  	if(threadContext)
  	{
  		threadContext->mCompressedCacheSize += totalRequiredSize;
  		threadContext->mTotalCompressedCacheSize += totalRequiredSize;
  	}
  #endif
  	compressedContactSize = Ps::to16(totalRequiredSize);
  
  	
  
  	//PxU32 startIndex = 0;
  
  	//Extract first material
  	PxU16 origMatIndex0 = pMaterial[0].mMaterialIndex0;
  	PxU16 origMatIndex1 = pMaterial[0].mMaterialIndex1;
  
  	PxReal staticFriction, dynamicFriction, combinedRestitution;
  	PxU32 materialFlags;
  	{
  		const PxsMaterialData& data0 = *materialManager->getMaterial(origMatIndex0);
  		const PxsMaterialData& data1 = *materialManager->getMaterial(origMatIndex1);
  
  		combinedRestitution = PxsMaterialCombiner::combineRestitution(data0, data1);
  		PxsMaterialCombiner combiner(1.0f, 1.0f);
  		PxsMaterialCombiner::PxsCombinedMaterial combinedMat = combiner.combineIsotropicFriction(data0, data1);
  		staticFriction  = combinedMat.staFriction;
  		dynamicFriction = combinedMat.dynFriction;
  		materialFlags = combinedMat.flags;
  	}
  
  
  	PxU8* PX_RESTRICT dataPlusOffset = patchData + additionalHeaderSize;
  	PxContactPatch* PX_RESTRICT patches = reinterpret_cast<PxContactPatch*>(dataPlusOffset);
  	PxU32* PX_RESTRICT faceIndice = triangleIndice;
  
  	outContactPatches = patchData;
  	outContactPoints = contactData;
  	outContactForces = forceData;
  
  	if(isModifiable)
  	{
  		
  		PxU32 flags = PxU32(isModifiable ? PxContactPatch::eMODIFIABLE : 0) |
  			(forceNoResponse ? PxContactPatch::eFORCE_NO_RESPONSE : 0) |
  			(isMeshType ? PxContactPatch::eHAS_FACE_INDICES : 0);
  
  		PxU32 currentIndex = 0;
  
  		PxModifiableContact* PX_RESTRICT point = reinterpret_cast<PxModifiableContact*>(contactData);
  
  		for(PxU32 a = 0; a < numStrideHeaders; ++a)
  		{
  			StridePatch& rootPatch = stridePatches[a];
  			if(rootPatch.isRoot)
  			{
  				PxContactPatch* PX_RESTRICT patch = patches++;
  				
  				PxU32 startIndex = rootPatch.startIndex;
  
  				const PxU16 matIndex0 = pMaterial[startIndex].mMaterialIndex0;
  				const PxU16 matIndex1 = pMaterial[startIndex].mMaterialIndex1;
  				if(matIndex0 != origMatIndex0 || matIndex1 != origMatIndex1)
  				{
  					const PxsMaterialData& data0 = *materialManager->getMaterial(matIndex0);
  					const PxsMaterialData& data1 = *materialManager->getMaterial(matIndex1);
  
  					combinedRestitution = PxsMaterialCombiner::combineRestitution(data0, data1);
  					PxsMaterialCombiner combiner(1.0f, 1.0f);
  					PxsMaterialCombiner::PxsCombinedMaterial combinedMat = combiner.combineIsotropicFriction(data0, data1);
  					staticFriction = combinedMat.staFriction;
  					dynamicFriction = combinedMat.dynFriction;
  					materialFlags = combinedMat.flags;
  					origMatIndex0 = matIndex0;
  					origMatIndex1 = matIndex1;
  				}
  
  				patch->nbContacts = rootPatch.totalCount;
  
  				patch->startContactIndex = Ps::to8(currentIndex);
  				patch->materialFlags = PxU8(materialFlags);
  				patch->staticFriction = staticFriction;
  				patch->dynamicFriction = dynamicFriction;
  				patch->restitution = combinedRestitution;
  				patch->materialIndex0 = matIndex0;
  				patch->materialIndex1 = matIndex1;
  				patch->normal = contactPoints[0].normal;
  				patch->mMassModification.mInvMassScale0 = 1.0f;
  				patch->mMassModification.mInvMassScale1 = 1.0f;
  				patch->mMassModification.mInvInertiaScale0 = 1.0f;
  				patch->mMassModification.mInvInertiaScale1 = 1.0f;
  				patch->internalFlags = PxU8(flags);
  
  				//const PxU32 endIndex = strideHeader[a];
  				const PxU32 totalCountThisPatch = rootPatch.totalCount;
  				if(insertAveragePoint && totalCountThisPatch > 1)
  				{
  					PxVec3 avgPt(0.0f);
  					PxF32 avgPen(0.0f);
  					PxF32 recipCount = 1.0f/(PxF32(rootPatch.totalCount));
  
  					PxU32 index = a;
  					while(index != 0xFF)
  					{
  						StridePatch& p = stridePatches[index];
  						for(PxU32 b = p.startIndex; b < p.endIndex; ++b)
  						{
  							avgPt += contactPoints[b].point;
  							avgPen += contactPoints[b].separation;
  						}
  						index = p.nextIndex;
  					}
  
  					if (faceIndice)
  					{
  						StridePatch& p = stridePatches[index];
  						*faceIndice = contactPoints[p.startIndex].internalFaceIndex1;
  						faceIndice++;
  					}
  
  					patch->nbContacts++;
  					point->contact = avgPt * recipCount;
  					point->separation = avgPen * recipCount;
  					point->normal = contactPoints[0].normal;
  					point->maxImpulse = PX_MAX_REAL;
  					point->targetVelocity = PxVec3(0.0f);
  					point->staticFriction = staticFriction;
  					point->dynamicFriction = dynamicFriction;
  					point->restitution = combinedRestitution;
  					point->materialFlags = materialFlags;
  					point->materialIndex0 = matIndex0;
  					point->materialIndex1 = matIndex1;
  					point++;
  					currentIndex++;
  					Ps::prefetchLine(point, 128);
  				}
  
  				PxU32 index = a;
  				while(index != 0xFF)
  				{
  					StridePatch& p = stridePatches[index];
  						
  					for(PxU32 b = p.startIndex; b < p.endIndex; ++b)
  					{
  						copyContactPoint(point, &contactPoints[b]);
  						point->normal = contactPoints[b].normal;
  						point->maxImpulse = PX_MAX_REAL;
  						point->targetVelocity = PxVec3(0.0f);
  						point->staticFriction = staticFriction;
  						point->dynamicFriction = dynamicFriction;
  						point->restitution = combinedRestitution;
  						point->materialFlags = materialFlags;
  						point->materialIndex0 = matIndex0;
  						point->materialIndex1 = matIndex1;
  						if (faceIndice)
  						{
  							*faceIndice = contactPoints[b].internalFaceIndex1;
  							faceIndice++;
  						}
  						point++;
  						currentIndex++;
  						Ps::prefetchLine(point, 128);
  					}
  					index = p.nextIndex;
  				}				
  			}
  		}
  	}
  	else 
  	{
  		PxU32 flags = PxU32(isMeshType ? PxContactPatch::eHAS_FACE_INDICES : 0);
  
  		PxContact* PX_RESTRICT point = reinterpret_cast<PxContact*>(contactData);
  		
  		PxU32 currentIndex = 0;
  		{
  			for(PxU32 a = 0; a < numStrideHeaders; ++a)
  			{
  				StridePatch& rootPatch = stridePatches[a];
  
  				if(rootPatch.isRoot)
  				{
  					const PxU16 matIndex0 = pMaterial[rootPatch.startIndex].mMaterialIndex0;
  					const PxU16 matIndex1 = pMaterial[rootPatch.startIndex].mMaterialIndex1;
  					if(matIndex0 != origMatIndex0 || matIndex1 != origMatIndex1)
  					{
  						const PxsMaterialData& data0 = *materialManager->getMaterial(matIndex0);
  						const PxsMaterialData& data1 = *materialManager->getMaterial(matIndex1);
  
  						combinedRestitution = PxsMaterialCombiner::combineRestitution(data0, data1);
  						PxsMaterialCombiner combiner(1.0f, 1.0f);
  						PxsMaterialCombiner::PxsCombinedMaterial combinedMat = combiner.combineIsotropicFriction(data0, data1);
  						staticFriction = combinedMat.staFriction;
  						dynamicFriction = combinedMat.dynFriction;
  						materialFlags = combinedMat.flags;
  						origMatIndex0 = matIndex0;
  						origMatIndex1 = matIndex1;
  					}
  
  					PxContactPatch* PX_RESTRICT patch = patches++;
  					patch->normal = contactPoints[rootPatch.startIndex].normal;
  					patch->nbContacts = rootPatch.totalCount;
  					patch->startContactIndex = Ps::to8(currentIndex);
  					//KS - we could probably compress this further into the header but the complexity might not be worth it
  					patch->staticFriction = staticFriction;
  					patch->dynamicFriction = dynamicFriction;
  					patch->restitution = combinedRestitution;
  					patch->materialIndex0 = matIndex0;
  					patch->materialIndex1 = matIndex1;
  					patch->materialFlags = PxU8(materialFlags);
  					patch->internalFlags = PxU8(flags);
  					patch->mMassModification.mInvMassScale0 = 1.0f;
  					patch->mMassModification.mInvMassScale1 = 1.0f;
  					patch->mMassModification.mInvInertiaScale0 = 1.0f;
  					patch->mMassModification.mInvInertiaScale1 = 1.0f;
  					if(insertAveragePoint && (rootPatch.totalCount) > 1)
  					{
  						patch->nbContacts++;
  						PxVec3 avgPt(0.0f);
  						PxF32 avgPen(0.0f);
  						PxF32 recipCount = 1.0f/(PxF32(rootPatch.totalCount));
  						PxU32 index = a;
  						
  						while(index != 0xFF)
  						{
  							StridePatch& p = stridePatches[index];
  							for(PxU32 b = p.startIndex; b < p.endIndex; ++b)
  							{
  								avgPt += contactPoints[b].point;
  								avgPen += contactPoints[b].separation;
  							}
  							index = stridePatches[index].nextIndex;
  						}
  
  						if (faceIndice)
  						{
  							StridePatch& p = stridePatches[index];
  							*faceIndice = contactPoints[p.startIndex].internalFaceIndex1;
  							faceIndice++;
  						}
  						point->contact = avgPt * recipCount;
  						point->separation = avgPen * recipCount;
  						
  						point++;
  						currentIndex++;
  						Ps::prefetchLine(point, 128);
  					}
  
  					PxU32 index = a;
  					while(index != 0xFF)
  					{
  						StridePatch& p = stridePatches[index];
  						for(PxU32 b = p.startIndex; b < p.endIndex; ++b)
  						{
  							copyContactPoint(point, &contactPoints[b]);
  							if (faceIndice)
  							{
  								*faceIndice = contactPoints[b].internalFaceIndex1;
  								faceIndice++;
  							}
  							point++;
  							currentIndex++;
  							Ps::prefetchLine(point, 128);
  						}
  						index = stridePatches[index].nextIndex;
  					}
  				}
  			}
  		}
  	}
  
  	writtenContactCount = Ps::to8(totalContactPoints);
  
  	return totalRequiredSize;
  }
  