#include <cmath>

#include "Quaternions.h"

bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3)
{
	bool bInter = false;
	float pointv[3] { p->x, p->y, p->z };
	float p1v[3] { p1->x, p1->y, p1->z };
	float p2v[3] { p2->x, p2->y, p2->z };
	float p3v[3] { p3->x, p3->y, p3->z };
	float normalv[3] { normal.x, normal.y, normal.z };

	int i = 0, j = 0;
#define ABS(X) (((X)<0.f)?-(X):(X) )
#define MAX(A, B) (((A)<(B))?(B):(A))
	float max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
#undef MAX
	if (max == ABS(normalv[0])) {i = 1; j = 2;} // y, z
	if (max == ABS(normalv[1])) {i = 0; j = 2;} // x, z
	if (max == ABS(normalv[2])) {i = 0; j = 1;} // x, y
#undef ABS

	float u0 = pointv[i] - p1v[i];
	float v0 = pointv[j] - p1v[j];
	float u1 = p2v[i] - p1v[i];
	float v1 = p2v[j] - p1v[j];
	float u2 = p3v[i] - p1v[i];
	float v2 = p3v[j] - p1v[j];

	if (u1 > -1.0e-05f && u1 < 1.0e-05f)// == 0.0f)
	{
		float b = u0 / u2;
		if (0.0f <= b && b <= 1.0f)
		{
			float a = (v0 - b * v2) / v1;
			if ((a >= 0.0f) && (( a + b ) <= 1.0f))
				bInter = 1;
		}
	}
	else
	{
		float b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
		if (0.0f <= b && b <= 1.0f)
		{
			float a = (u0 - b * u2) / u1;
			if ((a >= 0.0f) && (( a + b ) <= 1.0f ))
				bInter = 1;
		}
	}

	return bInter;
}

extern float d;
extern float a1,a2,a3;
extern float total,denom,mu;
extern XYZ pa1,pa2,pa3,n;

float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc, XYZ n, XYZ *p)
{

   //Calculate the parameters for the plane
   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 (abs(denom) < 0.0000001)        // Line and plane don't intersect
      return 0;
   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 0;

   if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;}
   return 1;
}

float findDistance(XYZ point1, XYZ point2){
	return sqrt((point1.x-point2.x)*(point1.x-point2.x)+(point1.y-point2.y)*(point1.y-point2.y)+(point1.z-point2.z)*(point1.z-point2.z));
}

XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){
	XYZ newpoint;
	if(xang){
		xang*=6.283185;
		xang/=360;
	}
	if(yang){
		yang*=6.283185;
		yang/=360;
	}
	if(zang){
		zang*=6.283185;
		zang/=360;
	}

	if(yang){
	newpoint.z=thePoint.z*cos(yang)-thePoint.x*sin(yang);
	newpoint.x=thePoint.z*sin(yang)+thePoint.x*cos(yang);
	thePoint.z=newpoint.z;
	thePoint.x=newpoint.x;
	}

	if(zang){
	newpoint.x=thePoint.x*cos(zang)-thePoint.y*sin(zang);
	newpoint.y=thePoint.y*cos(zang)+thePoint.x*sin(zang);
	thePoint.x=newpoint.x;
	thePoint.y=newpoint.y;
	}

	if(xang){
	newpoint.y=thePoint.y*cos(xang)-thePoint.z*sin(xang);
	newpoint.z=thePoint.y*sin(xang)+thePoint.z*cos(xang);
	thePoint.z=newpoint.z;
	thePoint.y=newpoint.y;
	}

	return thePoint;
}