/*==============================================================*/
/* E3D library							*/
/*								*/
/* Math functions						*/
/*								*/
/* AUTHOR:	Gabor Nagy					*/
/* DATE:	1996-Nov-01 23:39:24				*/
/*								*/
/* Copyright (C) 1995 by Gabor Nagy. All rights reserved.	*/
/*==============================================================*/
#include <float.h>
#include <math.h>

#include <E3D/Math.h>

#include <E3D/Matrix.h>

/*======================================*/
/* Get angle of a 2D vector		*/
/*======================================*/
E3dAngle E3d_GetAngle2D(E3dCoordinate LXPos, E3dCoordinate LYPos)
{
 E3dAngle	LAtnVal;


 if(LXPos==0.0)
 {
  if(LYPos==0.0) return((E3dAngle)0.0);
  if(LYPos>0.0) return((E3dAngle)90.0);
  if(LYPos<0.0) return((E3dAngle)-90.0);
 }
 else
 {
  if(LYPos==0.0)
  {
   if(LXPos>0.0) return((E3dAngle)0.0);
   if(LXPos<0.0) return((E3dAngle)180.0);
  }
  else
  {
   LAtnVal=E3dRADIANS_TO_DEGREES*atan(LYPos/LXPos);
   if(LXPos>0.0) return((E3dAngle)LAtnVal);
   else
   {
    if(LYPos>0.0) return((E3dAngle)(180.0+LAtnVal));
    else
    {
     if(LYPos<0.0) return((E3dAngle)(-180.0+LAtnVal));
     else return((E3dAngle)180.0);
    }
   }
  }
 }
 return((E3dAngle)0.0);
}


/*==============================================================*/
/* Compute XYZ from Longitude, Latitude and Distance		*/
/*==============================================================*/
void E3d_Orbit(double LLongitude, double LLatitude, double LDistance, E3d3DPosition* L3DPosition)
{
 E3dMatrix	LMatrix;


 if(LLongitude<0.0) LLongitude+=360.0;
 if(LLongitude>360.0) LLongitude-=360.0;

 E3d_MatrixLoadIdentity(LMatrix);
 E3d_MatrixRotate(LMatrix, 'y', LLongitude);
 E3d_MatrixRotate(LMatrix, 'x', -LLatitude);

 L3DPosition->X=LDistance*LMatrix[M20];
 L3DPosition->Y=LDistance*LMatrix[M21];
 L3DPosition->Z=LDistance*LMatrix[M22];
}


/*==============================================================*/
/* Compute quaternion that rotates Vector0 into Vector1		*/
/*==============================================================*/
void E3d_VectorsToQuaternion(E3dVector* LVector0, E3dVector* LVector1, E3dQuaternion* LQuaternion)
{
 E3dAngle	LAngleSin=LVector0->X*LVector1->X+LVector0->Y*LVector1->Y+LVector0->Z*LVector1->Z;	// Sine of angle:  LVector0 . LVector1
 E3dAngle	LAngleCos=sqrt(1.0-LAngleSin*LAngleSin);						// Cosine of angle: sqrt(1-sin(a)*sin(a))


 E3dM_CrossProduct3D(LVector0->X, LVector0->Y, LVector0->Z,  LVector1->X, LVector1->Y, LVector1->Z,  LQuaternion->X, LQuaternion->Y, LQuaternion->Z);
 LQuaternion->X*=LAngleCos;LQuaternion->Y*=LAngleCos;LQuaternion->Z*=LAngleCos;
 LQuaternion->Angle=LAngleSin;
}


/*======================================*/
/* Normal vector of a triangle		*/
/*======================================*/
void E3d_NormalV(E3dVector* LNormal, E3d3DPosition* LPoint0, E3d3DPosition* LPoint1, E3d3DPosition* LPoint2)
{
 double	LX0, LY0, LZ0, LX1, LY1, LZ1, LX2, LY2, LZ2;
 double	i, p, q;
 double	LR;

 LX0=LPoint0->X;LY0=LPoint0->Y;LZ0=LPoint0->Z;
 LX1=LPoint1->X;LY1=LPoint1->Y;LZ1=LPoint1->Z;
 LX2=LPoint2->X;LY2=LPoint2->Y;LZ2=LPoint2->Z;

 i=-((LY2-LY1)*(LZ2-LZ0)-(LZ2-LZ1)*(LY2-LY0));
 p=(LX2-LX1)*(LZ2-LZ0)-(LZ2-LZ1)*(LX2-LX0);
 q=-((LX2-LX1)*(LY2-LY0)-(LY2-LY1)*(LX2-LX0));
 LR=sqrt(i*i+p*p+q*q);if(LR>0.0) LR=1.0/LR;
 LNormal->X=i*LR;LNormal->Y=p*LR;LNormal->Z=q*LR;
 LNormal->Length=1.0;
}


