#pragma once
#include "../../stdafx.h"

#include "glLibMathLA.h"
#include "glLibMathVec.h"
#include "glLibMathMisc.h"

namespace glLib { namespace Math { namespace Intersect {

#pragma region Defines
#define GLLIBINTERNAL_CLASSIFY_TRI_BY_FACE(v1,v2,v3,faceplane,out_of_face) {\
	_distances[0] = DISTANCE_PLANE_POINT(faceplane,v1);\
	_distances[1] = _distances[0] * DISTANCE_PLANE_POINT(faceplane,v2);\
	_distances[2] = _distances[0] * DISTANCE_PLANE_POINT(faceplane,v3);\
	if(_distances[1]>0.0f && _distances[2]>0.0f)\
	{\
		out_of_face = 1;\
	}\
	else\
	{\
		out_of_face = 0;\
	}\
}\

#define GLLIBINTERNAL_CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri_points,tri_edge_planes, clipped_points, clipped_point_count) {\
	clipped_point_count = 0;\
	_temp_clip_count = 0;\
	PLANE_CLIP_POLYGON(tri_edge_planes[0],tri_points,3,_temp_clip,_temp_clip_count,MAX_TRI_CLIPPING);\
	if (_temp_clip_count>0)\
	{\
		_temp_clip_count2 = 0;\
		PLANE_CLIP_POLYGON(tri_edge_planes[1],_temp_clip,_temp_clip_count,_temp_clip2,_temp_clip_count2,MAX_TRI_CLIPPING);\
		if (_temp_clip_count2>0)\
		{\
			PLANE_CLIP_POLYGON(tri_edge_planes[2],_temp_clip2,_temp_clip_count2,clipped_points,clipped_point_count,MAX_TRI_CLIPPING);\
		}\
	}\
}\

#define GLLIBINTERNAL_MOST_DEEP_POINTS(plane,points,point_count,deep_points,deep_points_count,maxdeep) {\
	maxdeep=-1000.0f;\
	GUINT32 _k;\
	GREAL _dist;\
	deep_points_count = 0;\
	for(_k=0;_k<point_count;_k++) {\
		_dist = -DISTANCE_PLANE_POINT(plane,points[_k]);\
		if(_dist>maxdeep) {\
			maxdeep = _dist;\
			_max_candidates[0] = _k;\
			deep_points_count=1;\
		}\
		else if((_dist+G_EPSILON)>=maxdeep) {\
			_max_candidates[deep_points_count] = _k;\
			deep_points_count++;\
		}\
	}\
	if(maxdeep<0.0f) {\
		deep_points_count = 0;\
	}\
	else {\
		for(_k=0;_k<deep_points_count;_k++) {\
			VEC_COPY(deep_points[_k],points[_max_candidates[_k]]);\
		}\
	}\
}\

#pragma endregion

