/*******************************************************************
  mathlib.cpp                 Copyright © 1998 Virtual Paradigm  
  ----------------------------------------------------------------
  $Release  : 1.0.0 
  $Revision : 												       
	r1.00	27-05-98	Initial release
                                                                   
  $Author(s):						
			Dennis Predovnik
																   
  $Purpose  : 
			Contains common math routines for use in project 
			COMA, under development by Virtual paradigm ltd.
  
*******************************************************************/

#include <math.h>
#include <stdlib.h>

#include <dsound.h>
#include "../sound/sound.h"

#include "../math/math3d.h"
#include "../datastructure/baseDS.h"
#include "../render/view.h"
#include "../physics/rigidbody.h"
#include "../physics/simulate.h"
#include "../math/mathlib.h"


#define	EPS	0.000001


extern "C" {
	void buildTables(void);

//	float COS[360];
//	float SIN[360];
}

// This function builds all the look up tables for the engine
void buildTables(void){
	int angle;
	float rad;

	// generate sin/cos look up tables
	for(angle=0; angle<=360; angle++){
	    rad = (float) (3.14159*(float)angle/(float)180);
//	    COS[angle] = (float) cos(rad);
//		SIN[angle] = (float) sin(rad);
    }
}

// MATH FUNCTIONS
void makeVector(T3F init, T3F term, T3F vector){
    vector[0] = term[0] - init[0];
    vector[1] = term[1] - init[1];
    vector[2] = term[2] - init[2];
}

//****************************************************************
//
// Routine calculates the object's xsphere,ysphere and zsphere.
//
//****************************************************************

void calcObjectStats(void){



}

//****************************************************************
//
// This function computes maximum radius of a bounding box.
//
//****************************************************************

void objSphere(OBJECT *obj){
	float newRadius,      // Used in average radius calculation of poly
		  x,y,z;          // A single vertex
	int   index,vCount=0; // Looping variable
	T3F	  cent;

	obj->radius=0;
	for(int loop=0; loop<obj->numVerts; loop++){
		// Reset poly's radius and centre
		x = 0 - obj->vertex->v[0];
		y = 0 - obj->vertex->v[1];
		z = 0 - obj->vertex->v[2];

		// Compute distance to point
		newRadius = (float) sqrt(x*x + y*y + z*z);

		// Is this radius bigger than last?
		if(newRadius>obj->radius){
			obj->radius = newRadius;
			obj->radius = newRadius;
		}
	}
}

/*******************************************************************

  $Desc: Calculates the distane of a point from a plane.			

		 The equation of a plane is a x + b y + c z + d = 0
			                        n.x x + n.y y + n.z z + d = 0
		 or 

		 N.(P-P0) = 0

		 - This routine uses the above formula.	 

  $Author   : Dennis Predovnik

  $Arguments:
		bool absPlane(T3F *point, T3F *pa, T3F *pb, T3F *pc);

  $Return value:
		angle difference. (left/right)

*******************************************************************/

float pointPlane(T3F point, T3F pa, T3F pb, T3F pc){
	T3F	      n;
	float     dp,nt;
	const int x=0, y=1, z=2;

	//******** Calculate normal
	float ax = pb[x]-pa[x];    //pa pb
	float ay = pb[y]-pa[y];
	float az = pb[z]-pa[z]; 
	float bx = pc[x]-pa[x];    //pc pb
	float by = pc[y]-pa[y];
	float bz = pc[z]-pa[z];

	n[x] = ay*bz-az*by; 	   // Compute cross product.
	n[y] = az*bx-ax*bz;
	n[z] = ax*by-ay*bx;
	//******** End calculate normal

	dp = DOT(n, pa);
	nt = DOT(n, point);

	return nt-dp;
}

/*****************************************************************

  $Desc	: Determines if a surface is visible from a 
  		  given vector.

  $Author : Dennis Predovnik

  $Arguments:
	int visFace(TVERTEX *norm, TVERTEX *vector);

  $Return value:
		0 = Invisible
		1 = Visible


*****************************************************************/

int visFace(T3F n, T3F v){

    float dp = ( n[0] * v[0] + n[1] * v[1] + n[2] * v[2] );

//	float dp = dotProduct(n, v);

	if(dp<=0)
		return 1;

	return 0;
}

/*******************************************************************

  $Desc: Determine whether or not the line segment p1,p2
		 Intersects the 3 vertex facet bounded by pa,pb,pc

		 The equation of the line is p = p1 + mu (p2 - p1)
		 The equation of the plane is a x + b y + c z + d = 0
			                          n.x x + n.y y + n.z z + d = 0

  $Arguments:
		int lineFacet(T3F p1, T3F p2, 
					  T3F pa, T3F pb, T3F pc, float *p)

  $Return value:
		 Return true/false and the intersection point p

*******************************************************************/

