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




#include	"..\Collision\ZooOldColl.h"








/////////////////////////////////////////////////////////////////////////////////////////////////////
///		ComputeWorldMatrix : calcule la matrice d'un objet dans le World Space
/////////////////////////////////////////////////////////////////////////////////////////////////////
void ComputeWorldMatrix(ZNode *node)
{
	ZPRS	*prs;
	ZMatrix	mat;
	float	scale;

	mat.IdentityMatrix();
	scale = 1.0f;
	
	ZNode	*curNode = node;

	while(curNode->type <= MAX_NODE_SCENE)
	{
		if(curNode->type != ANI_TYPE_ID)
		{
			prs = ((ZNodeGraph*)node)->GetPRS();
			
			mat = prs->mat * mat;
			scale *= prs->GetScale();
		}
		
		curNode = curNode->father;
	}

	((ZNodeGraph*)node)->worldMat	= mat;
	((ZNodeGraph*)node)->worldScale	= scale;
}



/////////////////////////////////////////////////////////////////////////////////////////////////////
///		ComputeWorldMatrixRec : calcule les matrices de la descendance d'un objet
/////////////////////////////////////////////////////////////////////////////////////////////////////
void ComputeWorldMatrixRec(ZNode *node, const ZMatrix &refMat, float refScale)
{
	ZNode	*curNode = node->son;
	ZPRS	*prs;
	
	while(curNode!=NULL)
	{
		prs = ((ZNodeGraph*)curNode)->GetPRS();
		
		((ZNodeGraph*)curNode)->worldMat	= refMat * prs->mat;
		((ZNodeGraph*)curNode)->worldScale	= prs->GetScale() * refScale;

		if(curNode->son != NULL )		ComputeWorldMatrixRec(curNode, ((ZNodeGraph*)curNode)->worldMat, ((ZNodeGraph*)curNode)->worldScale);

		curNode = curNode->next;
	}
}



/////////////////////////////////////////////////////////////////////////////////////////////////////
///		ComputeWorldMatrixTree : calcule la matrice d'un objet et da descance dans le World Space
/////////////////////////////////////////////////////////////////////////////////////////////////////
void ComputeWorldMatrixTree(ZScene *scene, ZNode *node)
{
	ComputeWorldMatrix(node);

	ComputeWorldMatrixRec(node, ((ZNodeGraph*)node)->worldMat, ((ZNodeGraph*)node)->worldScale);
}













///////////////////////////////////////////////////////////////////////////////////////////////////////////////
///																											///
///										Test de collision en INTERSECTION									///
///																											///
///////////////////////////////////////////////////////////////////////////////////////////////////////////////



