/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///																																  ///
///		FICHIER :	Collide.cpp																									  ///
///																																  ///
///		NATURE	:	Management of the collision by rapid algortihm																  ///
///																																  ///
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////









#include "RAPID_version.H"

static char rapidtag_data[] = "RAPIDTAG  file: "__FILE__"    date: "__DATE__"    time: "__TIME__;

// to silence the compiler's complaints about unreferenced identifiers.
static void r1(char *f){  r1(f);  r1(rapidtag_data);  r1(rapid_version);}


extern int	RAPID_initialized;
void		RAPID_initialize();



#include	"..\Collision\RAPID.H"
#include	"..\Collision\matvec.H"
#include	"..\Basic\ZooMatrix.h"
#include	"..\Collision\overlap.H"



float	RAPID_mR[3][3];
float	RAPID_mT[3];
float	RAPID_mV[3];
float	RAPID_ms;


int		RAPID_first_contact;
int		RAPID_num_box_tests;
int		RAPID_num_tri_tests;
int		RAPID_num_contacts;
int		RAPID_num_cols_alloced = 0;

int		RAPID_is_contact_trans = 0;		// flag pour savoir s'il ya eu contact en translation ou au départ
										//		 1 -> contact en translation
										//		-1 -> contact au départ
										//		 0 -> pas de contact

float	RAPID_coeff;					// coefficient de vecteur de translation (dans [0,1])
int		RAPID_tri_id1,	RAPID_tri_id2;	// IDs des deux triangles qui sont correspondent à la collision en translation "retenue" (les plus proches)
int		RAPID_axid1,	RAPID_axid2;	// IDs des axes de collision (opposés, pour b1 et b2)


collision_pair *RAPID_contact = 0;




int add_collision(int id1, int id2);





///////////////////////////////////////////////////////////////////////////
/// tri_contact															///
///////////////////////////////////////////////////////////////////////////
int tri_contact(box *b1, box *b2)
{
	// assume just one triangle in each box.

	// the vertices of the tri in b2 is in model1 C.S.  The vertices of
	// the other triangle is in model2 CS.  Use RAPID_mR, RAPID_mT, and
	// RAPID_ms to transform into model2 CS.

	float	i1[3];
	float	i2[3];
	float	i3[3];
	int		rc;  // return code

	tri	*t1 = (tri*) ((unsigned int)(b1->P) - 1);
	tri	*t2 = (tri*) ((unsigned int)(b2->P) - 1);
	
	sMxVpV(i1, RAPID_ms, RAPID_mR, t1->p1, RAPID_mT);
	sMxVpV(i2, RAPID_ms, RAPID_mR, t1->p2, RAPID_mT);
	sMxVpV(i3, RAPID_ms, RAPID_mR, t1->p3, RAPID_mT);

	RAPID_num_tri_tests++;

	int f = tri_contact(i1, i2, i3, t2->p1, t2->p2, t2->p3);

	if (f) 
    {
		// add_collision may be unable to allocate enough memory,
		// so be prepared to pass along an OUT_OF_MEMORY return code.
		if ((rc = add_collision(t1->id, t2->id)) != RAPID_OK)		return rc;
    }
  
	return RAPID_OK;
}