template <typename type_float>        bool ray_triangle(const type_float ray_pos[3], const type_float ray_direction[3], const type_float triangle_v1[3], const type_float triangle_v2[3], const type_float triangle_v3[3], type_float* distance, type_float intersection[3]);
template <typename type_float> inline bool ray_triangle(const type_float ray_pos[3], const type_float ray_direction[3], const type_float triangle[3][3],                                                                   type_float* distance                            ) {
	type_float intersection[3];
	return ray_triangle(ray_pos,ray_direction,triangle[0],triangle[1],triangle[2],distance,intersection);
}
template <typename type_float> inline bool ray_triangle(const type_float ray_pos[3], const type_float ray_direction[3], const type_float triangle[3][3],                                                                   type_float* distance, type_float intersection[3]) {
	return ray_triangle(ray_pos,ray_direction,triangle[0],triangle[1],triangle[2],distance,intersection);
}
template <typename type_float> inline bool ray_triangle(const type_float ray_pos[3], const type_float ray_direction[3], const type_float triangle_v1[3], const type_float triangle_v2[3], const type_float triangle_v3[3], type_float* distance                            ) {
	type_float intersection[3];
	return ray_triangle(ray_pos,ray_direction,triangle_v1,triangle_v2,triangle_v3,distance,intersection);
}
template <typename type_float>        bool ray_triangle(const type_float ray_pos[3], const type_float ray_direction[3], const type_float triangle_v1[3], const type_float triangle_v2[3], const type_float triangle_v3[3], type_float* distance, type_float intersection[3]) {
	//adapted from http://geometryalgorithms.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
	type_float u[3], v[3], temp[3], n[3], v1[3], I[3], w[3];
	::glLib::Math::Vec::subtract3(triangle_v2,triangle_v1,u);
	::glLib::Math::Vec::subtract3(triangle_v3,triangle_v1,v);
	::glLib::Math::Vec::cross(u,v,temp);
	::glLib::Math::Vec::normalize3(temp,n);

	::glLib::Math::Vec::subtract3(triangle_v1,ray_pos,v1);

	::glLib::Math::Vec::negate3(v1,temp);
	type_float b = ::glLib::Math::Vec::dot3(n,ray_direction);
	if (fabs(b)<=(type_float)(0.0)) { //ray is parallel to triangle plane
		return false; //=> no intersect
	}
	type_float a = -::glLib::Math::Vec::dot3(n,temp);

	//get intersect point of ray with triangle plane
	type_float r = a / b;
	if (r<(type_float)(0.0)) { //ray goes away from triangle
		return false; //=> no intersect
	}

	//for a segment, also test if (r > 1.0) => no intersect
	::glLib::Math::Vec::scale3(r,ray_direction,I); //intersect point of ray and plane
	::glLib::Math::Vec::add3(I,ray_pos,intersection);

	//is I inside the triangle?
	type_float uu = ::glLib::Math::Vec::dot3(u,u);
	type_float uv = ::glLib::Math::Vec::dot3(u,v);
	type_float vv = ::glLib::Math::Vec::dot3(v,v);
	::glLib::Math::Vec::subtract3(I,v1,w);
	type_float wu = ::glLib::Math::Vec::dot3(w,u);
	type_float wv = ::glLib::Math::Vec::dot3(w,v);
	type_float D = uv * uv - uu * vv;

	//get and test parametric coords
	type_float s = (uv * wv - vv * wu) / D;
	if ( (s<0) || (s>1) ) { //I is outside the triangle
		return false; //=> no intersect
	}
	type_float t = (uv * wu - uu * wv) / D;
	if ( (t<0) || (s+t>1) ) { //I is outside the triangle
		return false; //=> no intersect
	}

	//I is in the triangle
	*distance = r;
	return true;
}
/*double intersect_triangle(const double ray_direction[3], const double ray_origin[3], const double p1[3], const double p2[3], const double p3[3]) {
	//http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()

	double u[3],v[3],n[3];
	//Get triangle edge vectors and plane normal
	glLibMathVec::subtract3(p2,p1,u);
	glLibMathVec::subtract3(p3,p1,v);
	glLibMathVec::cross(u,v,n);

	if (n[0]==0.0&&n[1]==0.0&&n[2]==0.0) return 0.0; //degenerate triangle

	double w0[3];
	glLibMathVec::subtract3(ray_origin,p1,w0);
	double a = -glLibMathVec::dot3(n,w0);
	double b = glLibMathVec::dot3(n,ray_direction);
	if (b==0.0) return 0.0; //ray is parallel to triangle plane ((a==0.0)?in:disjoint)

	//Get intersection point of ray with triangle plane
	double r = a / b;
	if (r<0.0) return 0.0; //Ray goes away from triangle
	//For a segment, also test if (r>1.0) = no intersect

	//distance to plane is r
	double I[3], temp[3];
	glLibMathVec::scale3(r,ray_direction,temp);
	glLibMathVec::add3(ray_origin,temp,I); //I is point of intersection

	// is I inside T?
	double uu = glLibMathVec::dot3(u,u);
	double uv = glLibMathVec::dot3(u,v);
	double vv = glLibMathVec::dot3(v,v);
	double w[3];
	glLibMathVec::subtract3(I,p1,w);
	double wu = glLibMathVec::dot3(w,u);
	double wv = glLibMathVec::dot3(w,v);
	double D = uv * uv - uu * vv;

	// get and test parametric coords
	double s = (uv * wv - vv * wu) / D;
	if (s<0.0||s>1.0) return 0.0;
	double t = (uv * wu - uu * wv) / D;
	if (t<0.0||(s+t)>1.0) return 0.0;

	return r;
}*/

