#pragma once
#include "includes.h"

double intersect_sphere(double ray_direction[3], double ray_origin[3], double sphere_center[3], double sphere_radius) {
	double delta[3];
	double a = 1.0;//glLibMathVec::dot3(ray_direction,ray_direction);

	glLibMathVec::subtract3(ray_origin,sphere_center,delta);
	double b = 2.0*glLibMathVec::dot3(ray_direction,delta);

	double c = glLibMathVec::dot3(delta,delta) - sphere_radius*sphere_radius;

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

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

	return ( (temp<0.0) ? (sqrt_radical-b) : temp ) / (2.0*a);
	//return (sqrt_radical-b) / (2.0*a);
}
/*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) : ;
}*/
double intersect_plane(double ray_direction[3], double ray_origin[3], double plane_point[3], double plane_normal[3]) {
	//http://en.wikipedia.org/wiki/Line-plane_intersection
	double n_dot_l = glLibMathVec::dot3(ray_direction,plane_normal);
	if (n_dot_l==0.0) return 0.0;
	double delta[3];
	glLibMathVec::subtract3(plane_point,ray_origin,delta);
	return glLibMathVec::dot3(delta,plane_normal) / n_dot_l;
}
double intersect_triangle(double ray_direction[3], double ray_origin[3], double p1[3], double p2[3], 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;
}
