// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of NVIDIA CORPORATION nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.  

#ifndef GU_VEC_CONVEXHULL_H
#define GU_VEC_CONVEXHULL_H

#include "common/PxPhysXCommonConfig.h"
#include "geometry/PxMeshScale.h"
#include "GuConvexMesh.h"
#include "GuVecConvex.h"
#include "GuConvexMeshData.h"
#include "GuBigConvexData.h"
#include "GuConvexSupportTable.h"
#include "GuCubeIndex.h"
#include "foundation/PxFPU.h"
#include "foundation/PxVecQuat.h"
#include "GuShapeConvex.h"

namespace physx
{
namespace Gu
{
#define CONVEX_MARGIN_RATIO			0.1f
#define CONVEX_MIN_MARGIN_RATIO		0.05f
#define	CONVEX_SWEEP_MARGIN_RATIO	0.025f
#define TOLERANCE_MARGIN_RATIO		0.08f
#define TOLERANCE_MIN_MARGIN_RATIO	0.05f

	//This margin is used in Persistent contact manifold
	PX_SUPPORT_FORCE_INLINE aos::FloatV CalculatePCMConvexMargin(const Gu::ConvexHullData* hullData, const aos::Vec3VArg scale, 
		const PxReal toleranceLength, const PxReal toleranceRatio = TOLERANCE_MIN_MARGIN_RATIO)
	{
		
		using namespace aos;
		const Vec3V extents = V3Mul(V3LoadU_SafeReadW(hullData->mInternal.mInternalExtents), scale);
		const FloatV min = V3ExtractMin(extents);
		const FloatV toleranceMargin = FLoad(toleranceLength * toleranceRatio);
		//ML: 25% of the minimum extents of the internal AABB as this convex hull's margin
		return FMin(FMul(min, FLoad(0.25f)), toleranceMargin);
	}

	PX_SUPPORT_FORCE_INLINE aos::FloatV CalculateMTDConvexMargin(const Gu::ConvexHullData* hullData, const aos::Vec3VArg scale)
	{
		using namespace aos;
		const Vec3V extents = V3Mul(V3LoadU_SafeReadW(hullData->mInternal.mInternalExtents), scale);
		const FloatV min = V3ExtractMin(extents);
		//ML: 25% of the minimum extents of the internal AABB as this convex hull's margin
		return FMul(min, FLoad(0.25f));
	}

	//This minMargin is used in PCM contact gen
	PX_SUPPORT_FORCE_INLINE void CalculateConvexMargin(const InternalObjectsData& internalObject, PxReal& margin, PxReal& minMargin, PxReal& sweepMargin,
		const aos::Vec3VArg scale)
	{
		using namespace aos;
		
		const Vec3V extents = V3Mul(V3LoadU_SafeReadW(internalObject.mInternalExtents), scale);
		const FloatV min_ = V3ExtractMin(extents);

		PxReal minExtent;
		FStore(min_, &minExtent);

		//margin is used as acceptanceTolerance for overlap.
		margin = minExtent * CONVEX_MARGIN_RATIO;
		//minMargin is used in the GJK termination condition
		minMargin = minExtent * CONVEX_MIN_MARGIN_RATIO;
		//this is used for sweep(gjkRaycast)
		sweepMargin = minExtent * CONVEX_SWEEP_MARGIN_RATIO;
	}


	PX_SUPPORT_FORCE_INLINE aos::Mat33V ConstructSkewMatrix(const aos::Vec3VArg scale, const aos::QuatVArg rotation) 
	{
		using namespace aos;
		Mat33V rot;
		QuatGetMat33V(rotation, rot.col0, rot.col1, rot.col2);
		Mat33V trans = M33Trnsps(rot);
		trans.col0 = V3Scale(trans.col0, V3GetX(scale));
		trans.col1 = V3Scale(trans.col1, V3GetY(scale));
		trans.col2 = V3Scale(trans.col2, V3GetZ(scale));
		return M33MulM33(trans, rot);
	}

