#include "../include/sharedIncludes.h"
#include <float.h>


float PlaneDistance(CVector Normal, CVector Point)
{	
	return -(Normal % Point);
}


bool IntersectedPlane(CVector vPoly[], CVector vLine[], CVector vNormal, float &originDistance)
{
	float distance1=0, distance2=0;						// The distances from the 2 points of the line from the plane
	originDistance = PlaneDistance(vNormal, vPoly[0]);
	distance1 = ((vNormal.x * vLine[0].x) + (vNormal.y * vLine[0].y) + (vNormal.z * vLine[0].z)) + originDistance;  // Get the distance from point1 and point2 from the plane using: Ax + By + Cz + D = (The distance from the plane)
  distance2 = ((vNormal.x * vLine[1].x) + (vNormal.y * vLine[1].y) + (vNormal.z * vLine[1].z)) + originDistance;

	if(distance1 * distance2 >= 0)			// Check to see if both point's distances are both negative or both positive
	   return false;
	return true;
}


double AngleBetweenVectors(CVector Vector1, CVector Vector2)
{							
	float dotProduct = (Vector1 % Vector2);
	float vectorsMagnitude = Vector1.Length() * Vector2.Length();
	double angle = acos( dotProduct / vectorsMagnitude );

	if(_isnan(angle))
		return 0;
	return( angle ); // Return the angle in radians
}


CVector IntersectionPoint(CVector vNormal, CVector vLine[], double distance)
{
	CVector vPoint, vLineDir;		// Variables to hold the point and the line's direction
	double Numerator = 0.0, Denominator = 0.0, dist = 0.0;

	vLineDir = vLine[1] - vLine[0];		// Get the Vector of the line
	vLineDir.Normalize();				// Normalize the lines vector
  Numerator = - (vNormal.x * vLine[0].x + vNormal.y * vLine[0].y + vNormal.z * vLine[0].z + distance);
	Denominator = vNormal % vLineDir;		// Get the dot product of the line's vector and the normal of the plane

  if( Denominator == 0.0)						// Check so we don't divide by zero
		return vLine[0];						// Return an arbitrary point on the line

  dist = Numerator / Denominator;				// Divide to get the multiplying (percentage) factor
	vPoint.x = (float)(vLine[0].x + (vLineDir.x * dist));
	vPoint.y = (float)(vLine[0].y + (vLineDir.y * dist));
	vPoint.z = (float)(vLine[0].z + (vLineDir.z * dist));

	return vPoint;								// Return the intersection point
}


bool InsidePolygon(CVector vIntersection, CVector Poly[], long verticeCount)
{
	const double MATCH_FACTOR = 0.9999;		// Used to cover up the error in floating point
	double Angle = 0.0;						// Initialize the angle
	CVector vA, vB;						// Create temp vectors

  for (int i = 0; i < verticeCount; i++)		// Go in a circle to each vertex and get the angle between
	{	
		vA = Poly[i] - vIntersection;	// Subtract the intersection point from the current vertex
		vB = Poly[(i + 1) % verticeCount] - vIntersection;  // Subtract the point from the next vertex
		Angle += AngleBetweenVectors(vA, vB);	// Find the angle between the 2 vectors and add them all up as we go along
	}
	if(Angle >= (MATCH_FACTOR * (2.0 * MLpi)) )	// If the angle is greater than 2 PI, (360 degrees)
		return true;							// The point is inside of the polygon
	return false;								// If you get here, it obviously wasn't inside the polygon, so Return FALSE
}


bool LinePolyIntersect2(CVector vPoly[], CVector vNormal, CVector vLine[], int verticeCount)
{
	float originDistance = 0;

	if(!IntersectedPlane(vPoly, vLine, vNormal, originDistance))
		return false;

  CVector vIntersection = IntersectionPoint(vNormal, vLine, originDistance);

	if(InsidePolygon(vIntersection, vPoly, verticeCount))
		return true;							// We collided!	  Return success
	return false;								// There was no collision, so return false
}