template <typename type_float> char line_line(
	type_float p1[3], type_float p2[3],
	type_float p3[3], type_float p4[3],
	type_float pa[3], type_float pb[3], type_float eps)
{
	//http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/

	type_float len;

	type_float p21[3], p21n[3];
	::glLib::Math::Vec::subtract3(p2,p1,p21);
	len = length(p21);
	if (len<eps) return 0;
	::glLib::Math::Vec::scale3(1/len,p21,p21n);

	type_float p43[3], p43n[3];
	::glLib::Math::Vec::subtract3(p4,p3,p43);
	len = length(p43);
	if (len<eps) return 0;
	::glLib::Math::Vec::scale3(1/len,p43,p43n);

	type_float p13[3], p24[3];
	::glLib::Math::Vec::subtract3(p1,p3,p13);
	::glLib::Math::Vec::subtract3(p2,p4,p24);

	type_float temp[3];

	if ( fabs(fabs(::glLib::Math::Vec::dot3(p43n,p21n))-1.0) < eps ) {
		//lines are parallel
		::glLib::Math::Vec::normalize3(p13,temp);
		if ( fabs(fabs(::glLib::Math::Vec::dot3(temp,p21n))-1.0) < eps ) {
			//lines overlap
			return 2;
		}
		return 0;
	}

	type_float d1343 = ::glLib::Math::Vec::dot3(p13,p43);
	type_float d4321 = ::glLib::Math::Vec::dot3(p43,p21);
	type_float d1321 = ::glLib::Math::Vec::dot3(p13,p21);
	type_float d4343 = ::glLib::Math::Vec::dot3(p43,p43);
	type_float d2121 = ::glLib::Math::Vec::dot3(p21,p21);

	type_float denom = d2121 * d4343 - d4321 * d4321;
	if (denom==0.0) return 0;
	type_float numer = d1343 * d4321 - d1321 * d4343;

	type_float mua = numer / denom;
	type_float mub = (d1343 + d4321*mua) / d4343;

	::glLib::Math::Vec::scale3(mua,p21,temp); ::glLib::Math::Vec::add3(p1,temp,pa);
	::glLib::Math::Vec::scale3(mub,p43,temp); ::glLib::Math::Vec::add3(p3,temp,pb);
	return 1;
}