//////////////////////////////////////////////////////////////////////////////////////////////
///		InterAxis
//////////////////////////////////////////////////////////////////////////////////////////////
//	Test d'intersection de deux intervalles sur un axe d ; Les rayons sont deja projetes sur l'axe
//	Retourne -1 si intersection, 0 sinon
int InterAxis(const ZVector4 &ab, float ra, float rb, const ZVector4 &d)
{
	float	r, s, abd;

	s	= ra+rb;
	abd	= ab*d;
	r	= abd*abd - s*s;
	
	if(r<=0)	return -1;
	return 0;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		InterObbSph
//////////////////////////////////////////////////////////////////////////////////////////////
//	Test d'intersection d'un Obb et d'une sphere
//	retourne -1 si intersection, 0 sinon
int InterObbSph(const ZVector4 &uA, ZVector4 &xA, ZVector4 &yA, ZVector4 &zA, const ZVector4 &uB, float ray)
{
	ZVector4	ab;
	ab = uB-uA;

	// 3 faces de A
	if (!InterAxis(ab, xA*xA, xA.Length()*ray, xA))		return 0;
	if (!InterAxis(ab, yA*yA, yA.Length()*ray, yA))		return 0;
	if (!InterAxis(ab, zA*zA, zA.Length()*ray, zA))		return 0;

	return -1;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		InterObbObb
//////////////////////////////////////////////////////////////////////////////////////////////
//	Test d'intersection de deux Obb
//	Retourne -1 si intersection, 0 sinon
int InterObbObb(const ZVector4 &uA, const ZVector4 &xA, const ZVector4 &yA, const ZVector4 &zA, 
				const ZVector4 &uB, const ZVector4 &xB, const ZVector4 &yB, const ZVector4 &zB)
{
	ZVector4	ab;
	ZVector4	d;

	ab = uB-uA;

	// 3 faces de A
	if(!InterAxis(ab, xA*xA, fabs(xA*xB)+fabs(xA*yB)+fabs(xA*zB), xA))			return 0;
	if(!InterAxis(ab, yA*yA, fabs(yA*xB)+fabs(yA*yB)+fabs(yA*zB), yA))			return 0;
	if(!InterAxis(ab, zA*zA, fabs(zA*xB)+fabs(zA*yB)+fabs(zA*zB), zA))			return 0;
	
	// 3 faces de B
	if(!InterAxis(ab, xB*xB, fabs(xB*xA)+fabs(xB*yA)+fabs(xB*zA), xB))			return 0;
	if(!InterAxis(ab, yB*yB, fabs(yB*xA)+fabs(yB*yA)+fabs(yB*zA), yB))			return 0;
	if(!InterAxis(ab, zB*zB, fabs(zB*xA)+fabs(zB*yA)+fabs(zB*zA), zB))			return 0;
	
	// 9 hybrides
	d = xA ^ xB;
	if(!InterAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), d))		return 0;
	d = xA ^ yB;
	if(!InterAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), d))		return 0;
	d = xA ^ zB;
	if(!InterAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), d))		return 0;

	d = yA ^ xB;
	if(!InterAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), d))		return 0;
	d = yA ^ yB;
	if(!InterAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), d))		return 0;
	d = yA ^ zB;
	if(!InterAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), d))		return 0;

	d = zA ^ xB;
	if(!InterAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), d))		return 0;
	d = zA ^ yB;
	if(!InterAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), d))		return 0;
	d = zA ^ zB;
	if(!InterAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), d))		return 0;
	
	return -1;
}




