/*
	Twilight Prophecy 3D/Multimedia SDK
	A multi-platform development system for virtual reality and multimedia.

	Copyright (C) 1997-2001 by Twilight 3D Finland Oy Ltd.

	This program is free software; you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation; either version 2 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program; if not, write to the Free Software
	Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

	Please read the file LICENSE.TXT for additional details.


	source: 
		intersection implementation

	revision history:
		Jan/31/2001 - Jukka Liimatta - initial revision
*/
#define PRMATH_TYPENAME
#include <prmath/prmath.hpp>
using namespace prmath;



bool prmath::IntersectConeSphere(const Cone& cone, const Sphere& sphere)
{
	// test if cone vertex is in sphere
	Vector3 diff = sphere.center - cone.origin;
	
	float r2 = sphere.radius * sphere.radius;
	float len2 = DotProduct(diff,diff);
	float len2mr2 = len2 - r2;
	if ( len2mr2 <= 0 ) 
		return true;

	// test if sphere center is in cone
	float dot1 = DotProduct(cone.target,diff);
	float dot2 = dot1 * dot1;
	float cs = (float)cos(cone.angle);
	float cs2 = cs * cs;
	if ( dot2 >= len2*cs2 ) 
		return true;
	
	float ulen = (float)sqrt(fabs(len2-dot2));
	float sn = (float)sin(cone.angle);
	float test = cs*dot1 + sn*ulen;
	float discr = test*test - len2mr2;
	
	return discr >= 0 && test >= 0;
}


bool prmath::IntersectBoxSphere(const Box& box, const Sphere& sphere)
{
	float fmin = 0;
	for ( int i=0; i<3; i++ )
	{
		const float& center = sphere.center[i];
		const float& min = box.min[i];
		const float& max = box.max[i];

		if (center < min)
		{
			float delta = center - min;
			fmin += delta*delta;
		}
		else if (center > max)
		{
			float delta = center - max;
			fmin += delta*delta;
		}
	}
	float r2 = sphere.radius * sphere.radius;
	return (fmin <= r2) ? true : false;
}


bool prmath::IntersectRaySphere(const Line& ray, const Sphere& sphere, Vector3& point)
{
	Vector3 dep = sphere.center - ray.origin;
	float dot3 = DotProduct(dep, ray.direction);
	float det = dot3*dot3 - LengthSquared(dep) + sphere.radius*sphere.radius;

	// no intersection
	if ( det < 0 )
		return false;

	// positive solution, or tangent (det==0)
	det = (float)sqrt(det);
	float t = dot3 + det;

	// intersection behind the origin for positive solution?
	if ( t < 0 )
	{
		// try negative solution, since the positive was behind the origin
		t = dot3 - det;
		if ( t < 0 ) 
			return false; // also behind origin
	}
	else
	{
		// negative solution
		if ( det > 0 )
		{
			float to = dot3 - det;
			// use negative solution only if closer to origin, and not behind it
			if ( (to < t) && (to >= 0) ) 
				t = to;
		}
	}

	// solve intersection
	point = ray.origin + ray.direction * t;
	return true;
}


bool prmath::IntersectRaySphere(const Line& ray, const Sphere& sphere, Vector3& enter, Vector3& leave)
{
	Vector3 direction = Normalize(ray.direction);
	float b = 2 * (DotProduct(ray.origin,direction) - DotProduct(sphere.center,direction));
	float c = DotProduct(ray.origin,ray.origin) +	DotProduct(sphere.center,sphere.center) - 2 * DotProduct(ray.origin,sphere.center) - sphere.radius*sphere.radius;
		
	float det = b * b - 4 * c;
	if ( det >= 0 )
	{
		float sd = (float)sqrt(det);
		float t0 = (-b + sd) * 0.5f;
		float t1 = (-b - sd) * 0.5f;
		if ( t0 > t1 )
		{
			enter = ray.origin + direction * t1;
			leave = ray.origin + direction * t0;
		}
		else
		{
			enter = ray.origin + direction * t0;
			leave = ray.origin + direction * t1;
		}
		return true;
	}
	return false;
}


bool prmath::IntersectRayPlane(const Line& ray, const Plane& plane, Vector3& point)
{
	float ndot = (DotProduct(ray.origin,plane.normal) + plane.dist);
	if ( ndot > 0 )
		return false;
		
	float t = ndot / DotProduct(ray.direction,plane.normal);
	point = ray.origin + t * ray.direction;

	return true;
}


