
//////////////////////////////////////////////////////////////////////
//
//	Crytek Common Source code
//	
//	File:Vector.h
//	Description:Implementation of the vector class.
//
//	History:
//	-Jan 31,2001:Created by Marco Corbetta
//	-1.2.2003 - ??? updated by Ivo Herzeg
//
//////////////////////////////////////////////////////////////////////

#ifndef VECTOR_H
#define VECTOR_H

#if _MSC_VER > 1000
# pragma once
#endif






////////////////////////////////////////////////////////////////
//#include "Cry_Math.h"
#include <math.h>
#include <assert.h>





///////////////////////////////////////////////////////////////////////////////
// Definitions                                                               //
///////////////////////////////////////////////////////////////////////////////
const float gf_PI       =  3.14159265358979323846264338327950288419716939937510f; // pi
const float gf_PI_MUL_2 =  6.28318530717958623200f; // 2*pi
const float gf_PI_DIV_2 =  3.14159265358979323846264338327950288419716939937510f*0.5f; // pi/2
const float gf_PI_DIV_4 =  0.78539816339744827900f; // pi/4
const float gf_INV_PI   =  0.31830988618379069122f; // 1/Pi
const float gf_DEGTORAD =  0.01745329251994329547f; // Degrees to Radians
const float gf_RADTODEG = 57.29577951308232286465f; // Radians to Degrees
const float gf_HUGE     =  1.0e+38f;                // Huge number for a float
const float gf_EPSILON  =  1.0e-5f;                 // Tolerance for a float

enum type_zero { zero };
enum type_min { MIN };
enum type_max { MAX };



///////////////////////////////////////////////////////////////////////////////
// Forward declarations                                                      //
///////////////////////////////////////////////////////////////////////////////
template <class ftype> class Vec3_tpl;
template <class ftype> class Ang3_tpl;
template <typename ftype> class Quaternion_tpl;
template <typename ftype> class Matrix33_tpl;
template <typename ftype> class Matrix34_tpl;
template <typename ftype> class Matrix44_tpl;






///////////////////////////////////////////////////////////////////////////////
// Typedefs                                                                  //
///////////////////////////////////////////////////////////////////////////////
typedef double real;
typedef int index;
typedef int index;

#include "CryVector2.h"



typedef Vec3_tpl<float>	Vec3d;
typedef Vec3_tpl<float>	Vec3;   //we will use only this throughout the project
typedef Vec3_tpl<double> Vec3d_f64;
typedef Vec3_tpl<float>	Point;
typedef Vec3_tpl<float>	Vector;
typedef Vec3_tpl<float>	vectorf;
typedef Vec3_tpl<double> vector;
typedef Vec3_tpl<int>		vectori;


typedef Ang3_tpl<float> Ang3d;
typedef Ang3_tpl<float> Ang3;


//-----------------------------------------------------------------------

//this can easily be confused with square-root?

inline double cos_tpl(double op) { return cos(op); }
inline float  cos_tpl(float op) { return cosf(op); }
inline double sin_tpl(double op) { return sin(op); }
inline float  sin_tpl(float op) { return sinf(op); }
inline double acos_tpl(double op) { return acos(op); }
inline float  acos_tpl(float op) { return acosf(op); }
inline double asin_tpl(double op) { return asin(op); }
inline float  asin_tpl(float op) { return asinf(op); }
inline double atan_tpl(double op) { return atan(op); }
inline float  atan_tpl(float op) { return atanf(op); }
inline double atan2_tpl(double op1,double op2) { return atan2(op1,op2); }
inline float  atan2_tpl(float op1,float op2) { return atan2f(op1,op2); }
inline double exp_tpl(double op) { return exp(op); }
inline float  exp_tpl(float op) { return expf(op); }
inline double log_tpl(double op) { return log(op); }
inline float  log_tpl(float op) { return logf(op); }
inline double sqrt_tpl(double op) { return sqrt(op); }
inline float  sqrt_tpl(float op) { return sqrtf(op); }
inline double fabs_tpl(double op) { return fabs(op); }
inline float  fabs_tpl(float op) { return fabsf(op); }
inline int    fabs_tpl(int op) { int mask=op>>31; return op+mask^mask; }

template <class ftype> inline ftype square(ftype fOp) { return(fOp*fOp); }

inline float Snap_s180( float val );
inline float Snap_s360( float val );


