//
// Convex-Convex collision algorithm
//
void btConvexConvexAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
{
	if (!m_manifoldPtr)
	{
		//swapped?
		m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject());
		m_ownManifold = true;
	}

	resultOut->setPersistentManifold(m_manifoldPtr);

	//comment-out next line to test multi-contact generation
	//resultOut->getPersistentManifold()->clearManifold();
	

	const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
	const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());

	btVector3  normalOnB;
		btVector3  pointOnBWorld;
#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
	if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
	{
		btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
		btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
	//	btVector3 localScalingA = capsuleA->getLocalScaling();
	//	btVector3 localScalingB = capsuleB->getLocalScaling();
		
		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();

		btScalar dist = capsuleCapsuleDistance(normalOnB,	pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
			capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(),
			body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold);

		if (dist<threshold)
		{
			btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
			resultOut->addContactPoint(normalOnB, pointOnBWorld, dist);	
		}
		resultOut->refreshContactPoints();
		return;
	}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER




#ifdef USE_SEPDISTANCE_UTIL2
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(), body1->getWorldTransform());
	}

	if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
#endif //USE_SEPDISTANCE_UTIL2

	btGjkPairDetector::ClosestPointInput input;

	btGjkPairDetector	gjkPairDetector(min0, min1, &m_simplexSolver, m_pdSolver);
	//TODO: if (dispatchInfo.m_useContinuous)
	gjkPairDetector.setMinkowskiA(min0);
	gjkPairDetector.setMinkowskiB(min1);

#ifdef USE_SEPDISTANCE_UTIL2
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
	} else
#endif //USE_SEPDISTANCE_UTIL2
	{
		//if (dispatchInfo.m_convexMaxDistanceUseCPT)
		//{
		//	input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
		//} else
		//{
		input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
//		}

		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
	}

	input.m_stackAlloc = dispatchInfo.m_stackAllocator;
	input.m_transformA = body0Wrap->getWorldTransform();
	input.m_transformB = body1Wrap->getWorldTransform();



	

#ifdef USE_SEPDISTANCE_UTIL2
	btScalar sepDist = 0.f;
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		sepDist = gjkPairDetector.getCachedSeparatingDistance();
		if (sepDist>SIMD_EPSILON)
		{
			sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
			//now perturbe directions to get multiple contact points
			
		}
	}