#pragma region OLD
/*template <typename type_float> bool point_triangle(type_float point[3],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*point[i0]+b*point[i1]+c

	a=U2[i1]-U1[i1];
	b=-(U2[i0]-U1[i0]);
	c=-a*U1[i0]-b*U1[i1];
	d1 = a*point[i0]+b*point[i1]+c;

	a=U0[i1]-U2[i1];
	b=-(U0[i0]-U2[i0]);
	c=-a*U2[i0]-b*U2[i1];
	d2 = a*point[i0]+b*point[i1]+c;

	if ((d0*d1>0.0)&&(d0*d2>0.0)) return true;
	return false;
}

//TODO
template <typename type_float> bool triangle_triangle(
	type_float triangle_v[3][3], type_float triangle_u[3][3],
	type_float intersection1[3], type_float intersection2[3], type_float intersection3[3])
{
	type_float E1[3],E2[3];
	type_float N1[3],N2[3],d1,d2;
	type_float du0,du1,du2,dv0,dv1,dv2;
	type_float D[3];
	type_float isect1[2], isect2[2];
	type_float isectpointA1[3],isectpointA2[3];
	type_float isectpointB1[3],isectpointB2[3];
	type_float du0du1,du0du2,dv0dv1,dv0dv2;
	int index;
	type_float vp0,vp1,vp2;
	type_float up0,up1,up2;
	type_float b,c,max;
	type_float tmp,diff[3];
	int smallest1,smallest2;

	//Compute plane equation of triangle_v
	glLibMathVec::subtract(triangle_v[1],triangle_v[0],E1,3);
	glLibMathVec::subtract(triangle_v[2],triangle_v[0],E2,3);
	glLibMathVec::cross(E1,E2,N1);
	d1 = -glLibMathVec::dot(N1,triangle_v[0],3);
	//Plane equation 1: N1.X+d1=0

	//Put U0,U1,U2 into plane equation 1 to compute signed distances to the plane
	du0 = glLibMathVec::dot(N1,triangle_u[0],3) + d1;
	du1 = glLibMathVec::dot(N1,triangle_u[1],3) + d1;
	du2 = glLibMathVec::dot(N1,triangle_u[2],3) + 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_u
	glLibMathVec::subtract(triangle_u[1],triangle_u[0],E1,3);
	glLibMathVec::subtract(triangle_u[2],triangle_u[0],E2,3);
	glLibMathVec::cross(E1,E2,N2);
	d2 = -glLibMathVec::dot(N2,triangle_u[0],3);
	//Plane equation 2: N2.X+d2=0

	//Put triangle_v into plane equation 2
	dv0 = glLibMathVec::dot(N2,triangle_v[0],3) + d2;
	dv1 = glLibMathVec::dot(N2,triangle_v[1],3) + d2;
	dv2 = glLibMathVec::dot(N2,triangle_v[2],3) + 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 false;                 //No intersection occurs
	}

	//Compute direction of intersection line
	glLibMathVec::cross(N1,N2,D);

	//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 = triangle_v[0][index];
	vp1 = triangle_v[1][index];
	vp2 = triangle_v[2][index];

	up0 = triangle_u[0][index];
	up1 = triangle_u[1][index];
	up2 = triangle_u[2][index];

	//Compute interval for triangle 1
	/**coplanar = compute_intervals_isectline(
		V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2,
		dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2);
	if (*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);


	//Compute interval for triangle 2
	compute_intervals_isectline(
		U0,U1,U2,up0,up1,up2,du0,du1,du2,
		du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2);

	SORT2(isect1[0],isect1[1],smallest1);
	SORT2(isect2[0],isect2[1],smallest2);

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

	//At this point, we know that the triangles intersect
	if (isect2[0]<isect1[0]) {
		if (smallest1==0) { glLibMathVec::copy(isectpointA1,intersection1,3); }
		else { glLibMathVec::copy(isectpointA2,intersection1,3); }

		if (isect2[1]<isect1[1]) {
			if (smallest2==0) { glLibMathVec::copy(isectpointB2,intersection2,3); }
			else { glLibMathVec::copy(isectpointB1,intersection2,3); }
		}
		else {
			if (smallest1==0) { glLibMathVec::copy(isectpointA2,intersection2,3); }
			else { glLibMathVec::copy(isectpointA1,intersection2,3); }
		}
	}
	else {
		if (smallest2==0) { glLibMathVec::copy(isectpointB1,intersection1,3); }
		else { glLibMathVec::copy(isectpointB2,intersection1,3); }

		if (isect2[1]>isect1[1]) {
			if (smallest1==0) { glLibMathVec::copy(isectpointA2,intersection2,3); }
			else { glLibMathVec::copy(isectpointA1,intersection2,3); }
		}
		else {
			if (smallest2==0) { glLibMathVec::copy(isectpointB2,intersection2,3); }
			else { glLibMathVec::copy(isectpointB1,intersection2,3); }
		}
	}
	return true;
}*/