//////////////////////////////////////////////////////////////////////////////////////////////
///		testInterTree
//////////////////////////////////////////////////////////////////////////////////////////////
int testInterTree(ZColl *nodeA, const ZMatrix &mA, float scaleA, ZColl *nodeB, const ZMatrix &mB, float scaleB)
{
	// position des [ZColl] dans le World-Space
	ZVector4	uA, uB, uAB;
	uA = mA * nodeA->pos;
	uB = mB * nodeB->pos;

	// SPHERE-SPHERE
	if( (nodeA->typeColl==0) && (nodeB->typeColl==0) )
	{
		uAB = uB - uA;
		float s	= nodeA->rayon * scaleA + nodeB->rayon * scaleB;

		if( (uAB*uAB-s*s) >= 0)		return 0;			// sortie ssi disjointes
	}
	else
	// OBB-OBB
	if( (nodeA->typeColl==1) && (nodeB->typeColl==1) )
	{
		ZVector4	xA, yA, zA;
		ZVector4	xB, yB, zB;

		xA  = nodeA->u;			// Passage en dim 4 *OBLIGATOIRE* pour supprimer la translation (w=0) ..
		xA *= mA;				// .. et ensuite on effectue le calcul matriciel
		yA  = nodeA->v;
		yA *= mA;
		zA  = nodeA->w;
		zA *= mA;

		xB  = nodeB->u;
		xB *= mB;
		yB  = nodeB->v;
		yB *= mB;
		zB  = nodeB->w;
		zB *= mB;

		if (!InterObbObb(uA, xA, yA, zA, uB, xB, yB, zB))			return 0;
	}
	else
	// SPHERE-OBB
	if( (nodeA->typeColl==0) && (nodeB->typeColl==1) )
	{
		ZVector4	xB, yB, zB;
		
		xB  = nodeB->u;
		xB *= mB;
		yB  = nodeB->v;
		yB *= mB;
		zB  = nodeB->w;
		zB *= mB;

		if(!InterObbSph(uB, xB, yB, zB, uA, nodeA->rayon * scaleA))			return 0;
	}
	else
	// OBB-SPHERE
	if( (nodeA->typeColl==1) && (nodeB->typeColl==0) )
	{
		ZVector4	xA, yA, zA;

		xA  = nodeA->u;
		xA *= mA;
		yA  = nodeA->v;
		yA *= mA;
		zA  = nodeA->w;
		zA *= mA;

		if(!InterObbSph(uA, xA, yA, zA, uB, nodeB->rayon * scaleB))			return 0;
	}

	// RECURSION
	if(nodeA->colB)
	{
		if(nodeB->colB)
		{
			if(testInterTree(nodeA->colA, mA, scaleA, nodeB->colA, mB, scaleB))		return 1;
			if(testInterTree(nodeA->colA, mA, scaleA, nodeB->colB, mB, scaleB))		return 1;
			if(testInterTree(nodeA->colB, mA, scaleA, nodeB->colA, mB, scaleB))		return 1;
			return testInterTree(nodeA->colB, mA, scaleA, nodeB->colB, mB, scaleB);
		}

		if(testInterTree(nodeA->colA, mA, scaleA, nodeB, mB, scaleB))			return 1;
		return testInterTree(nodeA->colB, mA, scaleA, nodeB, mB, scaleB);
	}

	if(nodeA->colA)
	{
		if(nodeB->colB)
		{
			if(testInterTree(nodeA, mA, scaleA, nodeB->colA, mB, scaleB))			return 1;
			return testInterTree(nodeA, mA, scaleA, nodeB->colB, mB, scaleB);
		}

		if(nodeB->colA)		return testInterTree(nodeA->colA, mA, scaleA, nodeB->colA, mB, scaleB);
		
		return testInterTree(nodeA->colA, mA, scaleA, nodeB, mB, scaleB);
	}
	
	if(nodeB->colB)
	{
		if(testInterTree(nodeA, mA, scaleA, nodeB->colA, mB, scaleB))				return 1;
		return testInterTree(nodeA, mA, scaleA, nodeB->colB, mB, scaleB);
	}
	
	if(nodeB->colA)		return testInterTree(nodeA, mA, scaleA, nodeB->colA, mB, scaleB);

	
	return 1;
}