int lineFacet(T3F p1, T3F p2, 
			  T3F pa, T3F pb, T3F pc, float *p){
	double	d;
	double	a1,a2,a3;
	double	total,denom,mu;
	T3F     n,pa1,pa2,pa3;
	const int x=0, y=1, z=2;

	/* Calculate the parameters for the plane */
	n[x] = (pb[y] - pa[y])*(pc[z] - pa[z]) - (pb[z] - pa[z])*(pc[y] - pa[y]);
	n[y] = (pb[z] - pa[z])*(pc[x] - pa[x]) - (pb[x] - pa[x])*(pc[z] - pa[z]);
	n[z] = (pb[x] - pa[x])*(pc[y] - pa[y]) - (pb[y] - pa[y])*(pc[x] - pa[x]);
	NORMALIZE(n);
	d = - n[x] * pa[x] - n[y] * pa[y] - n[z] * pa[z];

	/* Calculate the position on the line that intersects the plane */
	denom = n[x] * (p2[x] - p1[x]) + n[y] * (p2[y] - p1[y]) + n[z] * (p2[z] - p1[z]);
	if(fabs(denom)<EPS)       /* Line and plane don't intersect */
		return(false);
	mu = - (d + n[x] * p1[x] + n[y] * p1[y] + n[z] * p1[z]) / denom;
	p[x] = p1[x] + mu * (p2[x] - p1[x]);
	p[y] = p1[y] + mu * (p2[y] - p1[y]);
	p[z] = p1[z] + mu * (p2[z] - p1[z]);
	if(mu < 0 || mu > 1)      /* Intersection not along line segment */
		return(false);

	/* Determine whether or not the intersection point is bounded by pa,pb,pc */
/*
	pa1.x = pa.x - p->x;
	pa1.y = pa.y - p->y;
	pa1.z = pa.z - p->z;
	normalize(&pa1);
	pa2.x = pb.x - p->x;
	pa2.y = pb.y - p->y;
	pa2.z = pb.z - p->z;
	normalize(&pa2);
	pa3.x = pc.x - p->x;
	pa3.y = pc.y - p->y;
	pa3.z = pc.z - p->z;
	normalize(&pa3);
	a1 = pa1.x*pa2.x + pa1.y*pa2.y + pa1.z*pa2.z;
	a2 = pa2.x*pa3.x + pa2.y*pa3.y + pa2.z*pa3.z;
	a3 = pa3.x*pa1.x + pa3.y*pa1.y + pa3.z*pa1.z;
	total = (acos(a1) + acos(a2) + acos(a3)) * RADIANS_TO_DEGREES;
	if(fabs(total - 360)>EPS)
		return(FALSE);
//*/
	return(true);
}


/*******************************************************************
  $Desc: 
		Triangle/Triangle intersection test routine,
		See article "A Fast Triangle-Triangle Intersection Test",
		Journal of Graphics Tools, 2(2), 1997

  $Arguments:
		int triTriIntersect(float V0[3],float V1[3],float V2[3],
			                float U0[3],float U1[3],float U2[3])

		Parameters: Vertices of triangle 1: V0,V1,V2
			        Vertices of triangle 2: U0,U1,U2

  $Return value:
		1 if the triangles intersect, otherwise 0

*******************************************************************/

//   if USE_EPSILON_TEST is true then we do a check: 
//         if |dv|<EPSILON then dv=0.0;
//   else no check is done (which is less robust)

#define USE_EPSILON_TEST TRUE  
#define EPSILON   0.000001  //COLLEPS  //DEPEPS   //0.000001

#define CROSS(dest,v1,v2)                      \
              dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
              dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
              dest[2]=v1[0]*v2[1]-v1[1]*v2[0];

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2)          \
            dest[0]=v1[0]-v2[0]; \
            dest[1]=v1[1]-v2[1]; \
            dest[2]=v1[2]-v2[2]; 

/* sort so that a<=b */
#define SORT(a,b)       \
             if(a>b)    \
             {          \
               float c; \
               c=a;     \
               a=b;     \
               b=c;     \
             }

#define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \
              isect0=VV0+(VV1-VV0)*D0/(D0-D1);    \
              isect1=VV0+(VV2-VV0)*D0/(D0-D2);


