/*
 * $Id: bvolumes.h 4451 2017-03-18 16:35:58Z cejka-mail.muni.cz $
 *
 * Description  : One line description of file.
 * Author       : Vaclav Kyba <mail/Jabber: vaseo1@gmail.com>  <ICQ: 98576293>
 *
 * Purpose      :
 *     Long description of what the file is for.
 */


#ifndef __BVOLUMES__H__
#define __BVOLUMES__H__

#include "common.h"
#include "float.h"


namespace VRUT
{
	///AABB class
	class VRUT_CORE_API AABB
	{
	private:
	public:
		///Minimum point of AABB
		Vector3 MinBound;
		///Maximum point of AABB
		Vector3 MaxBound;

		///Class constructor
		AABB() : MinBound(FLT_MAX, FLT_MAX, FLT_MAX), MaxBound(-FLT_MAX, -FLT_MAX, -FLT_MAX)
		{
		}
		AABB(const Vector3 &min_bound, const Vector3 &max_bound) : MinBound(min_bound), MaxBound(max_bound) {}
		///Copy constructor
		AABB(const AABB & aabb) : MinBound(aabb.MinBound), MaxBound(aabb.MaxBound)	{}
		///Class destructor
		~AABB()
		{
		}
		///Set AABB as invalid
		void SetInvalid()
		{
			MinBound=Vector3(FLT_MAX, FLT_MAX, FLT_MAX);
			MaxBound=Vector3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
		}
		///Get center of the AABB
		Vector3 Center() const { return 0.5f*(MinBound + MaxBound); }
		///Get radius of the ABB
		float Radius() const { return (MaxBound-MinBound).Length()*0.5f; }
		///Get diagonal vector
		Vector3 Extent() const { return MaxBound-MinBound; }
		///Get area of AABB surface
		float SurfaceArea() const {
			if (!IsValid())
				return 0;
			Vector3 ext = MaxBound - MinBound;
			return 2.0f * (ext.x * ext.y + ext.x * ext.z + ext.y * ext.z);
		}
		static float MergedSurfaceArea(const AABB &aabb1, const AABB &aabb2) {
			Vector3 minB((aabb1.MinBound.x<aabb2.MinBound.x)?aabb1.MinBound.x:aabb2.MinBound.x, (aabb1.MinBound.y<aabb2.MinBound.y)?aabb1.MinBound.y:aabb2.MinBound.y, (aabb1.MinBound.z<aabb2.MinBound.z)?aabb1.MinBound.z:aabb2.MinBound.z);
			Vector3 maxB((aabb1.MaxBound.x>aabb2.MaxBound.x)?aabb1.MaxBound.x:aabb2.MaxBound.x, (aabb1.MaxBound.y>aabb2.MaxBound.y)?aabb1.MaxBound.y:aabb2.MaxBound.y, (aabb1.MaxBound.z>aabb2.MaxBound.z)?aabb1.MaxBound.z:aabb2.MaxBound.z);
			Vector3 ext = maxB - minB;
			return 2.0f * (ext.x * ext.y + ext.x * ext.z + ext.y * ext.z);
		}
		///Get volume of AABB
		float Volume() const
		{
			return (MaxBound.x - MinBound.x) * (MaxBound.y - MinBound.y) * (MaxBound.z - MinBound.z);
		}

		///Check if point is in AABB
		bool PointIsIn(const Vector3 & p) const
		{
			const float * pt(p._v);
			return *pt >= MinBound.x && *pt++ <= MaxBound.x
				&& *pt >= MinBound.y && *pt++ <= MaxBound.y
				&& *pt >= MinBound.z && *pt <= MaxBound.z;
		}

		///Get \c n-vertex (closest) and \c p-vertex (farthest) of AABB for given plane
		///\param[in] plane Given plane
		///\param[out] nvert Will be filled with n-vertex
		///\param[out] pvert Will be filled with p-vertex
		void ComputeNPvertices(const Vector4 &plane, Vector3 * nvert, Vector3 * pvert) const
		{
			ComputeNPvertices(Vector3(plane), nvert, pvert);
		}

