#include <math.h>
#include "MixedIntersection.h"


/* This returns a perpendicular vector from 2 given vectors by taking the cross product. */
CVector3 Cross(CVector3 vVector1, CVector3 vVector2)
{
	CVector3 vNormal;									
	vNormal.x = ((vVector1.y * vVector2.z) - (vVector1.z * vVector2.y));
	vNormal.y = ((vVector1.z * vVector2.x) - (vVector1.x * vVector2.z));
	vNormal.z = ((vVector1.x * vVector2.y) - (vVector1.y * vVector2.x));

	return vNormal;										
}


/* This returns the magnitude of a normal (or any other vector) */
float Magnitude(CVector3 vNormal)
{
	return (float)sqrt( (vNormal.x * vNormal.x) + (vNormal.y * vNormal.y) + (vNormal.z * vNormal.z) );
}


/* This returns a normalize vector (A vector exactly of length 1) */
CVector3 Normalize(CVector3 vNormal)
{
	float magnitude = Magnitude(vNormal);				
	vNormal.x /= magnitude;						
	vNormal.y /= magnitude;						
	vNormal.z /= magnitude;						
	return vNormal;						
}


/* This returns the normal of a polygon (The direction the polygon is facing) */
CVector3 Normal(CVector3 vTriangle[])
{														
	CVector3 vVector1 = vTriangle[2] - vTriangle[0];
	CVector3 vVector2 = vTriangle[1] - vTriangle[0];
	CVector3 vNormal = Cross(vVector1, vVector2);		
	vNormal = Normalize(vNormal);				
	return vNormal;						
}


/* This computers the dot product of 2 vectors */
float Dot(CVector3 vVector1, CVector3 vVector2) 
{
	return ( (vVector1.x * vVector2.x) + (vVector1.y * vVector2.y) + (vVector1.z * vVector2.z) );
}


/* This checks to see if a point is inside the ranges of a polygon */
double AngleBetweenVectors(CVector3 Vector1, CVector3 Vector2)
{							
	float dotProduct = Dot(Vector1, Vector2);				
	float vectorsMagnitude = Magnitude(Vector1) * Magnitude(Vector2) ;
	return( acos( dotProduct / vectorsMagnitude ) );
}


/* This returns the distance between a plane and the origin */
float PlaneDistance(CVector3 Normal, CVector3 Point)
{	
	float distance = 0;									
	distance = - ((Normal.x * Point.x) + (Normal.y * Point.y) + (Normal.z * Point.z));
	return distance;									
}


/* This returns the intersection point of the line that intersects the plane */
CVector3 IntersectionPoint(CVector3 vNormal, CVector3 vLine[], double distance)
{
	CVector3 vPoint, vLineDir;					
	double Numerator = 0.0, Denominator = 0.0, dist = 0.0;
	vLineDir = vLine[1] - vLine[0];		
	vLineDir = Normalize(vLineDir);		
	Numerator = - (vNormal.x * vLine[0].x +		
				   vNormal.y * vLine[0].y +
				   vNormal.z * vLine[0].z + distance);

	Denominator = Dot(vNormal, vLineDir);		
	if( Denominator == 0.0)						
		return vLine[0];					
	dist = Numerator / Denominator;				
	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;
}


/* This checks to see if a point is inside the ranges of a polygon */
bool InsidePolygon(CVector3 vIntersection, CVector3 Poly[], long verticeCount)
{
	const double MATCH_FACTOR = 0.99;		
	double Angle = 0.0;				
	CVector3 vA, vB;				
	
	for (int i = 0; i < verticeCount; i++)		
	{	
		vA = Poly[i] - vIntersection;			
		vB = Poly[(i + 1) % verticeCount] - vIntersection;
		Angle += AngleBetweenVectors(vA, vB);	
	}
											
	if(Angle >= (MATCH_FACTOR * (2.0 * MLpi)) )	
		return true;				
	return false;								
}


