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-2017 NVIDIA Corporation. All rights reserved.
  // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
  // Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
  
  #pragma once
  
  #ifndef CU_SOLVER_KERNEL_CU
  #error include CuCollision.h only from CuSolverKernel.cu
  #endif
  
  #include "../IndexPair.h"
  
  namespace
  {
  struct CuCollision
  {
  	struct ShapeMask
  	{
  		uint32_t mSpheres;
  		uint32_t mCones;
  
  		__device__ friend ShapeMask& operator &= (ShapeMask& left, const ShapeMask& right)
  		{
  			left.mSpheres = left.mSpheres & right.mSpheres;
  			left.mCones = left.mCones & right.mCones;
  			return left;
  		}
  	};
  
  	struct CollisionData
  	{
  		Pointer<Shared, float> mSphereX;
  		Pointer<Shared, float> mSphereY;
  		Pointer<Shared, float> mSphereZ;
  		Pointer<Shared, float> mSphereW;
  
  		Pointer<Shared, float> mConeCenterX;
  		Pointer<Shared, float> mConeCenterY;
  		Pointer<Shared, float> mConeCenterZ;
  		Pointer<Shared, float> mConeRadius;
  		Pointer<Shared, float> mConeAxisX;
  		Pointer<Shared, float> mConeAxisY;
  		Pointer<Shared, float> mConeAxisZ;
  		Pointer<Shared, float> mConeSlope;
  		Pointer<Shared, float> mConeSqrCosine;
  		Pointer<Shared, float> mConeHalfLength;
  	};
  
    public:
  	__device__ CuCollision(Pointer<Shared, uint32_t>);
  
  	template <typename CurrentT, typename PreviousT>
  	__device__ void operator()(CurrentT& current, PreviousT& previous, float alpha);
  
    private:
  	__device__ void buildSphereAcceleration(const CollisionData&);
  	__device__ void buildConeAcceleration();
  	__device__ void mergeAcceleration();
  
  	template <typename CurrentT>
  	__device__ bool buildAcceleration(const CurrentT&, float);
  
  	__device__ static ShapeMask readShapeMask(const float&, Pointer<Shared, const uint32_t>);
  	template <typename CurPos>
  	__device__ ShapeMask getShapeMask(const CurPos&) const;
  	template <typename PrevPos, typename CurPos>
  	__device__ ShapeMask getShapeMask(const PrevPos&, const CurPos&) const;
  
  	template <typename CurPos>
  	__device__ int32_t collideCapsules(const CurPos&, float3&, float3&) const;
  	template <typename PrevPos, typename CurPos>
  	__device__ int32_t collideCapsules(const PrevPos&, CurPos&, float3&, float3&) const;
  
  	template <typename CurrentT, typename PreviousT>
  	__device__ void collideCapsules(CurrentT& current, PreviousT& previous) const;
  	template <typename CurrentT, typename PreviousT>
  	__device__ void collideVirtualCapsules(CurrentT& current, PreviousT& previous) const;
  	template <typename CurrentT, typename PreviousT>
  	__device__ void collideContinuousCapsules(CurrentT& current, PreviousT& previous) const;
  
  	template <typename CurrentT, typename PreviousT>
  	__device__ void collideConvexes(CurrentT& current, PreviousT& previous, float alpha);
  	template <typename CurPos>
  	__device__ int32_t collideConvexes(const CurPos&, float3&) const;
  
  	template <typename CurrentT>
  	__device__ void collideTriangles(CurrentT& current, float alpha);
  	template <typename CurrentT>
  	__device__ void collideTriangles(CurrentT& current, int32_t i);
  
    public:
  	Pointer<Shared, uint32_t> mCapsuleIndices;
  	Pointer<Shared, uint32_t> mCapsuleMasks;
  	Pointer<Shared, uint32_t> mConvexMasks;
  
  	CollisionData mPrevData;
  	CollisionData mCurData;
  
  	// acceleration structure
  	Pointer<Shared, uint32_t> mShapeGrid;
  	float mGridScale[3];
  	float mGridBias[3];
  	static const uint32_t sGridSize = 8;
  };
  
  template <typename T>
  __device__ void swap(T& a, T& b)
  {
  	T c = a;
  	a = b;
  	b = c;
  }
  }
  
  __shared__ uninitialized<CuCollision> gCollideParticles;
  
  namespace
  {
  // initializes one pointer past data!
  __device__ void allocate(CuCollision::CollisionData& data)
  {
  	if (threadIdx.x < 15)
  	{
  		Pointer<Shared, float>* ptr = &data.mSphereX;
  		ptr[threadIdx.x] = *ptr + threadIdx.x * gClothData.mNumCapsules +
  		                   min(threadIdx.x, 4) * (gClothData.mNumSpheres - gClothData.mNumCapsules);
  	}
  }
  
  __device__ void generateSpheres(CuCollision::CollisionData& data, float alpha)
  {
  	// interpolate spheres and transpose
  	if (threadIdx.x < gClothData.mNumSpheres * 4)
  	{
  		float start = __ldg(gFrameData.mStartCollisionSpheres + threadIdx.x);
  		float target = __ldg(gFrameData.mTargetCollisionSpheres + threadIdx.x);
  		float value = start + (target - start) * alpha;
  		if (threadIdx.x % 4 == 3)
  			value = max(value, 0.0f);
  		int32_t j = threadIdx.x % 4 * gClothData.mNumSpheres + threadIdx.x / 4;
  		data.mSphereX[j] = value;
  	}
  
  	__syncthreads();
  }
  
  __device__ void generateCones(CuCollision::CollisionData& data, Pointer<Shared, const uint32_t> iIt)
  {
  	// generate cones
  	if (threadIdx.x < gClothData.mNumCapsules)
  	{
  		uint32_t firstIndex = iIt[0];
  		uint32_t secondIndex = iIt[1];
  
  		float firstX = data.mSphereX[firstIndex];
  		float firstY = data.mSphereY[firstIndex];
  		float firstZ = data.mSphereZ[firstIndex];
  		float firstW = data.mSphereW[firstIndex];
  
  		float secondX = data.mSphereX[secondIndex];
  		float secondY = data.mSphereY[secondIndex];
  		float secondZ = data.mSphereZ[secondIndex];
  		float secondW = data.mSphereW[secondIndex];
  
  		float axisX = (secondX - firstX) * 0.5f;
  		float axisY = (secondY - firstY) * 0.5f;
  		float axisZ = (secondZ - firstZ) * 0.5f;
  		float axisW = (secondW - firstW) * 0.5f;
  
  		float sqrAxisLength = axisX * axisX + axisY * axisY + axisZ * axisZ;
  		float sqrConeLength = sqrAxisLength - axisW * axisW;
  
  		float invAxisLength = rsqrtf(sqrAxisLength);
  		float invConeLength = rsqrtf(sqrConeLength);
  
  		if (sqrConeLength <= 0.0f)
  			invAxisLength = invConeLength = 0.0f;
  
  		float axisLength = sqrAxisLength * invAxisLength;
  
  		data.mConeCenterX[threadIdx.x] = (secondX + firstX) * 0.5f;
  		data.mConeCenterY[threadIdx.x] = (secondY + firstY) * 0.5f;
  		data.mConeCenterZ[threadIdx.x] = (secondZ + firstZ) * 0.5f;
  		data.mConeRadius[threadIdx.x] = (axisW + firstW) * invConeLength * axisLength;
  
  		data.mConeAxisX[threadIdx.x] = axisX * invAxisLength;
  		data.mConeAxisY[threadIdx.x] = axisY * invAxisLength;
  		data.mConeAxisZ[threadIdx.x] = axisZ * invAxisLength;
  		data.mConeSlope[threadIdx.x] = axisW * invConeLength;
  
  		float sine = axisW * invAxisLength;
  		data.mConeSqrCosine[threadIdx.x] = 1 - sine * sine;
  		data.mConeHalfLength[threadIdx.x] = axisLength;
  	}
  
  	__syncthreads();
  }
  }
  
  __device__ CuCollision::CuCollision(Pointer<Shared, uint32_t> scratchPtr)
  {
  	int32_t numCapsules2 = 2 * gClothData.mNumCapsules;
  	int32_t numCapsules4 = 4 * gClothData.mNumCapsules;
  	int32_t numConvexes = gClothData.mNumConvexes;
  
  	if (threadIdx.x < 3)
  	{
  		(&mCapsuleIndices)[threadIdx.x] = scratchPtr + threadIdx.x * numCapsules2;
  		(&mShapeGrid)[-14 * int32_t(threadIdx.x)] = scratchPtr + numCapsules4 + numConvexes;
  	}
  
  	Pointer<Shared, uint32_t> indexPtr = scratchPtr + threadIdx.x;
  	if (threadIdx.x < numCapsules2)
  	{
  		uint32_t index = (&gClothData.mCapsuleIndices->first)[threadIdx.x];
  		*indexPtr = index;
  
  		volatile uint32_t* maskPtr = generic(indexPtr + numCapsules2);
  		*maskPtr = 1u << index;
  		*maskPtr |= maskPtr[-int32_t(threadIdx.x & 1)];
  	}
  	indexPtr += numCapsules4;
  
  	if (threadIdx.x < numConvexes)
  		*indexPtr = gClothData.mConvexMasks[threadIdx.x];
  
  	if (gClothData.mEnableContinuousCollision || gClothData.mFrictionScale > 0.0f)
  	{
  		allocate(mPrevData);
  
  		__syncthreads(); // mPrevData raw hazard
  
  		generateSpheres(mPrevData, 0.0f);
  		generateCones(mPrevData, mCapsuleIndices + 2 * threadIdx.x);
  	}
  
  	allocate(mCurData); // also initializes mShapeGrid (!)
  }
  
  template <typename CurrentT, typename PreviousT>
  __device__ void CuCollision::operator()(CurrentT& current, PreviousT& previous, float alpha)
  {
  	ProfileDetailZone zone(cloth::CuProfileZoneIds::COLLIDE);
  
  	// if (current.w > 0) current.w = previous.w (see SwSolverKernel::computeBounds())
  	for (int32_t i = threadIdx.x; i < gClothData.mNumParticles; i += blockDim.x)
  	{
  		if (current(i, 3) > 0.0f)
  			current(i, 3) = previous(i, 3);
  	}
  
  	collideConvexes(current, previous, alpha);
  	collideTriangles(current, alpha);
  
  	if (buildAcceleration(current, alpha))
  	{
  		if (gClothData.mEnableContinuousCollision)
  			collideContinuousCapsules(current, previous);
  		else
  			collideCapsules(current, previous);
  
  		collideVirtualCapsules(current, previous);
  	}
  
  	// sync otherwise first threads overwrite sphere data before
  	// remaining ones have had a chance to use it leading to incorrect
  	// velocity calculation for friction / ccd
  
  	__syncthreads();
  
  	if (gClothData.mEnableContinuousCollision || gClothData.mFrictionScale > 0.0f)
  	{
  		// store current collision data for next iteration
  		Pointer<Shared, float> dstIt = mPrevData.mSphereX + threadIdx.x;
  		Pointer<Shared, const float> srcIt = mCurData.mSphereX + threadIdx.x;
  		for (; dstIt < mCurData.mSphereX; dstIt += blockDim.x, srcIt += blockDim.x)
  			*dstIt = *srcIt;
  	}
  
  	// __syncthreads() called in updateSleepState()
  }
  
  // build per-axis mask arrays of spheres on the right/left of grid cell
  __device__ void CuCollision::buildSphereAcceleration(const CollisionData& data)
  {
  	if (threadIdx.x >= 192)
  		return;
  	unsigned int mask = __ballot_sync(0xffffffff, !(threadIdx.x >= 192));
  
  	int32_t sphereIdx = threadIdx.x & 31;
  	int32_t axisIdx = threadIdx.x >> 6;             // coordinate index (x, y, or z)
  	int32_t signi = threadIdx.x << 26 & 0x80000000; // sign bit (min or max)
  
  	float signf = copysignf(1.0f, reinterpret_cast<const float&>(signi));
  	float pos = signf * data.mSphereW[sphereIdx] + data.mSphereX[sphereIdx + gClothData.mNumSpheres * axisIdx];
  
  	// use overflow so we can test for non-positive
  	uint32_t index = signi - uint32_t(floorf(pos * mGridScale[axisIdx] + mGridBias[axisIdx]));
  
  	axisIdx += (uint32_t(signi) >> 31) * 3;
  	Pointer<Shared, uint32_t> dst = mShapeGrid + sGridSize * axisIdx;
  	// #pragma unroll
  	for (int32_t i = 0; i < sGridSize; ++i, ++index)
  		dst[i] |= __ballot_sync(mask, int32_t(index) <= 0);
  }
  
  // generate cone masks from sphere masks
  __device__ void CuCollision::buildConeAcceleration()
  {
  	if (threadIdx.x >= 192)
  		return;
  	unsigned int mask = __ballot_sync(0xffffffff, !(threadIdx.x >= 192));
  	int32_t coneIdx = threadIdx.x & 31;
  
  	uint32_t sphereMask =
  	    mCurData.mConeRadius[coneIdx] && coneIdx < gClothData.mNumCapsules ? mCapsuleMasks[2 * coneIdx + 1] : 0;
  
  	int32_t offset = threadIdx.x / 32 * sGridSize;
  	Pointer<Shared, uint32_t> src = mShapeGrid + offset;
  	Pointer<Shared, uint32_t> dst = src + 6 * sGridSize;
  
  	// #pragma unroll
  	for (int32_t i = 0; i < sGridSize; ++i)
  		dst[i] |= __ballot_sync(mask,src[i] & sphereMask);
  }
  
  // convert right/left mask arrays into single overlap array
  __device__ void CuCollision::mergeAcceleration()
  {
  	if (threadIdx.x < sGridSize * 12)
  	{
  		Pointer<Shared, uint32_t> dst = mShapeGrid + threadIdx.x;
  		if (!(gClothData.mEnableContinuousCollision || threadIdx.x * 43 & 1024))
  			*dst &= dst[sGridSize * 3]; // above is same as 'threadIdx.x/24 & 1'
  
  		// mask garbage bits from build * Acceleration
  		int32_t shapeIdx = threadIdx.x >= sGridSize * 6; // spheres=0, cones=1
  		*dst &= (1 << (&gClothData.mNumSpheres)[shapeIdx]) - 1;
  	}
  }
  
  namespace
  {
  #if __CUDA_ARCH__ >= 300
  __device__ float mergeBounds(unsigned int mask, Pointer<Shared, float> buffer)
  {
  	float value = *buffer;
  	value = max(value, __shfl_down_sync(mask, value, 1));
  	value = max(value, __shfl_down_sync(mask, value, 2));
  	value = max(value, __shfl_down_sync(mask, value, 4));
  	value = max(value, __shfl_down_sync(mask, value, 8));
  	return max(value, __shfl_down_sync(mask, value, 16));
  }
  #else
  __device__ float mergeBounds(unsigned int mask, Pointer<Shared, float> buffer)
  {
  	// ensure that writes to buffer are visible to all threads
  	__threadfence_block();
  
  	volatile float* ptr = generic(buffer);
  	*ptr = max(*ptr, ptr[16]);
  	*ptr = max(*ptr, ptr[8]);
  	*ptr = max(*ptr, ptr[4]);
  	*ptr = max(*ptr, ptr[2]);
  	return max(*ptr, ptr[1]);
  }
  #endif
  // computes maxX, -minX, maxY, ... with a stride of 32, threadIdx.x must be < 192
  __device__ float computeSphereBounds(const CuCollision::CollisionData& data, Pointer<Shared, float> buffer)
  {
  	assert(threadIdx.x < 192);
  	unsigned int mask = __ballot_sync(0xffffffff, threadIdx.x < 192);
  
  	int32_t sphereIdx = min(threadIdx.x & 31, gClothData.mNumSpheres - 1); // sphere index
  	int32_t axisIdx = threadIdx.x >> 6;                                    // coordinate index (x, y, or z)
  	int32_t signi = threadIdx.x << 26;                                     // sign bit (min or max)
  	float signf = copysignf(1.0f, reinterpret_cast<const float&>(signi));
  
  	*buffer = data.mSphereW[sphereIdx] + signf * data.mSphereX[sphereIdx + gClothData.mNumSpheres * axisIdx];
  
  	return mergeBounds(mask, buffer);
  }
  
  #if __CUDA_ARCH__ >= 300
  template <typename CurrentT>
  __device__ float computeParticleBounds(const CurrentT& current, Pointer<Shared, float> buffer)
  {
  	int32_t numThreadsPerAxis = blockDim.x * 342 >> 10 & ~31; // same as / 3
  	int32_t axis = (threadIdx.x >= numThreadsPerAxis) + (threadIdx.x >= 2 * numThreadsPerAxis);
  	int32_t threadIdxInAxis = threadIdx.x - axis * numThreadsPerAxis;
  	int laneIdx = threadIdx.x & 31;
  
  	if (threadIdxInAxis < numThreadsPerAxis)
  	{
  		unsigned int mask = __ballot_sync(0xffffffff, threadIdxInAxis < numThreadsPerAxis);
  		typename CurrentT::ConstPointerType posIt = current[axis];
  		int32_t i = min(threadIdxInAxis, gClothData.mNumParticles - 1);
  		float minX = posIt[i], maxX = minX;
  		while (i += numThreadsPerAxis, i < gClothData.mNumParticles)
  		{
  			float posX = posIt[i];
  			minX = min(minX, posX);
  			maxX = max(maxX, posX);
  		}
  
  		minX = min(minX, __shfl_down_sync(mask,minX, 1));
  		maxX = max(maxX, __shfl_down_sync(mask,maxX, 1));
  		minX = min(minX, __shfl_down_sync(mask,minX, 2));
  		maxX = max(maxX, __shfl_down_sync(mask,maxX, 2));
  		minX = min(minX, __shfl_down_sync(mask,minX, 4));
  		maxX = max(maxX, __shfl_down_sync(mask,maxX, 4));
  		minX = min(minX, __shfl_down_sync(mask,minX, 8));
  		maxX = max(maxX, __shfl_down_sync(mask,maxX, 8));
  		minX = min(minX, __shfl_down_sync(mask,minX, 16));
  		maxX = max(maxX, __shfl_down_sync(mask,maxX, 16));
  
  		if (!laneIdx)
  		{
  			Pointer<Shared, float> dst = buffer - threadIdx.x + (threadIdxInAxis >> 5) + (axis << 6);
  			dst[0] = maxX;
  			dst[32] = -minX;
  		}
  	}
  
  	__syncthreads();
  
  	if (threadIdx.x >= 192)
  		return 0.0f;
  	unsigned int mask = __ballot_sync(0xffffffff, !(threadIdx.x >= 192));
  	float value = *buffer;
  	if (laneIdx >= (numThreadsPerAxis >> 5))
  		value = -FLT_MAX;
  
  	// blockDim.x <= 3 * 512, increase to 3 * 1024 by adding a shfl by 16
  	assert(numThreadsPerAxis <= 16 * 32);
  
  	value = max(value, __shfl_down_sync(mask,value, 1));
  	value = max(value, __shfl_down_sync(mask,value, 2));
  	value = max(value, __shfl_down_sync(mask,value, 4));
  	return max(value,  __shfl_down_sync(mask,value, 8));
  }
  #else
  template <typename CurrentT>
  __device__ float computeParticleBounds(const CurrentT& current, Pointer<Shared, float> buffer)
  {
  	if (threadIdx.x >= 192)
  		return 0.0f;
  	unsigned int mask = __ballot_sync(0xffffffff, !(threadIdx.x >= 192));
  
  	int32_t axisIdx = threadIdx.x >> 6; // x, y, or z
  	int32_t signi = threadIdx.x << 26;  // sign bit (min or max)
  	float signf = copysignf(1.0f, reinterpret_cast<const float&>(signi));
  
  	typename CurrentT::ConstPointerType pIt = current[axisIdx];
  	typename CurrentT::ConstPointerType pEnd = pIt + gClothData.mNumParticles;
  	pIt += min(threadIdx.x & 31, gClothData.mNumParticles - 1);
  
  	*buffer = *pIt * signf;
  	while (pIt += 32, pIt < pEnd)
  		*buffer = max(*buffer, *pIt * signf);
  
  	return mergeBounds(mask, buffer);
  }
  #endif
  }
  
  // build mask of spheres/cones touching a regular grid along each axis
  template <typename CurrentT>
  __device__ bool CuCollision::buildAcceleration(const CurrentT& current, float alpha)
  {
  	ProfileDetailZone zone(cloth::CuProfileZoneIds::COLLIDE_ACCELERATION);
  
  	// use still unused cone data as buffer for bounds computation
  	Pointer<Shared, float> buffer = mCurData.mConeCenterX + threadIdx.x;
  	float curParticleBounds = computeParticleBounds(current, buffer);
  	int32_t warpIdx = threadIdx.x >> 5;
  
  	if (!gClothData.mNumSpheres)
  	{
  		if (threadIdx.x < 192 && !(threadIdx.x & 31))
  			gFrameData.mParticleBounds[warpIdx] = curParticleBounds;
  		return false;
  	}
  
  	generateSpheres(mCurData, alpha);
  
  	if (threadIdx.x < 192)
  	{
  		float sphereBounds = computeSphereBounds(mCurData, buffer);
  		float particleBounds = curParticleBounds;
  		if (gClothData.mEnableContinuousCollision)
  		{
  			sphereBounds = max(sphereBounds, computeSphereBounds(mPrevData, buffer));
  			float prevParticleBounds = gFrameData.mParticleBounds[warpIdx];
  			particleBounds = max(particleBounds, prevParticleBounds);
  		}
  
  		float bounds = min(sphereBounds, particleBounds);
  		float expandedBounds = bounds + abs(bounds) * 1e-4f;
  
  		// store bounds data in shared memory
  		if (!(threadIdx.x & 31))
  		{
  			mGridScale[warpIdx] = expandedBounds;
  			gFrameData.mParticleBounds[warpIdx] = curParticleBounds;
  		}
  	}
  
  	__syncthreads(); // mGridScale raw hazard
  
  	if (threadIdx.x < 3)
  	{
  		float negativeLower = mGridScale[threadIdx.x * 2 + 1];
  		float edgeLength = mGridScale[threadIdx.x * 2] + negativeLower;
  		float divisor = max(edgeLength, FLT_EPSILON);
  		mGridScale[threadIdx.x] = __fdividef(sGridSize - 1e-3, divisor);
  		mGridBias[threadIdx.x] = negativeLower * mGridScale[threadIdx.x];
  		if (edgeLength < 0.0f)
  			mGridScale[0] = 0.0f; // mark empty intersection
  	}
  
  	// initialize sphere *and* cone grid to 0
  	if (threadIdx.x < 2 * 6 * sGridSize)
  		mShapeGrid[threadIdx.x] = 0;
  
  	__syncthreads(); // mGridScale raw hazard
  
  	// generate cones even if test below fails because
  	// continuous collision might need it in next iteration
  	generateCones(mCurData, mCapsuleIndices + 2 * threadIdx.x);
  
  	if (mGridScale[0] == 0.0f)
  		return false; // early out for empty intersection
  
  	if (gClothData.mEnableContinuousCollision)
  		buildSphereAcceleration(mPrevData);
  	buildSphereAcceleration(mCurData);
  	__syncthreads(); // mCurData raw hazard
  
  	buildConeAcceleration();
  	__syncthreads(); // mShapeGrid raw hazard
  
  	mergeAcceleration();
  	__syncthreads(); // mShapeGrid raw hazard
  
  	return true;
  }
  
  __device__ CuCollision::ShapeMask CuCollision::readShapeMask(const float& position,
                                                               Pointer<Shared, const uint32_t> sphereGrid)
  {
  	ShapeMask result;
  	int32_t index = int32_t(floorf(position));
  	uint32_t outMask = (index < sGridSize) - 1;
  
  	Pointer<Shared, const uint32_t> gridPtr = sphereGrid + (index & sGridSize - 1);
  	result.mSpheres = gridPtr[0] & ~outMask;
  	result.mCones = gridPtr[sGridSize * 6] & ~outMask;
  
  	return result;
  }
  
  // lookup acceleration structure and return mask of potential intersectors
  template <typename CurPos>
  __device__ CuCollision::ShapeMask CuCollision::getShapeMask(const CurPos& positions) const
  {
  	ShapeMask result;
  
  	result = readShapeMask(positions.x * mGridScale[0] + mGridBias[0], mShapeGrid);
  	result &= readShapeMask(positions.y * mGridScale[1] + mGridBias[1], mShapeGrid + 8);
  	result &= readShapeMask(positions.z * mGridScale[2] + mGridBias[2], mShapeGrid + 16);
  
  	return result;
  }
  
  template <typename PrevPos, typename CurPos>
  __device__ CuCollision::ShapeMask CuCollision::getShapeMask(const PrevPos& prevPos, const CurPos& curPos) const
  {
  	ShapeMask result;
  
  	float prevX = prevPos.x * mGridScale[0] + mGridBias[0];
  	float prevY = prevPos.y * mGridScale[1] + mGridBias[1];
  	float prevZ = prevPos.z * mGridScale[2] + mGridBias[2];
  
  	float curX = curPos.x * mGridScale[0] + mGridBias[0];
  	float curY = curPos.y * mGridScale[1] + mGridBias[1];
  	float curZ = curPos.z * mGridScale[2] + mGridBias[2];
  
  	float maxX = min(max(prevX, curX), 7.0f);
  	float maxY = min(max(prevY, curY), 7.0f);
  	float maxZ = min(max(prevZ, curZ), 7.0f);
  
  	result = readShapeMask(maxX, mShapeGrid);
  	result &= readShapeMask(maxY, mShapeGrid + 8);
  	result &= readShapeMask(maxZ, mShapeGrid + 16);
  
  	float minX = max(min(prevX, curX), 0.0f);
  	float minY = max(min(prevY, curY), 0.0f);
  	float minZ = max(min(prevZ, curZ), 0.0f);
  
  	result &= readShapeMask(minX, mShapeGrid + 24);
  	result &= readShapeMask(minY, mShapeGrid + 32);
  	result &= readShapeMask(minZ, mShapeGrid + 40);
  
  	return result;
  }
  
  template <typename CurPos>
  __device__ int32_t CuCollision::collideCapsules(const CurPos& curPos, float3& delta, float3& velocity) const
  {
  	ShapeMask shapeMask = getShapeMask(curPos);
  
  	delta.x = delta.y = delta.z = 0.0f;
  	velocity.x = velocity.y = velocity.z = 0.0f;
  
  	int32_t numCollisions = 0;
  	bool frictionEnabled = gClothData.mFrictionScale > 0.0f;
  
  	// cone collision
  	for (; shapeMask.mCones; shapeMask.mCones &= shapeMask.mCones - 1)
  	{
  		int32_t j = __ffs(shapeMask.mCones) - 1;
  
  		float deltaX = curPos.x - mCurData.mConeCenterX[j];
  		float deltaY = curPos.y - mCurData.mConeCenterY[j];
  		float deltaZ = curPos.z - mCurData.mConeCenterZ[j];
  
  		float axisX = mCurData.mConeAxisX[j];
  		float axisY = mCurData.mConeAxisY[j];
  		float axisZ = mCurData.mConeAxisZ[j];
  		float slope = mCurData.mConeSlope[j];
  
  		float dot = deltaX * axisX + deltaY * axisY + deltaZ * axisZ;
  		float radius = max(dot * slope + mCurData.mConeRadius[j], 0.0f);
  		float sqrDistance = deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ - dot * dot;
  
  		Pointer<Shared, const uint32_t> mIt = mCapsuleMasks + 2 * j;
  		uint32_t bothMask = mIt[1];
  
  		if (sqrDistance > radius * radius)
  		{
  			shapeMask.mSpheres &= ~bothMask;
  			continue;
  		}
  
  		sqrDistance = max(sqrDistance, FLT_EPSILON);
  		float invDistance = rsqrtf(sqrDistance);
  
  		float base = dot + slope * sqrDistance * invDistance;
  
  		float halfLength = mCurData.mConeHalfLength[j];
  		uint32_t leftMask = base < -halfLength;
  		uint32_t rightMask = base > halfLength;
  
  		uint32_t firstMask = mIt[0];
  		uint32_t secondMask = firstMask ^ bothMask;
  
  		shapeMask.mSpheres &= ~(firstMask & leftMask - 1);
  		shapeMask.mSpheres &= ~(secondMask & rightMask - 1);
  
  		if (!leftMask && !rightMask)
  		{
  			deltaX = deltaX - base * axisX;
  			deltaY = deltaY - base * axisY;
  			deltaZ = deltaZ - base * axisZ;
  
  			float sqrCosine = mCurData.mConeSqrCosine[j];
  			float scale = radius * invDistance * sqrCosine - sqrCosine;
  
  			delta.x = delta.x + deltaX * scale;
  			delta.y = delta.y + deltaY * scale;
  			delta.z = delta.z + deltaZ * scale;
  
  			if (frictionEnabled)
  			{
  				int32_t s0 = mCapsuleIndices[2 * j];
  				int32_t s1 = mCapsuleIndices[2 * j + 1];
  
  				// load previous sphere pos
  				float s0vx = mCurData.mSphereX[s0] - mPrevData.mSphereX[s0];
  				float s0vy = mCurData.mSphereY[s0] - mPrevData.mSphereY[s0];
  				float s0vz = mCurData.mSphereZ[s0] - mPrevData.mSphereZ[s0];
  
  				float s1vx = mCurData.mSphereX[s1] - mPrevData.mSphereX[s1];
  				float s1vy = mCurData.mSphereY[s1] - mPrevData.mSphereY[s1];
  				float s1vz = mCurData.mSphereZ[s1] - mPrevData.mSphereZ[s1];
  
  				// interpolate velocity between the two spheres
  				float t = dot * 0.5f + 0.5f;
  
  				velocity.x += s0vx + t * (s1vx - s0vx);
  				velocity.y += s0vy + t * (s1vy - s0vy);
  				velocity.z += s0vz + t * (s1vz - s0vz);
  			}
  
  			++numCollisions;
  		}
  	}
  
  	// sphere collision
  	for (; shapeMask.mSpheres; shapeMask.mSpheres &= shapeMask.mSpheres - 1)
  	{
  		int32_t j = __ffs(shapeMask.mSpheres) - 1;
  
  		float deltaX = curPos.x - mCurData.mSphereX[j];
  		float deltaY = curPos.y - mCurData.mSphereY[j];
  		float deltaZ = curPos.z - mCurData.mSphereZ[j];
  
  		float sqrDistance = FLT_EPSILON + deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ;
  		float relDistance = rsqrtf(sqrDistance) * mCurData.mSphereW[j];
  
  		if (relDistance > 1.0f)
  		{
  			float scale = relDistance - 1.0f;
  
  			delta.x = delta.x + deltaX * scale;
  			delta.y = delta.y + deltaY * scale;
  			delta.z = delta.z + deltaZ * scale;
  
  			if (frictionEnabled)
  			{
  				velocity.x += mCurData.mSphereX[j] - mPrevData.mSphereX[j];
  				velocity.y += mCurData.mSphereY[j] - mPrevData.mSphereY[j];
  				velocity.z += mCurData.mSphereZ[j] - mPrevData.mSphereZ[j];
  			}
  
  			++numCollisions;
  		}
  	}
  
  	return numCollisions;
  }
  
  static const __device__ float gSkeletonWidth = (1 - 0.2f) * (1 - 0.2f) - 1;
  
  template <typename PrevPos, typename CurPos>
  __device__ int32_t
  CuCollision::collideCapsules(const PrevPos& prevPos, CurPos& curPos, float3& delta, float3& velocity) const
  {
  	ShapeMask shapeMask = getShapeMask(prevPos, curPos);
  
  	delta.x = delta.y = delta.z = 0.0f;
  	velocity.x = velocity.y = velocity.z = 0.0f;
  
  	int32_t numCollisions = 0;
  	bool frictionEnabled = gClothData.mFrictionScale > 0.0f;
  
  	// cone collision
  	for (; shapeMask.mCones; shapeMask.mCones &= shapeMask.mCones - 1)
  	{
  		int32_t j = __ffs(shapeMask.mCones) - 1;
  
  		float prevAxisX = mPrevData.mConeAxisX[j];
  		float prevAxisY = mPrevData.mConeAxisY[j];
  		float prevAxisZ = mPrevData.mConeAxisZ[j];
  		float prevSlope = mPrevData.mConeSlope[j];
  
  		float prevX = prevPos.x - mPrevData.mConeCenterX[j];
  		float prevY = prevPos.y - mPrevData.mConeCenterY[j];
  		float prevZ = prevPos.z - mPrevData.mConeCenterZ[j];
  		float prevT = prevY * prevAxisZ - prevZ * prevAxisY;
  		float prevU = prevZ * prevAxisX - prevX * prevAxisZ;
  		float prevV = prevX * prevAxisY - prevY * prevAxisX;
  		float prevDot = prevX * prevAxisX + prevY * prevAxisY + prevZ * prevAxisZ;
  		float prevRadius = max(prevDot * prevSlope + mCurData.mConeRadius[j], 0.0f);
  
  		float curAxisX = mCurData.mConeAxisX[j];
  		float curAxisY = mCurData.mConeAxisY[j];
  		float curAxisZ = mCurData.mConeAxisZ[j];
  		float curSlope = mCurData.mConeSlope[j];
  
  		float curX = curPos.x - mCurData.mConeCenterX[j];
  		float curY = curPos.y - mCurData.mConeCenterY[j];
  		float curZ = curPos.z - mCurData.mConeCenterZ[j];
  		float curT = curY * curAxisZ - curZ * curAxisY;
  		float curU = curZ * curAxisX - curX * curAxisZ;
  		float curV = curX * curAxisY - curY * curAxisX;
  		float curDot = curX * curAxisX + curY * curAxisY + curZ * curAxisZ;
  		float curRadius = max(curDot * curSlope + mCurData.mConeRadius[j], 0.0f);
  
  		float curSqrDistance = FLT_EPSILON + curT * curT + curU * curU + curV * curV;
  
  		float dotPrevPrev = prevT * prevT + prevU * prevU + prevV * prevV - prevRadius * prevRadius;
  		float dotPrevCur = prevT * curT + prevU * curU + prevV * curV - prevRadius * curRadius;
  		float dotCurCur = curSqrDistance - curRadius * curRadius;
  
  		float discriminant = dotPrevCur * dotPrevCur - dotCurCur * dotPrevPrev;
  		float sqrtD = sqrtf(discriminant);
  		float halfB = dotPrevCur - dotPrevPrev;
  		float minusA = dotPrevCur - dotCurCur + halfB;
  
  		// time of impact or 0 if prevPos inside cone
  		float toi = __fdividef(min(0.0f, halfB + sqrtD), minusA);
  		bool hasCollision = toi < 1.0f && halfB < sqrtD;
  
  		// skip continuous collision if the (un-clamped) particle
  		// trajectory only touches the outer skin of the cone.
  		float rMin = prevRadius + halfB * minusA * (curRadius - prevRadius);
  		hasCollision = hasCollision && (discriminant > minusA * rMin * rMin * gSkeletonWidth);
  
  		// a is negative when one cone is contained in the other,
  		// which is already handled by discrete collision.
  		hasCollision = hasCollision && minusA < -FLT_EPSILON;
  
  		if (hasCollision)
  		{
  			float deltaX = prevX - curX;
  			float deltaY = prevY - curY;
  			float deltaZ = prevZ - curZ;
  
  			// interpolate delta at toi
  			float posX = prevX - deltaX * toi;
  			float posY = prevY - deltaY * toi;
  			float posZ = prevZ - deltaZ * toi;
  
  			float curHalfLength = mCurData.mConeHalfLength[j];
  			float curScaledAxisX = curAxisX * curHalfLength;
  			float curScaledAxisY = curAxisY * curHalfLength;
  			float curScaledAxisZ = curAxisZ * curHalfLength;
  
  			float prevHalfLength = mPrevData.mConeHalfLength[j];
  			float deltaScaledAxisX = curScaledAxisX - prevAxisX * prevHalfLength;
  			float deltaScaledAxisY = curScaledAxisY - prevAxisY * prevHalfLength;
  			float deltaScaledAxisZ = curScaledAxisZ - prevAxisZ * prevHalfLength;
  
  			float oneMinusToi = 1.0f - toi;
  
  			// interpolate axis at toi
  			float axisX = curScaledAxisX - deltaScaledAxisX * oneMinusToi;
  			float axisY = curScaledAxisY - deltaScaledAxisY * oneMinusToi;
  			float axisZ = curScaledAxisZ - deltaScaledAxisZ * oneMinusToi;
  			float slope = prevSlope * oneMinusToi + curSlope * toi;
  
  			float sqrHalfLength = axisX * axisX + axisY * axisY + axisZ * axisZ;
  			float invHalfLength = rsqrtf(sqrHalfLength);
  			float dot = (posX * axisX + posY * axisY + posZ * axisZ) * invHalfLength;
  
  			float sqrDistance = posX * posX + posY * posY + posZ * posZ - dot * dot;
  			float invDistance = sqrDistance > 0.0f ? rsqrtf(sqrDistance) : 0.0f;
  
  			float base = dot + slope * sqrDistance * invDistance;
  			float scale = base * invHalfLength;
  
  			if (abs(scale) < 1.0f)
  			{
  				deltaX = deltaX + deltaScaledAxisX * scale;
  				deltaY = deltaY + deltaScaledAxisY * scale;
  				deltaZ = deltaZ + deltaScaledAxisZ * scale;
  
  				// reduce ccd impulse if (clamped) particle trajectory stays in cone skin,
  				// i.e. scale by exp2(-k) or 1/(1 + k) with k = (tmin - toi) / (1 - toi)
  				float minusK = __fdividef(sqrtD, minusA * oneMinusToi);
  				oneMinusToi = __fdividef(oneMinusToi, 1.f - minusK);
  
  				curX = curX + deltaX * oneMinusToi;
  				curY = curY + deltaY * oneMinusToi;
  				curZ = curZ + deltaZ * oneMinusToi;
  
  				curDot = curX * curAxisX + curY * curAxisY + curZ * curAxisZ;
  				curRadius = max(curDot * curSlope + mCurData.mConeRadius[j], 0.0f);
  				curSqrDistance = curX * curX + curY * curY + curZ * curZ - curDot * curDot;
  
  				curPos.x = mCurData.mConeCenterX[j] + curX;
  				curPos.y = mCurData.mConeCenterY[j] + curY;
  				curPos.z = mCurData.mConeCenterZ[j] + curZ;
  			}
  		}
  
  		// curPos inside cone (discrete collision)
  		bool hasContact = curRadius * curRadius > curSqrDistance;
  
  		Pointer<Shared, const uint32_t> mIt = mCapsuleMasks + 2 * j;
  		uint32_t bothMask = mIt[1];
  
  		uint32_t cullMask = bothMask & (hasCollision | hasContact) - 1;
  		shapeMask.mSpheres &= ~cullMask;
  
  		if (!hasContact)
  			continue;
  
  		float invDistance = curSqrDistance > 0.0f ? rsqrtf(curSqrDistance) : 0.0f;
  		float base = curDot + curSlope * curSqrDistance * invDistance;
  
  		float halfLength = mCurData.mConeHalfLength[j];
  		uint32_t leftMask = base < -halfLength;
  		uint32_t rightMask = base > halfLength;
  
  		// can only skip continuous sphere collision if post-ccd position
  		// is on code side *and* particle had cone-ccd collision.
  		uint32_t firstMask = mIt[0];
  		uint32_t secondMask = firstMask ^ bothMask;
  		cullMask = (firstMask & leftMask - 1) | (secondMask & rightMask - 1);
  		shapeMask.mSpheres &= ~cullMask | hasCollision - 1;
  
  		if (!leftMask && !rightMask)
  		{
  			float deltaX = curX - base * curAxisX;
  			float deltaY = curY - base * curAxisY;
  			float deltaZ = curZ - base * curAxisZ;
  
  			float sqrCosine = mCurData.mConeSqrCosine[j];
  			float scale = curRadius * invDistance * sqrCosine - sqrCosine;
  
  			delta.x = delta.x + deltaX * scale;
  			delta.y = delta.y + deltaY * scale;
  			delta.z = delta.z + deltaZ * scale;
  
  			if (frictionEnabled)
  			{
  				int32_t s0 = mCapsuleIndices[2 * j];
  				int32_t s1 = mCapsuleIndices[2 * j + 1];
  
  				// load previous sphere pos
  				float s0vx = mCurData.mSphereX[s0] - mPrevData.mSphereX[s0];
  				float s0vy = mCurData.mSphereY[s0] - mPrevData.mSphereY[s0];
  				float s0vz = mCurData.mSphereZ[s0] - mPrevData.mSphereZ[s0];
  
  				float s1vx = mCurData.mSphereX[s1] - mPrevData.mSphereX[s1];
  				float s1vy = mCurData.mSphereY[s1] - mPrevData.mSphereY[s1];
  				float s1vz = mCurData.mSphereZ[s1] - mPrevData.mSphereZ[s1];
  
  				// interpolate velocity between the two spheres
  				float t = curDot * 0.5f + 0.5f;
  
  				velocity.x += s0vx + t * (s1vx - s0vx);
  				velocity.y += s0vy + t * (s1vy - s0vy);
  				velocity.z += s0vz + t * (s1vz - s0vz);
  			}
  
  			++numCollisions;
  		}
  	}
  
  	// sphere collision
  	for (; shapeMask.mSpheres; shapeMask.mSpheres &= shapeMask.mSpheres - 1)
  	{
  		int32_t j = __ffs(shapeMask.mSpheres) - 1;
  
  		float prevX = prevPos.x - mPrevData.mSphereX[j];
  		float prevY = prevPos.y - mPrevData.mSphereY[j];
  		float prevZ = prevPos.z - mPrevData.mSphereZ[j];
  		float prevRadius = mPrevData.mSphereW[j];
  
  		float curX = curPos.x - mCurData.mSphereX[j];
  		float curY = curPos.y - mCurData.mSphereY[j];
  		float curZ = curPos.z - mCurData.mSphereZ[j];
  		float curRadius = mCurData.mSphereW[j];
  
  		float sqrDistance = FLT_EPSILON + curX * curX + curY * curY + curZ * curZ;
  
  		float dotPrevPrev = prevX * prevX + prevY * prevY + prevZ * prevZ - prevRadius * prevRadius;
  		float dotPrevCur = prevX * curX + prevY * curY + prevZ * curZ - prevRadius * curRadius;
  		float dotCurCur = sqrDistance - curRadius * curRadius;
  
  		float discriminant = dotPrevCur * dotPrevCur - dotCurCur * dotPrevPrev;
  		float sqrtD = sqrtf(discriminant);
  		float halfB = dotPrevCur - dotPrevPrev;
  		float minusA = dotPrevCur - dotCurCur + halfB;
  
  		// time of impact or 0 if prevPos inside sphere
  		float toi = __fdividef(min(0.0f, halfB + sqrtD), minusA);
  		bool hasCollision = toi < 1.0f && halfB < sqrtD;
  
  		// skip continuous collision if the (un-clamped) particle
  		// trajectory only touches the outer skin of the cone.
  		float rMin = prevRadius + halfB * minusA * (curRadius - prevRadius);
  		hasCollision = hasCollision && (discriminant > minusA * rMin * rMin * gSkeletonWidth);
  
  		// a is negative when one cone is contained in the other,
  		// which is already handled by discrete collision.
  		hasCollision = hasCollision && minusA < -FLT_EPSILON;
  
  		if (hasCollision)
  		{
  			float deltaX = prevX - curX;
  			float deltaY = prevY - curY;
  			float deltaZ = prevZ - curZ;
  
  			float oneMinusToi = 1.0f - toi;
  
  			// reduce ccd impulse if (clamped) particle trajectory stays in cone skin,
  			// i.e. scale by exp2(-k) or 1/(1 + k) with k = (tmin - toi) / (1 - toi)
  			float minusK = __fdividef(sqrtD, minusA * oneMinusToi);
  			oneMinusToi = __fdividef(oneMinusToi, 1 - minusK);
  
  			curX = curX + deltaX * oneMinusToi;
  			curY = curY + deltaY * oneMinusToi;
  			curZ = curZ + deltaZ * oneMinusToi;
  
  			curPos.x = mCurData.mSphereX[j] + curX;
  			curPos.y = mCurData.mSphereY[j] + curY;
  			curPos.z = mCurData.mSphereZ[j] + curZ;
  
  			sqrDistance = FLT_EPSILON + curX * curX + curY * curY + curZ * curZ;
  		}
  
  		float relDistance = rsqrtf(sqrDistance) * curRadius;
  
  		if (relDistance > 1.0f)
  		{
  			float scale = relDistance - 1.0f;
  
  			delta.x = delta.x + curX * scale;
  			delta.y = delta.y + curY * scale;
  			delta.z = delta.z + curZ * scale;
  
  			if (frictionEnabled)
  			{
  				velocity.x += mCurData.mSphereX[j] - mPrevData.mSphereX[j];
  				velocity.y += mCurData.mSphereY[j] - mPrevData.mSphereY[j];
  				velocity.z += mCurData.mSphereZ[j] - mPrevData.mSphereZ[j];
  			}
  
  			++numCollisions;
  		}
  	}
  
  	return numCollisions;
  }
  
  namespace
  {
  template <typename PrevPos, typename CurPos>
  __device__ inline float3 calcFrictionImpulse(const PrevPos& prevPos, const CurPos& curPos, const float3& shapeVelocity,
                                               float scale, const float3& collisionImpulse)
  {
  	const float frictionScale = gClothData.mFrictionScale;
  
  	// calculate collision normal
  	float deltaSq = collisionImpulse.x * collisionImpulse.x + collisionImpulse.y * collisionImpulse.y +
  	                collisionImpulse.z * collisionImpulse.z;
  
  	float rcpDelta = rsqrtf(deltaSq + FLT_EPSILON);
  
  	float nx = collisionImpulse.x * rcpDelta;
  	float ny = collisionImpulse.y * rcpDelta;
  	float nz = collisionImpulse.z * rcpDelta;
  
  	// calculate relative velocity scaled by number of collision
  	float rvx = curPos.x - prevPos.x - shapeVelocity.x * scale;
  	float rvy = curPos.y - prevPos.y - shapeVelocity.y * scale;
  	float rvz = curPos.z - prevPos.z - shapeVelocity.z * scale;
  
  	// calculate magnitude of relative normal velocity
  	float rvn = rvx * nx + rvy * ny + rvz * nz;
  
  	// calculate relative tangential velocity
  	float rvtx = rvx - rvn * nx;
  	float rvty = rvy - rvn * ny;
  	float rvtz = rvz - rvn * nz;
  
  	// calculate magnitude of vt
  	float rcpVt = rsqrtf(rvtx * rvtx + rvty * rvty + rvtz * rvtz + FLT_EPSILON);
  
  	// magnitude of friction impulse (cannot be larger than -|vt|)
  	float j = max(-frictionScale * deltaSq * rcpDelta * scale * rcpVt, -1.0f);
  
  	return make_float3(rvtx * j, rvty * j, rvtz * j);
  }
  }
  
  template <typename CurrentT, typename PreviousT>
  __device__ void CuCollision::collideCapsules(CurrentT& current, PreviousT& previous) const
  {
  	ProfileDetailZone zone(cloth::CuProfileZoneIds::COLLIDE_CAPSULES);
  
  	bool frictionEnabled = gClothData.mFrictionScale > 0.0f;
  	bool massScaleEnabled = gClothData.mCollisionMassScale > 0.0f;
  
  	for (int32_t i = threadIdx.x; i < gClothData.mNumParticles; i += blockDim.x)
  	{
  		typename CurrentT::VectorType curPos = current(i);
  
  		float3 delta, velocity;
  		if (int32_t numCollisions = collideCapsules(curPos, delta, velocity))
  		{
  			float scale = __fdividef(1.0f, numCollisions);
  
  			if (frictionEnabled)
  			{
  				typename PreviousT::VectorType prevPos = previous(i);
  				float3 frictionImpulse = calcFrictionImpulse(prevPos, curPos, velocity, scale, delta);
  
  				prevPos.x -= frictionImpulse.x;
  				prevPos.y -= frictionImpulse.y;
  				prevPos.z -= frictionImpulse.z;
  
  				previous(i) = prevPos;
  			}
  
  			curPos.x += delta.x * scale;
  			curPos.y += delta.y * scale;
  			curPos.z += delta.z * scale;
  
  			current(i) = curPos;
  
  			if (massScaleEnabled)
  			{
  				float deltaLengthSq = delta.x * delta.x + delta.y * delta.y + delta.z * delta.z;
  				float massScale = 1.0f + gClothData.mCollisionMassScale * deltaLengthSq;
  				current(i, 3) = __fdividef(current(i, 3), massScale);
  			}
  		}
  	}
  }
  
  #define NEW_LERP_AND_APPLY 1
  namespace
  {
  #if NEW_LERP_AND_APPLY
  template <typename ParticleDataT>
  __device__ float3 lerp(const ParticleDataT& particleData, const int4& indices, const float4& weights)
  {
  	typename ParticleDataT::VectorType posX, posY, posZ;
  	posX = particleData(indices.x);
  	posY = particleData(indices.y);
  	posZ = particleData(indices.z);
  
  	float3 result;
  	result.x = posX.x * weights.x + posY.x * weights.y + posZ.x * weights.z;
  	result.y = posX.y * weights.x + posY.y * weights.y + posZ.y * weights.z;
  	result.z = posX.z * weights.x + posY.z * weights.y + posZ.z * weights.z;
  	return result;
  }
  
  template <typename ParticleDataT>
  __device__ void apply(ParticleDataT& particleData, const int4& indices, const float4& weights, float3 delta, float scale)
  {
  	typename ParticleDataT::VectorType posX, posY, posZ;
  	posX = particleData(indices.x);
  	posY = particleData(indices.y);
  	posZ = particleData(indices.z);
  
  	delta.x *= scale;
  	delta.y *= scale;
  	delta.z *= scale;
  
  	posX.x += delta.x * weights.x; posY.x += delta.x * weights.y; posZ.x += delta.x * weights.z;
  	posX.y += delta.y * weights.x; posY.y += delta.y * weights.y; posZ.y += delta.y * weights.z;
  	posX.z += delta.z * weights.x; posY.z += delta.z * weights.y; posZ.z += delta.z * weights.z;
  
  	//if(particle is not static)        store new position
  	if(particleData(indices.x,3)!= 0) particleData(indices.x) = posX;
  	if(particleData(indices.y,3)!= 0) particleData(indices.y) = posY;
  	if(particleData(indices.z,3)!= 0) particleData(indices.z) = posZ;
  }
  #else
  template <typename PointerT>
  __device__ float lerp(PointerT pos, const int4& indices, const float4& weights)
  {
  	return pos[indices.x] * weights.x + pos[indices.y] * weights.y + pos[indices.z] * weights.z;
  }
  
  template <typename PointerT>
  __device__ void apply(PointerT pos, const int4& indices, const float4& weights, float delta)
  {
  	pos[indices.x] += delta * weights.x;
  	pos[indices.y] += delta * weights.y;
  	pos[indices.z] += delta * weights.z;
  }
  #endif
  }
  
  template <typename CurrentT, typename PreviousT>
  __device__ void CuCollision::collideVirtualCapsules(CurrentT& current, PreviousT& previous) const
  {
  	const uint32_t* __restrict setSizeIt = gClothData.mVirtualParticleSetSizesBegin;
  
  	if (!setSizeIt)
  		return;
  
  	if (gClothData.mEnableContinuousCollision)
  	{
  		// copied from mergeAcceleration
  		Pointer<Shared, uint32_t> dst = mShapeGrid + threadIdx.x;
  		if (!(threadIdx.x * 43 & 1024) && threadIdx.x < sGridSize * 12)
  			*dst &= dst[sGridSize * 3];
  		__syncthreads(); // mShapeGrid raw hazard
  	}
  
  	ProfileDetailZone zone(cloth::CuProfileZoneIds::COLLIDE_VIRTUAL_CAPSULES);
  
  	const uint32_t* __restrict setSizeEnd = gClothData.mVirtualParticleSetSizesEnd;
  	const uint16_t* __restrict indicesEnd = gClothData.mVirtualParticleIndices;
  	const float4* __restrict weightsIt = reinterpret_cast<const float4*>(gClothData.mVirtualParticleWeights);
  
  	bool frictionEnabled = gClothData.mFrictionScale > 0.0f;
  	bool massScaleEnabled = gClothData.mCollisionMassScale > 0.0f;
  
  	for (; setSizeIt != setSizeEnd; ++setSizeIt)
  	{
  		__syncthreads();
  
  		const uint16_t* __restrict indicesIt = indicesEnd + threadIdx.x * 4;
  		for (indicesEnd += *setSizeIt * 4; indicesIt < indicesEnd; indicesIt += blockDim.x * 4)
  		{
  			int4 indices = make_int4(indicesIt[0], indicesIt[1], indicesIt[2], indicesIt[3]);
  
  			float4 weights = weightsIt[indices.w];
  
  #if NEW_LERP_AND_APPLY
  			float3 curPos = lerp(current, indices, weights);
  #else
  			float3 curPos;
  			curPos.x = lerp(current[0], indices, weights);
  			curPos.y = lerp(current[1], indices, weights);
  			curPos.z = lerp(current[2], indices, weights);
  
  #endif
  			float3 delta, velocity;
  			if (int32_t numCollisions = collideCapsules(curPos, delta, velocity))
  			{
  				float scale = __fdividef(1.0f, numCollisions);
  				float wscale = weights.w * scale;
  
  #if NEW_LERP_AND_APPLY
  				apply(current, indices, weights, delta, wscale);
  #else
  				apply(current[0], indices, weights, delta.x * wscale);
  				apply(current[1], indices, weights, delta.y * wscale);
  				apply(current[2], indices, weights, delta.z * wscale);
  #endif
  				if (frictionEnabled)
  				{
  #if NEW_LERP_AND_APPLY
  					float3 prevPos = lerp(previous, indices, weights);
  #else
  					float3 prevPos;
  					prevPos.x = lerp(previous[0], indices, weights);
  					prevPos.y = lerp(previous[1], indices, weights);
  					prevPos.z = lerp(previous[2], indices, weights);
  #endif
  
  					float3 frictionImpulse = calcFrictionImpulse(prevPos, curPos, velocity, scale, delta);
  
  #if NEW_LERP_AND_APPLY
  					apply(previous, indices, weights, frictionImpulse, -weights.w);
  #else
  					apply(previous[0], indices, weights, frictionImpulse.x * -weights.w);
  					apply(previous[1], indices, weights, frictionImpulse.y * -weights.w);
  					apply(previous[2], indices, weights, frictionImpulse.z * -weights.w);
  #endif
  				}
  
  				if (massScaleEnabled)
  				{
  					float deltaLengthSq = (delta.x * delta.x + delta.y * delta.y + delta.z * delta.z) * scale * scale;
  					float invMassScale = __fdividef(1.0f, 1.0f + gClothData.mCollisionMassScale * deltaLengthSq);
  
  					// not multiplying by weights[3] here because unlike applying velocity
  					// deltas where we want the interpolated position to obtain a particular
  					// value, we instead just require that the total change is equal to invMassScale
  					invMassScale = invMassScale - 1.0f;
  					current(indices.x, 3) *= 1.0f + weights.x * invMassScale;
  					current(indices.y, 3) *= 1.0f + weights.y * invMassScale;
  					current(indices.z, 3) *= 1.0f + weights.z * invMassScale;
  				}
  			}
  		}
  	}
  }
  
  template <typename CurrentT, typename PreviousT>
  __device__ void CuCollision::collideContinuousCapsules(CurrentT& current, PreviousT& previous) const
  {
  	ProfileDetailZone zone(cloth::CuProfileZoneIds::COLLIDE_CONTINUOUS_CAPSULES);
  
  	bool frictionEnabled = gClothData.mFrictionScale > 0.0f;
  	bool massScaleEnabled = gClothData.mCollisionMassScale > 0.0f;
  
  	for (int32_t i = threadIdx.x; i < gClothData.mNumParticles; i += blockDim.x)
  	{
  		typename PreviousT::VectorType prevPos = previous(i);
  		typename CurrentT::VectorType curPos = current(i);
  
  		float3 delta, velocity;
  		if (int32_t numCollisions = collideCapsules(prevPos, curPos, delta, velocity))
  		{
  			float scale = __fdividef(1.0f, numCollisions);
  
  			if (frictionEnabled)
  			{
  				float3 frictionImpulse = calcFrictionImpulse(prevPos, curPos, velocity, scale, delta);
  
  				prevPos.x -= frictionImpulse.x;
  				prevPos.y -= frictionImpulse.y;
  				prevPos.z -= frictionImpulse.z;
  
  				previous(i) = prevPos;
  			}
  
  			curPos.x += delta.x * scale;
  			curPos.y += delta.y * scale;
  			curPos.z += delta.z * scale;
  
  			current(i) = curPos;
  
  			if (massScaleEnabled)
  			{
  				float deltaLengthSq = delta.x * delta.x + delta.y * delta.y + delta.z * delta.z;
  				float massScale = 1.0f + gClothData.mCollisionMassScale * deltaLengthSq;
  				current(i, 3) = __fdividef(current(i, 3), massScale);
  			}
  		}
  	}
  }
  
  template <typename CurPos>
  __device__ int32_t CuCollision::collideConvexes(const CurPos& positions, float3& delta) const
  {
  	ProfileDetailZone zone(cloth::CuProfileZoneIds::COLLIDE_CONVEXES);
  
  	delta.x = delta.y = delta.z = 0.0f;
  
  	Pointer<Shared, const float> planeX = mCurData.mSphereX;
  	Pointer<Shared, const float> planeY = planeX + gClothData.mNumPlanes;
  	Pointer<Shared, const float> planeZ = planeY + gClothData.mNumPlanes;
  	Pointer<Shared, const float> planeW = planeZ + gClothData.mNumPlanes;
  
  	int32_t numCollisions = 0;
  	Pointer<Shared, const uint32_t> cIt = mConvexMasks;
  	Pointer<Shared, const uint32_t> cEnd = cIt + gClothData.mNumConvexes;
  	for (; cIt != cEnd; ++cIt)
  	{
  		uint32_t mask = *cIt;
  
  		int32_t maxIndex = __ffs(mask) - 1;
  		float maxDist = planeW[maxIndex] + positions.z * planeZ[maxIndex] + positions.y * planeY[maxIndex] +
  		                positions.x * planeX[maxIndex];
  
  		while ((maxDist < 0.0f) && (mask &= mask - 1))
  		{
  			int32_t i = __ffs(mask) - 1;
  			float dist = planeW[i] + positions.z * planeZ[i] + positions.y * planeY[i] + positions.x * planeX[i];
  			if (dist > maxDist)
  				maxDist = dist, maxIndex = i;
  		}
  
  		if (maxDist < 0.0f)
  		{
  			delta.x -= planeX[maxIndex] * maxDist;
  			delta.y -= planeY[maxIndex] * maxDist;
  			delta.z -= planeZ[maxIndex] * maxDist;
  
  			++numCollisions;
  		}
  	}
  
  	return numCollisions;
  }
  
  template <typename CurrentT, typename PreviousT>
  __device__ void CuCollision::collideConvexes(CurrentT& current, PreviousT& previous, float alpha)
  {
  	if (!gClothData.mNumConvexes)
  		return;
  
  	// interpolate planes and transpose
  	if (threadIdx.x < gClothData.mNumPlanes * 4)
  	{
  		float start = gFrameData.mStartCollisionPlanes[threadIdx.x];
  		float target = gFrameData.mTargetCollisionPlanes[threadIdx.x];
  		int32_t j = threadIdx.x % 4 * gClothData.mNumPlanes + threadIdx.x / 4;
  		mCurData.mSphereX[j] = start + (target - start) * alpha;
  	}
  
  	__syncthreads();
  
  	bool frictionEnabled = gClothData.mFrictionScale > 0.0f;
  
  	for (int32_t i = threadIdx.x; i < gClothData.mNumParticles; i += blockDim.x)
  	{
  		typename CurrentT::VectorType curPos = current(i);
  
  		float3 delta;
  		if (int32_t numCollisions = collideConvexes(curPos, delta))
  		{
  			float scale = __fdividef(1.0f, numCollisions);
  
  			if (frictionEnabled)
  			{
  				typename PreviousT::VectorType prevPos = previous(i);
  
  				float3 frictionImpulse =
  				    calcFrictionImpulse(prevPos, curPos, make_float3(0.0f, 0.0f, 0.0f), scale, delta);
  
  				prevPos.x -= frictionImpulse.x;
  				prevPos.y -= frictionImpulse.y;
  				prevPos.z -= frictionImpulse.z;
  
  				previous(i) = prevPos;
  			}
  
  			curPos.x += delta.x * scale;
  			curPos.y += delta.y * scale;
  			curPos.z += delta.z * scale;
  
  			current(i) = curPos;
  		}
  	}
  
  	__syncthreads();
  }
  
  namespace
  {
  struct TriangleData
  {
  	float baseX, baseY, baseZ;
  	float edge0X, edge0Y, edge0Z;
  	float edge1X, edge1Y, edge1Z;
  	float normalX, normalY, normalZ;
  
  	float edge0DotEdge1;
  	float edge0SqrLength;
  	float edge1SqrLength;
  
  	float det;
  	float denom;
  
  	float edge0InvSqrLength;
  	float edge1InvSqrLength;
  
  	// initialize struct after vertices have been stored in first 9 members
  	__device__ void initialize()
  	{
  		edge0X -= baseX, edge0Y -= baseY, edge0Z -= baseZ;
  		edge1X -= baseX, edge1Y -= baseY, edge1Z -= baseZ;
  
  		normalX = edge0Y * edge1Z - edge0Z * edge1Y;
  		normalY = edge0Z * edge1X - edge0X * edge1Z;
  		normalZ = edge0X * edge1Y - edge0Y * edge1X;
  
  		float normalInvLength = rsqrtf(normalX * normalX + normalY * normalY + normalZ * normalZ);
  		normalX *= normalInvLength;
  		normalY *= normalInvLength;
  		normalZ *= normalInvLength;
  
  		edge0DotEdge1 = edge0X * edge1X + edge0Y * edge1Y + edge0Z * edge1Z;
  		edge0SqrLength = edge0X * edge0X + edge0Y * edge0Y + edge0Z * edge0Z;
  		edge1SqrLength = edge1X * edge1X + edge1Y * edge1Y + edge1Z * edge1Z;
  
  		det = __fdividef(1.0f, edge0SqrLength * edge1SqrLength - edge0DotEdge1 * edge0DotEdge1);
  		denom = __fdividef(1.0f, edge0SqrLength + edge1SqrLength - edge0DotEdge1 - edge0DotEdge1);
  
  		edge0InvSqrLength = __fdividef(1.0f, edge0SqrLength);
  		edge1InvSqrLength = __fdividef(1.0f, edge1SqrLength);
  	}
  };
  }
  
  template <typename CurrentT>
  __device__ void CuCollision::collideTriangles(CurrentT& current, int32_t i)
  {
  	ProfileDetailZone zone(cloth::CuProfileZoneIds::COLLIDE_TRIANGLES);
  
  	float posX = current(i, 0);
  	float posY = current(i, 1);
  	float posZ = current(i, 2);
  
  	const TriangleData* __restrict tIt = reinterpret_cast<const TriangleData*>(generic(mCurData.mSphereX));
  	const TriangleData* __restrict tEnd = tIt + gClothData.mNumCollisionTriangles;
  
  	float normalX, normalY, normalZ, normalD = 0.0f;
  	float minSqrLength = FLT_MAX;
  
  	for (; tIt != tEnd; ++tIt)
  	{
  		float dx = posX - tIt->baseX;
  		float dy = posY - tIt->baseY;
  		float dz = posZ - tIt->baseZ;
  
  		float deltaDotEdge0 = dx * tIt->edge0X + dy * tIt->edge0Y + dz * tIt->edge0Z;
  		float deltaDotEdge1 = dx * tIt->edge1X + dy * tIt->edge1Y + dz * tIt->edge1Z;
  		float deltaDotNormal = dx * tIt->normalX + dy * tIt->normalY + dz * tIt->normalZ;
  
  		float s = tIt->edge1SqrLength * deltaDotEdge0 - tIt->edge0DotEdge1 * deltaDotEdge1;
  		float t = tIt->edge0SqrLength * deltaDotEdge1 - tIt->edge0DotEdge1 * deltaDotEdge0;
  
  		s = t > 0.0f ? s * tIt->det : deltaDotEdge0 * tIt->edge0InvSqrLength;
  		t = s > 0.0f ? t * tIt->det : deltaDotEdge1 * tIt->edge1InvSqrLength;
  
  		if (s + t > 1.0f)
  		{
  			s = (tIt->edge1SqrLength - tIt->edge0DotEdge1 + deltaDotEdge0 - deltaDotEdge1) * tIt->denom;
  		}
  
  		s = fmaxf(0.0f, fminf(1.0f, s));
  		t = fmaxf(0.0f, fminf(1.0f - s, t));
  
  		dx = dx - tIt->edge0X * s - tIt->edge1X * t;
  		dy = dy - tIt->edge0Y * s - tIt->edge1Y * t;
  		dz = dz - tIt->edge0Z * s - tIt->edge1Z * t;
  
  		float sqrLength = dx * dx + dy * dy + dz * dz;
  
  		if (0.0f > deltaDotNormal)
  			sqrLength *= 1.0001f;
  
  		if (sqrLength < minSqrLength)
  		{
  			normalX = tIt->normalX;
  			normalY = tIt->normalY;
  			normalZ = tIt->normalZ;
  			normalD = deltaDotNormal;
  			minSqrLength = sqrLength;
  		}
  	}
  
  	if (normalD < 0.0f)
  	{
  		current(i, 0) = posX - normalX * normalD;
  		current(i, 1) = posY - normalY * normalD;
  		current(i, 2) = posZ - normalZ * normalD;
  	}
  }
  
  namespace
  {
  static const int32_t sTrianglePadding = sizeof(TriangleData) / sizeof(float) - 9;
  }
  
  template <typename CurrentT>
  __device__ void CuCollision::collideTriangles(CurrentT& current, float alpha)
  {
  	if (!gClothData.mNumCollisionTriangles)
  		return;
  
  	// interpolate triangle vertices and store in shared memory
  	for (int32_t i = threadIdx.x, n = gClothData.mNumCollisionTriangles * 9; i < n; i += blockDim.x)
  	{
  		float start = gFrameData.mStartCollisionTriangles[i];
  		float target = gFrameData.mTargetCollisionTriangles[i];
  		int32_t idx = i * 7282 >> 16; // same as i/9
  		int32_t offset = i + idx * sTrianglePadding;
  		mCurData.mSphereX[offset] = start + (target - start) * alpha;
  	}
  
  	__syncthreads();
  
  	for (int32_t i = threadIdx.x; i < gClothData.mNumCollisionTriangles; i += blockDim.x)
  	{
  		reinterpret_cast<TriangleData*>(generic(mCurData.mSphereX))[i].initialize();
  	}
  
  	__syncthreads();
  
  	for (int32_t i = threadIdx.x; i < gClothData.mNumParticles; i += blockDim.x)
  		collideTriangles(current, i);
  
  	__syncthreads();
  }
  