/*======================================================================*/
/* Distance of a point and a line given by 2 points, in 3D space	*/
/*======================================================================*/
E3dCoordinate E3d_PointDistanceFromLine(double LX, double LY, double LZ, double LX0, double LY0, double LZ0, double LX1, double LY1, double LZ1, double LX2, double LY2, double LZ2)
{
 E3dCoordinate	LTc;
 E3dCoordinate	m, n, p;

 m=LX1-LX0;n=LY1-LY0;p=LZ1-LZ0;
 LTc=(m*(LX2-LX0)+n*(LY2-LY0)+p*(LZ2-LZ0))/(m*m+n*n+p*p);
 LX=LX0+m*LTc-LX2;LY=LY0+n*LTc-LY2;LZ=LZ0+p*LTc-LZ2;
 return(sqrt(LX*LX+LY*LY+LZ*LZ));
}


/*======================================================*/
/* Returns TRUE if the three points are colinear or	*/
/* the first point in the list is at one of the ends	*/
/*======================================================*/
EBool E3d_Colinear(double LP1X, double LP1Y, double LP2X, double LP2Y, double LP3X, double LP3Y)
{
 E3dCoordinate	LSX, LSY, LTX, LTY, LDet, LErr;

 LSX=LP2X-LP1X;LSY=LP2Y-LP1Y;
 LTX=LP3X-LP1X;LTY=LP3Y-LP1Y;

 LDet=LSX*LTY-LSY*LTX;
 LErr=E3dCOLINERR*(E3dM_ABS(LSX)+E3dM_ABS(LSY)+E3dM_ABS(LTX)+E3dM_ABS(LTY));

// LErr must be positive
//

// The points are colinear
//
 if(E3dM_ABS(LDet)<LErr) 
 {
// For a to be on an end, the direction of the s and t must be the same
//
  if((LSX*LTX<-E3dCOLINERR)||(LSY*LTY<-E3dCOLINERR)) return(FALSE);
  return(TRUE);
 }
 return(FALSE);
}


/*==============================================================*/
/* Determine whether two 2D line segments intersect		*/
/*								*/
/* Return value:						*/
/*  - TRUE if:							*/
/*    - P1-P2 intersects Q1-Q2,					*/
/*    - P1-P2 and Q1-Q2 share a point and are co-linear		*/
/*    - P1-P2 and Q1-Q2 are the exact opposite of each-other	*/
/*    - P1-P2 and Q1-Q2 are identical				*/
/*  - FALSE otherwise						*/
/*==============================================================*/
EBool E3d_Intersect(double LP1X, double LP1Y, double LP2X, double LP2Y, double LQ1X, double LQ1Y, double LQ2X, double LQ2Y)
{
 E3dType	LUX, LUY, LVX, LVY, LDX, LDY, LDelta;
 E3dType 	LT1, LT2;
 EBool		LIntersect;

 LT1=0;LT2=0;
 LUX=LP2X-LP1X;LUY=LP2Y-LP1Y;LVX=LQ2X-LQ1X;LVY=LQ2Y-LQ1Y;

 LIntersect=FALSE;
 LDelta=(LUY*LVX)-(LUX*LVY);

 if(E3dM_ABS(LDelta)>E3dMACHINE_ERROR)
 {
  if(LP1X==LQ1X&&LP1Y==LQ1Y)			// P1 = Q1
  {
   if(LP2X==LQ2X&&LP2Y==LQ2Y) return(TRUE);	// The two line segments are the same
   else return(E3d_Colinear(LP1X, LP1Y, LP2X, LP2Y, LQ2X, LQ2Y));
  }
  else if(LP1X==LQ2X&&LP1Y==LQ2Y)		// P1 = Q2
  {
   if(LP2X==LQ1X&&LP2Y==LQ1Y) return(TRUE);	// The two line segments are the opposite of each-other
   else return(E3d_Colinear(LP1X, LP1Y, LP2X, LP2Y, LQ1X, LQ1Y));
  }
  else if(LP2X==LQ1X&&LP2Y==LQ1Y)		// P2 = Q1
  {
   return(E3d_Colinear(LP2X, LP2Y, LP1X, LP1Y, LQ2X, LQ2Y));
  }
  else if(LP2X==LQ2X&&LP2Y==LQ2Y)		// P2 = Q2
  {
   return(E3d_Colinear(LP2X, LP2Y, LP1X, LP1Y, LQ1X, LQ1Y));
  }

  LDX=LP1X-LQ1X;LDY=LP1Y-LQ1Y;

  LT1=((LDX*LVY)-(LDY*LVX))/LDelta;
  if(E3dM_ABS(LVX)>E3dMACHINE_ERROR) LT2=(LDX+(LUX*LT1))/LVX;
  else
  {
   if(E3dM_ABS(LVY)>E3dMACHINE_ERROR) LT2=(LDY+(LUY*LT1))/LVY;
  }
  if(LT1>0.0&&LT1<1.0&&LT2>0.0&&LT2<1.0) return(TRUE);
 }
 return(LIntersect);
}