	PX_SUPPORT_FORCE_INLINE void ConstructSkewMatrix(const aos::Vec3VArg scale, const aos::QuatVArg rotation, aos::Mat33V& vertex2Shape, aos::Mat33V& shape2Vertex, aos::Vec3V& center, const bool idtScale) 
	{
		using namespace aos;

		PX_ASSERT(!V3AllEq(scale, V3Zero()));
	
		if(idtScale)
		{
			//create identity buffer
			const Mat33V identity = M33Identity();
			vertex2Shape = identity;
			shape2Vertex = identity;
		}
		else
		{
			const FloatV scaleX = V3GetX(scale);
			const Vec3V invScale = V3Recip(scale);

			//this is uniform scale
			if(V3AllEq(V3Splat(scaleX), scale))
			{	
				vertex2Shape = M33Diagonal(scale);
				shape2Vertex = M33Diagonal(invScale);
			}
			else
			{
				Mat33V rot;
				QuatGetMat33V(rotation, rot.col0, rot.col1, rot.col2);
				const Mat33V trans = M33Trnsps(rot);
				/*
					vertex2shape
					skewMat = Inv(R)*Diagonal(scale)*R;
				*/

				const Mat33V temp(V3Scale(trans.col0, scaleX), V3Scale(trans.col1, V3GetY(scale)), V3Scale(trans.col2, V3GetZ(scale)));
				vertex2Shape = M33MulM33(temp, rot);

				//don't need it in the support function
				/*
					shape2Vertex
					invSkewMat =(invSkewMat)= Inv(R)*Diagonal(1/scale)*R;
				*/
				
				shape2Vertex.col0 = V3Scale(trans.col0, V3GetX(invScale));
				shape2Vertex.col1 = V3Scale(trans.col1, V3GetY(invScale));
				shape2Vertex.col2 = V3Scale(trans.col2, V3GetZ(invScale));
				shape2Vertex = M33MulM33(shape2Vertex, rot);

				//shape2Vertex = M33Inverse(vertex2Shape);
			}

			//transform center to shape space
			center = M33MulV3(vertex2Shape, center);
		}
	}

	PX_SUPPORT_FORCE_INLINE aos::Mat33V ConstructVertex2ShapeMatrix(const aos::Vec3VArg scale, const aos::QuatVArg rotation) 
	{
		using namespace aos;
		Mat33V rot;
		QuatGetMat33V(rotation, rot.col0, rot.col1, rot.col2);
		const Mat33V trans = M33Trnsps(rot);
		/*
			vertex2shape
			skewMat = Inv(R)*Diagonal(scale)*R;
		*/

		const Mat33V temp(V3Scale(trans.col0, V3GetX(scale)), V3Scale(trans.col1, V3GetY(scale)), V3Scale(trans.col2, V3GetZ(scale)));
		return M33MulM33(temp, rot);
	}


	class ConvexHullV : public ConvexV
	{

		class TinyBitMap
		{
		public:
			PxU32 m[8];
			PX_FORCE_INLINE TinyBitMap() { m[0] = m[1] = m[2] = m[3] = m[4] = m[5] = m[6] = m[7] = 0; }
			PX_FORCE_INLINE void set(PxU8 v) { m[v >> 5] |= 1 << (v & 31); }
			PX_FORCE_INLINE bool get(PxU8 v) const { return (m[v >> 5] & 1 << (v & 31)) != 0; }
		};


	public:
		/**
		\brief Constructor
		*/
		PX_SUPPORT_INLINE ConvexHullV() : ConvexV(ConvexType::eCONVEXHULL)
		{
		}

		PX_SUPPORT_INLINE ConvexHullV(const Gu::ConvexHullData* _hullData, const aos::Vec3VArg _center, const aos::Vec3VArg scale, const aos::QuatVArg scaleRot,
			const bool idtScale) :
			ConvexV(ConvexType::eCONVEXHULL, _center)
		{
			using namespace aos;

			hullData = _hullData;
			const PxVec3* PX_RESTRICT tempVerts = _hullData->getHullVertices();
			verts = tempVerts;
			numVerts = _hullData->mNbHullVertices;
			CalculateConvexMargin(_hullData->mInternal, margin, minMargin, sweepMargin, scale);
			ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, center, idtScale);
			data = _hullData->mBigConvexRawData;
		}