		///Get \c n-vertex (closest) and \c p-vertex (farthest) of AABB for given plane normal
		///\param[in] normal Given plane normal
		///\param[out] nvert Will be filled with n-vertex
		///\param[out] pvert Will be filled with p-vertex
		void ComputeNPvertices(const Vector3 &normal, Vector3 * nvert, Vector3 * pvert) const
		{
			for (int i = 0; i < 3; i++)
			{
				bool pos = (normal._v[i] >= 0.0f);
				if (nvert)	nvert->_v[i] = ( pos ? MinBound[i] : MaxBound[i] );
				if (pvert)	pvert->_v[i] = ( pos ? MaxBound[i] : MinBound[i] );
			}
		}

		///Get distances of \c n-vertex (closest) and \c p-vertex (farthest) of AABB for given plane
		///\param[in] plane Given plane
		///\param[out] ndist Will be filled with the distance of n-vertex
		///\param[out] pdist Will be filled with the distance of p-vertex
		void ComputeNPDistances(const Vector4 &plane, float *ndist, float *pdist) const
		{
			Vector3 m1 = Vector3::MultiplyComp(MinBound, Vector3(plane));
			Vector3 m2 = Vector3::MultiplyComp(MaxBound, Vector3(plane));
			if (ndist)	{ Vector3 m = Vector3::MinComp(m1, m2); *ndist = m.x + m.y + m.z + plane.w; }
			if (pdist)	{ Vector3 m = Vector3::MaxComp(m1, m2); *pdist = m.x + m.y + m.z + plane.w; }
		}

		///Get distance of AABB from plane defined by point and normal
		///\param[in] pos Point on plane
		///\param[in] dir Plane normal
		///\return Distance from plane
		float GetDistFromPointNormal(const Vector3 &pos, const Vector3 &dir) const
		{
			Vector3 nvert;
			ComputeNPvertices(dir, &nvert, (Vector3 *)NULL);
			return (nvert - pos).Length();
		}

		///Get square distance (faster than normal) of AABB from plane defined by point and normal
		///\param[in] pos Point on plane
		///\param[in] dir Plane normal
		///\return Distance from plane
		float GetSqrDistFromPointNormal(const Vector3 &pos, const Vector3 &dir) const
		{
			Vector3 nvert;
			ComputeNPvertices(dir, &nvert, (Vector3 *)NULL);
			return (nvert - pos).LengthSq();
		}

		///Prevent copying NULL pointers - when debugging
		AABB & operator=(const AABB & aabb)
		{
			MinBound = aabb.MinBound;
			MaxBound = aabb.MaxBound;
			return *this;
		}

		///AABB union
		AABB operator+(const AABB & aabb) const
		{
			AABB ret;
			ret.MinBound = MinBound;
			ret.MaxBound = MaxBound;
			ret.Merge(aabb);
			return ret;
		}

		///Equal operator
		bool operator==(const AABB & aabb) const
		{
			return (aabb.MinBound == MinBound) && (aabb.MaxBound == MaxBound);
		}

		///not Equal operator
		bool operator!=(const AABB & aabb) const
		{
			return (aabb.MinBound != MinBound) || (aabb.MaxBound != MaxBound);
		}

		///AABB union
		AABB & Merge(const AABB & aabb)
		{
			for (unsigned i = 0; i < 3; i++)
			{
				if (aabb.MinBound._v[i] < MinBound._v[i])
					MinBound._v[i] = aabb.MinBound._v[i];
				if (aabb.MaxBound._v[i] > MaxBound._v[i])
					MaxBound._v[i] = aabb.MaxBound._v[i];
			}
			return *this;
		}

		bool IsValid() const
		{
			if ((MinBound.x>MaxBound.x) || (MinBound.y>MaxBound.y) || (MinBound.z>MaxBound.z))
				return false;
			return true;
		}

		void ExpandWithPoint(const Vector3 &point)
		{
			for (unsigned i = 0; i < 3; i++)
			{
				if (MaxBound._v[i] < point._v[i])
					MaxBound._v[i] = point._v[i];
				if (MinBound._v[i] > point._v[i])
					MinBound._v[i] = point._v[i];
			}
		}