////////////////////////////////////////////////////////////////
template <class ftype> class Vec3_tpl
{
public:
	
	ftype x,y,z;

	Vec3_tpl(){};

	Vec3_tpl( const ftype vx, const ftype vy, const ftype vz ) { x=vx; y=vy; z=vz; };

	//bracket operator
	void operator () ( const ftype vx,const ftype vy,const ftype vz ) { x=vx; y=vy; z=vz; };

	//float* fptr=vec;
	operator ftype* ()					{ return (ftype*)this; }

	//float farray[3]={1,2,3};
	//Vec3 v=Vec3(farray);
	//
	//float* fptr;
	//Vec3 newv=Vec3(fptr);
	//
	//Matrix44 tm=GetTranslationMat(Vec3(44,55,66));
	//Vec3 tvec=Vec3(tm[3]); //really dangerous! Should be replaced by "GetTranslation(tm)"
	template<class ftype> explicit Vec3_tpl(const ftype *src) { x=src[0]; y=src[1]; z=src[2]; }

	//CONSTRUCTOR: implement the copy/casting/assignement constructor:	
	template <class ftype>
	inline Vec3_tpl( const Vec3_tpl<ftype>& v ) {	x=v.x;	y=v.y;	z=v.z; }


	//overload = operator to copy double=doube or float=float
	//Vec3_tpl<float>=Vec3_tpl<float>
	//Vec3_tpl<double>=Vec3_tpl<double>
	Vec3_tpl& operator=(const Vec3_tpl<ftype> &v) { x=v.x; y=v.y; z=v.z; return *this; }

	//overload assignment operator for casting double/float
	//Vec3_tpl<float>=Vec3_tpl<double>
	//Vec3_tpl<double>=Vec3_tpl<float>
	template<typename U>
	Vec3_tpl& operator = (const U &v)	{	x=(ftype)v.x; y=(ftype)v.y; z=(ftype)v.z ;return *this; }

	ftype &operator [] (int index)		  { assert(index>=0 && index<=2); return ((ftype*)this)[index]; }
	ftype operator [] (int index) const { assert(index>=0 && index<=2); return ((ftype*)this)[index]; }





	////////////////////////////////////////////////////////////////		
	//overloaded arithmetic operators
	////////////////////////////////////////////////////////////////		

	//three methods for a "dot-product" operation
	ftype Dot (const Vec3_tpl<ftype> &vec2)	const	{ return x*vec2.x + y*vec2.y + z*vec2.z; }

	//two methods for a "cross-product" operation
	Vec3_tpl<ftype> Cross (const Vec3_tpl<ftype> &vec2) const	{	return Vec3_tpl<ftype>( y*vec2.z  -  z*vec2.y,     z*vec2.x -    x*vec2.z,   x*vec2.y  -  y*vec2.x); 	}	


	Vec3_tpl<ftype> operator*(ftype k) const { return Vec3_tpl<ftype>(x*k,y*k,z*k); }
	Vec3_tpl<ftype> operator/(ftype k) const { k=(ftype)1.0/k; return Vec3_tpl<ftype>(x*k,y*k,z*k); }
	friend Vec3_tpl operator * (float f, const Vec3_tpl &vec)	{ return Vec3_tpl(f*vec.x, f*vec.y, f*vec.z); }


	Vec3_tpl<ftype>& operator *= (ftype k) { x*=k;y*=k;z*=k; return *this; }
	Vec3_tpl<ftype>& operator /= (ftype k) { k=(ftype)1.0/k; x*=k;y*=k;z*=k; return *this; }



	//negate
	Vec3_tpl<ftype> operator - ( void ) const { return Vec3_tpl<ftype>(-x,-y,-z); }
	void	Negate() { x=-x; y=-y; z=-z; }	
	Vec3_tpl& flip() { x=-x;y=-y;z=-z; return *this; }




	__declspec( deprecated ) 
	friend bool operator ==(const Vec3_tpl<ftype> &vec1, const Vec3_tpl<ftype> &vec2)	{

		//please replace the "==" operator by the function " IsEquivalent(v0,v1,VEC_EPSILON); "

		//the function "PreciseEquals(v0,v1);" will be replaced by the "==" operator

		return (Ffabs(vec1.x-vec2.x) <= VEC_EPSILON)&&(Ffabs(vec1.y-vec2.y) <= VEC_EPSILON)&&(Ffabs(vec1.z-vec2.z) <= VEC_EPSILON); 
	}
	
	
	__declspec( deprecated ) 
	friend bool operator !=(const Vec3_tpl<ftype> &vec1, const Vec3_tpl<ftype> &vec2)	{ 

		//please replace the "!=" operator by the function " !IsEquivalent(v0,v1,VEC_EPSILON); "

		return !(vec1==vec2); 
	}



	__declspec( deprecated ) 
	friend bool operator < (const Vec3_tpl<ftype> &vec1, const Vec3_tpl<ftype> &vec2)		{ 
		//Vladimir, please remove this!! 
		return (vec1.x < vec2.x) && (vec1.y < vec2.y) && (vec1.z < vec2.z); 
	}
	__declspec( deprecated ) 
	friend bool operator > (const Vec3_tpl<ftype> &vec1, const Vec3_tpl<ftype> &vec2)		{ 
		//Vladimir, please remove this!!
		return (vec1.x > vec2.x) && (vec1.y > vec2.y) && (vec1.z > vec2.z); 
	}

	friend bool IsEquivalent(const Vec3_tpl<ftype>& v0, const Vec3_tpl<ftype>& v1, float epsilon) {
		return  ((Ffabs(v0.x-v1.x) <= epsilon) &&	(Ffabs(v0.y-v1.y) <= epsilon)&&	(Ffabs(v0.z-v1.z) <= epsilon));	
	}

	friend inline bool PreciseEquals(const Vec3_tpl<ftype>& v0, const Vec3_tpl<ftype>& v1)	{
    //to be replaced by "=="
		if ((v0.x==v1.x) && (v0.y==v1.y) && (v0.z==v1.z))	return true;
		return false;
	}

	//-----------------------------------------------------------------------------------------------







	////////////////////////////////////////////////////////////////
	//common methods
	////////////////////////////////////////////////////////////////
	Vec3_tpl& zero() { x=y=z=0; return *this; }
	void Set(const ftype xval,const ftype yval, const ftype zval) { x=xval; y=yval; z=zval; }
	Vec3_tpl<ftype>&	set(const ftype xval,const ftype yval, const ftype zval) { x=xval; y=yval; z=zval; return *this; }

	//! calcultae the length of the vector
	ftype	Length() const { return sqrt_tpl(x*x+y*y+z*z); }		
	ftype len() const { return sqrt_tpl(x*x+y*y+z*z); }
	friend ftype GetLength( const Vec3_tpl<ftype>& v ) { return sqrt_tpl(v.x*v.x + v.y*v.y + v.z*v.z); }

	//! calcultae the squared length of the vector
	friend ftype GetLengthSquared( const Vec3_tpl<ftype> &v ) { return v.x*v.x + v.y*v.y + v.z*v.z; }
	ftype len2() const { return x*x +y*y + z*z; }



	//! normalize the vector and return the length=1 if successfull
	float	Normalize() { 
		float fLen = Length();
	//	assert(fLen>0.00001f);
		if (fLen<0.00001f) return(0);  //no good idea! not everybody will check this
		float fInvLen=1.0f/fLen; 
		x*=fInvLen; y*=fInvLen; z*=fInvLen; 
		return fInvLen; 
	}

	//! return a normalized vector
	inline friend Vec3_tpl<ftype> GetNormalized( const Vec3_tpl<ftype> &v ) {
			ftype vlength=GetLength(v);	
			//assert(vlength>0.00001f);
			ftype ivlength=1.0f/vlength;
			return (v*ivlength);
		}


	Vec3_tpl& normalize() { 
		ftype rlen=sqrt_tpl(x*x+y*y+z*z); 
		//assert(rlen>0.00001f);
		if (rlen>0) { rlen=(ftype)1.0/rlen; x*=rlen;y*=rlen;z*=rlen; } 
		else set(0,0,1); return *this; 
	}
	Vec3_tpl normalized() const { 
		ftype rlen=sqrt_tpl(x*x+y*y+z*z);
		//assert(rlen>0.00001f);
		if (rlen>0) { rlen=(ftype)1.0/rlen; return Vec3_tpl(x*rlen,y*rlen,z*rlen); } 
		else return Vec3_tpl(0,0,1);
	}


	float	NormalizeFast() {
		float fLen = x*x + y*y + z*z;
		//assert(fLen>0.00001f);
		unsigned int *n1 = (unsigned int *)&fLen;
		unsigned int n = 0x5f3759df - (*n1 >> 1);
		float *n2 = (float *)&n;
		fLen = (1.5f - (fLen * 0.5f) * *n2 * *n2) * *n2;
		x*=fLen; y*=fLen; z*=fLen;
		return fLen;
	}



	friend	float	GetSquaredDistance(const Vec3_tpl &vec1, const Vec3_tpl &vec2)	{		
		return (vec2.x-vec1.x)*(vec2.x-vec1.x)+(vec2.y-vec1.y)*(vec2.y-vec1.y)+(vec2.z-vec1.z)*(vec2.z-vec1.z);
	}
	friend float GetDistance(const Vec3_tpl &vec1, const Vec3_tpl &vec2) { 
		return (float) sqrt((vec2.x-vec1.x)*(vec2.x-vec1.x)+(vec2.y-vec1.y)*(vec2.y-vec1.y)+(vec2.z-vec1.z)*(vec2.z-vec1.z)); 
	}	


	//! force vector length by normalizing it
	//! 08/26/2002 optimized a little by M.M.
	void  SetLen(const float fLen)	{ 
		float fLenMe = GetLengthSquared(*this);
		if(fLenMe<0.00001f*0.00001f)return;
		fLenMe=fLen/(float)sqrt((double)fLenMe);
		x*=fLenMe; y*=fLenMe; z*=fLenMe;
	}



	// permutate coordinates so that z goes to new_z slot
	Vec3_tpl permutated(int new_z) const { return Vec3_tpl(*(&x+inc_mod3[new_z]), *(&x+dec_mod3[new_z]), *(&x+new_z)); }

	// returns volume of a box with this vector as diagonal 
	ftype volume() const { return x*y*z; }

	// returns a vector orthogonal to this one
	Vec3_tpl orthogonal() const {
		int i = isneg(sqr((ftype)0.9)*GetLengthSquared(*this)-x*x);
		Vec3_tpl<ftype> res;
		res[i]=0; res[inc_mod3[i]]=(*this)[dec_mod3[i]]; res[dec_mod3[i]]=-(*this)[inc_mod3[i]];
		return res;
	}


	// returns a vector that consists of absolute values of this one's coordinates
	Vec3_tpl abs() const { return Vec3_tpl(fabs_tpl(x),fabs_tpl(y),fabs_tpl(z)); }



	//! check for min bounds
	void	CheckMin(const Vec3_tpl &other)	{ 
		if (other.x<x) x=other.x;
		if (other.y<y) y=other.y;
		if (other.z<z) z=other.z;
	}			

	//! check for max bounds
	void	CheckMax(const Vec3_tpl &other)	{
		if (other.x>x) x=other.x;
		if (other.y>y) y=other.y;
		if (other.z>z) z=other.z;
	}



	//this is a special case for CAngleAxis
	Vec3_tpl rotated(const Vec3_tpl &axis, ftype angle) const { 
		return rotated(axis,cos_tpl(angle),sin_tpl(angle)); 
	}
	Vec3_tpl rotated(const Vec3_tpl &axis, ftype cosa,ftype sina) const {
		Vec3_tpl zax = axis*(*this*axis); 
		Vec3_tpl xax = *this-zax; 
		Vec3_tpl yax = axis^xax;
		return xax*cosa + yax*sina + zax;
	}

	Vec3_tpl rotated(const Vec3_tpl &center,const Vec3_tpl &axis, ftype cosa,ftype sina) const { 
		return center+(*this-center).rotated(axis,cosa,sina); 
	}
	Vec3_tpl rotated(const Vec3_tpl &center,const Vec3_tpl &axis, ftype angle) const { 
		return center+(*this-center).rotated(axis,angle); 
	}

	//! calculate the angle between two vectors in 3d (by M.M.)
	//! /param invA direction of vector a (don't have to be normalized)
	//! /param invB direction of vector b (don't have to be normalized)
	//! /return angle in the range 0 to p radians. 
	friend inline float CalcAngleBetween( const Vec3d &invA, const Vec3d &invB )	{
		double LengthQ=GetLength(invA)*GetLength(invB);
		if(LengthQ<0.01)LengthQ=0.01;
		return((float)(acos((invA*invB)/LengthQ)));
	}


	Vec3_tpl(type_zero) { x=y=z=0; }
	Vec3_tpl(type_min) { x=y=z=-100; }
	Vec3_tpl(type_max) { x=y=z=100; }

};