/*======================================================================================================*/
/* Determine whether two 2D line segments intersect and	if they do,					*/
/* return the coordinates of the intersection point							*/
/*													*/
/* Return value	    Meaning							 LIntersection value	*/
/*------------------------------------------------------------------------------------------------------*/
/* E3dDISJOINT	    P1-P2 and Q1-Q2 are completely disjoint			 unchanged		*/
/* E3dINTERSECT	    P1-P2 intersects Q1-Q2					 intersection point	*/
/* E3dINTERSECT_P1  P1-P2 intersects Q1-Q2 at P1				 unchanged		*/
/* E3dINTERSECT_P2  P1-P2 intersects Q1-Q2 at P2				 unchanged		*/
/* E3dINTERSECT_Q1  P1-P2 intersects Q1-Q2 at Q1				 unchanged		*/
/* E3dINTERSECT_Q2  P1-P2 intersects Q1-Q2 at Q2				 unchanged		*/
/* E3dSHARE_P1	    P1-P2 and Q1-Q2 share P1					 unchanged		*/
/* E3dSHARE_P2	    P1-P2 and Q1-Q2 share P2					 unchanged		*/
/* E3dSHARE_Q1	    P1-P2 and Q1-Q2 share Q1					 unchanged		*/
/* E3dSHARE_Q2	    P1-P2 and Q1-Q2 share Q2					 unchanged		*/
/* E3dPARALLEL	    P1-P2 is parallel with Q1-Q2				 unchanged		*/
/* E3dOVERLAP	    P1-P2 and Q1-Q2 overlap, but they don't share any points	 unchanged		*/
/* E3dOVERLAP_P1    P1-P2 and Q1-Q2 overlap and they share P1			 unchanged		*/
/* E3dOVERLAP_P2    P1-P2 and Q1-Q2 overlap and they share P2			 unchanged		*/
/* E3dOPPOSITE	    P1-P2 is the same as Q2-Q1					 unchanged		*/
/* E3dEQUAL	    P1-P2 is the same as Q1-Q2					 unchanged		*/
/*======================================================================================================*/
EBool E3d_2DIntersection(double LP1X, double LP1Y, double LP2X, double LP2Y, double LQ1X, double LQ1Y, double LQ2X, double LQ2Y, E3d2DPosition* LIntersection)
{
 E3dType	LUX, LUY, LVX, LVY, LDX, LDY, LDelta;
 E3dType 	LT1, LT2;

 LT1=0;LT2=0;
 LUX=LP2X-LP1X;LUY=LP2Y-LP1Y;LVX=LQ2X-LQ1X;LVY=LQ2Y-LQ1Y;

 LDelta=(LUY*LVX)-(LUX*LVY);

 if(E3dM_ABS(LDelta)>E3dMACHINE_ERROR)
 {
// Check for special cases
//
  if(LP1X==LQ1X&&LP1Y==LQ1Y)		// P1 = Q1
  {
   if(LP2X==LQ2X&&LP2Y==LQ2Y) return(E3dEQUAL);
   else
   {
    if(E3d_Colinear(LP1X, LP1Y, LP2X, LP2Y, LQ2X, LQ2Y)) return(E3dOVERLAP_P1);
    else return(E3dSHARE_P1);
   }
  }
  else if(LP1X==LQ2X&&LP1Y==LQ2Y)	// P1 = Q2
  {
   if(LP2X==LQ1X&&LP2Y==LQ1Y) return(E3dOPPOSITE);
   else
   {
    if(E3d_Colinear(LP1X, LP1Y, LP2X, LP2Y, LQ1X, LQ1Y)) return(E3dOVERLAP_P1);
    else return(E3dSHARE_P2);
   }
  }
  else if(LP2X==LQ1X&&LP2Y==LQ1Y)	// P2 = Q2
  {
   if(E3d_Colinear(LP2X, LP2Y, LP1X, LP1Y, LQ2X, LQ2Y)) return(E3dOVERLAP_P2);
   else return(E3dSHARE_P2);
  }
  else if(LP2X==LQ2X&&LP2Y==LQ2Y)
  {
   if(E3d_Colinear(LP2X, LP2Y, LP1X, LP1Y, LQ1X, LQ1Y)) return(E3dOVERLAP_P2);
   else return(E3dSHARE_P2);
  }

  LDX=LP1X-LQ1X;LDY=LP1Y-LQ1Y;

  LT1=((LDX*LVY)-(LDY*LVX))/LDelta;
  if(E3dM_ABS(LVX)>E3dMACHINE_ERROR) LT2=(LDX+(LUX*LT1))/LVX;
  else
  {
   if(E3dM_ABS(LVY)>E3dMACHINE_ERROR) LT2=(LDY+(LUY*LT1))/LVY;
  }
  if(LT1>0.0&&LT1<1.0&&LT2>0.0&&LT2<1.0)
  {
   LIntersection->X=LP1X+LUX*LT1;
   LIntersection->Y=LP1Y+LUY*LT1;
   return(E3dINTERSECT);
  }
 }
 return(E3dDISJOINT);
}
