#include "btFractureBody.h"
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletDynamics/Dynamics/btDynamicsWorld.h"



void	btFractureBody::recomputeConnectivity(btCollisionWorld* world)
{
	m_connections.clear();
	//@todo use the AABB tree to avoid N^2 checks

	if (getCollisionShape()->isCompound())
	{
		btCompoundShape* compound = (btCompoundShape*)getCollisionShape();
		for (int i=0;i<compound->getNumChildShapes();i++)
		{
			for (int j=i+1;j<compound->getNumChildShapes();j++)
			{

				struct   MyContactResultCallback : public btCollisionWorld::ContactResultCallback
				{
					bool m_connected;
					btScalar m_margin;
					MyContactResultCallback() :m_connected(false),m_margin(0.05)
					{
					}
					virtual   btScalar   addSingleResult(btManifoldPoint& cp,   const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
					{
						if (cp.getDistance()<=m_margin)
							m_connected = true;
						return 1.f;
					}
			   };

				MyContactResultCallback result;

				btCollisionObject obA;
				obA.setWorldTransform(compound->getChildTransform(i));
				obA.setCollisionShape(compound->getChildShape(i));
				btCollisionObject obB;
				obB.setWorldTransform(compound->getChildTransform(j));
				obB.setCollisionShape(compound->getChildShape(j));
				world->contactPairTest(&obA,&obB,result);
				if (result.m_connected)
				{
					btConnection tmp;
					tmp.m_childIndex0 = i;
					tmp.m_childIndex1 = j;
					tmp.m_childShape0 = compound->getChildShape(i);
					tmp.m_childShape1 = compound->getChildShape(j);
					tmp.m_strength = 1.f;//??
					m_connections.push_back(tmp);
				}
			}
		}
	}
	

}

btCompoundShape* btFractureBody::shiftTransformDistributeMass(btCompoundShape* boxCompound,btScalar mass,btTransform& shift)
{

	btVector3 principalInertia;

	btScalar* masses = new btScalar[boxCompound->getNumChildShapes()];
	for (int j=0;j<boxCompound->getNumChildShapes();j++)
	{
		//evenly distribute mass
		masses[j]=mass/boxCompound->getNumChildShapes();
	}

	return shiftTransform(boxCompound,masses,shift,principalInertia);

}


btCompoundShape* btFractureBody::shiftTransform(btCompoundShape* boxCompound,btScalar* masses,btTransform& shift, btVector3& principalInertia)
{
	btTransform principal;

	boxCompound->calculatePrincipalAxisTransform(masses,principal,principalInertia);


	///create a new compound with world transform/center of mass properly aligned with the principal axis

	///non-recursive compound shapes perform better
	
#ifdef USE_RECURSIVE_COMPOUND

	btCompoundShape* newCompound = new btCompoundShape();
	newCompound->addChildShape(principal.inverse(),boxCompound);
	newBoxCompound = newCompound;
	//m_collisionShapes.push_back(newCompound);

	//btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	//btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,newCompound,principalInertia);

#else
#ifdef CHANGE_COMPOUND_INPLACE
	newBoxCompound = boxCompound;
	for (int i=0;i<boxCompound->getNumChildShapes();i++)
	{
		btTransform newChildTransform = principal.inverse()*boxCompound->getChildTransform(i);
		///updateChildTransform is really slow, because it re-calculates the AABB each time. todo: add option to disable this update
		boxCompound->updateChildTransform(i,newChildTransform);
	}
	bool isDynamic = (mass != 0.f);
	btVector3 localInertia(0,0,0);
	if (isDynamic)
		boxCompound->calculateLocalInertia(mass,localInertia);
	
#else
	///creation is faster using a new compound to store the shifted children
	btCompoundShape* newBoxCompound = new btCompoundShape();
	for (int i=0;i<boxCompound->getNumChildShapes();i++)
	{
		btTransform newChildTransform = principal.inverse()*boxCompound->getChildTransform(i);
		///updateChildTransform is really slow, because it re-calculates the AABB each time. todo: add option to disable this update
		newBoxCompound->addChildShape(newChildTransform,boxCompound->getChildShape(i));
	}



#endif

#endif//USE_RECURSIVE_COMPOUND

	shift = principal;
	return newBoxCompound;
}