Vec3_tpl<float>::Vec3_tpl(type_min) { x=y=z=-3.3E38f; }
Vec3_tpl<float>::Vec3_tpl(type_max) { x=y=z=3.3E38f; }
Vec3_tpl<double>::Vec3_tpl(type_min) { x=y=z=-1.7E308; }
Vec3_tpl<double>::Vec3_tpl(type_max) { x=y=z=1.7E308; }



// dot product (2 versions)
template<class ftype1,class ftype2> 
ftype1 operator * (const Vec3_tpl<ftype1> &op1, const Vec3_tpl<ftype2> &op2) { 
	return op1.x*op2.x+op1.y*op2.y+op1.z*op2.z; 
} 
template<class ftype1,class ftype2> 
ftype1 operator | (const Vec3_tpl<ftype1> &op1, const Vec3_tpl<ftype2> &op2) { 
	return op1.x*op2.x+op1.y*op2.y+op1.z*op2.z; 
} 

// cross product
template<class ftype1,class ftype2> 
Vec3_tpl<ftype1> operator ^ (const Vec3_tpl<ftype1> &op1, const Vec3_tpl<ftype2> &op2) {
	return Vec3_tpl<ftype1>(op1.y*op2.z-op1.z*op2.y, op1.z*op2.x-op1.x*op2.z, op1.x*op2.y-op1.y*op2.x); 
} 
template<class ftype1,class ftype2> 
Vec3_tpl<ftype1> operator % (const Vec3_tpl<ftype1> &op1, const Vec3_tpl<ftype2> &op2) {
	return Vec3_tpl<ftype1>(op1.y*op2.z-op1.z*op2.y, op1.z*op2.x-op1.x*op2.z, op1.x*op2.y-op1.y*op2.x); 
} 


