text: /*
   * Copyright (c) 2008-2015, NVIDIA CORPORATION.  All rights reserved.
   *
   * NVIDIA CORPORATION and its licensors retain all intellectual property
   * and proprietary rights in and to this software, 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.
   */
  
  // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
  // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  
  
  #include "PxPhysXConfig.h"
  #if PX_USE_CLOTH_API
  
  #include "ExtClothFabricCooker.h"
  #include "ExtClothTetherCooker.h"
  #include "PxVec4.h"
  #include "PxPhysics.h"
  #include "PxStrideIterator.h"
  #include "PsFoundation.h"
  #include "PsArray.h"
  #include "PsHashMap.h"
  #include "PsSort.h"
  #include "foundation/PxIO.h"
  
  #pragma warning(disable:4127)
  
  using namespace physx;
  
  struct physx::PxFabricCookerImpl
  {
  	bool cook(const PxClothMeshDesc& desc, PxVec3 gravity, bool useGeodesicTether);
  
  	PxClothFabricDesc getDescriptor() const;
  	void save(PxOutputStream& stream, bool platformMismatch) const;
  
  public:
  	PxU32 mNumParticles;
  
  	shdfnd::Array<PxClothFabricPhase> mPhases;
  	shdfnd::Array<PxU32> mSets; // with 0 prefix
  	shdfnd::Array<PxReal> mRestvalues;
  	shdfnd::Array<PxU32> mIndices;
  
  	shdfnd::Array<PxU32> mTetherAnchors;
  	shdfnd::Array<PxReal> mTetherLengths;
  };
  
  PxClothFabricCooker::PxClothFabricCooker(const PxClothMeshDesc& desc, const PxVec3& gravity, bool useGeodesicTether)
  : mImpl(new PxFabricCookerImpl())
  {
  	mImpl->cook(desc, gravity, useGeodesicTether);
  }
  
  PxClothFabricCooker::~PxClothFabricCooker()
  {
  	delete mImpl;
  }
  
  PxClothFabricDesc PxClothFabricCooker::getDescriptor() const
  {
  	return mImpl->getDescriptor();
  }
  
  void PxClothFabricCooker::save(PxOutputStream& stream, bool platformMismatch) const
  {
  	mImpl->save(stream, platformMismatch);
  }
  
  
  PxClothFabric* physx::PxClothFabricCreate( PxPhysics& physics, const PxClothMeshDesc& desc, const PxVec3& gravity, bool useGeodesicTether )
  {
  	PxFabricCookerImpl impl;
  
  	if(!impl.cook(desc, gravity, useGeodesicTether))
  		return 0;
  
  	return physics.createClothFabric(impl.getDescriptor());
  }
  
  namespace
  {
  	// calculate the inclusive prefix sum, equivalent of std::partial_sum
  	template <typename T>
  	void prefixSum(const T* first, const T* last, T* dest)
  	{
  		if (first != last)
  		{	
  			*(dest++) = *(first++);
  			for (; first != last; ++first, ++dest)
  				*dest = *(dest-1) + *first;
  		}
  	}
  
  	template <typename T>
  	void gatherAdjacencies(shdfnd::Array<PxU32>& valency, shdfnd::Array<PxU32>& adjacencies, 
  		const PxBoundedData& triangles, const PxBoundedData& quads)
  	{
  		// count number of edges per vertex
  		PxStrideIterator<const T> tIt, qIt;
  		tIt = PxMakeIterator((const T*)triangles.data, triangles.stride);
  		for(PxU32 i=0; i<triangles.count; ++i, ++tIt, ++qIt)
  		{
  			for(PxU32 j=0; j<3; ++j)
  				valency[tIt.ptr()[j]] += 2;
  		}
  		qIt = PxMakeIterator((const T*)quads.data, quads.stride);
  		for(PxU32 i=0; i<quads.count; ++i, ++tIt, ++qIt)
  		{
  			for(PxU32 j=0; j<4; ++j)
  				valency[qIt.ptr()[j]] += 2;
  		}
  
  		prefixSum(valency.begin(), valency.end(), valency.begin());
  		adjacencies.resize(valency.back());
  
  		// gather adjacent vertices
  		tIt = PxMakeIterator((const T*)triangles.data, triangles.stride);
  		for(PxU32 i=0; i<triangles.count; ++i, ++tIt)
  		{
  			for(PxU32 j=0; j<3; ++j)
  			{
  				adjacencies[--valency[tIt.ptr()[j]]] = tIt.ptr()[(j+1)%3];
  				adjacencies[--valency[tIt.ptr()[j]]] = tIt.ptr()[(j+2)%3];
  			}
  		}
  		qIt = PxMakeIterator((const T*)quads.data, quads.stride);
  		for(PxU32 i=0; i<quads.count; ++i, ++qIt)
  		{
  			for(PxU32 j=0; j<4; ++j)
  			{
  				adjacencies[--valency[qIt.ptr()[j]]] = qIt.ptr()[(j+1)%4];
  				adjacencies[--valency[qIt.ptr()[j]]] = qIt.ptr()[(j+3)%4];
  			}
  		}
  	}
  
  	
  	struct Edge
  	{
  		Edge() : mStretching(0.0f), mBending(0.0f), mShearing(0.0f) {}
  
  		void classify()
  		{
  			mStretching += 0.1f;
  		}
  
  		// classify v0-v2 edge based on alternative v0-v1-v2 path 
  		void classify(const PxVec4& v0, const PxVec4& v1, const PxVec4& v2)
  		{
  			const PxVec3& p0 = reinterpret_cast<const PxVec3&>(v0);
  			const PxVec3& p1 = reinterpret_cast<const PxVec3&>(v1);
  			const PxVec3& p2 = reinterpret_cast<const PxVec3&>(v2);
  
  			PxReal area = (p1-p0).cross(p2-p1).magnitude();
  			// triangle height / base length
  			// 1.0 = quad edge, 0.2 = quad diagonal + quad edge, 
  			PxReal ratio = area / (p2-p0).magnitudeSquared();
  
  			// 0.5 = quad diagonal
  			mShearing += PxMax(0.0f, 0.15f - fabsf(0.45f - ratio));
  			// 0.0 = collinear points
  			mBending += PxMax(0.0f, 0.1f - ratio) * 3;
  		}
  
  		PxReal mStretching;
  		PxReal mBending;
  		PxReal mShearing;
  	};
  
  	typedef shdfnd::Pair<PxU32, PxU32> Pair;
  	typedef shdfnd::Pair<Pair, PxClothFabricPhaseType::Enum> Entry;
  
  	// maintain heap status after elements have been pushed (heapify)
  	template<typename T>
  	void pushHeap(shdfnd::Array<T> &heap, const T &value)
  	{
  		heap.pushBack(value);
  		T* begin = heap.begin();
  		T* end = heap.end();
  
  		if (end <= begin)
  			return;
  	
  		PxU32 current = PxU32(end - begin) - 1;
  		while (current > 0)
  		{
  			const PxU32 parent = (current - 1) / 2;
  			if (!(begin[parent] < begin[current]))
  				break;
  
  			shdfnd::swap(begin[parent], begin[current]);
  			current = parent;
  		}
  	}
  
  	// pop one element from the heap
  	template<typename T>
  	T popHeap(shdfnd::Array<T> &heap)
  	{
  		T* begin = heap.begin();
  		T* end = heap.end();
  
  		shdfnd::swap(begin[0], end[-1]); // exchange elements
  
  		// shift down
  		end--;
  
  		PxU32 current = 0;
  		while (begin + (current * 2 + 1) < end)
  		{
  			PxU32 child = current * 2 + 1;
  			if (begin + child + 1 < end && begin[child] < begin[child + 1])
  				++child;
  
  			if (!(begin[current] < begin[child]))
  				break;
  
  			shdfnd::swap(begin[current], begin[child]);
  			current = child;
  		}
  
  		return heap.popBack();
  	}
  
  	// ---------------------------------------------------------------------------------------
  	// Heap element to sort constraint based on graph color count
  	struct ConstraintGraphColorCount
  	{
  		ConstraintGraphColorCount(int cid, int count) 
  			: constraint((PxU32)cid), colorCount((PxU32)count) {}
  
  		PxU32 constraint;
  		PxU32 colorCount; 
  
  		bool operator < (const ConstraintGraphColorCount& c) const
  		{
  			return colorCount < c.colorCount;
  		}
  	};
  
  	struct ConstraintSorter
  	{
  	public:
  
  		ConstraintSorter(PxU32* constraints_) : constraints(constraints_) {}
  
  		bool operator()(PxU32 i, PxU32 j) const
  		{
  			PxU32 ci = i*2;
  			PxU32 cj = j*2;
  
  			if (constraints[ci] == constraints[cj])
  				return constraints[ci+1] < constraints[cj+1];
  			else
  				return constraints[ci] < constraints[cj];
  		}
  
  		PxU32* constraints;
  	};
  
  } // anonymous namespace
  
  bool PxFabricCookerImpl::cook(const PxClothMeshDesc& desc, PxVec3 gravity, bool useGeodesicTether)
  {	
  	if(!desc.isValid())
  	{
  		shdfnd::getFoundation().error(PxErrorCode::eINVALID_PARAMETER, __FILE__, __LINE__, 
  			"PxFabricCookerImpl::cook: desc.isValid() failed!");
  		return false;
  	}
  
  	gravity = gravity.getNormalized();
  
  	mNumParticles = desc.points.count;
  
  	// assemble points
  	shdfnd::Array<PxVec4> particles;
  	particles.reserve(mNumParticles);
  	PxStrideIterator<const PxVec3> pIt((const PxVec3*)desc.points.data, desc.points.stride);
  	PxStrideIterator<const PxReal> wIt((const PxReal*)desc.invMasses.data, desc.invMasses.stride);
  	for(PxU32 i=0; i<mNumParticles; ++i)
  		particles.pushBack(PxVec4(*pIt++, wIt.ptr() ? *wIt++ : 1.0f));
  
  	// build adjacent vertex list
  	shdfnd::Array<PxU32> valency(mNumParticles+1, 0);
  	shdfnd::Array<PxU32> adjacencies;
  	if(desc.flags & PxMeshFlag::e16_BIT_INDICES)
  		gatherAdjacencies<PxU16>(valency, adjacencies, desc.triangles, desc.quads);
  	else
  		gatherAdjacencies<PxU32>(valency, adjacencies, desc.triangles, desc.quads);
  
  	// build unique neighbors from adjacencies
  	shdfnd::Array<PxU32> mark(valency.size(), 0);
  	shdfnd::Array<PxU32> neighbors; neighbors.reserve(adjacencies.size());
  	for(PxU32 i=1, j=0; i<valency.size(); ++i)
  	{
  		for(; j<valency[i]; ++j)
  		{
  			PxU32 k = adjacencies[j];
  			if(mark[k] != i)
  			{
  				mark[k] = i;
  				neighbors.pushBack(k);
  			}
  		}
  		valency[i] = neighbors.size();
  	}
  
  	// build map of unique edges and classify
  	shdfnd::HashMap<Pair, Edge> edges;
  	for(PxU32 i=0; i<mNumParticles; ++i)
  	{
  		PxReal wi = particles[i].w;
  		// iterate all neighbors
  		PxU32 jlast = valency[i+1];
  		for(PxU32 j=valency[i]; j<jlast; ++j)
  		{
  			// add 1-ring edge
  			PxU32 m = neighbors[j];
  			if(wi + particles[m].w > 0.0f)
  				edges[Pair(PxMin(i, m), PxMax(i, m))].classify();
  
  			// iterate all neighbors of neighbor
  			PxU32 klast = valency[m+1];
  			for(PxU32 k=valency[m]; k<klast; ++k)
  			{
  				PxU32 n = neighbors[k];
  				if(n != i && wi + particles[n].w > 0.0f)
  				{
  					// add 2-ring edge
  					edges[Pair(PxMin(i, n), PxMax(i, n))].classify(
  						particles[i], particles[m], particles[n]);
  				}
  			}
  		}
  	}
  
  	// copy classified edges to constraints array
  	// build histogram of constraints per vertex
  	shdfnd::Array<Entry> constraints; 	
  	constraints.reserve(edges.size());
  	valency.resize(0); valency.resize(mNumParticles+1, 0);
  
  	const PxReal sqrtHalf = PxSqrt(0.4f);
  	for(shdfnd::HashMap<Pair, Edge>::Iterator eIt = edges.getIterator(); !eIt.done(); ++eIt)
  	{
  		const Edge& edge = eIt->second;
  		const Pair& pair = eIt->first;
  		if((edge.mStretching + edge.mBending + edge.mShearing) > 0.0f)
  		{	
  			PxClothFabricPhaseType::Enum type = PxClothFabricPhaseType::eINVALID;
  			if(edge.mBending > PxMax(edge.mStretching, edge.mShearing))
  				type = PxClothFabricPhaseType::eBENDING;
  			else if(edge.mShearing > PxMax(edge.mStretching, edge.mBending))
  				type = PxClothFabricPhaseType::eSHEARING;
  			else 
  			{
  				PxVec4 diff = particles[pair.first]-particles[pair.second];
  				PxReal dot = gravity.dot(reinterpret_cast<const PxVec3&>(diff).getNormalized());
  				type = fabsf(dot) < sqrtHalf ? PxClothFabricPhaseType::eHORIZONTAL : PxClothFabricPhaseType::eVERTICAL;
  			}
  			++valency[pair.first];
  			++valency[pair.second];
  			constraints.pushBack(Entry(pair, type));
  		}
  	} 
  
  	prefixSum(valency.begin(), valency.end(), valency.begin());
  
  	PxU32 numConstraints = constraints.size();
  
  	// build adjacent constraint list
  	adjacencies.resize(0); adjacencies.resize(valency.back(), 0);
  	for(PxU32 i=0; i<numConstraints; ++i)
  	{
  		adjacencies[--valency[constraints[i].first.first]] = i;
  		adjacencies[--valency[constraints[i].first.second]] = i;
  	}
  	
  	shdfnd::Array<PxU32>::ConstIterator aFirst = adjacencies.begin();
  	shdfnd::Array<PxU32> colors(numConstraints, numConstraints); // constraint -> color, initialily not colored
  	mark.resize(0); mark.resize(numConstraints+1, PX_MAX_U32); // color -> constraint index
  	shdfnd::Array<PxU32> adjColorCount(numConstraints, 0); // # of neighbors that are already colored
  
  	shdfnd::Array<ConstraintGraphColorCount> constraintHeap; 
  	constraintHeap.reserve(numConstraints); // set of constraints to color (added in edge distance order)
  
  	// Do graph coloring based on edge distance.
  	// For each constraint, we add its uncolored neighbors to the heap
  	// ,and we pick the constraint with most colored neighbors from the heap.
  	while ( true )
  	{
  		PxU32 constraint = 0;
  		while ( (constraint < numConstraints) && (colors[constraint] != numConstraints))
  			constraint++; // start with the first uncolored constraint
  	
  		if (constraint >= numConstraints)
  			break;
  
  		constraintHeap.clear();
  		pushHeap(constraintHeap, ConstraintGraphColorCount((int)constraint, (int)adjColorCount[constraint]));
  		PxClothFabricPhaseType::Enum type = constraints[constraint].second;
  		
  		while (!constraintHeap.empty())
  		{		
  			ConstraintGraphColorCount heapItem = popHeap(constraintHeap);
  			constraint = heapItem.constraint;
  			if (colors[constraint] != numConstraints)
  				continue; // skip if already colored 
  
  			const Pair& pair = constraints[constraint].first;			
  			for(PxU32 j=0; j<2; ++j)
  			{
  				PxU32 index = j ? pair.first : pair.second;
  				if(particles[index].w == 0.0f)
  					continue; // don't mark adjacent particles if attached
  
  				for(shdfnd::Array<PxU32>::ConstIterator aIt = aFirst + valency[index], aEnd = aFirst + valency[index+1]; aIt != aEnd; ++aIt)
  				{				
  					PxU32 adjacentConstraint = *aIt;
  					if ((constraints[adjacentConstraint].second != type) || (adjacentConstraint == constraint))
  						continue;
  
  					mark[colors[adjacentConstraint]] = constraint; 
  					++adjColorCount[adjacentConstraint];
  					pushHeap(constraintHeap, ConstraintGraphColorCount((int)adjacentConstraint, (int)adjColorCount[adjacentConstraint]));
  				}
  			}
  
  			// find smallest color with matching type
  			PxU32 color = 0;
  			while((color < mPhases.size() && mPhases[color].phaseType != type) || mark[color] == constraint)
  				++color;
  
  			// create a new color set
  			if(color == mPhases.size())
  			{
  				PxClothFabricPhase phase(type, mPhases.size());
  				mPhases.pushBack(phase);
  				mSets.pushBack(0);
  			}
  
  			colors[constraint] = color;
  			++mSets[color];
  		} 
  	}
  
  #if 0 // PX_DEBUG
  	printf("set[%u] = ", mSets.size());
  	for(PxU32 i=0; i<mSets.size(); ++i)
  		printf("%u ", mSets[i]);
  #endif
  
  	prefixSum(mSets.begin(), mSets.end(), mSets.begin());
  
  #if 0 // PX_DEBUG
  	printf(" = %u\n", mSets.back());
  #endif
  
  	// write indices and rest lengths
  	// convert mSets to exclusive sum
  	PxU32 back = mSets.back();
  	mSets.pushBack(back);
  	mIndices.resize(numConstraints*2);
  	mRestvalues.resize(numConstraints);
  	for(PxU32 i=0; i<numConstraints; ++i)
  	{
  		PxU32 first = constraints[i].first.first;
  		PxU32 second = constraints[i].first.second;
  
  		PxU32 index = --mSets[colors[i]];
  
  		mIndices[2*index  ] = first;
  		mIndices[2*index+1] = second;
  
  		PxVec4 diff = particles[second] - particles[first];
  		mRestvalues[index] = reinterpret_cast<
  			const PxVec3&>(diff).magnitude();
  	} 
  	
  	// reorder constraints and rest values for more efficient cache access (linear)
  	shdfnd::Array<PxU32> newIndices(mIndices.size());
  	shdfnd::Array<PxF32> newRestValues(mRestvalues.size());
  
  	// sort each constraint set in vertex order
  	for (PxU32 i=0; i < mSets.size()-1; ++i)
  	{
  		// create a re-ordering list
  		shdfnd::Array<PxU32> reorder(mSets[i+1]-mSets[i]);
  
  		for (PxU32 r=0; r < reorder.size(); ++r)
  			reorder[r] = r;
  
  		const PxU32 indicesOffset = mSets[i]*2;
  		const PxU32 restOffset = mSets[i];
  
  		ConstraintSorter predicate(&mIndices[indicesOffset]);
  		shdfnd::sort(&reorder[0], reorder.size(), predicate);
  		
  		for (PxU32 r=0; r < reorder.size(); ++r)
  		{
  			newIndices[indicesOffset + r*2] = mIndices[indicesOffset + reorder[r]*2];
  			newIndices[indicesOffset + r*2+1] = mIndices[indicesOffset + reorder[r]*2+1];
  			newRestValues[restOffset + r] = mRestvalues[restOffset + reorder[r]];
  		}
  	}
  
  	mIndices = newIndices;
  	mRestvalues = newRestValues;
  
  	PX_ASSERT(mIndices.size() == mRestvalues.size()*2);
  	PX_ASSERT(mRestvalues.size() == mSets.back());
  
  #if 0 // PX_DEBUG
  	for (PxU32 i = 1; i < mSets.size(); i++)
  	{
  		PxClothFabricPhase phase = mPhases[i-1];
  		printf("%d : type %d, size %d\n", 
  			i-1, phase.phaseType, mSets[i] - mSets[i-1]);
  	}
  #endif
  
  	if (useGeodesicTether)
  	{
  		PxClothGeodesicTetherCooker tetherCooker(desc);
  		if (tetherCooker.getCookerStatus() == 0)
  		{
  			PxU32 numTethersPerParticle = tetherCooker.getNbTethersPerParticle();
  			PxU32 tetherSize = mNumParticles * numTethersPerParticle;
  			mTetherAnchors.resize(tetherSize);
  			mTetherLengths.resize(tetherSize);
  			tetherCooker.getTetherData(mTetherAnchors.begin(), mTetherLengths.begin());
  		}
  		else
  			useGeodesicTether = false;
  	}
  
  	if (!useGeodesicTether)
  	{
  		PxClothSimpleTetherCooker tetherCooker(desc);
  		if (tetherCooker.getCookerStatus() == 0)
  		{
  			mTetherAnchors.resize(mNumParticles);
  			mTetherLengths.resize(mNumParticles);
  			tetherCooker.getTetherData(mTetherAnchors.begin(), mTetherLengths.begin());
  		}
  	}
  
  	return true;
  }
  
  physx::PxClothFabricDesc physx::PxFabricCookerImpl::getDescriptor() const
  {
  	PxClothFabricDesc result;
  
  	result.nbParticles = mNumParticles;
  	result.nbPhases = mPhases.size();
  	result.phases = mPhases.begin();
  	result.nbSets = mSets.size()-1;
  	result.sets = mSets.begin()+1;
  	result.restvalues = mRestvalues.begin();
  	result.indices = mIndices.begin();
  	result.nbTethers = mTetherAnchors.size();
  	result.tetherAnchors = mTetherAnchors.begin();
  	result.tetherLengths = mTetherLengths.begin();
  
  	return result;
  }
  
  void physx::PxFabricCookerImpl::save( PxOutputStream& stream, bool /*platformMismatch*/ ) const
  {
  	// version 1 is equivalent to 0x030300 and 0x030301 (PX_PHYSICS_VERSION of 3.3.0 and 3.3.1).
  	// If the stream format changes, the loader code in ScClothFabricCore.cpp 
  	// and the version number need to change too. 
  	PxU32 version = 1;
  	stream.write(&version, sizeof(PxU32));
  
  	PxClothFabricDesc desc = getDescriptor();
  
  	// write explicit sizes, others are implicit
  	stream.write(&mNumParticles, sizeof(PxU32));
  	stream.write(&desc.nbPhases, sizeof(PxU32));
  	stream.write(&desc.nbSets, sizeof(PxU32));
  	stream.write(&desc.nbTethers, sizeof(PxU32));
  
  	PxU32 nbConstraints = desc.sets[desc.nbSets-1];
  
  	// write actual data
  	PX_COMPILE_TIME_ASSERT(sizeof(PxClothFabricPhaseType::Enum) == sizeof(PxU32));
  	stream.write(desc.phases, desc.nbPhases*sizeof(PxClothFabricPhase));
  	stream.write(desc.sets, desc.nbSets*sizeof(PxU32));
  
  	stream.write(desc.restvalues, nbConstraints*sizeof(PxReal));
  	stream.write(desc.indices, nbConstraints*2*sizeof(PxU32));
  
  	stream.write(desc.tetherAnchors, desc.nbTethers*sizeof(PxU32));
  	stream.write(desc.tetherLengths, desc.nbTethers*sizeof(PxReal));
  }
  
  #endif //PX_USE_CLOTH_API
  