//////////////////////////////////////////////////////////////////////////////////////////////
///		testInterRecB
//////////////////////////////////////////////////////////////////////////////////////////////
int testInterRecB(ZScene *scene, ZNode *nodeA0, ZNode *nodeA, ZNode *nodeB0, ZNode *nodeB, ZNode **collA, ZNode **collB)
{
	ZColl	*p, *q;

	if(nodeA0 != nodeB)
	{
		if(nodeB->type==COL_TYPE_ID)
		{
			p = (ZColl*) nodeA;
			q = (ZColl*) nodeB;

			if(testInterTree(p, p->worldMat, p->worldScale, q, q->worldMat, q->worldScale))
			{
				*collA = nodeA;
				*collB = nodeB;
				return 1;
			}
	    }
		
		if(nodeB->son!=NULL)
		{
			if(testInterRecB(scene, nodeA0, nodeA, nodeB0, nodeB->son, collA, collB))	return 1;
		}
	}
	
	if( (nodeB!=nodeB0) && (nodeB->next!=NULL) )
    {
		if(testInterRecB(scene, nodeA0, nodeA, nodeB0, nodeB->next, collA, collB))		return 1;
    }
  
	return 0;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		testInterRecA
//////////////////////////////////////////////////////////////////////////////////////////////
int testInterRecA(ZScene *scene, ZNode *nodeA0, ZNode *nodeA, ZNode *nodeB, ZNode **collA, ZNode **collB)
{
	if(nodeA != nodeB)
	{
		if(nodeA->type==COL_TYPE_ID)
	    {
			if(testInterRecB(scene, nodeA0, nodeA, nodeB, nodeB, collA, collB))		return 1;
		}
		if(nodeA->son!=NULL)
		{
			if(testInterRecA(scene, nodeA0, nodeA->son, nodeB, collA, collB))		return 1;
		}
	}
	
	if( (nodeA!=nodeA0) && (nodeA->next!=NULL))
    {
		if(testInterRecA(scene, nodeA0, nodeA->next, nodeB, collA, collB))			return 1;
    }
	
	return 0;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		M3DtestInter
//////////////////////////////////////////////////////////////////////////////////////////////
int M3DtestInter(ZScene *scene, ZNode *nodeA, ZNode *nodeB, ZNode **collA, ZNode **collB)
{
	if(nodeA==nodeB)	return -1;
	
	ComputeWorldMatrixTree(scene, nodeA);
	ComputeWorldMatrixTree(scene, nodeB);
	
	*collA = *collB = NULL;

	return testInterRecA(scene, nodeA, nodeA, nodeB, collA, collB);
}
















///////////////////////////////////////////////////////////////////////////////////////////////////////////////
///																											///
///										Test de collision en TRANSLATION									///
///																											///
///////////////////////////////////////////////////////////////////////////////////////////////////////////////



//////////////////////////////////////////////////////////////////////////////////////////////
///		SquareSolve
//////////////////////////////////////////////////////////////////////////////////////////////
//	fonction calculant si la plus petite racine est entre 0 et 1
//	suppose a>0
//	retourne 0 si pas de racine, 1 si racine (calculee dans res)*/
int SquareSolve(float a, float b, float c, float *res)
{
	float	d;
	
	*res = 1;
	
	if( (c<0) || (b>=0) )		return 0;
	
	if(a+b+c < 0)
	{
		if(a==0)
		{
			if(b==0)		return 0;
			*res = -c/b;
		}
		else 
			*res = (-b-sqrt(b*b-4*a*c)) / (2*a);
		return 1;
	}
	
	if(a==0)
	{
		if(b==0)		return 0;
		*res = -c/b;
	}
	
	if(2*a+b<0)			return 0;
	
	d = b*b - 4*a*c;
	if(d<0)				return 0;
	
	*res = (-b-sqrt(d)) / (2*a);
	
	return 1;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		CollAxis
//////////////////////////////////////////////////////////////////////////////////////////////
//	Test de collision de deux intervalles sur un axe d selon une translation u.
//	Les rayons sont deja projetes sur l'axe
//	Retourne -1 si deja intersection
//	Retourne 0 si pas de collision
//	Retourne 1 si collision (distance calculee dans res)
int CollAxis(const ZVector4 &ab, float ra, float rb, const ZVector4 &depl, const ZVector4 &d, float *res)
{
	float	p, q, r, s, abd, ud;

	s	= ra+rb;
	abd = ab * d;
	r	= abd*abd-s*s;
	
	if(r<=0)
	{
		*res=-1;
		return -1;
	}

	ud	= depl * d;
	q	= -2*ud*abd;
	p	= ud*ud;
    
	return SquareSolve(p, q, r, res);
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		CollObbSph
//////////////////////////////////////////////////////////////////////////////////////////////
int CollObbSph(const ZVector4 &uA, ZVector4 &xA, ZVector4 &yA, ZVector4 &zA, const ZVector4 &uB, float ray, const ZVector4 &depl, float *res, ZVector4 *axis)
{
	ZVector4	ab;
	ZVector4	vcol;
	float		r, resmin=-1;
	int			k;
	
	ab = uB-uA;

	*res=1;


	// 3 faces de A
	if (!InterAxis(ab, xA*xA, xA.Length()*ray, xA))		return 0;
	if (!InterAxis(ab, yA*yA, yA.Length()*ray, yA))		return 0;
	if (!InterAxis(ab, zA*zA, zA.Length()*ray, zA))		return 0;


	k = CollAxis(ab, xA*xA, xA.Length()*ray, depl, xA, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=xA;	}
	
	k = CollAxis(ab, yA*yA, yA.Length()*ray, depl, yA, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=yA;	}
	
	k = CollAxis(ab, zA*zA, zA.Length()*ray, depl, zA, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=zA;	}
	
	
	// Résultat
	*res = resmin;
	if(resmin<0)	return -1;
	*axis = vcol;
	
	return 1;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		CollObbObb
//////////////////////////////////////////////////////////////////////////////////////////////
int CollObbObb(const ZVector4 &uA, const ZVector4 &xA, const ZVector4 &yA, const ZVector4 &zA, const ZVector4 &uB, const ZVector4 &xB, const ZVector4 &yB, const ZVector4 &zB,
			   const ZVector4 &depl, float *res, ZVector4 *axis)
{
	ZVector4	ab;
	ZVector4	d;
	ZVector4	vcol;
	float		r, resmin=-1;
	int			k;

	ab = uB-uA;

	*res = 1;

	
	// 3 faces de A
	k = CollAxis(ab, xA*xA,	fabs(xA*xB)+fabs(xA*yB)+fabs(xA*zB), depl, xA, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=xA;	}
	
	k = CollAxis(ab, yA*yA, fabs(yA*xB)+fabs(yA*yB)+fabs(yA*zB), depl, yA, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=yA;	}
	
	k = CollAxis(ab, zA*zA, fabs(zA*xB)+fabs(zA*yB)+fabs(zA*zB), depl, zA, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=zA;	}
	

	// 3 faces de B
	k = CollAxis(ab, xB*xB, fabs(xB*xA)+fabs(xB*yA)+fabs(xB*zA), depl, xB, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=xB;	}
	
	k = CollAxis(ab, yB*yB, fabs(yB*xA)+fabs(yB*yA)+fabs(yB*zA), depl, yB, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=yB;	}
	
	k = CollAxis(ab, zB*zB, fabs(zB*xA)+fabs(zB*yA)+fabs(zB*zA), depl, zB, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=zB;	}


	// 9 hybrides
	d = xA ^ xB;
	k = CollAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), depl, d, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=d;		}

	d = xA ^ yB;
	k = CollAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), depl, d, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=d;		}
	
	d = xA ^ zB;
	k = CollAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), depl, d, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=d;		}


	d = yA ^ xB;
	k = CollAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), depl, d, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=d;		}

	d = yA ^ yB;
	k = CollAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), depl, d, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=d;		}
	
	d = yA ^ zB;
	k = CollAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), depl, d, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=d;		}
		

	d = zA ^ xB;
	k = CollAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), depl, d, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=d;		}

	d = zA ^ yB;
	k = CollAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), depl, d, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=d;		}
	
	d = zA ^ zB;
	k = CollAxis(ab, fabs(d*xA)+fabs(d*yA)+fabs(d*zA),	fabs(d*xB)+fabs(d*yB)+fabs(d*zB), depl, d, &r);
	if(!k)		return 0;
	if((k==1)&&(r>resmin))		{	resmin=r;	vcol=d;		}


	// Résultat
	*res = resmin;
	if(resmin<0)		return -1;
	*axis = vcol;
	
	return 1;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		testCollTree