#define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \
  if(D0D1>0.0f)                                         \
  {                                                     \
	/* here we know that D0D2<=0.0 */                   \
    /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
    ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1);          \
  }                                                     \
  else if(D0D2>0.0f)                                    \
  {                                                     \
    /* here we know that d0d1<=0.0 */                   \
    ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1);          \
  }                                                     \
  else if(D1*D2>0.0f || D0!=0.0f)                       \
  {                                                     \
    /* here we know that d0d1<=0.0 or that D0!=0.0 */   \
    ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1);          \
  }                                                     \
  else if(D1!=0.0f)                                     \
  {                                                     \
    ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1);          \
  }                                                     \
  else if(D2!=0.0f)                                     \
  {                                                     \
    ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1);          \
  }                                                     \
  else                                                  \
  {                                                     \
    /* triangles are coplanar */                        \
    return coplanarTriTri(N1,V0,V1,V2,U0,U1,U2);      \
  }

/* this edge to edge test is based on Franlin Antonio's gem:
   "Faster Line Segment Intersection", in Graphics Gems III,
   pp. 199-202 */ 
#define EDGE_EDGE_TEST(V0,U0,U1)                      \
  Bx=U0[i0]-U1[i0];                                   \
  By=U0[i1]-U1[i1];                                   \
  Cx=V0[i0]-U0[i0];                                   \
  Cy=V0[i1]-U0[i1];                                   \
  f=Ay*Bx-Ax*By;                                      \
  d=By*Cx-Bx*Cy;                                      \
  if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f))  \
  {                                                   \
    e=Ax*Cy-Ay*Cx;                                    \
    if(f>0)                                           \
    {                                                 \
      if(e>=0 && e<=f) return 1;                      \
    }                                                 \
    else                                              \
    {                                                 \
      if(e<=0 && e>=f) return 1;                      \
    }                                                 \
  }                                

#define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
{                                              \
  float Ax,Ay,Bx,By,Cx,Cy,e,d,f;               \
  Ax=V1[i0]-V0[i0];                            \
  Ay=V1[i1]-V0[i1];                            \
  /* test edge U0,U1 against V0,V1 */          \
  EDGE_EDGE_TEST(V0,U0,U1);                    \
  /* test edge U1,U2 against V0,V1 */          \
  EDGE_EDGE_TEST(V0,U1,U2);                    \
  /* test edge U2,U1 against V0,V1 */          \
  EDGE_EDGE_TEST(V0,U2,U0);                    \
}

#define POINT_IN_TRI(V0,U0,U1,U2)           \
{                                           \
  float a,b,c,d0,d1,d2;                     \
  /* is T1 completly inside T2? */          \
  /* check if V0 is inside tri(U0,U1,U2) */ \
  a=U1[i1]-U0[i1];                          \
  b=-(U1[i0]-U0[i0]);                       \
  c=-a*U0[i0]-b*U0[i1];                     \
  d0=a*V0[i0]+b*V0[i1]+c;                   \
                                            \
  a=U2[i1]-U1[i1];                          \
  b=-(U2[i0]-U1[i0]);                       \
  c=-a*U1[i0]-b*U1[i1];                     \
  d1=a*V0[i0]+b*V0[i1]+c;                   \
                                            \
  a=U0[i1]-U2[i1];                          \
  b=-(U0[i0]-U2[i0]);                       \
  c=-a*U2[i0]-b*U2[i1];                     \
  d2=a*V0[i0]+b*V0[i1]+c;                   \
  if(d0*d1>0.0)                             \
  {                                         \
    if(d0*d2>0.0) return 1;                 \
  }                                         \
}

int coplanarTriTri(float N[3],float V0[3],float V1[3],float V2[3],
                   float U0[3],float U1[3],float U2[3]){
	float A[3];
	short i0,i1;
	/* first project onto an axis-aligned plane, that maximizes the area */
	/* of the triangles, compute indices: i0,i1. */
	A[0]=fabs(N[0]);
	A[1]=fabs(N[1]);
	A[2]=fabs(N[2]);
	if(A[0]>A[1]){
		if(A[0]>A[2]){
			i0=1;      /* A[0] is greatest */
			i1=2;
		}
		else{
			i0=0;      /* A[2] is greatest */
			i1=1;
		}
	}
	else{  /* A[0]<=A[1] */
		if(A[2]>A[1]){
			i0=0;      /* A[2] is greatest */
			i1=1;                                           
		}
		else{
			i0=0;      /* A[1] is greatest */
			i1=2;
		}
    }               
                
    /* test all edges of triangle 1 against the edges of triangle 2 */
    EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2);
    EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2);
    EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2);
                
    /* finally, test if tri1 is totally contained in tri2 or vice versa */
    POINT_IN_TRI(V0,U0,U1,U2);
    POINT_IN_TRI(U0,V0,V1,V2);

	return 0;
}