		///Build box from AABB, transform it and get its new AABB
		AABB Transform(const Matrix * transMat) const
		{
			if (!IsValid())
				return *this;
			///Get transformed box
			Vector3 transedAabb[8];
			transedAabb[0] = transMat->TransformCoord(MinBound);
			transedAabb[1] = transMat->TransformCoord(Vector3(MaxBound.x, MinBound.y, MinBound.z));
			transedAabb[2] = transMat->TransformCoord(Vector3(MinBound.x, MaxBound.y, MinBound.z));
			transedAabb[3] = transMat->TransformCoord(Vector3(MinBound.x, MinBound.y, MaxBound.z));

			Vector3 yaxis(transedAabb[2] - transedAabb[0]);
			transedAabb[4] = transedAabb[1] + yaxis;
			transedAabb[5] = transedAabb[3] + yaxis;
			transedAabb[6] = transedAabb[1] + transedAabb[3] - transedAabb[0];
			transedAabb[7] = transedAabb[6] + yaxis;
			///Get AABB for transformed aabb
			AABB ret;
			Vector3 * minB(&ret.MinBound);
			Vector3 * maxB(&ret.MaxBound);
			*minB = *maxB = transedAabb[0];
			for (int i = 1; i < 8; i++)
			{
				const float * pos = transedAabb[i]._v;
				minB->x = __min(minB->x, *pos);
				maxB->x = __max(maxB->x, *pos); pos++;
				minB->y = __min(minB->y, *pos);
				maxB->y = __max(maxB->y, *pos); pos++;
				minB->z = __min(minB->z, *pos);
				maxB->z = __max(maxB->z, *pos);
			}
			return ret;
		}

		///Check if it intersects another AABB
		bool Intersects(const VRUT::AABB &aabb) const
		{
			return !((MaxBound.x < aabb.MinBound.x) || (MaxBound.y < aabb.MinBound.y) || (MaxBound.z < aabb.MinBound.z) || (aabb.MaxBound.x < MinBound.x) || (aabb.MaxBound.y < MinBound.y) || (aabb.MaxBound.z < MinBound.z));
		}

		///Check if ray intersects AABB in given interval
		///\param[in] r Ray to be checked
		///\param[in] t0 Minimum distance from ray origin
		///\param[in] t1 Maximum distance from ray origin
		///\param[out] dist Distance from ray origin where ray touches AABB if intersected
		///\return \c true if intersected
		bool Intersects(const Ray & r, float t0 = 0.0f, float t1 = FLT_MAX, float * dist = NULL) const
		{
			float tmin = -FLT_MAX;
			float tmax = FLT_MAX;
			if (r.direction.x != 0.0f)
			{
				float invDirX = 1.0f/r.direction.x;
				if (invDirX < 0.0f)
				{
					tmin = (MaxBound.x - r.origin.x) * invDirX;
					tmax = (MinBound.x - r.origin.x) * invDirX;
				}
				else
				{
					tmin = (MinBound.x - r.origin.x) * invDirX;
					tmax = (MaxBound.x - r.origin.x) * invDirX;
				}
			}
			else if (r.origin.x < MinBound.x || r.origin.x > MaxBound.x)
				return false;

			if (r.direction.y != 0.0f)
			{
				float tymin, tymax;
				float invDirY = 1.0f/r.direction.y;
				if (invDirY < 0.0f)
				{
					tymin = (MaxBound.y - r.origin.y) * invDirY;
					tymax = (MinBound.y - r.origin.y) * invDirY;
				}
				else
				{
					tymin = (MinBound.y - r.origin.y) * invDirY;
					tymax = (MaxBound.y - r.origin.y) * invDirY;
				}
				if ( (tmin > tymax) || (tymin > tmax) )
					return false;
				if (tymin > tmin)
					tmin = tymin;
				if (tymax < tmax)
					tmax = tymax;
			}
			else if (r.origin.y < MinBound.y || r.origin.y > MaxBound.y)
				return false;

			if (r.direction.z != 0.0f)
			{
				float tzmin, tzmax;
				float invDirZ = 1.0f/r.direction.z;
				if (invDirZ < 0.0f)
				{
					tzmin = (MaxBound.z - r.origin.z) * invDirZ;
					tzmax = (MinBound.z - r.origin.z) * invDirZ;
				}
				else
				{
					tzmin = (MinBound.z - r.origin.z) * invDirZ;
					tzmax = (MaxBound.z - r.origin.z) * invDirZ;
				}
				if ( (tmin > tzmax) || (tzmin > tmax) )
					return false;
				if (tzmin > tmin)
					tmin = tzmin;
				if (tzmax < tmax)
					tmax = tzmax;
			}
			else if (r.origin.z < MinBound.z || r.origin.z > MaxBound.z)
				return false;

			bool ret = ( (tmin < t1) && (tmax > t0) );
			if (ret && dist)
				*dist = tmin;

			return ret;
		}