///////////////////////////////////////////////////////////////////////////
/// collide_recursive													///
///////////////////////////////////////////////////////////////////////////
int collide_recursive(box *b1, box *b2, float R[3][3], float T[3], float s)
{
	float d[3];		// temp storage for scaled dimensions of box b2.
	int rc;			// return codes
  
	if (1)
    {
		if (RAPID_first_contact && (RAPID_num_contacts > 0)) return RAPID_OK;

		// test top level

		RAPID_num_box_tests++;
  
		int f1;
  
		d[0] = s * b2->d[0];
		d[1] = s * b2->d[1];
		d[2] = s * b2->d[2];
		f1 = obb_disjoint(R, T, b1->d, d);

		if (f1 != 0)	return RAPID_OK;  // stop processing this test, go to top of loop

		// contact between boxes
		if (b1->leaf() && b2->leaf()) 
		{
			// it is a leaf pair - compare the polygons therein
			// tri_contact uses the model-to-model transforms stored in
			// RAPID_mR, RAPID_mT, RAPID_ms.
	
			// this will pass along any OUT_OF_MEMORY return codes which
			// may be generated.
			return tri_contact(b1, b2);
		}

		float U[3];

		float cR[3][3], cT[3], cs;
      
		// Currently, the transform from model 2 to model 1 space is
		// given by [B T s], where y = [B T s].x = s.B.x + T.

		if (b2->leaf() || (!b1->leaf() && (b1->size() > b2->size())))
		{
			// here we descend to children of b1.  The transform from
			// a child of b1 to b1 is stored in [b1->N->pR,b1->N->pT],
			// but we will denote it [B1 T1 1] for short.  Notice that
			// children boxes always have same scaling as parent, so s=1
			// for such nested transforms.
	
			// Here, we compute [B1 T1 1]'[B T s] = [B1'B B1'(T-T1) s]
			// for each child, and store the transform into the collision
			// test queue.
			box *N = b1->P+1;

			MTxM(cR, N->pR, R); 
			VmV(U, T, N->pT);		MTxV(cT, N->pR, U);
			cs = s;
	
			if ((rc = collide_recursive(N, b2, cR, cT, cs)) != RAPID_OK)	    return rc;
	  
			MTxM(cR, b1->P->pR, R); 
			VmV(U, T, b1->P->pT);	MTxV(cT, b1->P->pR, U);
			cs = s;

			if ((rc = collide_recursive(b1->P, b2, cR, cT, cs)) != RAPID_OK)	    return rc;
	  
			return RAPID_OK;
		}
		else 
		{
			// here we descend to the children of b2.  See comments for
			// other 'if' clause for explanation.
			box *N = b2->P+1;

			MxM(cR, R, N->pR);		sMxVpV(cT, s, R, N->pT, T);
			cs = s;
	  
			if ((rc = collide_recursive(b1, N, cR, cT, cs)) != RAPID_OK)	    return rc;
	  
			MxM(cR, R, b2->P->pR); 	sMxVpV(cT, s, R, b2->P->pT, T);
			cs = s;

			if ((rc = collide_recursive(b1, b2->P, cR, cT, cs)) != RAPID_OK)	    return rc;
	  
			return RAPID_OK; 
		}
	}
  
	return RAPID_OK;
}



///////////////////////////////////////////////////////////////////////////
/// RAPID_Collide														///
///////////////////////////////////////////////////////////////////////////
int RAPID_Collide(float R1[3][3], float T1[3], RAPID_model *RAPID_model1,
   				  float R2[3][3], float T2[3], RAPID_model *RAPID_model2,
				  int flag)
{
	return RAPID_Collide(R1, T1, 1.0, RAPID_model1, R2, T2, 1.0, RAPID_model2, flag);
}



///////////////////////////////////////////////////////////////////////////
/// RAPID_Collide														///
///////////////////////////////////////////////////////////////////////////
int RAPID_Collide(float R1[3][3], float T1[3], float s1, RAPID_model *RAPID_model1,
				  float R2[3][3], float T2[3], float s2, RAPID_model *RAPID_model2,
				  int flag)
{
	if (!RAPID_initialized) RAPID_initialize();

	if (RAPID_model1->build_state != RAPID_BUILD_STATE_PROCESSED)		return RAPID_ERR_UNPROCESSED_MODEL;

	if (RAPID_model2->build_state != RAPID_BUILD_STATE_PROCESSED)		return RAPID_ERR_UNPROCESSED_MODEL;

	box *b1 = RAPID_model1->b;
	box *b2 = RAPID_model2->b;

	RAPID_first_contact = 0; 
	if (flag == RAPID_FIRST_CONTACT)	RAPID_first_contact = 1;
  
	float tR1[3][3], tR2[3][3], R[3][3];
	float tT1[3], tT2[3], T[3], U[3];
	float s;
  
	// [R1,T1,s1] and [R2,T2,s2] are how the two triangle sets
	// (i.e. models) are positioned in world space.  [tR1,tT1,s1] and
	// [tR2,tT2,s2] are how the top level boxes are positioned in world
	// space
  
	MxM(tR1, R1, b1->pR);                  // tR1 = R1 * b1->pR;
	sMxVpV(tT1, s1, R1, b1->pT, T1);       // tT1 = s1 * R1 * b1->pT + T1;
	MxM(tR2, R2, b2->pR);                  // tR2 = R2 * b2->pR;
	sMxVpV(tT2, s2, R2, b2->pT, T2);       // tT2 = s2 * R2 * b2->pT + T2;
  
	// (R,T,s) is the placement of b2's top level box within
	// the coordinate system of b1's top level box.

	MTxM(R, tR1, tR2);								// R = tR1.T()*tR2;
	VmV(U, tT2, tT1);  sMTxV(T, 1.0f/s1, tR1, U);	// T = tR1.T()*(tT2-tT1)/s1;
  
	s = s2/s1;

	// To transform tri's from model1's CS to model2's CS use this:
	//    x2 = ms . mR . x1 + mT

	{
	    MTxM(RAPID_mR, R2, R1);
		VmV(U, T1, T2);  sMTxV(RAPID_mT, 1.0f/s2, R2, U);
		RAPID_ms = s1/s2;
	}
  

	// reset the report fields
	RAPID_num_box_tests = 0;
	RAPID_num_tri_tests = 0;
	RAPID_num_contacts  = 0;

	// make the call
	return collide_recursive(b1, b2, R, T, s);
}