		PX_SUPPORT_INLINE ConvexHullV(const Gu::ConvexHullData* _hullData, const aos::Vec3VArg _center) :
			ConvexV(ConvexType::eCONVEXHULL, _center)
		{
			using namespace aos;

			hullData = _hullData;
			verts = _hullData->getHullVertices();
			numVerts = _hullData->mNbHullVertices;
			data = _hullData->mBigConvexRawData;
		}

		//this is used by CCD system
		PX_SUPPORT_INLINE ConvexHullV(const PxGeometry& geom) : ConvexV(ConvexType::eCONVEXHULL, aos::V3Zero())
		{
			using namespace aos;
			const PxConvexMeshGeometry& convexGeom = static_cast<const PxConvexMeshGeometry&>(geom);
			const Gu::ConvexHullData* hData = _getHullData(convexGeom);

			const Vec3V vScale = V3LoadU_SafeReadW(convexGeom.scale.scale);	// PT: safe because 'rotation' follows 'scale' in PxMeshScale
			const QuatV vRot = QuatVLoadU(&convexGeom.scale.rotation.x);
			const bool idtScale = convexGeom.scale.isIdentity();

			hullData = hData;
			const PxVec3* PX_RESTRICT tempVerts = hData->getHullVertices();
			verts = tempVerts;
			numVerts = hData->mNbHullVertices;
			CalculateConvexMargin(hData->mInternal, margin, minMargin, sweepMargin, vScale);
			ConstructSkewMatrix(vScale, vRot, vertex2Shape, shape2Vertex, center, idtScale);

			data = hData->mBigConvexRawData;
		}

		//this is used by convex vs tetrahedron collision
		PX_SUPPORT_INLINE ConvexHullV(const Gu::PolygonalData& polyData, const Cm::FastVertex2ShapeScaling& convexScale) :
			ConvexV(ConvexType::eCONVEXHULL, aos::V3LoadU(polyData.mCenter))
		{
			using namespace aos;

			const Vec3V vScale = V3LoadU(polyData.mScale.scale);

			verts = polyData.mVerts;
			numVerts = PxU8(polyData.mNbVerts);
			CalculateConvexMargin(polyData.mInternal, margin, minMargin, sweepMargin, vScale);

			const PxMat33& v2s = convexScale.getVertex2ShapeSkew();
			const PxMat33& s2v = convexScale.getShape2VertexSkew();

			vertex2Shape.col0 = V3LoadU(v2s.column0);
			vertex2Shape.col1 = V3LoadU(v2s.column1);
			vertex2Shape.col2 = V3LoadU(v2s.column2);

			shape2Vertex.col0 = V3LoadU(s2v.column0);
			shape2Vertex.col1 = V3LoadU(s2v.column1);
			shape2Vertex.col2 = V3LoadU(s2v.column2);

			data = polyData.mBigData;

		}

		PX_SUPPORT_INLINE void initialize(const Gu::ConvexHullData* _hullData, const aos::Vec3VArg _center, const aos::Vec3VArg scale,
			const aos::QuatVArg scaleRot, const bool idtScale)
		{
			using namespace aos;

			const PxVec3* tempVerts = _hullData->getHullVertices();
			CalculateConvexMargin(_hullData->mInternal, margin, minMargin, sweepMargin, scale);
			ConstructSkewMatrix(scale, scaleRot, vertex2Shape, shape2Vertex, center, idtScale);

			verts = tempVerts;
			numVerts = _hullData->mNbHullVertices;
			//rot = _rot;	

			center = _center;

			//	searchIndex = 0;
			data = _hullData->mBigConvexRawData;

			hullData = _hullData;
			if (_hullData->mBigConvexRawData)
			{
				PxPrefetchLine(hullData->mBigConvexRawData->mValencies);
				PxPrefetchLine(hullData->mBigConvexRawData->mValencies, 128);
				PxPrefetchLine(hullData->mBigConvexRawData->mAdjacentVerts);
			}
		}


	
		PX_FORCE_INLINE void resetMargin(const PxReal toleranceLength)
		{
			const PxReal toleranceMinMargin = toleranceLength * TOLERANCE_MIN_MARGIN_RATIO;
			const PxReal toleranceMargin = toleranceLength * TOLERANCE_MARGIN_RATIO;

			margin = PxMin(margin, toleranceMargin);
			minMargin = PxMin(minMargin, toleranceMinMargin);
		}

