///////////////////////////////////////////////////////////////////////////////
///  @file OgreOpcodeMath.h
///  @brief <TODO: insert file description here>
///
///  @author The OgreOpcode Team
///  
///////////////////////////////////////////////////////////////////////////////
///  
///  This file is part of OgreOpcode.
///  
///  A lot of the code is based on the Nebula Opcode Collision module, see docs/Nebula_license.txt
///  
///  OgreOpcode is free software; you can redistribute it and/or
///  modify it under the terms of the GNU Lesser General Public
///  License as published by the Free Software Foundation; either
///  version 2.1 of the License, or (at your option) any later version.
///  
///  OgreOpcode 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
///  Lesser General Public License for more details.
///  
///  You should have received a copy of the GNU Lesser General Public
///  License along with OgreOpcode; if not, write to the Free Software
///  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
///  
///////////////////////////////////////////////////////////////////////////////
#ifndef __OgreOpcodeMath_h__
#define __OgreOpcodeMath_h__

#include <Ogre.h>
#include "OgreOpcodeExports.h"

namespace OgreOpcode
{
	namespace Details
	{
		// -----------------------------------------------------------------------
		// Overridden operators

		/// Dot product operator
		inline Ogre::Real operator | ( const Ogre::Vector3& u,  const Ogre::Vector3& v ) 
		{
			return (u.x*v.x + u.y*v.y + u.z*v.z);
		}

		/// Cross product operator
		inline Ogre::Vector3 operator ^ ( const Ogre::Vector3& u,  const Ogre::Vector3& v ) 
		{
			return u.crossProduct( v );
		}

		// forward declarations
		class Capsule;
		class Line;
		class OrientedBox;

		class line3
		{
		public:
			Ogre::Vector3 b;
			Ogre::Vector3 m;

			line3() {};
			line3(const Ogre::Vector3& v0, const Ogre::Vector3& v1) : b(v0), m(v1-v0) {};
			line3(const line3& l) : b(l.b), m(l.m) {};

			void set(const Ogre::Vector3& v0, const Ogre::Vector3& v1)
			{
				b = v0;
				m = v1-v0;
			};

			const Ogre::Vector3& start(void) const
			{
				return b;
			};

			Ogre::Vector3 end(void) const
			{
				return (b+m);
			};

			Ogre::Real length(void) const
			{
				return m.length();
			};

			//--- minimal distance of point to line -------------------------
			Ogre::Real distance(const Ogre::Vector3& p)
			{
				Ogre::Vector3 diff(p-b);
				Ogre::Real l = m.dotProduct(m);
				if (l > 0.0f) {
					Ogre::Real t = m.dotProduct(diff) / l;
					diff = diff - m*t;
					return diff.length();
				} else
				{
					// line is really a point...
					Ogre::Vector3 v(p-b);
					return v.length();
				}
			};

			//--- get 3d point on line given t ------------------------------
			Ogre::Vector3 ipol(const Ogre::Real t) const {
				return Ogre::Vector3(b + m*t);
			};
		};

		//-------------------------------------------------------------------
		//  Triangle points are tri(s,t)=b + s*e0 + t*e1 where
		//  0<=s<=1, 0<=t<=1 and 0<=s+t<=1
		//-------------------------------------------------------------------
		class triangle
		{
		public:
			Ogre::Vector3 b,e0,e1;

			triangle() {};
			triangle(const Ogre::Vector3& v0, const Ogre::Vector3& v1, const Ogre::Vector3& v2) 
				: b(v0), e0(v1-v0), e1(v2-v0) {};
			triangle(const triangle& t) 
				: b(t.b), e0(t.e0), e1(t.e1) {}; 

			void set(const Ogre::Vector3& v0, const Ogre::Vector3& v1, const Ogre::Vector3& v2) {
				b  = v0;
				e0 = v1-v0;
				e1 = v2-v0;
			};