//////////////////////////////////////////////////////////////////////////////////////////////
int testCollTree(ZColl *nodeA, const ZMatrix &mA, float scaleA, ZColl *nodeB, const ZMatrix &mB, float scaleB, ZVector3 *depl, float *res, ZVector3 *vcol)
{
	// position des [ZColl] dans le World-Space
	ZVector4	uA, uB, uAB;
	uA = mA * nodeA->pos;
	uB = mB * nodeB->pos;

	ZVector4	axis, axismin;

	ZVector4	transl;
	transl = (*depl);

	int			kfin, k;
	
	float	r, rmin=-1;

	// SPHERE-SPHERE
	if( (nodeA->typeColl==0) && (nodeB->typeColl==0) )
	{
		rmin = 1;
		
		uAB = uB - uA;
		float s	= nodeA->rayon * scaleA + nodeB->rayon * scaleB;
		float rtest = (uAB*uAB-s*s);
		
		// détermination de 'r'
		if(rtest < 0)		r = -1;
		else
		{
			float	q = -2*(uAB*transl);
			float	p = depl->SqLength();
			SquareSolve(p, q, rtest, &r);
		}
		
		// traitement suivant la valeur de 'r'
		if(r < rmin)
		{
			rmin = r;
			
			if( (r>0) && (nodeA->colA==NULL) && (nodeA->colB==NULL) && (nodeB->colA==NULL) && (nodeB->colB==NULL) )
			{
				axismin = uAB - r * transl;
			}
		}

		// cette partie doit dégager à priori, car 'rmin' n'est modifié que si on trouve une valeur + petite que lui-même, en l'ocurrence <= 1....
		// donc ce test est ridicule... (sauf pour l'égalité où il a une raison d'être)
		if(rmin >= 1) 
		{
			*res = 1;
			return 0;
		}
		else if(rmin < 0)		kfin = -1;
		else					kfin = 1;
		// .......
	}
	else 
	// OBB-OBB
	if( (nodeA->typeColl==1) && (nodeB->typeColl==1) )
	{
		rmin = 1;

		ZVector4	xA, yA, zA;
		ZVector4	xB, yB, zB;

		xA  = nodeA->u;			// Passage en dim 4 OBLIGATOIRE pour supprimer la translation (w=0) ..
		xA *= mA;				// .. et ensuite on effectue le calcul matriciel
		yA  = nodeA->v;
		yA *= mA;
		zA  = nodeA->w;
		zA *= mA;

		xB  = nodeB->u;
		xB *= mB;
		yB  = nodeB->v;
		yB *= mB;
		zB  = nodeB->w;
		zB *= mB;

		k = CollObbObb(uA, xA, yA, zA, uB, xB, yB, zB, transl, &r, &axis);
		
		if(r < rmin)
		{
			rmin = r;
			axismin = axis;
		}

		if(rmin >= 1) 
		{
			*res=1;
			return 0;
		}
		else if(rmin < 0)		kfin = -1;
		else					kfin = 1;
	}
	else 
	// SPHERE-OBB
	if( (nodeA->typeColl==0) && (nodeB->typeColl==1) )
	{
		rmin = 1;

		ZVector4	xB, yB, zB;
		xB  = nodeB->u;
		xB *= mB;
		yB  = nodeB->v;
		yB *= mB;
		zB  = nodeB->w;
		zB *= mB;

		k = CollObbSph(uB, xB, yB, zB, uA, nodeA->rayon * scaleA, -transl, &r, &axis);
		if(r < rmin)
		{
			rmin = r;
			axismin = axis;
		}

		if(rmin >= 1)
		{
			*res = 1;
			return 0;
		}
		else if(rmin < 0)		kfin = -1;
		else					kfin = 1;
	}
	else 
	// OBB-SPHERE
	if( (nodeA->typeColl==1) && (nodeB->typeColl==0) )
	{
		rmin = 1;

		ZVector4	xA, yA, zA;
		xA  = nodeA->u;
		xA *= mA;
		yA  = nodeA->v;
		yA *= mA;
		zA  = nodeA->w;
		zA *= mA;

		k = CollObbSph(uA, xA, yA, zA, uB, nodeB->rayon * scaleB, transl, &r, &axis);
		if(r < rmin)
		{
			rmin = r;
			axismin = axis;
		}

		if(rmin >= 1)
		{
			*res = 1;
			return 0;
		}
		else if(rmin < 0)		kfin=-1;
		else					kfin=1;
	}


	ZVector3	deplTMP;

	// RECURSION
	if(nodeA->colB)
	{
		deplTMP = *depl;
		*res = 1;

		if(nodeB->colB)
		{
			k = testCollTree(nodeA->colA, mA, scaleA, nodeB->colA, mB, scaleB, &deplTMP, &r, vcol);
			if(k<0)			{	*res = -1;			return k;		}
			if(k)			{	deplTMP *= r;		(*res) *= r;	}
			
			k = testCollTree(nodeA->colA, mA, scaleA, nodeB->colB, mB, scaleB, &deplTMP, &r, vcol);
			if(k<0)			{	*res = -1;			return k;		}
			if(k)			{	deplTMP *= r;		(*res) *= r;	}
			
			k = testCollTree(nodeA->colB, mA, scaleA, nodeB->colA, mB, scaleB, &deplTMP, &r, vcol);
			if(k<0)			{	*res = -1;			return k;		}
			if(k)			{	deplTMP *= r;		(*res) *= r;	}

			k = testCollTree(nodeA->colB, mA, scaleA, nodeB->colB, mB, scaleB, &deplTMP, &r, vcol);
			if(k<0)			{	*res = -1;			return k;		}
			if(k)			{	deplTMP *= r;		(*res) *= r;	}

			if((*res)<1)		return 1;
			return 0;
		}
		
		k = testCollTree(nodeA->colA, mA, scaleA, nodeB, mB, scaleB, &deplTMP, &r, vcol);
		if(k<0)			{	*res = -1;		return k;			}
		if(k)			{	deplTMP *= r;		(*res) *= r;	}
		
		k = testCollTree(nodeA->colB, mA, scaleA, nodeB, mB, scaleB, &deplTMP, &r, vcol);
		if(k<0)			{	*res = -1;		return k;			}
		if(k)			{	deplTMP *= r;		(*res) *= r;	}

		if((*res) < 1)		return 1;
		return 0;
	}

	if(nodeA->colA)
	{
		if(nodeB->colB)
		{
			deplTMP = *depl;
			*res = 1;
			
			k = testCollTree(nodeA, mA, scaleA, nodeB->colA, mB, scaleB, &deplTMP, &r, vcol);
			if(k<0)			{	*res = -1;			return k;		}
			if(k)			{	deplTMP *= r;		(*res) *= r;	}

			k = testCollTree(nodeA, mA, scaleA, nodeB->colB, mB, scaleB, &deplTMP, &r, vcol);
			if(k<0)			{	*res = -1;			return k;		}
			if(k)			{	deplTMP *= r;		(*res) *= r;	}

			if((*res)<1)		return 1;
			return 0;
		}
		else 
		if(nodeB->colA)		return testCollTree(nodeA->colA, mA, scaleA, nodeB->colA, mB, scaleB, depl, res, vcol);
		
		return testCollTree(nodeA->colA, mA, scaleA, nodeB, mB, scaleB, depl, res, vcol);
	}

	if(nodeB->colB)
	{
		deplTMP = *depl;
		*res = 1;

		k = testCollTree(nodeA, mA, scaleA, nodeB->colA, mB, scaleB, &deplTMP, &r, vcol);
		if(k<0)			{	*res = -1;			return k;		}
		if(k)			{	deplTMP *= r;		(*res) *= r;	}

		k = testCollTree(nodeA, mA, scaleA, nodeB->colB, mB, scaleB, &deplTMP, &r, vcol);
		if(k<0)			{	*res = -1;			return k;		}
		if(k)			{	deplTMP *= r;		(*res) *= r;	}

		if((*res)<1)		return 1;
		return 0;
	}
	
	if(nodeB->colA)		return testCollTree(nodeA, mA, scaleA, nodeB->colA, mB, scaleB, depl, res, vcol);
	
	*res  = rmin;
	*vcol = axis;
	
	return kfin;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		testCollRecB
//////////////////////////////////////////////////////////////////////////////////////////////
int testCollRecB(ZScene *scene, ZNode *nodeA0, ZNode *nodeA, ZNode *nodeB0, ZNode *nodeB, ZVector3 *depl, float *res, ZVector3 *axis, ZNode **collA, ZNode **collB)
{
	float	r;
	int		k;
	
	ZColl	*p, *q;

	*res=1;
  
	if(nodeA0 != nodeB)
	{
		if(nodeB->type==COL_TYPE_ID)
		{
			p = (ZColl*) nodeA;
			q = (ZColl*) nodeB;

			k = testCollTree(p, p->worldMat, p->worldScale, q, q->worldMat, q->worldScale, depl, &r, axis);

			if(k<0)
			{
				*collA = nodeA;
				*collB = nodeB;
				*res   = -1;
				return k;
			}
			if(k)
			{
				*collA = nodeA;
				*collB = nodeB;
				(*depl) *= r;
				(*res)  *= r;
			}
		}

		if(nodeB->son!=NULL)
		{
			k = testCollRecB(scene, nodeA0, nodeA, nodeB0, nodeB->son, depl, &r, axis, collA, collB);
			if(k<0)
			{
				*res = -1;
				return k;
			}
			if(k)
			{
				(*depl) *= r;
				(*res)  *= r;
			}
		}
	}

	if( (nodeB!=nodeB0) && (nodeB->next!=NULL) )
	{
		k = testCollRecB(scene, nodeA0, nodeA, nodeB0, nodeB->next, depl, &r, axis, collA, collB);
		if(k<0)
		{
			*res = -1;
			return k;
		}
		if(k)
		{
			(*depl) *= r;
			(*res)  *= r;
		}
	}

	if((*res)<1)	return 1;
  
	return 0;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		testCollRecA
//////////////////////////////////////////////////////////////////////////////////////////////
int testCollRecA(ZScene *scene, ZNode *nodeA0, ZNode *nodeA, ZNode *nodeB, ZVector3 *depl, float *res, ZVector3 *axis, ZNode **collA, ZNode **collB)
{
	float	r;
	int		k;

	*res = 1;

	if(nodeA != nodeB)
	{
		if(nodeA->type==COL_TYPE_ID)
		{
			k = testCollRecB(scene, nodeA0, nodeA, nodeB, nodeB, depl, &r, axis, collA, collB);
			if(k<0) 
			{
				*res = -1;
				return k;
			}
			if(k)
			{
				(*depl) *= r;
				(*res)  *= r;
			}
		}
		if(nodeA->son!=NULL)
		{
			k = testCollRecA(scene, nodeA0, nodeA->son, nodeB, depl, &r, axis, collA, collB);
			if(k<0)
			{
				*res = -1;
				return k;
			}
			if(k)
			{
				(*depl) *= r;
				(*res)  *= r;
			}
		}
	}

	if( (nodeA!=nodeA0) && (nodeA->next!=NULL))
	{
		k = testCollRecA(scene, nodeA0, nodeA->next, nodeB, depl, &r, axis, collA, collB);
		if(k<0)
		{
			*res = -1;
			return k;
		}
		if(k)
		{
			(*depl) *= r;
			(*res)  *= r;
		}
	}

	if((*res)<1)	return 1;
  
	return 0;
}



//////////////////////////////////////////////////////////////////////////////////////////////
///		M3DtestColl
//////////////////////////////////////////////////////////////////////////////////////////////
int M3DtestColl(ZScene *scene, ZNode *nodeA, ZNode *nodeB, const ZVector3 &transl, float *res, ZVector3 *vcol, float garde, ZNode **collA, ZNode **collB)
{
	if(nodeA==nodeB)	return -1;
	
	ComputeWorldMatrixTree(scene, nodeA);
	ComputeWorldMatrixTree(scene, nodeB);

	ZVector3		axis, depl;
	axis.SetNull();

	depl = transl;
	*collA = *collB = NULL;

	int		k = testCollRecA(scene, nodeA, nodeA, nodeB, &depl, res, &axis, collA, collB);
	if(k<=0)		return k;
	
	float	l = *res;
	if(l<0)		*res = 0; 
	

	float	x;

	if( (x=axis*axis) != 0 )
	{
		float a = transl * axis;
		if(a>0)			*vcol = axis * (-(1-l)*a/x-garde/sqrt(x));
		else			*vcol = axis * (-(1-l)*a/x+garde/sqrt(x));
	}
	else
		vcol->SetNull();

	return k;
}