#include "stdafx.h"
#include "dTriColliderCommon.h"
#include "dTriColliderMath.h"
#include "dTriSphere.h"
#include "dctrilistcollider.h"
#include "../phworld.h"
#include "../../xrcdb/xr_area.h"
////////////////////////////////////////////////////////////////////////////
IC dReal dcTriListCollider::PointSphereTest(const dReal* center, const dReal radius,
							 const dReal* pt,dReal* norm)
{

	norm[0]=center[0]-pt[0];
	norm[1]=center[1]-pt[1];
	norm[2]=center[2]-pt[2];
	dReal mag=dSqrt(dDOT(norm,norm));
	dReal depth=radius-mag;
	if(depth<0.f)	return -1.f;
	if(mag>0.f)
	{
		norm[0]/=mag;norm[1]/=mag;norm[2]/=mag;
	}
	else 
	{
		norm[0]=0;norm[1]=1;norm[2]=0;
	}
	return depth;

}


inline dReal dcTriListCollider::FragmentonSphereTest(const dReal* center, const dReal radius,
								  const dReal* pt1, const dReal* pt2,dReal* norm)
{
	dVector3 direction={pt2[0]-pt1[0],pt2[1]-pt1[1],pt2[2]-pt1[2]};
	cast_fv(direction).normalize();
	dReal center_prg=dDOT(center,direction);
	dReal pt1_prg=dDOT(pt1,direction);
	dReal pt2_prg=dDOT(pt2,direction);
	dReal from1_dist=center_prg-pt1_prg;
	if(center_prg<pt1_prg||center_prg>pt2_prg) return -1;
	dVector3 line_to_center={
		-pt1[0]-direction[0]*from1_dist+center[0],
			-pt1[1]-direction[1]*from1_dist+center[1],
			-pt1[2]-direction[2]*from1_dist+center[2]
	};

	float mag=dSqrt(dDOT(line_to_center,line_to_center));
	//dNormalize3(norm);


	dReal depth=radius-mag;
	if(depth<0.f) return -1.f;
	if(mag>0.f)
	{
		norm[0]=line_to_center[0]/mag;
		norm[1]=line_to_center[1]/mag;
		norm[2]=line_to_center[2]/mag;
	}
	else
	{
		norm[0]=0;
		norm[1]=1;
		norm[2]=0;
	}
	return depth;
}


///////////////////////////////////////////////////////////////////////////////
IC bool dcTriListCollider::FragmentonSphereTest(const dReal* center, const dReal radius,
								  const dReal* pt1, const dReal* pt2,dReal* norm,dReal& depth)
{
	dVector3 V={pt2[0]-pt1[0],pt2[1]-pt1[1],pt2[2]-pt1[2]};
	dVector3 L={pt1[0]-center[0],pt1[1]-center[1],pt1[2]-center[2]};
	dReal sq_mag_V=dDOT(V,V);
	dReal dot_L_V=dDOT(L,V);
	dReal t=-dot_L_V/sq_mag_V;//t
	if(t<0.f||t>1.f)return false;
	dVector3 Pc={pt1[0]+t*V[0],pt1[1]+t*V[1],pt1[2]+t*V[2]};
	dVector3 Dc={center[0]-Pc[0],center[1]-Pc[1],center[2]-Pc[2]};
	dReal sq_mag_Dc=dDOT(Dc,Dc);
	if(sq_mag_Dc>radius*radius)return false;
	//dReal dot_V_Pc=dDOT(V,Pc);
	//if(dDOT(V,pt1)>dot_V_Pc||dDOT(V,pt2)<dot_V_Pc) return false;
	
	dReal mag=dSqrt(sq_mag_Dc);
	depth=radius-mag;
	if(mag>0.f)
	{
		norm[0]=Dc[0]/mag;
		norm[1]=Dc[1]/mag;
		norm[2]=Dc[2]/mag;
	}
	else
	{
		norm[0]=0;
		norm[1]=1;
		norm[2]=0;
	}
	return true;
}