///////////////////////////////////////////////////////////////////////////
/// tri_contact_trans													///
///////////////////////////////////////////////////////////////////////////
///
///	utilise le vecteur translation dans le CS de b2 --> RAPID_mV
///
int tri_contact_trans(box *b1, box *b2)
{
	// assume just one triangle in each box.

	// the vertices of the tri in b2 is in model1 C.S.  The vertices of
	// the other triangle is in model2 CS.  Use RAPID_mR, RAPID_mT, and
	// RAPID_ms to transform into model2 CS.

	float	i1[3];
	float	i2[3];
	float	i3[3];
	int		rc;			// return code
  
	// Passage des vertices de b1 dans le CS de b2
	tri	*t1 = (tri*) ((unsigned int)(b1->P) - 1);
	tri	*t2 = (tri*) ((unsigned int)(b2->P) - 1);

	sMxVpV(i1, RAPID_ms, RAPID_mR, t1->p1, RAPID_mT);
	sMxVpV(i2, RAPID_ms, RAPID_mR, t1->p2, RAPID_mT);
	sMxVpV(i3, RAPID_ms, RAPID_mR, t1->p3, RAPID_mT);

	RAPID_num_tri_tests++;
	float	coeff = RAPID_coeff;
	int f = tri_contact_trans(i1, i2, i3, t2->p1, t2->p2, t2->p3, RAPID_mV);
	
	// Cas de collision en translation
	if (f==1)
    {
		if(RAPID_is_contact_trans!=-1)
		{
			RAPID_is_contact_trans = 1;					// si contact primaire, alors pas de prise en compte du contact en transl, sinon mise à 1
			if(RAPID_coeff!=coeff)						// stockage des deux IDs des triangles, ssi ils postulent pour les (plus proches) triangles en collision
			{	
				RAPID_tri_id1 = t1->id;		
				RAPID_tri_id2 = t2->id;		
			}
		}
		
		if((rc = add_collision(t1->id, t2->id)) != RAPID_OK)		return rc;
		return 1;
    }

	// Cas de prime intersection
	if (f==-1)
	{
		RAPID_is_contact_trans = -1;														// contact primaire mis à jour (prioritaire)
		if ((rc = add_collision(t1->id, t2->id)) != RAPID_OK)		return rc;
		return -1;
    }

	// Cas DISJOINT
	return RAPID_OK;
}