bool prmath::IntersectRayTriangle(const Line& ray, const Vector3& v0, const Vector3& v1, const Vector3& v2, Vector3& point)
{
	// intersect the ray with plane
	plane3f plane(v0,v1,v2);
	if ( !IntersectRayPlane(ray,plane,point) )
		return false;

	// test if intersection point is inside the triangle
	const vec3f& origin = ray.origin;
	vec3f normal;
	
	// edge 1
	normal = CrossProduct(origin-v1,origin-v0);
	if ( DotProduct(point,normal) < DotProduct(origin,normal) )
		return false;
		
	// edge 2
	normal = CrossProduct(origin-v2,origin-v1);
	if ( DotProduct(point,normal) < DotProduct(origin,normal) )
		return false;
		
	// edge 3
	normal = CrossProduct(origin-v0,origin-v2);
	if ( DotProduct(point,normal) < DotProduct(origin,normal) )
		return false;
		
	return true;
}


bool prmath::IntersectRayBox(const Line& ray, const Box& box, Vector3& point)
{
	// -------------------------------------
	// Fast Ray-Box Intersection
	// by Andrew Woo
	// Graphics Gems, Academic Press, 1990
	// -------------------------------------

	enum
	{
		ITR_RIGHT	= 0,
		ITR_LEFT	= 1,
		ITR_MIDDLE	= 2
	};


	// find candidate planes; this loop can be avoided if
	// rays cast all from the eye ( assume perspective view )
	int i;
	int quadrant[3];
	bool inside = true;
	Vector3 candidatePlane;

	for ( i=0; i<3; i++ )
	{
		if ( ray.origin[i] < box.min[i] )
		{
			quadrant[i] = ITR_LEFT;
			candidatePlane[i] = box.min[i];
			inside = false;
		}
		else if ( ray.origin[i] > box.max[i] )
		{
			quadrant[i] = ITR_RIGHT;
			candidatePlane[i] = box.max[i];
			inside = false;
		}
		else
		{
			quadrant[i] = ITR_MIDDLE;
		}
	}

	// ray origin inside bounding box
	if ( inside )
	{
		point = ray.origin;
		return true;
	}

	// calculate t distances to candidate planes
	Vector3 maxt;

	for ( i=0; i<3; i++ )
	{
		maxt[i] = (quadrant[i] != ITR_MIDDLE && ray.direction[i] != 0) ?
			(candidatePlane[i] - ray.origin[i]) / ray.direction[i] : -1;
	}

	// get largest of the maxt's for final choice of intersection
	int whichPlane = 0;

	for ( i=1; i<3; i++ )
	{
		if ( maxt[whichPlane] < maxt[i] )
			whichPlane = i;
	}

	// check final candidate actually inside box
	if ( maxt[whichPlane] < 0 )
		return false;

	for ( i=0; i<3; i++ )
	{
		if ( whichPlane != i )
		{
			point[i] = ray.origin[i] + maxt[whichPlane] * ray.direction[i];
			if ( point[i] < box.min[i] || point[i] > box.max[i] )
				return false;
		}
		else
		{
			point[i] = candidatePlane[i];
		}
	}

	return true; // ray hits box
}


bool prmath::PlaneIntersect(const Plane& p0, const Plane& p1, Vector3& point, Vector3& direction)
{
	direction = CrossProduct(p0.normal,p1.normal);
	float det = LengthSquared(direction);
	
	// parallel?
	if ( det < (float)0.000001 ) 
		return false;

	// intersection
	point = (p0.dist * CrossProduct(p1.normal,direction) + p1.dist * CrossProduct(direction,p0.normal)) / det;
	direction = Normalize(direction);

	return true;
}


bool prmath::PlaneIntersect(const Plane& p0, const Plane& p1, const Plane& p2, Vector3& point)
{
	// determinant
	Vector3 cp01 = CrossProduct(p0.normal,p1.normal);
	float det = DotProduct(cp01,p2.normal);
	
	// parallel?
	if ( det < (float)0.001 ) 
		return false;

	// intersection
	Vector3 cp12 = CrossProduct(p1.normal,p2.normal) * p0.dist;
	Vector3 cp20 = CrossProduct(p2.normal,p0.normal) * p1.dist;

	point = (cp12 + cp20 + p2.dist * cp01) / det;
	return true;
}


bool prmath::PointInsideTriangle(const Vector3& v0, const Vector3& v1, const Vector3& v2, const Vector3& point)
{
	Vector3 v10 = v1 - v0;
	Vector3 v20 = v2 - v0;
	Vector3 vp0 = point - v0;
	float b = DotProduct(v10,v20);
	float c = DotProduct(v20,v20);
	float d = DotProduct(vp0,v10);
	float e = DotProduct(vp0,v20);
	
	float x = d*c - e*b;
	if ( x < 0 )
		return false;

	float a = DotProduct(v10,v10);
	float y = e*a - d*b;
	if ( y < 0 )
		return false;

	return (x+y) <= (a*c - b*b);
}


float prmath::GetTriangleArea(float edge0, float edge1, float edge2)
{
	// Heron's Formula
	float s = (edge0 + edge1 + edge2) * 0.5f;
	return (float)sqrt( s * (s-edge0) * (s-edge1) * (s-edge2) );
}