		//actually is this version significantly slower
		bool IntersectsSSE(const Ray & r, float * dist = NULL) const
		{
			static const float flt_plus_inf = -logf(0);	// let's keep C and C++ compilers happy.
			static const float VRUT_ALIGNED(16)
				ps_cst_plus_inf[4]	= {  flt_plus_inf,  flt_plus_inf,  flt_plus_inf,  flt_plus_inf },
				ps_cst_minus_inf[4]	= { -flt_plus_inf, -flt_plus_inf, -flt_plus_inf, -flt_plus_inf };

			//bool ray_box_intersect(const aabb_t &box, const ray_t &ray, ray_segment_t &rs) {
			// you may already have those values hanging around somewhere
			const __m128
				plus_inf	= _mm_load_ps(ps_cst_plus_inf),
				minus_inf	= _mm_load_ps(ps_cst_minus_inf);

			// use whatever's apropriate to load.
			const __m128
				box_min	= _mm_loadu_ps(MinBound._v),
				box_max	= _mm_loadu_ps(MaxBound._v),
				pos	= _mm_loadu_ps(r.origin._v),
				dir	= _mm_loadu_ps(r.direction._v);

			// use a div if inverted directions aren't available
			const __m128 l1 = _mm_div_ps(_mm_sub_ps(box_min, pos), dir);
			const __m128 l2 = _mm_div_ps(_mm_sub_ps(box_max, pos), dir);

			// the order we use for those min/max is vital to filter out
			// NaNs that happens when an inv_dir is +/- inf and
			// (box_min - pos) is 0. inf * 0 = NaN
			const __m128 filtered_l1a = _mm_min_ps(l1, plus_inf);
			const __m128 filtered_l2a = _mm_min_ps(l2, plus_inf);

			const __m128 filtered_l1b = _mm_max_ps(l1, minus_inf);
			const __m128 filtered_l2b = _mm_max_ps(l2, minus_inf);

			// now that we're back on our feet, test those slabs.
			__m128 lmax = _mm_max_ps(filtered_l1a, filtered_l2a);
			__m128 lmin = _mm_min_ps(filtered_l1b, filtered_l2b);

			// unfold back. try to hide the latency of the shufps & co.
			const __m128 lmax0 = _mm_shuffle_ps(lmax, lmax, 0x39);
			const __m128 lmin0 = _mm_shuffle_ps(lmin, lmin, 0x39);
			lmax = _mm_min_ss(lmax, lmax0);
			lmin = _mm_max_ss(lmin, lmin0);

			const __m128 lmax1 = _mm_movehl_ps(lmax,lmax);
			const __m128 lmin1 = _mm_movehl_ps(lmin,lmin);
			lmax = _mm_min_ss(lmax, lmax1);
			lmin = _mm_max_ss(lmin, lmin1);

			const bool ret = (_mm_comige_ss(lmax, _mm_setzero_ps()) & _mm_comige_ss(lmax,lmin))!=0;

			if (dist)
				_mm_store_ss((float * const)dist, lmin);
				//_mm_store_ss((float * const)dist, lmax);

			return  ret;
		}