int triTriIntersect(float V0[3],float V1[3],float V2[3],
                    float U0[3],float U1[3],float U2[3]){
	float E1[3],E2[3];
	float N1[3],N2[3],d1,d2;
	float du0,du1,du2,dv0,dv1,dv2;
	float D[3];
	float isect1[2], isect2[2];
	float du0du1,du0du2,dv0dv1,dv0dv2;
	short index;
	float vp0,vp1,vp2;
	float up0,up1,up2;
	float b,c,max;

	/* compute plane equation of triangle(V0,V1,V2) */
	SUB(E1,V1,V0);
	SUB(E2,V2,V0);
	CROSS(N1,E1,E2);
	d1=-DOT(N1,V0);
	/* plane equation 1: N1.X+d1=0 */

	/* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
	du0=DOT(N1,U0)+d1;
	du1=DOT(N1,U1)+d1;
	du2=DOT(N1,U2)+d1;

	/* coplanarity robustness check */
#if USE_EPSILON_TEST==TRUE
	if(fabs(du0)<EPSILON) du0=0.0;
	if(fabs(du1)<EPSILON) du1=0.0;
	if(fabs(du2)<EPSILON) du2=0.0;
#endif
	du0du1=du0*du1;
	du0du2=du0*du2;

	if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
		return 0;                  /* no intersection occurs */

	/* compute plane of triangle (U0,U1,U2) */
	SUB(E1,U1,U0);
	SUB(E2,U2,U0);
	CROSS(N2,E1,E2);
	d2=-DOT(N2,U0);
	/* plane equation 2: N2.X+d2=0 */

	/* put V0,V1,V2 into plane equation 2 */
	dv0=DOT(N2,V0)+d2;
	dv1=DOT(N2,V1)+d2;
	dv2=DOT(N2,V2)+d2;

#if USE_EPSILON_TEST==TRUE
	if(fabs(dv0)<EPSILON) dv0=0.0;
	if(fabs(dv1)<EPSILON) dv1=0.0;
	if(fabs(dv2)<EPSILON) dv2=0.0;
#endif

	dv0dv1=dv0*dv1;
	dv0dv2=dv0*dv2;
        
	if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
		return 0;                  /* no intersection occurs */

	/* compute direction of intersection line */
	CROSS(D,N1,N2);

	/* compute and index to the largest component of D */
	max=fabs(D[0]);
	index=0;
	b=fabs(D[1]);
	c=fabs(D[2]);
	if(b>max) max=b,index=1;
	if(c>max) max=c,index=2;

    /* this is the simplified projection onto L*/
    vp0=V0[index];
    vp1=V1[index];
    vp2=V2[index];

    up0=U0[index];
    up1=U1[index];
    up2=U2[index];

	/* compute interval for triangle 1 */
	COMPUTE_INTERVALS(vp0,vp1,vp2,dv0,dv1,dv2,dv0dv1,dv0dv2,isect1[0],isect1[1]);

	/* compute interval for triangle 2 */
	COMPUTE_INTERVALS(up0,up1,up2,du0,du1,du2,du0du1,du0du2,isect2[0],isect2[1]);

	SORT(isect1[0],isect1[1]);
	SORT(isect2[0],isect2[1]);

	if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
	return 1;
}


int polyTriIntersect(struct TAGBSPSURF *poly, struct TAGMDLSURF *tri){
	int test, index=3;


	//*********** Check to see if triangle
	if(poly->numVerts==3){

		// 1st tri
		test = triTriIntersect(poly->poly[0]->wv, poly->poly[1]->wv, poly->poly[2]->wv,
				   			    tri->poly[0]->wv,  tri->poly[1]->wv,  tri->poly[2]->wv );
		if(test) return 1;

		// 2nd tri
		test = triTriIntersect(poly->poly[0]->wv, poly->poly[1]->wv, poly->poly[2]->wv,
				   			    tri->poly[0]->wv,  tri->poly[2]->wv,  tri->poly[3]->wv );
		if(test) return 1;

		return 0;
	}
	//*********** End check to see if triangle


	//*********** Tessalate 
	while(index<=poly->numVerts){

		// 1st tri
		test = triTriIntersect(poly->poly[0]->wv, poly->poly[index-2]->wv, poly->poly[index-1]->wv,
				   			    tri->poly[0]->wv, tri->poly[1]->wv, tri->poly[2]->wv );
		if(test) return 1;

		// 2nd tri
		test = triTriIntersect(poly->poly[0]->wv, poly->poly[index-2]->wv, poly->poly[index-1]->wv,
				   			    tri->poly[0]->wv,  tri->poly[2]->wv,  tri->poly[3]->wv );
		if(test) return 1;

		index++;
	}
	//*********** End Tessalate 

	return 0;
}

// END MATH FUNCTIONS  