#endif //USE_SEPDISTANCE_UTIL2

	if (min0->isPolyhedral() && min1->isPolyhedral())
	{


		struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
		{
			virtual void setShapeIdentifiersA(int partId0, int index0){}
			virtual void setShapeIdentifiersB(int partId1, int index1){}
			virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) 
			{
			}
		};

		
		struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
		{
			btDiscreteCollisionDetectorInterface::Result* m_originalResult;
			btVector3	m_reportedNormalOnWorld;
			btScalar m_marginOnA;
			btScalar m_marginOnB;
			btScalar	m_reportedDistance;
			
			bool		m_foundResult;
			btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
			:m_originalResult(result),
			m_marginOnA(marginOnA),
			m_marginOnB(marginOnB),
			m_foundResult(false)
			{
			}
			
			virtual void setShapeIdentifiersA(int partId0, int index0){}
			virtual void setShapeIdentifiersB(int partId1, int index1){}
			virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorldOrg, btScalar depthOrg) 
			{
				m_reportedDistance = depthOrg;
				m_reportedNormalOnWorld = normalOnBInWorld;
				
				btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
				m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
				if (m_reportedDistance<0.f)
				{
					m_foundResult = true;					
				}
				m_originalResult->addContactPoint(normalOnBInWorld, adjustedPointB, m_reportedDistance);
			}
		};

		
		btDummyResult dummy;

		///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it

		btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
		btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();

		btWithoutMarginResult	withoutMargin(resultOut, min0Margin, min1Margin);

		btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
		btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
		if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
		{
			btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();

			btScalar minDist = -1e30f;
			btVector3 sepNormalWorldSpace;
			bool foundSepAxis  = true;

			if (dispatchInfo.m_enableSatConvex)
			{
				foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
					body0Wrap->getWorldTransform(), 
					body1Wrap->getWorldTransform(),
					sepNormalWorldSpace,*resultOut);
			} else
			{
#ifdef ZERO_MARGIN
				gjkPairDetector.setIgnoreMargin(true);
				gjkPairDetector.getClosestPoints(input,*resultOut, dispatchInfo.m_debugDraw);
#else


				gjkPairDetector.getClosestPoints(input, withoutMargin, dispatchInfo.m_debugDraw);
				//gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN
				//btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
				//if (l2>SIMD_EPSILON)
				{
					sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
					//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
					minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
	
#ifdef ZERO_MARGIN
					foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
					foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
#endif
				}
			}
			if (foundSepAxis)
			{
				
				//printf("sepNormalWorldSpace=%f, %f, %f\n", sepNormalWorldSpace.getX(), sepNormalWorldSpace.getY(), sepNormalWorldSpace.getZ());

				btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
					body0Wrap->getWorldTransform(), 
					body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut);
 				
			}
			if (m_ownManifold)
			{
				resultOut->refreshContactPoints();
			}
			return;

		} else
		{
			//we can also deal with convex versus triangle (without connectivity data)
			if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
			{

				btVertexArray vertices;
				btTriangleShape* tri = (btTriangleShape*)polyhedronB;
				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
				vertices.push_back(	body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
				
				//tri->initializePolyhedralFeatures();

				btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();

				btVector3 sepNormalWorldSpace;
				btScalar minDist =-1e30f;
				btScalar maxDist = threshold;
				
				bool foundSepAxis = false;
				if (0)
				{
					polyhedronB->initializePolyhedralFeatures();
					 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
					*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
					body0Wrap->getWorldTransform(), 
					body1Wrap->getWorldTransform(),
					sepNormalWorldSpace,*resultOut);
				//	 printf("sepNormalWorldSpace=%f, %f, %f\n", sepNormalWorldSpace.getX(), sepNormalWorldSpace.getY(), sepNormalWorldSpace.getZ());

				} else
				{
#ifdef ZERO_MARGIN
					gjkPairDetector.setIgnoreMargin(true);
					gjkPairDetector.getClosestPoints(input,*resultOut, dispatchInfo.m_debugDraw);
#else
					gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw);
#endif//ZERO_MARGIN
					
					btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
					if (l2>SIMD_EPSILON)
					{
						sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
						//minDist = gjkPairDetector.getCachedSeparatingDistance();
						//maxDist = threshold;
						minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
						foundSepAxis = true;
					}
				}

				
				if (foundSepAxis)
				{
					btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
						body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
				}
				
				
				if (m_ownManifold)
				{
					resultOut->refreshContactPoints();
				}
				
				return;
			}
			
		}


	}
	
	gjkPairDetector.getClosestPoints(input,*resultOut, dispatchInfo.m_debugDraw);

	//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
	
	//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
	if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
	{
		
		int i;
		btVector3 v0, v1;
		btVector3 sepNormalWorldSpace;
		btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
	
		if (l2 > SIMD_EPSILON)
		{
			sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
			
			btPlaneSpace1(sepNormalWorldSpace, v0, v1);


			bool perturbeA = true;
			const btScalar angleLimit = 0.125f * SIMD_PI;
			btScalar perturbeAngle;
			btScalar radiusA = min0->getAngularMotionDisc();
			btScalar radiusB = min1->getAngularMotionDisc();
			if (radiusA < radiusB)
			{
				perturbeAngle = gContactBreakingThreshold /radiusA;
				perturbeA = true;
			} else
			{
				perturbeAngle = gContactBreakingThreshold / radiusB;
				perturbeA = false;
			}
			if ( perturbeAngle > angleLimit ) 
					perturbeAngle = angleLimit;

			btTransform unPerturbedTransform;
			if (perturbeA)
			{
				unPerturbedTransform = input.m_transformA;
			} else
			{
				unPerturbedTransform = input.m_transformB;
			}
			
			for ( i=0;i<m_numPerturbationIterations;i++)
			{
				if (v0.length2()>SIMD_EPSILON)
				{
					btQuaternion perturbeRot(v0, perturbeAngle);
					btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
					btQuaternion rotq(sepNormalWorldSpace, iterationAngle);
				
				
					if (perturbeA)
					{
						input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
						input.m_transformB = body1Wrap->getWorldTransform();
#ifdef DEBUG_CONTACTS
						dispatchInfo.m_debugDraw->drawTransform(input.m_transformA, 5.0);
#endif //DEBUG_CONTACTS
					} else
					{
						input.m_transformA = body0Wrap->getWorldTransform();
						input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
#ifdef DEBUG_CONTACTS
						dispatchInfo.m_debugDraw->drawTransform(input.m_transformB, 5.0);
#endif
					}
				
					btPerturbedContactResult perturbedResultOut(resultOut, input.m_transformA, input.m_transformB, unPerturbedTransform, perturbeA, dispatchInfo.m_debugDraw);
					gjkPairDetector.getClosestPoints(input, perturbedResultOut, dispatchInfo.m_debugDraw);
				}
			}
		}
	}

	