//---------------------------------------------------------------------------

//vector addition
template<class ftype1,class ftype2>
Vec3_tpl<ftype1> operator + (const Vec3_tpl<ftype1> &op1, const Vec3_tpl<ftype2> &op2) {
	return Vec3_tpl<ftype1>(op1.x+op2.x, op1.y+op2.y, op1.z+op2.z);
}
//vector subtraction
template<class ftype1,class ftype2>
Vec3_tpl<ftype1> operator - (const Vec3_tpl<ftype1> &op1, const Vec3_tpl<ftype2> &op2) {
	return Vec3_tpl<ftype1>(op1.x-op2.x, op1.y-op2.y, op1.z-op2.z);
}


//---------------------------------------------------------------------------


//vector self-addition
template<class ftype1,class ftype2>
Vec3_tpl<ftype1>& operator += (Vec3_tpl<ftype1> &op1, const Vec3_tpl<ftype2> &op2) {
	op1.x+=op2.x; op1.y+=op2.y; op1.z+=op2.z; return op1;
}
//vector self-subtraction
template<class ftype1,class ftype2>
Vec3_tpl<ftype1>& operator -= (Vec3_tpl<ftype1> &op1, const Vec3_tpl<ftype2> &op2) {
	op1.x-=op2.x; op1.y-=op2.y; op1.z-=op2.z; return op1;
}