			//--- get the face normal of the triangle ---------------------------------
			Ogre::Vector3 normal(void) const
			{
				Ogre::Vector3 cross = e0.crossProduct(e1);
				cross.normalise();
				return cross;
			};

			//--- get the midpoint (center of gravity) of the triangle ----------------
			Ogre::Vector3 midpoint(void) const {
				return b + ((e0+e1)/3.0f);
			};

			//--- get the plane of the triangle ---------------------------------------
			Ogre::Plane getplane(void) const
			{
				return Ogre::Plane(b,b+e0,b+e1);
			};

			//--- get one the edge points ---------------------------------------------
			Ogre::Vector3 point(int i) const
			{
				switch (i)
				{
				case 0: return b;
				case 1: return b + e0;
				case 2: return b + e1;
				default: return Ogre::Vector3(0.0f, 0.0f, 0.0f);
				}
			};

			bool isPointInsideFast(Ogre::Vector3 &p)
			{
				Ogre::Vector3 f = point(1) - point(0);
				Ogre::Vector3 g = point(2) - point(0);

				Ogre::Real a = f.dotProduct(f);
				Ogre::Real _b = f.dotProduct(g);
				Ogre::Real c = g.dotProduct(g);

				Ogre::Real ac_bb = (a*c)-(_b*_b);
				Ogre::Vector3 vp = p - point(0);

				Ogre::Real d = vp.dotProduct(f);
				Ogre::Real e = vp.dotProduct(g);
				Ogre::Real x = (d*c)-(e*_b);
				Ogre::Real y = (e*a)-(d*_b);
				Ogre::Real z = x+y-ac_bb;

				return (( ((unsigned int&)z)& ~(((unsigned int&)x)|((unsigned int&)y))) & 0x80000000)!=0;
			}

			//--- check if and where line intersects triangle -------------------------
			//  Taken from Magic Software (http://www.cs.unc.edu/~eberly)
			//  Return false if line is parallel to triangle or hits its backside.
			//
			bool intersect(const line3& line, Ogre::Real& ipos)
			{

				// Compute plane of triangle, Dot(normal,X-tri.b) = 0 where 'normal' is
				// the plane normal.  If the angle between the line direction and normal
				// is small, then the line is effectively parallel to the triangle.
				const Ogre::Real fTolerance = 1e-04f;
				Ogre::Vector3 norm = e0.crossProduct(e1);
				Ogre::Real fDenominator = norm.dotProduct(line.m);
				//Real fLLenSqr     = line.m % line.m;
				//Real fNLenSqr     = norm % norm;

				// check if intersecting backface or parallel...
				if (fDenominator >= -fTolerance) return false;

				//if ((fDenominator*fDenominator) <= (fTolerance*fLLenSqr*fNLenSqr)) {
				//    // line and triangle are parallel
				//    return false;
				//}

				// The line is X(t) = line.b + t*line.m.  Compute line parameter t for
				// intersection of line and plane of triangle.  Substitute in the plane
				// equation to get Dot(normal,line.b-tri.b) + t*Dot(normal,line.m)   
				Ogre::Vector3 kDiff0(line.b - b);
				Ogre::Real fTime = -(norm.dotProduct(kDiff0)) / fDenominator;
				if ((fTime<-fTolerance) || (fTime>(1.0f+fTolerance))) return false;

				// Find difference of intersection point of line with plane and vertex
				// of triangle.
				Ogre::Vector3 kDiff1(kDiff0 + line.m*fTime);

				// Compute if intersection point is inside triangle.  Write
				// kDiff1 = s0*E0 + s1*E1 and solve for s0 and s1.
				Ogre::Real fE00 = e0.dotProduct(e0);
				Ogre::Real fE01 = e0.dotProduct(e1);
				Ogre::Real fE11 = e1.dotProduct(e1);
				Ogre::Real fDet = (Ogre::Real) fabs(fE00*fE11-fE01*fE01);     // = |normal|^2 > 0
				Ogre::Real fR0  = e0.dotProduct(kDiff1);
				Ogre::Real fR1  = e1.dotProduct(kDiff1);

				Ogre::Real fS0 = fE11*fR0 - fE01*fR1;
				Ogre::Real fS1 = fE00*fR1 - fE01*fR0;

				if ((fS0>=-fTolerance) && (fS1>=-fTolerance) && (fS0+fS1<=fDet+fTolerance)) {
					// intersection is inside triangle
					ipos = fTime;
					return true;
				} else {
					// intersection is outside triangle
					return false;
				}
			};