		///Copmpute interval in which ray intersects AABB
		///\param[in] r Ray to be checked
		///\param[in] t0 Minimum distance from ray origin
		///\param[in] t1 Maximum distance from ray origin
		///\return \c true if intersected
		bool Intersects(const Ray & r, float *t0, float *t1) const
		{
			float tmin = -FLT_MAX;
			float tmax = FLT_MAX;
			if (r.direction.x != 0.0f)
			{
				float invDirX = 1.0f/r.direction.x;
				if (invDirX < 0.0f)
				{
					tmin = (MaxBound.x - r.origin.x) * invDirX;
					tmax = (MinBound.x - r.origin.x) * invDirX;
				}
				else
				{
					tmin = (MinBound.x - r.origin.x) * invDirX;
					tmax = (MaxBound.x - r.origin.x) * invDirX;
				}
			}
			else if (r.origin.x < MinBound.x || r.origin.x > MaxBound.x)
				return false;

			if (r.direction.y != 0.0f)
			{
				float tymin, tymax;
				float invDirY = 1.0f/r.direction.y;
				if (invDirY < 0.0f)
				{
					tymin = (MaxBound.y - r.origin.y) * invDirY;
					tymax = (MinBound.y - r.origin.y) * invDirY;
				}
				else
				{
					tymin = (MinBound.y - r.origin.y) * invDirY;
					tymax = (MaxBound.y - r.origin.y) * invDirY;
				}
				if ( (tmin > tymax) || (tymin > tmax) )
					return false;
				if (tymin > tmin)
					tmin = tymin;
				if (tymax < tmax)
					tmax = tymax;
			}
			else if (r.origin.y < MinBound.y || r.origin.y > MaxBound.y)
				return false;

			if (r.direction.z != 0.0f)
			{
				float tzmin, tzmax;
				float invDirZ = 1.0f/r.direction.z;
				if (invDirZ < 0.0f)
				{
					tzmin = (MaxBound.z - r.origin.z) * invDirZ;
					tzmax = (MinBound.z - r.origin.z) * invDirZ;
				}
				else
				{
					tzmin = (MinBound.z - r.origin.z) * invDirZ;
					tzmax = (MaxBound.z - r.origin.z) * invDirZ;
				}
				if ( (tmin > tzmax) || (tzmin > tmax) )
					return false;
				if (tzmin > tmin)
					tmin = tzmin;
				if (tzmax < tmax)
					tmax = tzmax;
			}
			else if (r.origin.z < MinBound.z || r.origin.z > MaxBound.z)
				return false;

			*t0=__max(*t0, tmin);
			*t1=__min(*t1, tmax);
			return *t0<=*t1;
		}