//Returns the number of points of intersection (0, 1, 2, or 3) and sets the appropriate
//intersection points
/*template <typename type_float> char triangle_triangle(
	type_float triangle1[3][3], type_float triangle2[3][3],
	type_float intersection1[3], type_float intersection2[3], type_float intersection3[3])
{
	type_float vec1[3], vec2[3], n2[3], d2, dv0, dv1, dv2;

	//Get "N" of containing plane for triangle 2
	glLibMathVec::subtract(triangle2[1],triangle2[0],vec1);
	glLibMathVec::subtract(triangle2[2],triangle2[0],vec2);
	glLibMathVec::cross(vec1,vec2,n2);
	d2 = -glLibMathVec::dot(n2,triangle2[0]);

	//Signed distances multiplied by n2*n2 (n2 isn't normalized)
	dv0 = glLibMathVec::dot(n2,triangle1[0]) + d2;
	dv1 = glLibMathVec::dot(n2,triangle1[1]) + d2;
	dv2 = glLibMathVec::dot(n2,triangle1[2]) + d2;

	if ((dv0>0)&&(dv1>0)&&(dv2>0)) return 0; //All points of triangle 1 on same side; no intersection
	if ((dv0<0)&&(dv1<0)&&(dv2<0)) return 0; //All points of triangle 1 on same side; no intersection
	if ((dv0==0)&&(dv1==0)&&(dv2==0)) {
		//Conatining planes are coplanar
	}

	type_float n1[3], d1, D[3];

	//Get "N" of containing plane for triangle 1
	glLibMathVec::subtract(triangle1[1],triangle1[0],vec1);
	glLibMathVec::subtract(triangle1[2],triangle1[0],vec2);
	glLibMathVec::cross(vec1,vec2,n1);
	//d1 = -glLibMathVec::dot(n1,triangle2[0]);

	//Parallel to planes' intersection
	glLibMathVec::cross(n1,n2,D);

	//Find O such that O + t*D is the line of intersection of the triangles' planes
	//This wouldn't be necessary if we weren't going to compute the intersections later
	type_float a1=n1[0], b1=n1[1], c1=n1[2];
	type_float a2=n2[0], b2=n2[1], c2=n2[2];
	if (D[0]!=0) {
		type_float denom = a1*b2 - a2*b1;
		O[0] = (b1*d2 - b2*d1)  /  denom;
		O[1] = (a2*d1 - a1*d2)  /  denom;
		O[2] = 0;
	}
	else if (D[1]!=0) {
		type_float denom = a1*c2 - a2*c1;
		O[0] = (c1*d2 - c2*d1)  /  denom;
		O[1] = 0;
		O[2] = (a2*d1 - a1*d2)  /  denom;
	}
	else {
		type_float denom = b1*c2 - b2*c1;
		O[0] = 0;
		O[1] = (c1*d2 - c2*d1)  /  denom;
		O[2] = (b2*d1 - b1*d2)  /  denom;
	}

	//Project points of triangle 1 onto the intersection-line of the triangles' planes
	type_float Tri1P[3][3];
	type_float Tri2P[3][3];
	glLibMathVec::subtract(triangle1[0],O,Tri1P[0],3); glLibMathVec::scale(D,Tri1P[0],3);
	glLibMathVec::subtract(triangle1[1],O,Tri1P[1],3); glLibMathVec::scale(D,Tri1P[1],3);
	glLibMathVec::subtract(triangle1[2],O,Tri1P[2],3); glLibMathVec::scale(D,Tri1P[2],3);

	//Compute intervals
	type_float tri1_t1, tri1_t2;
	type_float tri2_t1, tri2_t2;
	tri1_t1 =
}*/
#pragma endregion

/*char triangle_triangle(
	GIM_TRIANGLE_DATA* tri1,
	GIM_TRIANGLE_DATA* tri2,
	GIM_TRIANGLE_CONTACT_DATA* contact_data)
{
	typedef type_float float;

	type_float _distances[3];
	char out_of_face = 0;

	GLLIBINTERNAL_CLASSIFY_TRI_BY_FACE(tri1->m_vertices[0],tri1->m_vertices[1],tri1->m_vertices[2],tri2->m_planes.m_planes[0],out_of_face);
	if (out_of_face==1) return 0;

	GLLIBINTERNAL_CLASSIFY_TRI_BY_FACE(tri2->m_vertices[0],tri2->m_vertices[1],tri2->m_vertices[2],tri1->m_planes.m_planes[0],out_of_face);
	if (out_of_face==1) return 0;

	//Cache variables for triangle intersection
	unsigned int _max_candidates[MAX_TRI_CLIPPING];
	vec3f _temp_clip[MAX_TRI_CLIPPING];
	unsigned int _temp_clip_count = 0;
	vec3f _temp_clip2[MAX_TRI_CLIPPING];
	unsigned int _temp_clip_count2 = 0;
	vec3f clipped_points2[MAX_TRI_CLIPPING];
	vec3f deep_points2[MAX_TRI_CLIPPING];
	vec3f clipped_points1[MAX_TRI_CLIPPING];
	vec3f deep_points1[MAX_TRI_CLIPPING];

	//State variabnles
	unsigned int mostdir = 0;
	unsigned int clipped2_count = 0;

	//Clip tri2 by tri1 edges
	GLLIBINTERNAL_CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri2->m_vertices,(&tri1->m_planes.m_planes[1]), clipped_points2, clipped2_count);

	if (clipped2_count==0) {
		return 0;
	}

	//find most deep interval face1
	GUINT32 deep2_count = 0;

	type_float maxdeep;

	GLLIBINTERNAL_MOST_DEEP_POINTS((tri1->m_planes.m_planes[0]), clipped_points2, clipped2_count, deep_points2, deep2_count, maxdeep);
	if (deep2_count==0) {
		//*perror = 0.0f;
		return 0;
	}

	//Normal pointing to triangle1
	VEC_SCALE(contact_data->m_separating_normal,-1.0f,(tri1->m_planes.m_planes[0]));

	//Clip tri1 by tri2 edges
	unsigned int clipped1_count = 0;

	GLLIBINTERNAL_CLIP_TRI_POINTS_BY_TRI_EDGE_PLANES(tri1->m_vertices,(&tri2->m_planes.m_planes[1]), clipped_points1, clipped1_count);

	if (clipped2_count == 0 ) {
		//*perror = 0.0f;
		return 0;
	}


	//find interval face2
	GUINT32 deep1_count=0;

	type_float dist;

	GLLIBINTERNAL_MOST_DEEP_POINTS((tri2->m_planes.m_planes[0]), clipped_points1, clipped1_count, deep_points1, deep1_count, dist);

	if (deep1_count==0) {
		//*perror = 0.0f;
		return 0;
	}

	if (dist<maxdeep) {
		maxdeep = dist;
		mostdir = 1;
		VEC_COPY(contact_data->m_separating_normal,(tri2->m_planes.m_planes[0]));
	}
	//set deep
	contact_data->m_penetration_depth = maxdeep;

	//check most dir for contacts
	if (mostdir==0) {
		contact_data->m_point_count = deep2_count;
		for (mostdir=0;mostdir<deep2_count;mostdir++) {
			VEC_COPY(contact_data->m_points[mostdir],deep_points2[mostdir]);
		}
	}
	else {
		contact_data->m_point_count = deep1_count;
		for (mostdir=0;mostdir<deep1_count;mostdir++) {
			VEC_COPY(contact_data->m_points[mostdir],deep_points1[mostdir]);
		}
	}
	return 1;
}*/