		PX_FORCE_INLINE aos::Vec3V supportPoint(const PxI32 index)const
		{
			using namespace aos;
			
			return M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[index]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
		}

		PX_NOINLINE PxU32 hillClimbing(const aos::Vec3VArg _dir)const
		{
			using namespace aos;

			const Gu::Valency* valency = data->mValencies;
			const PxU8* adjacentVerts = data->mAdjacentVerts;
			
			//NotSoTinyBitMap visited;
			PxU32 smallBitMap[8] = {0,0,0,0,0,0,0,0};

		//	PxU32 index = searchIndex;
			PxU32 index = 0;

			{
				PxVec3 vertexSpaceDirection;
				V3StoreU(_dir, vertexSpaceDirection);
				const PxU32 offset = ComputeCubemapNearestOffset(vertexSpaceDirection, data->mSubdiv);
				//const PxU32 offset = ComputeCubemapOffset(vertexSpaceDirection, data->mSubdiv);
				index = data->mSamples[offset];
			}

			Vec3V maxPoint = V3LoadU_SafeReadW(verts[index]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
			FloatV max = V3Dot(maxPoint, _dir);
	
			PxU32 initialIndex = index;
			
			do
			{
				initialIndex = index;
				const PxU32 numNeighbours = valency[index].mCount;
				const PxU32 offset = valency[index].mOffset;

				for(PxU32 a = 0; a < numNeighbours; ++a)
				{
					const PxU32 neighbourIndex = adjacentVerts[offset + a];

					const Vec3V vertex = V3LoadU_SafeReadW(verts[neighbourIndex]);	// PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
					const FloatV dist = V3Dot(vertex, _dir);
					if(FAllGrtr(dist, max))
					{
						const PxU32 ind = neighbourIndex>>5;
						const PxU32 mask = PxU32(1 << (neighbourIndex & 31));
						if((smallBitMap[ind] & mask) == 0)
						{
							smallBitMap[ind] |= mask;
							max = dist;
							index = neighbourIndex;
						}
					}
				}

			}while(index != initialIndex);

			return index;
		}

		PX_SUPPORT_INLINE PxU32 bruteForceSearch(const aos::Vec3VArg _dir)const 
		{
			using namespace aos;
			//brute force
			PxVec3 dir;
			V3StoreU(_dir, dir);

			PxReal max = verts[0].dot(dir);
			PxU32 maxIndex = 0;

			for (PxU32 i = 1; i < numVerts; ++i)
			{
				const PxReal dist = verts[i].dot(dir);
				if (dist > max)
				{
					max = dist;
					maxIndex = i;
				}
			}
			return maxIndex;
		}

		//points are in vertex space, _dir in vertex space
		PX_NOINLINE PxU32 supportVertexIndex(const aos::Vec3VArg _dir)const
		{
			using namespace aos;
			if(data)
				return hillClimbing(_dir);
			else
				return bruteForceSearch(_dir);
		}

		//dir is in the vertex space
		PX_SUPPORT_INLINE void bruteForceSearchMinMax(const aos::Vec3VArg _dir, aos::FloatV& min, aos::FloatV& max)const 
		{
			using namespace aos;

			//brute force
			PxVec3 dir;
			V3StoreU(_dir, dir);
			//get the support point from the orignal margin
			PxReal _max = verts[0].dot(dir);
			PxReal _min = _max;

			for(PxU32 i = 1; i < numVerts; ++i)
			{ 
				const PxReal dist = verts[i].dot(dir);
				_max = PxMax(dist, _max);
				_min = PxMin(dist, _min);
			}
			min = FLoad(_min);
			max = FLoad(_max);
		}

		//This function is used in the full contact manifold generation code, points are in vertex space.
		//This function support scaling, _dir is in the shape space	
		PX_SUPPORT_INLINE void supportVertexMinMax(const aos::Vec3VArg _dir, aos::FloatV& min, aos::FloatV& max)const
		{
			using namespace aos;

			//dir is in the vertex space
			const Vec3V dir = M33TrnspsMulV3(vertex2Shape, _dir);

			if(data)
			{
				const PxU32 maxIndex = hillClimbing(dir);
				const PxU32 minIndex = hillClimbing(V3Neg(dir));
				const Vec3V maxPoint = M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[maxIndex]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
				const Vec3V minPoint = M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[minIndex]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
				min = V3Dot(_dir, minPoint);
				max = V3Dot(_dir, maxPoint);
			}
			else
			{
				//dir is in the vertex space
				bruteForceSearchMinMax(dir, min, max);
			}
		}  
 
		//This function is used in the full contact manifold generation code
		PX_SUPPORT_INLINE void populateVerts(const PxU8* inds, PxU32 numInds, const PxVec3* originalVerts, aos::Vec3V* _verts)const
		{
			using namespace aos;

			for(PxU32 i=0; i<numInds; ++i)
				_verts[i] = M33MulV3(vertex2Shape, V3LoadU_SafeReadW(originalVerts[inds[i]]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'populateVerts' is always called with polyData.mVerts)
		}

		//This function is used in epa
		//dir is in the shape space
		PX_SUPPORT_INLINE aos::Vec3V supportLocal(const aos::Vec3VArg dir)const
		{
			using namespace aos;
			//scale dir and put it in the vertex space
			const Vec3V _dir = M33TrnspsMulV3(vertex2Shape, dir);
			const PxU32 maxIndex = supportVertexIndex(_dir);
			return M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[maxIndex]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
		}

		//this is used in the sat test for the full contact gen
		PX_SUPPORT_INLINE void supportLocal(const aos::Vec3VArg dir, aos::FloatV& min, aos::FloatV& max)const
		{
			using namespace aos;
			//dir is in the shape space
			supportVertexMinMax(dir, min, max);
		}

		//This function is used in epa
		PX_SUPPORT_INLINE aos::Vec3V supportRelative(const aos::Vec3VArg dir, const aos::PxMatTransformV& aTob, const aos::PxMatTransformV& aTobT) const
		{
			using namespace aos;
		
			//transform dir into the shape space
//			const Vec3V dir_ = aTob.rotateInv(dir);//relTra.rotateInv(dir);
			const Vec3V dir_ = aTobT.rotate(dir);//relTra.rotateInv(dir);
			const Vec3V maxPoint = supportLocal(dir_);
			//translate maxPoint from shape space of a back to the b space
			return aTob.transform(maxPoint);//relTra.transform(maxPoint);
		}

		//dir in the shape space, this function is used in gjk	
		PX_SUPPORT_INLINE aos::Vec3V supportLocal(const aos::Vec3VArg dir, PxI32& index)const
		{
			using namespace aos;
			//scale dir and put it in the vertex space, for non-uniform scale, we don't want the scale in the dir, therefore, we are using
			//the transpose of the inverse of shape2Vertex(which is vertex2shape). This will allow us igore the scale and keep the rotation
			const Vec3V dir_ = M33TrnspsMulV3(vertex2Shape, dir);
			//get the extreme point index
			const PxU32 maxIndex = supportVertexIndex(dir_);
			index = PxI32(maxIndex);
			//p is in the shape space
			return M33MulV3(vertex2Shape, V3LoadU_SafeReadW(verts[index]));	// PT: safe because of the way vertex memory is allocated in ConvexHullData (and 'verts' is initialized with ConvexHullData::getHullVertices())
		}

		//this function is used in gjk	
		PX_SUPPORT_INLINE aos::Vec3V supportRelative(	const aos::Vec3VArg dir, const aos::PxMatTransformV& aTob,
															const aos::PxMatTransformV& aTobT, PxI32& index)const
		{
			using namespace aos;

			//transform dir from b space to the shape space of a space
//			const Vec3V dir_ = aTob.rotateInv(dir);//relTra.rotateInv(dir);//M33MulV3(skewInvRot, dir);
			const Vec3V dir_ = aTobT.rotate(dir);//relTra.rotateInv(dir);//M33MulV3(skewInvRot, dir);
			const Vec3V p = supportLocal(dir_, index);
			//transfrom from a to b space
			return aTob.transform(p);
		}

		aos::Mat33V vertex2Shape;//inv(R)*S*R
		aos::Mat33V shape2Vertex;//inv(vertex2Shape)

		const Gu::ConvexHullData* hullData;
		const BigConvexRawData* data;  
		const PxVec3* verts;
		PxU8 numVerts;
	};

}

}

#endif	//