///////////////////////////////////////////////////////////////////////////
/// collide_recursive_trans												///
///////////////////////////////////////////////////////////////////////////
///
///	Vb1		= vecteur tranlation de b2 par rapport a b1, dans le CS de b1.
/// R,T,s	= placement de OBB2 par rapport à OBB1
///
int collide_recursive_trans(box *b1, box *b2, float R[3][3], float T[3], float s, float Vb1[3])
{
	float	d[3];		// temp storage for scaled dimensions of box b2.
	int		rc;			// return codes
  
	if (1)
    {
		if (RAPID_first_contact && (RAPID_num_contacts > 0))	return RAPID_OK;

		// test top level
		RAPID_num_box_tests++;
  
		int f1;

		d[0] = s * b2->d[0];
		d[1] = s * b2->d[1];
		d[2] = s * b2->d[2];
		f1 = obb_inter_trans(R, T, b1->d, d, Vb1);


		if (f1==0)		return RAPID_OK;  // stop processing this test, go to top of loop  --- OBB disjointes

		// contact between boxes
		if (b1->leaf() && b2->leaf())
		{
			// it is a leaf pair - compare the polygons therein
			// tri_contact uses the model-to-model transforms stored in
			// RAPID_mR, RAPID_mT, RAPID_ms, and use RAPID_mV for the translation vector in b2's CS
	
			// this will pass along any OUT_OF_MEMORY return codes which
			// may be generated.
			return tri_contact_trans(b1, b2);
		}

		float U[3];
		float cR[3][3], cT[3], cV[3], cs;
      
		// Currently, the transform from model 2 to model 1 space is
		// given by [B T s], where y = [B T s].x = s.B.x + T.

		if (b2->leaf() || (!b1->leaf() && (b1->size() > b2->size())))
		{
			// here we descend to children of b1.  The transform from
			// a child of b1 to b1 is stored in [b1->N->pR,b1->N->pT],
			// but we will denote it [B1 T1 1] for short.  Notice that
			// children boxes always have same scaling as parent, so s=1
			// for such nested transforms.
	
			// Here, we compute [B1 T1 1]'[B T s] = [B1'B B1'(T-T1) s]
			// for each child, and store the transform into the collision
			// test queue.
			box *N = b1->P+1;

			MTxM(cR, N->pR, R);											// cR = b1->N->R.T() * R
			VmV(U, T, N->pT);			MTxV(cT, N->pR, U);				// cT = b1->N->R.T() * (T - b1->N->T) / 1.0
			MTxV(cV, N->pR, Vb1);										// cV = b1->N->R.T() * V / 1.0

			cs = s;
	
			rc = collide_recursive_trans(N, b2, cR, cT, cs, cV);
			if ((rc!=RAPID_OK) && (rc!=1) && (rc!=-1))	    return rc;

			MTxM(cR, b1->P->pR, R);											// cR = b1->P->R.T() * R
			VmV(U, T, b1->P->pT); 		MTxV(cT, b1->P->pR, U);				// cT = b1->P->R.T() * (T - b1->P->T) / 1.0
			MTxV(cV, b1->P->pR, Vb1);										// cV = b1->P->R.T() * V / 1.0

			cs = s;

			rc = collide_recursive_trans(b1->P, b2, cR, cT, cs, cV);
			if ((rc!=RAPID_OK) && (rc!=1) && (rc!=-1))	    return rc;
	  
			return RAPID_OK;
		}
		else 
		{
			// here we descend to the children of b2.  See comments for
			// other 'if' clause for explanation.
			box *N = b2->P+1;

			MxM(cR, R, N->pR);												// cR = R * b2->N->R
			sMxVpV(cT, s, R, N->pT, T);										// cT = s*R*b2->N->T + T
			
			cs = s;
	  
			rc = collide_recursive_trans(b1, N, cR, cT, cs, Vb1);
			if ((rc!=RAPID_OK) && (rc!=1) && (rc!=-1))	    return rc;
	  
			
			MxM(cR, R, b2->P->pR); 											// cR = R * b2->N->R
			sMxVpV(cT, s, R, b2->P->pT, T);									// cT = s*R*b2->N->T + T
			
			cs = s;

			rc = collide_recursive_trans(b1, b2->P, cR, cT, cs, Vb1);
			if ((rc!=RAPID_OK) && (rc!=1) && (rc!=-1))	    return rc;
	  
			return RAPID_OK; 
		}
	}
  
	return RAPID_OK;
}



///////////////////////////////////////////////////////////////////////////
/// RAPID_Collide_trans													///
///////////////////////////////////////////////////////////////////////////
int RAPID_Collide_trans(float R1[3][3], float T1[3], float V1[3], RAPID_model *RAPID_model1,
   						float R2[3][3], float T2[3], float V2[3], RAPID_model *RAPID_model2,
						int flag)
{
	return RAPID_Collide_trans(R1, T1, 1.0, V1, RAPID_model1, R2, T2, 1.0, V2, RAPID_model2, flag);
}