template <typename type_float> inline bool            ray_AABB(const type_float ray_origin[3], const type_float            ray_direction[3], const type_float box_min[3], const type_float box_max[3], type_float* distance) {
	type_float ray_direction_reciprocal[3]; ::glLib::Math::Vec::reciprocal3(ray_direction,ray_direction_reciprocal,box_min,box_max);
	return ray_AABB_reciprocal(ray_origin,ray_direction_reciprocal,distance);
}
template <typename type_float>        bool ray_AABB_reciprocal(const type_float ray_origin[3], const type_float ray_direction_reciprocal[3], const type_float box_min[3], const type_float box_max[3], type_float* distance) {
	//http://www.cs.utah.edu/~awilliam/box/box.pdf
	type_float tmin, tmax, tymin, tymax, tzmin, tzmax;

	if (ray_direction_reciprocal[0]<(type_float)(0.0)) {
		tmin  = (box_max[0] - ray_origin[0]) * ray_direction_reciprocal[0];
		tmax  = (box_min[0] - ray_origin[0]) * ray_direction_reciprocal[0];
	}
	else {
		tmin  = (box_min[0] - ray_origin[0]) * ray_direction_reciprocal[0];
		tmax  = (box_max[0] - ray_origin[0]) * ray_direction_reciprocal[0];
	}
	if (ray_direction_reciprocal[1]<0.0) {
		tymin = (box_max[1] - ray_origin[1]) * ray_direction_reciprocal[1];
		tymax = (box_min[1] - ray_origin[1]) * ray_direction_reciprocal[1];
	}
	else {
		tymin = (box_min[1] - ray_origin[1]) * ray_direction_reciprocal[1];
		tymax = (box_max[1] - ray_origin[1]) * ray_direction_reciprocal[1];
	}

	if (tmin>tymax||tymin>tmax) return false;
	if (tymin>tmin) tmin = tymin;
	if (tymax<tmax) tmax = tymax;

	if (ray_direction_reciprocal[2]<(type_float)(0.0)) {
		tzmin = (box_max[2] - ray_origin[2]) * ray_direction_reciprocal[2];
		tzmax = (box_min[2] - ray_origin[2]) * ray_direction_reciprocal[2];
	}
	else {
		tzmin = (box_min[2] - ray_origin[2]) * ray_direction_reciprocal[2];
		tzmax = (box_max[2] - ray_origin[2]) * ray_direction_reciprocal[2];
	}

	if ( (tmin > tzmax) || (tzmin > tmax) ) return false;
	if (tzmin > tmin) tmin = tzmin;
	if (tzmax < tmax) tmax = tzmax;

	*distance = (tmin<(type_float)(0.0)) ? tmax : tmin; //tmin<t1 && tmax>t0;
	return true;
}
/*template <typename type_float> type_float ray_aabb(type_float aabb_min[3], type_float aabb_max[3], type_float origin[3], type_float direction[3], type_float eps=0.0001) {
	//Adapted from http://www.cs.utah.edu/~awilliam/box/

	float tmin, tmax, tymin, tymax, tzmin, tzmax;

	if (direction[0]==0) direction[0] = eps;
	if (direction[1]==0) direction[1] = eps;
	if (direction[2]==0) direction[2] = eps;

	tmin = (((direction[0]>0)?aabb_min[0]:aabb_max[0])-origin[0]) / direction[0];
	tmax = (((direction[0]<0)?aabb_min[0]:aabb_max[0])-origin[0]) / direction[0];
	tymin = (((direction[1]>0)?aabb_min[1]:aabb_max[1])-origin[1]) / direction[1];
	tymax = (((direction[1]<0)?aabb_min[1]:aabb_max[1])-origin[1]) / direction[1];
	if ( (tmin > tymax) || (tymin > tmax) ) return -1.0;
	if (tymin > tmin) tmin = tymin;
	if (tymax < tmax) tmax = tymax;
	tzmin = (((direction[2]>0)?aabb_min[2]:aabb_max[2])-origin[2]) / direction[2];
	tzmax = (((direction[2]<0)?aabb_min[2]:aabb_max[2])-origin[2]) / direction[2];
	if ( (tmin > tzmax) || (tzmin > tmax) ) return -1.0;
	//if (tzmin > tmin) tmin = tzmin;
	//if (tzmax < tmax) tmax = tzmax;
	//return ( (tmin < t1) && (tmax > t0) ) - 0.5;
	return 1.0;
}*/