			//--- check if and where line intersects triangle -------------------------
			//  Taken from Magic Software (http://www.cs.unc.edu/~eberly)
			//  Return false if line is parallel to triangle
			//
			bool intersect_both_sides(const line3& line, Ogre::Real& ipos) {

				// Compute plane of triangle, Dot(normal,X-tri.b) = 0 where 'normal' is
				// the plane normal.  If the angle between the line direction and normal
				// is small, then the line is effectively parallel to the triangle.
				const Ogre::Real fTolerance = 1e-04f;
				Ogre::Vector3 norm = e0.crossProduct(e1);
				Ogre::Real fDenominator = norm.dotProduct(line.m);
				Ogre::Real fLLenSqr     = line.m.dotProduct(line.m);
				Ogre::Real fNLenSqr     = norm.dotProduct(norm);

				// check if intersecting backface or parallel...
				if (fDenominator*fDenominator <= fTolerance*fLLenSqr*fNLenSqr) return false;

				//if ((fDenominator*fDenominator) <= (fTolerance*fLLenSqr*fNLenSqr)) {
				//    // line and triangle are parallel
				//    return false;
				//}

				// The line is X(t) = line.b + t*line.m.  Compute line parameter t for
				// intersection of line and plane of triangle.  Substitute in the plane
				// equation to get Dot(normal,line.b-tri.b) + t*Dot(normal,line.m)   
				Ogre::Vector3 kDiff0(line.b - b);
				Ogre::Real fTime = -(norm.dotProduct(kDiff0)) / fDenominator;
				if ((fTime<-fTolerance) || (fTime>(1.0f+fTolerance))) return false;

				// Find difference of intersection point of line with plane and vertex
				// of triangle.
				Ogre::Vector3 kDiff1(kDiff0 + line.m*fTime);

				// Compute if intersection point is inside triangle.  Write
				// kDiff1 = s0*E0 + s1*E1 and solve for s0 and s1.
				Ogre::Real fE00 = e0.dotProduct(e0);
				Ogre::Real fE01 = e0.dotProduct(e1);
				Ogre::Real fE11 = e1.dotProduct(e1);
				Ogre::Real fDet = (Ogre::Real) fabs(fE00*fE11-fE01*fE01);     // = |normal|^2 > 0
				Ogre::Real fR0  = e0.dotProduct(kDiff1);
				Ogre::Real fR1  = e1.dotProduct(kDiff1);

				Ogre::Real fS0 = fE11*fR0 - fE01*fR1;
				Ogre::Real fS1 = fE00*fR1 - fE01*fR0;

				if ((fS0>=-fTolerance) && (fS1>=-fTolerance) && (fS0+fS1<=fDet+fTolerance)) {
					// intersection is inside triangle
					ipos = fTime;
					return true;
				} else {
					// intersection is outside triangle
					return false;
				}
			};
		};

#define TINY (0.0000001)
#define n_max(a,b)      (((a) > (b)) ? (a) : (b))
#define n_min(a,b)      (((a) < (b)) ? (a) : (b))

		class bbox3
		{
		public:
			Ogre::Vector3 vmin;
			Ogre::Vector3 vmax;