		///Check if it intersects another AABB that is transformed by matrix
		//http://www.gamasutra.com/view/feature/3383/simple_intersection_tests_for_games.php?page=5
		//The VRUT implementation most probably contains bug, please check it and compare with original
		bool Intersects(const Matrix &matrix, const VRUT::AABB &aabb, const Matrix &aabbMatrix) const
		{
			Vector3 T(aabbMatrix.TransformCoord(aabb.Center())-matrix.TransformCoord(Center()));

			//B's basis with respect to A's local frame
			Matrix R(matrix*aabbMatrix);
			float ra, rb, t;
			long i, k;

			/*ALGORITHM: Use the separating axis test for all 15 potential
			separating axes. If a separating axis could not be found, the two
			boxes overlap. */

			Vector3 a(MaxBound-MinBound);
			Vector3 b(aabb.MaxBound-aabb.MinBound);
			//A's basis vectors
			for( i=0 ; i<3 ; i++ )
			{
				ra = a[i];
				rb = b[0]*fabs(R._m[i+4*0]) + b[1]*fabs(R._m[i+4*1]) + b[2]*fabs(R._m[i+4*2]);
				t = fabs( T[i] );

				if( t > ra + rb )
					return false;
			}

			//B's basis vectors
			for( k=0 ; k<3 ; k++ )
			{
				ra = a[0]*fabs(R._m[0+4*k]) + a[1]*fabs(R._m[1+4*k]) + a[2]*fabs(R._m[2+4*k]);
				rb = b[k];
				t = fabs( T[0]*R._m[0+4*k] + T[1]*R._m[1+4*k] + T[2]*R._m[2+4*k] );
				if( t > ra + rb )
					return false;
			}

			//9 cross products

			//L = A0 x B0
			ra = a[1]*fabs(R._m[2+4*0]) + a[2]*fabs(R._m[1+4*0]); 
			rb = b[1]*fabs(R._m[0+4*2]) + b[2]*fabs(R._m[0+4*1]); 
			t = fabs( T[2]*R._m[1+4*0] - T[1]*R._m[2+4*0] ); 
			if( t > ra + rb )
				return false; 

			//L = A0 x B1
			ra = a[1]*fabs(R._m[2+4*1]) + a[2]*fabs(R._m[1+4*1]); 
			rb = b[0]*fabs(R._m[0+4*2]) + b[2]*fabs(R._m[0+4*0]); 
			t = fabs( T[2]*R._m[1+4*1] - T[1]*R._m[2+4*1] ); 
			if( t > ra + rb )
				return false; 

			//L = A0 x B2
			ra = a[1]*fabs(R._m[2+4*2]) + a[2]*fabs(R._m[1+4*2]); 
			rb = b[0]*fabs(R._m[0+4*1]) + b[1]*fabs(R._m[0+4*0]); 
			t = fabs( T[2]*R._m[1+4*2] - T[1]*R._m[2+4*2] ); 
			if( t > ra + rb )
				return false; 

			//L = A1 x B0
			ra = a[0]*fabs(R._m[2+4*0]) + a[2]*fabs(R._m[0+4*0]); 
			rb = b[1]*fabs(R._m[1+4*2]) + b[2]*fabs(R._m[1+4*1]); 
			t = fabs( T[0]*R._m[2+4*0] - T[2]*R._m[0+4*0] ); 
			if( t > ra + rb )
				return false; 

			//L = A1 x B1
			ra = a[0]*fabs(R._m[2+4*1]) + a[2]*fabs(R._m[0+4*1]); 
			rb = b[0]*fabs(R._m[1+4*2]) + b[2]*fabs(R._m[1+4*0]); 
			t = fabs( T[0]*R._m[2+4*1] - T[2]*R._m[0+4*1] ); 
			if( t > ra + rb )
				return false; 

			//L = A1 x B2
			ra = a[0]*fabs(R._m[2+4*2]) + a[2]*fabs(R._m[0+4*2]); 
			rb = b[0]*fabs(R._m[1+4*1]) + b[1]*fabs(R._m[1+4*0]); 
			t = fabs( T[0]*R._m[2+4*2] - T[2]*R._m[0+4*2] ); 
			if( t > ra + rb )
				return false; 

			//L = A2 x B0
			ra = a[0]*fabs(R._m[1+4*0]) + a[1]*fabs(R._m[0+4*0]); 
			rb = b[1]*fabs(R._m[2+4*2]) + b[2]*fabs(R._m[2+4*1]); 
			t = fabs( T[1]*R._m[0+4*0] - T[0]*R._m[1+4*0] ); 
			if( t > ra + rb )
				return false; 

			//L = A2 x B1
			ra = a[0]*fabs(R._m[1+4*1]) + a[1]*fabs(R._m[0+4*1]); 
			rb = b[0] *fabs(R._m[2+4*2]) + b[2]*fabs(R._m[2+4*0]); 
			t = fabs( T[1]*R._m[0+4*1] - T[0]*R._m[1+4*1] ); 
			if( t > ra + rb )
				return false; 

			//L = A2 x B2
			ra = a[0]*fabs(R._m[1+4*2]) + a[1]*fabs(R._m[0+4*2]); 
			rb = b[0]*fabs(R._m[2+4*1]) + b[1]*fabs(R._m[2+4*0]); 
			t = fabs( T[1]*R._m[0+4*2] - T[0]*R._m[1+4*2] ); 
			if( t > ra + rb )
				return false;

			/*no separating axis found,
			the two boxes overlap */ 
			return true; 
		}

		///Check if it intersects another AABB
		float Distance(const VRUT::AABB &aabb) const
		{
			if (Intersects(aabb))
				return 0;
			float res=0;
			for (size_t i=0; i<3; ++i)
			{
				if (MinBound._v[i]>aabb.MaxBound._v[i])
					res+=(aabb.MaxBound._v[i]-MinBound._v[i])*(aabb.MaxBound._v[i]-MinBound._v[i]);
				else if (aabb.MinBound._v[i]>MaxBound._v[i])
					res+=(MaxBound._v[i]-aabb.MinBound._v[i])*(MaxBound._v[i]-aabb.MinBound._v[i]);
			}
			return sqrtf(res);
		}

		///Get string with bounds
		wxString ToString() const
		{
			return wxT("MinBound: ")+MinBound.ToString()+wxT("\nMaxBound: ")+MaxBound.ToString();
		}