template <typename type_float> bool ray_sphere(const type_float ray_origin[3], const type_float ray_direction[3], const type_float sphere_center[3], type_float sphere_radius, type_float* distance) {
	type_float delta[3];
	type_float a = (type_float)(1.0);//glLibMathVec::dot3(ray_direction,ray_direction);

	::glLib::Math::Vec::subtract3(ray_origin,sphere_center,delta);
	type_float b = (type_float)(2.0)*::glLib::Math::Vec::dot3(ray_direction,delta);

	type_float c = ::glLib::Math::Vec::dot3(delta,delta) - sphere_radius*sphere_radius;

	type_float radicand = b*b - (type_float)(4.0)*a*c;
	if (radicand<(type_float)(0.0)) return false;

	type_float sqrt_radical = sqrt(radicand);
	type_float temp = -(b+sqrt_radical);

	*distance = ( (temp<(type_float)(0.0)) ? (sqrt_radical-b) : temp ) / ((type_float)(2.0)*a);
	//return (sqrt_radical-b) / (2.0*a);
	return true;
}
/*float intersect_sphere(double ray_direction[3], double ray_origin[3], double sphere_center[3], double sphere_radius) {
	//http://www.devmaster.net/forums/showthread.php?t=413
	double delta[3];
	glLibMathVec::subtract3(ray_origin,sphere_center,delta);
	double B = glLibMathVec::dot3(delta,ray_direction);
	double C = glLibMathVec::dot3(delta,delta) - sphere_radius*sphere_radius;
	double D = B*B - C;
	return (D>0) ? -B-sqrt(D) : ;
}*/
template <typename type_float> bool ray_plane(const type_float ray_origin[3], const type_float ray_direction[3], const type_float plane_point[3], const type_float plane_normal[3], type_float* distance) {
	//http://en.wikipedia.org/wiki/Line-plane_intersection
	type_float n_dot_l = ::glLib::Math::Vec::dot3(ray_direction,plane_normal);
	if (n_dot_l==(type_float)(0.0)) return false;
	type_float delta[3];
	::glLib::Math::Vec::subtract3(plane_point,ray_origin,delta);
	type_float dist = ::glLib::Math::Vec::dot3(delta,plane_normal) / n_dot_l;
	if (dist<=(type_float)(0.0)) return false;
	*distance = dist;
	return true;
}

};};};