#ifdef USE_SEPDISTANCE_UTIL2
	if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
	{
		m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0->getWorldTransform(), body1->getWorldTransform());
	}
#endif //USE_SEPDISTANCE_UTIL2

	if (m_ownManifold)
	{
		resultOut->refreshContactPoints();
	}
}
//
// Convex-Convex collision algorithm
//
void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{

	if (!m_manifoldPtr)
	{
		//swapped?
		m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
		m_ownManifold = true;
	}
	resultOut->setPersistentManifold(m_manifoldPtr);

	//comment-out next line to test multi-contact generation
	//resultOut->getPersistentManifold()->clearManifold();
	

	btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
	btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());

	btVector3  normalOnB;
		btVector3  pointOnBWorld;
#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
	if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
	{
		btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
		btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
		btVector3 localScalingA = capsuleA->getLocalScaling();
		btVector3 localScalingB = capsuleB->getLocalScaling();
		
		btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();

		btScalar dist = capsuleCapsuleDistance(normalOnB,	pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
			capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
			body0->getWorldTransform(),body1->getWorldTransform(),threshold);

		if (dist<threshold)
		{
			btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
			resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);	
		}
		resultOut->refreshContactPoints();
		return;
	}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER


#ifdef USE_SEPDISTANCE_UTIL2
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
	}

	if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
#endif //USE_SEPDISTANCE_UTIL2

	{

	
	btGjkPairDetector::ClosestPointInput input;

	btGjkPairDetector	gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
	//TODO: if (dispatchInfo.m_useContinuous)
	gjkPairDetector.setMinkowskiA(min0);
	gjkPairDetector.setMinkowskiB(min1);

#ifdef USE_SEPDISTANCE_UTIL2
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
	} else
#endif //USE_SEPDISTANCE_UTIL2
	{
		if (dispatchInfo.m_convexMaxDistanceUseCPT)
		{
			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
		} else
		{
			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
		}
		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
	}

	input.m_stackAlloc = dispatchInfo.m_stackAllocator;
	input.m_transformA = body0->getWorldTransform();
	input.m_transformB = body1->getWorldTransform();

	gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);

	

#ifdef USE_SEPDISTANCE_UTIL2
	btScalar sepDist = 0.f;
	if (dispatchInfo.m_useConvexConservativeDistanceUtil)
	{
		sepDist = gjkPairDetector.getCachedSeparatingDistance();
		if (sepDist>SIMD_EPSILON)
		{
			sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
			//now perturbe directions to get multiple contact points
			
		}
	}
#endif //USE_SEPDISTANCE_UTIL2

	//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
	
	//perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
	if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
	{
		
		int i;
		btVector3 v0,v1;
		btVector3 sepNormalWorldSpace;
	
		sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
		btPlaneSpace1(sepNormalWorldSpace,v0,v1);


		bool perturbeA = true;
		const btScalar angleLimit = 0.125f * SIMD_PI;
		btScalar perturbeAngle;
		btScalar radiusA = min0->getAngularMotionDisc();
		btScalar radiusB = min1->getAngularMotionDisc();
		if (radiusA < radiusB)
		{
			perturbeAngle = gContactBreakingThreshold /radiusA;
			perturbeA = true;
		} else
		{
			perturbeAngle = gContactBreakingThreshold / radiusB;
			perturbeA = false;
		}
		if ( perturbeAngle > angleLimit ) 
				perturbeAngle = angleLimit;

		btTransform unPerturbedTransform;
		if (perturbeA)
		{
			unPerturbedTransform = input.m_transformA;
		} else
		{
			unPerturbedTransform = input.m_transformB;
		}
		
		for ( i=0;i<m_numPerturbationIterations;i++)
		{
			if (v0.length2()>SIMD_EPSILON)
			{
			btQuaternion perturbeRot(v0,perturbeAngle);
			btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
			btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
			
			
			if (perturbeA)
			{
				input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
				input.m_transformB = body1->getWorldTransform();
#ifdef DEBUG_CONTACTS
				dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
#endif //DEBUG_CONTACTS
			} else
			{
				input.m_transformA = body0->getWorldTransform();
				input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
#ifdef DEBUG_CONTACTS
				dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
#endif
			}
			
			btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
			gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
			}
			
		}
	}

	

#ifdef USE_SEPDISTANCE_UTIL2
	if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
	{
		m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
	}
#endif //USE_SEPDISTANCE_UTIL2


	}

	if (m_ownManifold)
	{
		resultOut->refreshContactPoints();
	}

}