///////////////////////////////////////////////////////////////
//	represents angle. components are always clamped to -180 180 range
template <class ftype> class Ang3_tpl : public Vec3_tpl<ftype>
{
public:

	Ang3_tpl() {}

	Ang3_tpl(const Vec3d &v) { x=v.x;	y=v.y; z=v.z;	}  

	Ang3_tpl	&operator = (const Vec3d &v)  { x=v.x; y=v.y; z=v.z; return *this; 	}

	inline Ang3d_tpl( const ftype vx, const ftype vy, const ftype vz )	{	x=vx; y=vy; z=vz;	}  


	//! normalize the vector ANGLE to -180, 180 range 
	void	Snap180()	{
		x = Snap_s180(x);
		y = Snap_s180(y);
		z = Snap_s180(z);
	}

	//! normalize the vector ANGLE to 0-360 range 
	inline void	Snap360()	{
		x = Snap_s360(x);
		y = Snap_s360(y);
		z = Snap_s360(z);
	}

	//! convert from radians to degrees
	void  Rad2Deg()	{		x=RAD2DEG(x);		y=RAD2DEG(y);		z=RAD2DEG(z);	}
	//! convert from degrees to radians
	void  Deg2Rad() {   x=DEG2RAD(x);   y=DEG2RAD(y); 	z=DEG2RAD(z);	}


	/*!
	* converts Matrix or a Quaternion back into Euler angles.
	* if you pass a Matrix34 or Matrix44 the factorising function  
	* uses just the 3x3 part. 
	* 
	* Example 1:
	*
	*   Matrix m44 = GetRotationAA44( 0.5f, normalize( CVector(1,2,3)) );
	*   CEuler e = GetAnglesXYZ(m33);
	*
	* Example 2:
	*
	*   CQuaternion quat = GetRotationAA44( 0.5f, normalize( CVector(1,2,3)) );
	*   CEuler e = GetAnglesXYZ(quat); //use casting operator to convert quaternion into matrix, 
	*                                  //and then into Euler angles.
	*
	*/
	//member function
	inline SetAnglesXYZ( const Matrix44_tpl<float> &m ) {
		y = asin_tpl(-m.m_values[2][0]);
		if (y<gf_PI_DIV_2) {
			if (y>-gf_PI_DIV_2) {	z=atan2_tpl(m.m_values[1][0],m.m_values[0][0]);	x=atan2_tpl(m.m_values[2][1],m.m_values[2][2]);	} 
			else { z=-atan2_tpl(-m.m_values[0][1],m.m_values[0][2]); x=0; }
		} else{	z=atan2_tpl(-m.m_values[0][1],m.m_values[0][2]);x=0; } 
	}
	inline static Ang3_tpl<float> GetAnglesXYZ( const Matrix44_tpl<float> &m ) {	Ang3_tpl<float> a; a.SetAnglesXYZ(m);	return a;	}
	inline friend Ang3_tpl<float> GetAnglesXYZ( const Matrix44_tpl<float> &m ) {	Ang3_tpl<float> a; a.SetAnglesXYZ(m);	return a;	}


	inline void SetAngles( const Matrix33_tpl<ftype>& m ) {
		//check if we have an orthonormal-base (assuming we are using a right-handed coordinate system)
		assert( IsEquivalent(m.GetOrtX(),m.GetOrtY()%m.GetOrtZ(),0.01f) );
		assert( IsEquivalent(m.GetOrtY(),m.GetOrtZ()%m.GetOrtX(),0.01f) );
		assert( IsEquivalent(m.GetOrtZ(),m.GetOrtX()%m.GetOrtY(),0.01f) );
		y = asin_tpl(max((ftype)-1.0,min((ftype)1.0,-m(2,0))));
		if (fabs_tpl(fabs_tpl(y)-(ftype)(pi*0.5))<(ftype)0.01) {//data[stridei*2+stridej*0]*data[stridei*2+stridej*0]>1-1E-3) {
			x = 0;
			z = atan2_tpl(-m(0,1), m(1,1));
		} else {
			x = atan2_tpl( m(2,1), m(2,2));
			z = atan2_tpl( m(1,0), m(0,0));
		}
	}
	inline static Ang3_tpl<ftype> GetAngles( const Matrix33_tpl<ftype> &m ) {	Ang3_tpl<ftype> a; a.SetAngles(m);	return a;	}
	inline friend Ang3_tpl<ftype> GetAngles( const Matrix33_tpl<ftype> &m ) {	Ang3_tpl<ftype> a; a.SetAngles(m);	return a;	}


	/*!
	*  convert matrix back into Ang3d.
	*
	*/
	inline SetAnglesYXZ( const Matrix44_tpl<float> &m ) {
		x = asin_tpl(m.m_values[2][1]);
		if (x<gf_PI_DIV_2) {
			if (x>-gf_PI_DIV_2) {	z=atan2_tpl(-m.m_values[0][1],m.m_values[1][1]);	y=atan2_tpl(-m.m_values[2][0],m.m_values[2][2]);	} 
			else {  z=-atan2_tpl(m.m_values[0][2],m.m_values[0][0]); y=0; }
		} else {	z= atan2_tpl(m.m_values[0][2],m.m_values[0][0]); y=0; } 
	}
	inline static Ang3_tpl<float> GetAnglesYXZ( const Matrix44_tpl<float> &m ) {	Ang3_tpl<float> a; a.SetAnglesYXZ(m);	return a;	}
	inline friend Ang3_tpl<float> GetAnglesYXZ( const Matrix44_tpl<float> &m ) {	Ang3_tpl<float> a; a.SetAnglesYXZ(m);	return a;	}





};