/* This checks to see if a line intersects a plane and returns the point of intersection (if a return vector is given) */
bool LinePlaneIntersection(CVector3 vPoly[], CVector3 vLine[], CVector3 &vNormal, float &originDistance, CVector3 &ip = NULL)
{
	float distance1=0, distance2=0;							
	vNormal = Normal(vPoly);						
	originDistance = PlaneDistance(vNormal, vPoly[0]);
	distance1 = ((vNormal.x * vLine[0].x)  +					
		         (vNormal.y * vLine[0].y)  +					
				 (vNormal.z * vLine[0].z)) + originDistance;		
	distance2 = ((vNormal.x * vLine[1].x)  +					
		         (vNormal.y * vLine[1].y)  +					
				 (vNormal.z * vLine[1].z)) + originDistance;

	if(distance1 * distance2 >= 0)			
	   return false;
  if(ip)
    ip = IntersectionPoint(vNormal, vLine, originDistance);
	return true;							
}


/* This returns true if the line intersects the polygon and puts the intersection point in ip */
bool LinePolygonIntersection(CVector3 vPoly[], CVector3 vLine[], int verticeCount, CVector3 &ip)
{
	CVector3 vNormal;
	float originDistance = 0;
	if(!LinePlaneIntersection(vPoly, vLine,   vNormal,   originDistance))
		return false;
	CVector3 vIntersection = IntersectionPoint(vNormal, vLine, originDistance);
	if(InsidePolygon(vIntersection, vPoly, verticeCount))
  {
    ip = vIntersection;
		return true;
  }
	return false;								
}


float Absolute(float num)
{
	if(num < 0)
		return (0 - num);
	return num;
}

/* This tells if a sphere is BEHIND, in FRONT, or INTERSECTS a plane, also it's distance */
int SpherePlaneIntersection(CVector3 &vCenter, CVector3 &vNormal, CVector3 &vPoint, float radius, float &distance)
{
	float d = (float)PlaneDistance(vNormal, vPoint);
	distance = (vNormal.x * vCenter.x + vNormal.y * vCenter.y + vNormal.z * vCenter.z + d);
	if(Absolute(distance) < radius)
		return INTERSECTS;
	else if(distance >= radius)
		return FRONT;
	return BEHIND;
}


/* This finds the edge planes of the triangle and checks the sphere against each one */
bool EdgePlanesCollision(CVector3 &vCenter, CVector3 vTriangle[], CVector3 &vNormal, float radius)
{
	int edge1 = 0, edge2 = 0, edge3 = 0;
	float distance = 0;
  	//////////////// Calculate Edge 1 ////////////////
	CVector3 vVector = vTriangle[1] - vTriangle[0];
	CVector3 vEdgeNormal = Cross(vVector, vNormal);
	vEdgeNormal = Normalize(vEdgeNormal);
	edge1 = SpherePlaneIntersection(vCenter, vEdgeNormal, vTriangle[0], radius, distance);

	//////////////// Calculate Edge 2 ////////////////
	vVector = vTriangle[2] - vTriangle[1];
	vEdgeNormal = Cross(vVector, vNormal);
	vEdgeNormal = Normalize(vEdgeNormal);
	edge2 = SpherePlaneIntersection(vCenter, vEdgeNormal, vTriangle[1], radius, distance);

	//////////////// Calculate Edge 3 ////////////////
	vVector = vTriangle[0] - vTriangle[2];
	vEdgeNormal = Cross(vVector, vNormal);
	vEdgeNormal = Normalize(vEdgeNormal);
	edge3 = SpherePlaneIntersection(vCenter, vEdgeNormal, vTriangle[0], radius, distance);
	if(edge1 != BEHIND && edge2 != BEHIND && edge3 != BEHIND)
		return true;
	return false;
}


/* This returns true if our sphere collides with the triangle passed in */
bool SpherePolygonIntersection(CVector3 vTriangle[], CVector3 &vCenter, float radius)
{
	CVector3 vNormal = Normal(vTriangle);
	float distance = 0.0f;
	int classification = SpherePlaneIntersection(vCenter, vNormal, vTriangle[0], radius, distance);
	if(classification == INTERSECTS) 
	{
		CVector3 vOffset = vNormal * distance;
		CVector3 vPosition = vCenter - vOffset;
		if(InsidePolygon(vPosition, vTriangle, 3))
			return true;	// We collided!
		else
		{
			if(EdgePlanesCollision(vCenter, vTriangle, vNormal, radius))
			{
				return true;	// We collided! "And you doubted me..." - Sphere
			}
		}
	}
	return false;
}