		// compute the vertex for number N = <0..7>, N = 4.x + 2.y + z, where
		// x,y,z are either 0 or 1; (0 .. min coordinate, 1 .. max coordinate)
		Vector3 GetVertex(const int N) const {
			Vector3 vertex;
			switch (N) {
			case 0: vertex = MinBound; break;
			case 1: vertex=Vector3(MinBound.x, MinBound.y, MaxBound.z); break;
			case 2: vertex=Vector3(MinBound.x, MaxBound.y, MinBound.z); break;
			case 3: vertex=Vector3(MinBound.x, MaxBound.y, MaxBound.z); break;
			case 4: vertex=Vector3(MaxBound.x, MinBound.y, MinBound.z); break;
			case 5: vertex=Vector3(MaxBound.x, MinBound.y, MaxBound.z); break;
			case 6: vertex=Vector3(MaxBound.x, MaxBound.y, MinBound.z); break;
			case 7: vertex = MaxBound; break;
			}
			return vertex;
		}

		/// Transforms this AABB with given matrix, clips it with given clip planes and returns the AABB of the result.
		/// Works in homogeneous coordinates, assumes vertices with invalid w-coordinate are outside this volume.
		/// The result is divided by w, and than bounded into AABB.
		///
		/// The main usage of this is to transform AABB to screen coordinates.
		///
		/// \param[in] mvp Matrix that transforms this AABB. Usually Model-View-Projection matrix
		/// \param[in] clip_planes Clip planes that clips the transformed AABB.
		AABB TransformAndClipHomogeneous(const Matrix &mvp, const std::vector<Vector4> &clip_planes) const;

		/// Indices of four face quads. The order of quads is the same as the order of clip planes in CullingPlanes::FRUSTUM_PLANE.
		/// The indices are the same indices as returned by AABB::GetVertex. The faces are CCW in RH-system.
		static const unsigned int FaceQuadIndices[];

		// Default OpenGL clip volume (-1,-1,-1) - (1,1,1)
		static const AABB DefaultClipVolume;
	};


	///Bounding cone class
	class BCone
	{
	public:
		///BCone center
		Vector3 Center;
		///BCone axis (from top vertex to expanding far end)
		Vector3 Axis;
		///Angle of cone expand
		float Angle;

		///Class constructor
		BCone()
		{
			Center = Vector3(0, 0, 0);
			Axis = Vector3(0, 1, 0);
			Angle = 0.0f;
		}

		///Get string with bounds
		wxString ToString() const
		{
			return wxT("Center: ")+Center.ToString()+wxT("\nAxis: ")+Axis.ToString()+wxT("\nAngle: ")+TextWriter::WriteToWxString(Angle)+wxT("\n");
		}
	};


	///Bounding sphere class
	class BSphere
	{
	private:
	public:
		///BSphere center
		Vector3 Center;
		///BSphere radius
		float Radius;

		///Class constructor
		BSphere() : Center(0, 0, 0), Radius(0.0f)  {}
		///Copy constructor
		BSphere(const BSphere & s) : Center(s.Center), Radius(s.Radius)  {}
		///Class destructor
		~BSphere()	{}

		///BSphere-sphere intersection test
		bool Intersects(const BSphere * s) const
		{
			float r = Radius + s->Radius;
			return (Center - s->Center).LengthSq() < r * r;
		}

		///BSphere-sphere intersection test
		bool Intersects(const Vector3 &center, const float &radius) const
		{
			float r = Radius + radius;
			return (Center - center).LengthSq() < r * r;
		}

		///BSphere-cone intersection test
		bool Intersects(const BCone * K) const
		{
			Vector3 U(K->Center - (Radius / sinf(K->Angle)) * K->Axis);
			Vector3 D(Center - U);
			float dsqr = D.Dot(D);
			float e = K->Axis.Dot(D);
			float cs = cosf(K->Angle);
			if ( e > 0
				&& e * e >= dsqr * cs * cs )
			{
				D = Center - K->Center;
				dsqr = D.Dot(D);
				e = -K->Axis.Dot(D);
				float sn = sinf(K->Angle);
				if ( e > 0 && e * e >= dsqr * sn * sn )
					return dsqr <= Radius * Radius;
				else
					return true;
			}
			return false;
		}

		///Prevent copying NULL pointers - when debugging
		BSphere & operator=(const BSphere & s)
		{
			Center = s.Center;
			Radius = s.Radius;
			return *this;
		}

		///Get string with bounds
		wxString ToString() const
		{
			return wxT("Center: ")+Center.ToString()+wxT("\nRadius: ")+TextWriter::WriteToWxString(Radius)+wxT("\n");
		}
	};
}


#endif