//! normalize the val to 0-360 range 
inline float Snap_s360( float val )
{
	if( val < 0.0f )
		val =float( 360.0f + fmod(val,360.0f));
	else
		if(val >= 360.0f)
			val =float(fmod(val,360.0f));
	return val;
}

//! normalize the val to -180, 180 range 
inline float Snap_s180( float val )
{
	if( val > -180.0f && val < 180.0f)
		return val;
	val = Snap_s360( val );
	if( val>180.0f )
		return -(360.0f - val);
	return val;
}

//! normalize the val to -180, 180 range 
//inline float AngDiff( float ang1, float ang2 ) {	return Snap180( ang1 - ang2 ); }


////////////////////////////////////////////////////////////////		







































































//---------------------------------------------------------------------------------
//---------------------------------------------------------------------------------
//---------------------------------------------------------------------------------
//-------lots of intersection-tests -----------------------------------------------
//-------I'll move them into CryFIST ----------------------------------------------
//---------------------------------------------------------------------------------
//---------------------------------------------------------------------------------
//---------------------------------------------------------------------------------
//---------------------------------------------------------------------------------


inline void AddToBounds (const Vec3d& v, Vec3d& mins, Vec3d& maxs)
{
	/*int		i; float val;
	for (i=0; i<3; i++)	{
		val = (&x)[i];
		if (val < mins[i]) mins[i] = val;
		if (val > maxs[i])maxs[i] = val;
	}*/
	
	if (v.x < mins.x)	mins.x = v.x;
	if (v.x > maxs.x)	maxs.x = v.x;
	if (v.y < mins.y)	mins.y = v.y;
	if (v.y > maxs.y)	maxs.y = v.y;
	if (v.z < mins.z)	mins.z = v.z;
	if (v.z > maxs.z)	maxs.z = v.z;
}