			enum
			{
				CLIP_LEFT   = (1<<0),
				CLIP_RIGHT  = (1<<1),
				CLIP_BOTTOM = (1<<2),
				CLIP_TOP    = (1<<3),
				CLIP_NEAR   = (1<<4),
				CLIP_FAR    = (1<<5),
			};

			enum
			{
				OUTSIDE     = 0,
				ISEQUAL     = (1<<0),
				ISCONTAINED = (1<<1),
				CONTAINS    = (1<<2),
				CLIPS       = (1<<3),
			};

			//--- constructors ----------------------------------------------
			bbox3() {};
			bbox3(const Ogre::Vector3& _vmin, const Ogre::Vector3& _vmax) : vmin(_vmin), vmax(_vmax) {};
			bbox3(const bbox3& bb) : vmin(bb.vmin), vmax(bb.vmax) {};

			//--- initialize from Vector3 cloud -----------------------------
			bbox3(Ogre::Vector3 *varray, int num)
			{
				vmin = varray[0];
				vmax = varray[0];
				int i;
				for (i=0; i<num; i++)
				{
					if      (varray[i].x<vmin.x) vmin.x=varray[i].x;
					else if (varray[i].x>vmax.x) vmax.x=varray[i].x;
					if      (varray[i].y<vmin.y) vmin.y=varray[i].y;
					else if (varray[i].y>vmax.y) vmax.y=varray[i].y;
					if      (varray[i].z<vmin.z) vmin.z=varray[i].z;
					else if (varray[i].z>vmax.z) vmax.z=varray[i].z;
				}
			};

			//--- utility getters ------------------------------------------
			Ogre::Vector3 getCenter() const { return (vmin + vmax)*0.5; }
			void getCenter(Ogre::Vector3& v) const { v = (vmin + vmax)*0.5; }

			Ogre::Vector3 getExtents() const { return (vmax - vmin)*0.5; }
			void getExtents( Ogre::Vector3& v) const { v = (vmax - vmin)*0.5; }

			//--- setting elements ------------------------------------------
			void set(const Ogre::Vector3& _vmin, const Ogre::Vector3& _vmax)
			{
				vmin = _vmin;
				vmax = _vmax;
			};
			void set(Ogre::Vector3 *varray, int num)
			{
				vmin = varray[0];
				vmax = varray[0];
				int i;
				for (i=0; i<num; i++)
				{
					if      (varray[i].x<vmin.x) vmin.x=varray[i].x;
					else if (varray[i].x>vmax.x) vmax.x=varray[i].x;
					if      (varray[i].y<vmin.y) vmin.y=varray[i].y;
					else if (varray[i].y>vmax.y) vmax.y=varray[i].y;
					if      (varray[i].z<vmin.z) vmin.z=varray[i].z;
					else if (varray[i].z>vmax.z) vmax.z=varray[i].z;
				}
			};

			//--- invalidate bounding box to prepare for growing ------------
			void begin_grow(void)
			{
				vmin = Ogre::Vector3(+1000000.0f,+1000000.0f,+1000000.0f);
				vmax = Ogre::Vector3(-1000000.0f,-1000000.0f,-1000000.0f);
			};
			void grow(const Ogre::Vector3& v)
			{
				if (v.x<vmin.x) vmin.x=v.x;
				if (v.x>vmax.x) vmax.x=v.x;
				if (v.y<vmin.y) vmin.y=v.y;
				if (v.y>vmax.y) vmax.y=v.y;
				if (v.z<vmin.z) vmin.z=v.z;
				if (v.z>vmax.z) vmax.z=v.z;
			};
			void grow(Ogre::Vector3 *varray, int num)
			{
				int i;
				for (i=0; i<num; i++)
				{
					grow(varray[i]);
				}
			};

			void grow(const bbox3& bb)
			{
				if (bb.vmin.x<vmin.x) vmin.x=bb.vmin.x;
				if (bb.vmin.y<vmin.y) vmin.y=bb.vmin.y;