///////////////////////////////////////////////////////////////////////////
/// RAPID_Collide_trans													///
///////////////////////////////////////////////////////////////////////////
int RAPID_Collide_trans(float R1[3][3], float T1[3], float s1, float V1[3], RAPID_model *RAPID_model1,
						float R2[3][3], float T2[3], float s2, float V2[3], RAPID_model *RAPID_model2,
						int flag)
{
	if (!RAPID_initialized)		RAPID_initialize();

	if (RAPID_model1->build_state != RAPID_BUILD_STATE_PROCESSED)		return RAPID_ERR_UNPROCESSED_MODEL;
	if (RAPID_model2->build_state != RAPID_BUILD_STATE_PROCESSED)		return RAPID_ERR_UNPROCESSED_MODEL;

	box *b1 = RAPID_model1->b;
	box *b2 = RAPID_model2->b;

	RAPID_first_contact = 0; 
	if (flag == RAPID_FIRST_CONTACT)	RAPID_first_contact = 1;
  
	float tR1[3][3], tR2[3][3], R[3][3];
	float tT1[3], tT2[3], T[3], U[3], V[3];
	float s;
  
	// [R1,T1,s1] et [R2,T2,s2] sont les transformations des deux RAPID_models dans le world space.
	// [tR1,tT1,s1] et [tR2,tT2,s2] sont les transformations qui placent les top-OBB dans le world space.
	
	MxM(tR1, R1, b1->pR);									// tR1 = R1 * b1->pR
	sMxVpV(tT1, s1, R1, b1->pT, T1);						// tT1 = s1 * R1 * b1->pT + T1
	MxM(tR2, R2, b2->pR);									// tR2 = R2 * b2->pR
	sMxVpV(tT2, s2, R2, b2->pT, T2);						// tT2 = s2 * R2 * b2->pT + T2
  
	// (R,T,s) est le placement de la top-OBB de b2 dans le systeme de coord de
	// la top-OBB de b1.

	MTxM(R, tR1, tR2);										// R = tR1.T() * tR2
	VmV(U, tT2, tT1);	sMTxV(T, 1.0f/s1, tR1, U);			// T = tR1.T() * (tT2-tT1)/s1
	VmV(U, V2, V1);		sMTxV(V, 1.0f/s1, tR1, U);			// V = tR1.T() * (V2-V1)/s1			--> vecteur translation dans le CS de b1
	s = s2/s1;												// s = s2/s1

	// To transform tri's from model1's CS to model2's CS use this:
	//    x2 = ms . mR . x1 + mT

	{
	    MTxM(RAPID_mR, R2, R1);								// mR = R2.T() * R1
		VmV(U, T1, T2);  sMTxV(RAPID_mT, 1.0f/s2, R2, U);	// mT = R2.T() * (T1-T2)/s2
		VmV(U, V2, V1);  sMTxV(RAPID_mV, 1.0f/s2, R2, U);	// mV = R2.T() * (V2-V1)/s2
		RAPID_ms = s1/s2;									// ms = s1/s2
		
	}


	// reset the report fields
	RAPID_num_box_tests = 0;
	RAPID_num_tri_tests = 0;
	RAPID_num_contacts	= 0;
	
	RAPID_is_contact_trans = 0;
	RAPID_coeff		= 1;
	RAPID_tri_id1	= RAPID_tri_id2 = -1;
	RAPID_axid1		= RAPID_axid2	= -1;

	// make the call
	return collide_recursive_trans(b1, b2, R, T, s, V);
}



///////////////////////////////////////////////////////////////////////////
/// add_collision														///
///////////////////////////////////////////////////////////////////////////
int add_collision(int id1, int id2)
{
	if (!RAPID_contact)
    {
		RAPID_contact = new collision_pair[10];
		
		if (!RAPID_contact)		return RAPID_ERR_COLLIDE_OUT_OF_MEMORY;
		
		RAPID_num_cols_alloced  = 10;
		RAPID_num_contacts		= 0;
    }
  
	if (RAPID_num_contacts == RAPID_num_cols_alloced)
    {
		collision_pair *t = new collision_pair[RAPID_num_cols_alloced*2];
		
		if (!t)					return RAPID_ERR_COLLIDE_OUT_OF_MEMORY;
      
		RAPID_num_cols_alloced *= 2;
      
		for(int i=0; i<RAPID_num_contacts; i++) t[i] = RAPID_contact[i];
		delete [] RAPID_contact;
		RAPID_contact = t;
    }
  
	RAPID_contact[RAPID_num_contacts].id1 = id1;
	RAPID_contact[RAPID_num_contacts].id2 = id2;
	RAPID_num_contacts++;

	return RAPID_OK;
}