// line/sphere-intersection
// p1: 1st point of line
// p2: 2nd point of line
// p3: center of sphere
// r: radius of sphere
// i1: 1st intersection point
// i2: 2nd intersection point
// return number of intersections
inline int LineSphereIntersection(Vec3d p1, Vec3d p2, Vec3d p3, float r, Vec3d &i1, Vec3d i2)
{
	float dx=p2.x-p1.x;
	float dy=p2.y-p1.y;
	float dz=p2.z-p1.z;
	float a=dx*dx+dy*dy+dz*dz;
	float b=2.0f*(dx*(p1.x-p3.x)+dy*(p1.y-p3.y)+dz*(p1.z-p3.z));
	float c=p3.Dot(p3)+p2.Dot(p2)-2.0f*(p3.x*p1.x+p3.y*p1.y+p3.z-p1.z)-r*r;
	float d=b*b-4.0f*a*c;
	if (d<0.0f)
		return 0;
	float u;
	u=(-b+(float)sqrt((double)d))/a*a;
	i1=p1+((p2-p1)*u);
	if (d==0.0)
		return 1;
	u=(-b-(float)sqrt((double)d))/a*a;
	i2=p1+((p2-p1)*u);
	return 2;
}


////////////////////////////////////////////////////////////////		
//! check if the point is inside an AABB
inline bool	InsideBBox(const Vec3d &p, const Vec3d &mins,const Vec3d &maxs)
{
	if ((p.x>=mins.x && p.x<=maxs.x) && (p.y>=mins.y && p.y<=maxs.y) && (p.z>=mins.z && p.z<=maxs.z))
		return (true);

	return (false);
}


//! check if the point is inside a triangle
inline bool	PointInTriangle(const Vec3d& point, const Vec3d& v0,const Vec3d& v1,const Vec3d& v2,const Vec3d& normal)
{
	float xt,yt;
	Vec3d nn;

	int p1,p2;

	nn = normal;
	nn.x = Ffabs(nn.x);
	nn.y = Ffabs(nn.y);
	nn.z = Ffabs(nn.z);

	if ((nn.x>=nn.y) && (nn.x>=nn.z)) 
	{
		xt=point.y; yt=point.z;
		p1=PLANE_Y;p2=PLANE_Z;
	}
	else
		if ((nn.y>=nn.x) && (nn.y>=nn.z))
		{
			xt=point.x;yt=point.z;
			p1=PLANE_X;p2=PLANE_Z;
		}
		else
		{
			xt=point.x;yt=point.y;
			p1=PLANE_X;p2=PLANE_Y;
		}

		float Ax,Ay,Bx,By;
		float s;

		bool front=false;
		bool back=false;


		Ax=(v0)[p1];Bx=(v1)[p1];
		Ay=(v0)[p2];By=(v1)[p2];

		s=((Ay-yt)*(Bx-Ax)-(Ax-xt)*(By-Ay));

		if (s>=0) 
		{ 
			if (back) 
				return (false); 
			front=true;
		}
		else 
		{ 
			if (front) 
				return (false); 
			back=true; 
		}

		Ax=(v1)[p1];Bx=(v2)[p1];
		Ay=(v1)[p2];By=(v2)[p2];

		s=((Ay-yt)*(Bx-Ax)-(Ax-xt)*(By-Ay));

		if (s>=0) 
		{ 
			if (back) 
				return (false); 
			front=true;
		}
		else 
		{ 
			if (front) return (false); 
			back=true; 
		}

		Ax=(v2)[p1];Bx=(v0)[p1];
		Ay=(v2)[p2];By=(v0)[p2];

		s=((Ay-yt)*(Bx-Ax)-(Ax-xt)*(By-Ay));

		if (s>=0) 
		{ 
			if (back) 
				return (false); 
			front=true;
		}
		else 
		{ 
			if (front) 
				return (false); 
			back=true; 
		}

		return (true);
}



////////////////////////////////////////////////////////////////		
//misc functions
//! check if a ray hit an AABB
inline bool	RayHitBBox(const Vec3d &start,const Vec3d &end,const Vec3d &mins,const Vec3d &maxs)
{

	Vec3d vMid=(maxs+mins)*0.5f;	
	Vec3d	vExt=maxs-vMid;

  Vec3d l=(end-start)*0.5f;
	Vec3d m=(start-vMid)+l;
  Vec3d a=vExt; //maxs;
  
  Vec3d al(l.x<0?-l.x:l.x, l.y<0?-l.y:l.y, l.z<0?-l.z:l.z);

  if (m.x*m.x-square(a.x+al.x) > 1E-8)   return (false);
  if (m.y*m.y-square(a.y+al.y) > 1E-8)   return (false);
  if (m.z*m.z-square(a.z+al.z) > 1E-8)   return (false);
  
  if (square(m.z*l.y-m.y*l.z)-square(a.y*al.z+a.z*al.y) > 1E-8)  return (false);
  if (square(m.x*l.z-m.z*l.x)-square(a.x*al.z+a.z*al.x) > 1E-8)  return (false);
  if (square(m.x*l.y-m.y*l.x)-square(a.x*al.y+a.y*al.x) > 1E-8)  return (false);
  
  return (true);
}