IC bool	dcTriListCollider:: PointSphereTest(const dReal* center, const dReal radius,
							 const dReal* pt,dReal* norm,dReal &depth)
{

	norm[0]=center[0]-pt[0];
	norm[1]=center[1]-pt[1];
	norm[2]=center[2]-pt[2];
	dReal smag=dDOT(norm,norm);
	if(smag >radius*radius)return false;
	float mag=dSqrt(smag);
	depth=radius-mag;
	if(mag>0.f)
	{
		norm[0]/=mag;norm[1]/=mag;norm[2]/=mag;
	}
	else 
	{
		norm[0]=0;norm[1]=1;norm[2]=0;
	}
	return true;

}
/////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
int dcTriListCollider::dSortedTriSphere(const dReal*	/**v1/**/,const dReal*	/**v2/**/,
					 const dReal* triAx,
					 CDB::TRI* T,
					 dReal dist,
					 dxGeom* Sphere,
					 dxGeom* Geometry, 
					 int Flags, 
					 dContactGeom* Contacts,
					 int			skip
					 ){


						 //const dReal* v1=(dReal*)T->verts[1];
						 //const dReal* v2=(dReal*)T->verts[2];
						 const dReal* SphereCenter=dGeomGetPosition(Sphere);
						 const float SphereRadius = dGeomSphereGetRadius(Sphere);



						 //	dNormalize3(triAx);
						 const dReal *ContactNormal=triAx;//{triAx[0],triAx[1],triAx[2]};
						 dVector3 ContactPos={SphereCenter[0]-triAx[0]* SphereRadius,SphereCenter[1]-triAx[1]* SphereRadius,SphereCenter[2]-triAx[2]* SphereRadius};

						 float ContactDepth= -dist + SphereRadius;
						 if (ContactDepth >= 0){

							 Contacts->normal[0] =-ContactNormal[0];
							 Contacts->normal[1] =-ContactNormal[1];
							 Contacts->normal[2] =-ContactNormal[2];
							 Contacts->depth = ContactDepth;
							 ////////////////////

							 Contacts->pos[0]=ContactPos[0];
							 Contacts->pos[1]=ContactPos[1];
							 Contacts->pos[2]=ContactPos[2];
							 Contacts->g1 = Geometry;
							 Contacts->g2 = Sphere;
							 ((dxGeomUserData*)dGeomGetData(Sphere))->tri_material=T->material;
							 if(dGeomGetUserData(Sphere)->callback)dGeomGetUserData(Sphere)->callback(T,Contacts);
							 SURFACE(Contacts,0)->mode=T->material;
							 //////////////////////////////////
							 return 1;

						 }

						 return 0;
					 }


int dcTriListCollider::dTriSphere(const dReal* v0,const dReal* v1,const dReal* v2,
	 Triangle* T,
	 dxGeom* Sphere,dxGeom* Geometry, int Flags, 
	 dContactGeom* Contacts,int /**skip/**/)
 {
 
 
	 const dVector3 &triSideAx0	=T->side0	;
	 const dVector3 &triSideAx1	=T->side1	;
	 const dVector3 &triAx		=T->norm	;

	 //if(!TriPlaneContainPoint(triAx,v0,SphereCenter)) return 0;


	 const dReal radius=dGeomSphereGetRadius(Sphere);
	 float Depth=-T->dist+radius;
	 if(Depth<0.f)	return 0;
	 const dReal* pos=dGeomGetPosition(Sphere);
	 dVector3 ContactNormal;
	 if(TriContainPoint(v0,v1,v2,triAx,triSideAx0,triSideAx1,pos))
	 {
		 ContactNormal[0]=triAx[0];ContactNormal[1]=triAx[1];ContactNormal[2]=triAx[2];
		 //dVector3 ContactPos={pos[0]-triAx[0]* radius,pos[1]-triAx[1]* radius,pos[2]-triAx[2]* radius};
	 }
	 else
	 {
		//VERIFY( g_pGameLevel );
		CDB::TRI*       T_array      = inl_ph_world().ObjectSpace().GetStaticTris();
		flags8& gl_state=gl_cl_tries_state[I-B];
		if(gl_state.test(fl_engaged_s0)||gl_state.test(fl_engaged_s1)||gl_state.test(fl_engaged_s2))	
			return 0;
		if(FragmentonSphereTest(pos,radius,v0,v1,ContactNormal,Depth))
		{
			SideToGlClTriState(T->T->verts[0],T->T->verts[1],T_array);
		}
		else if(FragmentonSphereTest(pos,radius,v1,v2,ContactNormal,Depth))
		{
			SideToGlClTriState(T->T->verts[1],T->T->verts[2],T_array );
		}
		else if(FragmentonSphereTest(pos,radius,v2,v0,ContactNormal,Depth))
		{
			SideToGlClTriState(T->T->verts[2],T->T->verts[0],T_array );
		}
		else{
			if(gl_state.test(fl_engaged_v0)||gl_state.test(fl_engaged_v1)||gl_state.test(fl_engaged_v2))	
																									return 0;
			if(PointSphereTest(pos,radius,v0,ContactNormal,Depth))
			{
				VxToGlClTriState(T->T->verts[0],T_array );
			}
			else if(PointSphereTest(pos,radius,v1,ContactNormal,Depth))
			{
				VxToGlClTriState(T->T->verts[1],T_array );
			}
			else if(PointSphereTest(pos,radius,v2,ContactNormal,Depth))
			{
				VxToGlClTriState(T->T->verts[2],T_array );
			}
			else return 0;
		 }
	 }

	 Contacts->normal[0] =-ContactNormal[0];
	 Contacts->normal[1] =-ContactNormal[1];
	 Contacts->normal[2] =-ContactNormal[2];
	 Contacts->depth = Depth;
	 ////////////////////

	 Contacts->pos[0]=pos[0]-ContactNormal[0]*radius;
	 Contacts->pos[1]=pos[1]-ContactNormal[1]*radius;
	 Contacts->pos[2]=pos[2]-ContactNormal[2]*radius;
	 Contacts->g1 = Geometry;
	 Contacts->g2 = Sphere;
	 ((dxGeomUserData*)dGeomGetData(Sphere))->tri_material=T->T->material;
	 if(dGeomGetUserData(Sphere)->callback)dGeomGetUserData(Sphere)->callback(T->T,Contacts);
	 SURFACE(Contacts,0)->mode=T->T->material;
	 //////////////////////////////////
	 //	++OutTriCount;
	 return 1;
 }