////////////////////////////////////////////////////////////////		
//! check if a Ray hit a sphere
inline bool	RayHitSphere(const Vec3d &start,const Vec3d &end,const Vec3d &center, float radius2)
{
  Vec3d diff;
  Vec3d AC, AB;
  Vec3d point;

	//check if one of the two edpoints of the line is inside the sphere  
  diff.x = end.x-center.x;
  diff.y = end.y-center.y;
  diff.z = end.z-center.z;
  if (diff.x*diff.x+diff.y*diff.y+diff.z*diff.z <= radius2) 
    return (true);
  
  AC.x = center.x-start.x;
  AC.y = center.y-start.y;
  AC.z = center.z-start.z;
  if (AC.x*AC.x+AC.y*AC.y+AC.z*AC.z <= radius2) 
    return (true);

	//check distance from the sphere to the line
  AB.x = end.x-start.x;
  AB.y = end.y-start.y;
  AB.z = end.z-start.z;
  
  float r = (AC.x*AB.x+AC.y*AB.y+AC.z*AB.z) / (AB.x*AB.x+AB.y*AB.y+AB.z*AB.z);

	//projection falls outside the line
  if (r<0 || r>1) 
    return (false);

	//check if the distance from the line to the center of the sphere is less than radius
  point.x = start.x + r*AB.x;
  point.y = start.y + r*AB.y;
  point.z = start.z + r*AB.z;
  
  if ((point.x-center.x)*(point.x-center.x) + (point.y-center.y)*(point.y-center.y) + (point.z-center.z)*(point.z-center.z) > radius2)
    return (false);
  
  return (true);
}

////////////////////////////////////////////////////////////////		
//! calc the area of a polygon giving a list of vertices and normal
inline float	CalcArea(const Vec3d *vertices,int numvertices,const Vec3d &normal)
{	
  Vec3d csum(0,0,0);
  
  int n=numvertices;
  for (int i = 0, j = 1; i <= n-2; i++, j++)
  {
    csum.x += vertices[i].y*vertices[j].z-
				  vertices[i].z*vertices[j].y;
		csum.y += vertices[i].z*vertices[j].x-
				  vertices[i].x*vertices[j].z;
    csum.z += vertices[i].x*vertices[j].y-
				  vertices[i].y*vertices[j].x;
  }
  
  csum.x += vertices[n-1].y*vertices[0].z-
    vertices[n-1].z*vertices[0].y;
  csum.y += vertices[n-1].z*vertices[0].x-
    vertices[n-1].x*vertices[0].z;
  csum.z += vertices[n-1].x*vertices[0].y-
    vertices[n-1].y*vertices[0].x;
  
  float area=0.5f*(float)Ffabs(normal*csum);
  return (area);
}




//! calculate 2 vector that form a orthogonal base with a given input vector (by M.M.)
//! /param invDirection input direction (has to be normalized)
//! /param outvA first output vector that is perpendicular to the input direction
//! /param outvB second output vector that is perpendicular the input vector and the first output vector
inline void GetOtherBaseVec( const Vec3d &invDirection, Vec3d &outvA, Vec3d &outvB )
{
	if(invDirection.z<-0.5f || invDirection.z>0.5f)
	{
		outvA.x=invDirection.z;
		outvA.y=invDirection.y;
		outvA.z=-invDirection.x;
	}
	else
	{
		outvA.x=invDirection.y;
		outvA.y=-invDirection.x;
		outvA.z=invDirection.z;
	}
	outvB = invDirection.Cross(outvA);
	outvB.Normalize();
	outvA = invDirection.Cross(outvB);

	// without this normalization the results are not good enouth (Cross product introduce a errors)
	outvA.Normalize();
}









/////////////////////////////////////////////////////////////////////////
//this is some special engine stuff, should be moved to a better location
/////////////////////////////////////////////////////////////////////////

// for bbox's checks and calculations
#define MAX_BB	+99999.0f
#define MIN_BB	-99999.0f

//! checks if this has been set to minBB
inline bool IsMinBB( const Vec3& v ) {
	if (v.x<=MIN_BB) return (true);
	if (v.y<=MIN_BB) return (true);
	if (v.z<=MIN_BB) return (true);
	return (false);
}

//! checks if this has been set to maxBB
inline bool IsMaxBB( const Vec3& v ) {
	if (v.x>=MAX_BB) return (true);
	if (v.y>=MAX_BB) return (true);
	if (v.z>=MAX_BB) return (true);
	return (false);
}

inline Vec3d	SetMaxBB( void ) { return Vec3(MAX_BB,MAX_BB,MAX_BB); }
inline Vec3d	SetMinBB( void ) { return Vec3(MIN_BB,MIN_BB,MIN_BB); }




#endif //vector
