//written by Martin Ruzicka, email MartinRuzickaTM@seznam.cz

#ifndef MATHEMATICS_H
#define MATHEMATICS_H

#include <cassert>
const double DT_PI = 3.14159265;
const double INTERVAL_TOLERANCE=0.0001; //tolerance pri porovnavani double a floatu kdy sou cisla jeste stejne (pokud abs(cislo1-cislo2)<INTERVAL_TOLERANCE tak sou cisla povazovana za stejna)
const double EPSILON = 0.001;
#include <cmath>
#include <ostream>
#include <limits>


namespace ANALYTICAL_GEOMETRY
{
///Base structure to work with vectors and points in euclidian 3D space.
/**
*	Zakladni datova struktura pro praci s vektory a body v prostoru. Tridy Vektor a Point zni dedi.
**/
template<typename T>
class Base_3D_vector
{
public:
	T arr[4];		/**< Slozky reprezentujici bod nebo vektor v 3 rozmernem prostoru.*/
public:
	Base_3D_vector( const Base_3D_vector &);
	Base_3D_vector& operator=( const Base_3D_vector &);
	Base_3D_vector(void);					/**< Konstruktor nenastavujici zadne hodnoty slozkam x,y,z,w. */
	Base_3D_vector(const T& x,const T& y,const T& z, const T& w);			/**< Konstruktor nastavujici hodnoty slozek x,y,z,w. */

	Base_3D_vector( const T* arr);
	~Base_3D_vector(void);

	inline void clampTo( const T from, const T to );	/**< Clamp x,y,z values into interval<from, to> */
	
	T& x(void);
	T& y(void);
	T& z(void);
	T& w(void);
	T* getArrPtr(void); 

	const T& x(void) const;
	const T& y(void) const;
	const T& z(void) const;
	const T& w(void) const;
	const T* getArrPtr(void) const;

	void print(std::ostream& output);						//pouze k ladeni (vypisuje do output streamu informace o vektoru)
	inline void set_data(const T& x,const T& y,const T& z, const T& w);	/**< Nastavuje hodnoty slozek x,y,z,w teto tridy*/
	inline void set_data(const T& x,const T& y,const T& z );	/**< Nastavuje hodnoty slozek x,y,z teto tridy*/

	inline void set_zero(void);	//nastavi slozky x,y,z na hodnotu 0
};
template<typename T> inline void Base_3D_vector<T>::clampTo( const T from, const T to )
{
	this->arr[0] = max( min( this->arr[0], to), from );
	this->arr[1] = max( min( this->arr[0], to), from );
	this->arr[2] = max( min( this->arr[0], to), from );
}
template<typename T> inline void Base_3D_vector<T>::set_zero(void)	
{
	this->arr[0] = this->arr[1] = this->arr[2] = this->arr[3] = 0;
}
template<typename T> inline void Base_3D_vector<T>::set_data(const T &x,const T &y,const T &z)	
{
	this->arr[0] = x;
	this->arr[1] = y;
	this->arr[2] = z;
}
template<typename T> inline void Base_3D_vector<T>::set_data(const T &x,const T &y,const T &z, const T &w)	
{
	this->arr[0] = x;
	this->arr[1] = y;
	this->arr[2] = z;
	this->arr[3] = w;
}

template<typename T>  Base_3D_vector<T>& Base_3D_vector<T>::operator=( const Base_3D_vector &vec)
{
	this->arr[0] = vec.arr[0];
	this->arr[1] = vec.arr[1];
	this->arr[2] = vec.arr[2];
	this->arr[3] = vec.arr[3];

	return *this;
}
template<typename T> Base_3D_vector<T>::Base_3D_vector(void)	//konstruktor
{
	this->arr[0] = 0;
	this->arr[1] = 0;
	this->arr[2] = 0;
	this->arr[3] = 0;
}
template<typename T> Base_3D_vector<T>::Base_3D_vector(const T &x,const T &y,const T &z, const T &w)	//konstruktor
{
	this->arr[0] = x;
	this->arr[1] = y;
	this->arr[2] = z;
	this->arr[3] = w;
}
template<typename T> Base_3D_vector<T>::Base_3D_vector(const T *arr)	//konstruktor
{
	this->arr[0] = arr[0];
	this->arr[1] = arr[1];
	this->arr[2] = arr[2];
	this->arr[3] = arr[3];
}
template<typename T> Base_3D_vector<T>::Base_3D_vector( const Base_3D_vector<T> & vec) //copy constructor
{
	this->arr[0] = vec.arr[0];
	this->arr[1] = vec.arr[1];
	this->arr[2] = vec.arr[2];
	this->arr[3] = vec.arr[3];
}
template<typename T> Base_3D_vector<T>::~Base_3D_vector(void)
{
}

template<typename T> T& Base_3D_vector<T>::x(void)
{
	return this->arr[0];
}
template<typename T> T& Base_3D_vector<T>::y(void)
{
	return this->arr[1];
}
template<typename T> T& Base_3D_vector<T>::z(void)
{
	return this->arr[2];
}
template<typename T> T& Base_3D_vector<T>::w(void)
{
	return this->arr[3];
}
template<typename T> T* Base_3D_vector<T>::getArrPtr(void)
{
	return this->arr;
}

template<typename T> const T& Base_3D_vector<T>::x(void) const
{
	return this->arr[0];
}
template<typename T> const T& Base_3D_vector<T>::y(void) const
{
	return this->arr[1];
}
template<typename T> const T& Base_3D_vector<T>::z(void) const
{
	return this->arr[2];
}
template<typename T> const T& Base_3D_vector<T>::w(void) const
{
	return this->arr[3];
}
template<typename T> const T* Base_3D_vector<T>::getArrPtr(void) const
{
	return this->arr;
}
template<typename T> void Base_3D_vector<T>::print(std::ostream& output)
{
	output<<"Base_3D_vector je: ("<<this->arr[0]<<","<<this->arr[1]<<","<<this->arr[2]<<","<<this->arr[3]<<");"<<std::endl;
}

template<typename T>
class Vector_3;

template<typename T>
class Ray_3;


/// Represents point in 3d euclidian space.
/**
*	Trida reprezentujici bod v prostoru s vetsinou matematickych operaci ktere lze s bodem provadet.
**/
template<typename T>
class Point_3: public Base_3D_vector<T>
{
public:
	Point_3(const T& x,const T& y,const T& z);			/**< Konstruktor nastavujici hodnoty slozek x,y,z. */
	~Point_3(void);
	Point_3(void);
	//copy construktor defaultni staci


	inline void rotate_point_arround_ray( const Ray_3<T> &normalize_Ray_3, const T& angle);	/**< Orotuje bod kolem paprsku.  POZOR axis musi byt NORMALIZOVANA!!!*/
	inline Point_3<T> get_rotate_point_arround_ray( const Ray_3<T> &normalize_Ray_3, const T& angle);	/**< Vytvori novy bod, ktery orotuje bod kolem paprsku a vrati z funkce.  POZOR axis musi byt NORMALIZOVANA!!!*/

	inline void Point_3<T>::rotate_point( const Point_3<T> &pivot, const Vector_3<T> & normalize_axis,const T& angle);	

	inline Vector_3<T> operator-(const Point_3<T> &p1) const;	/**< Odecteni 2 bodu v prostoru vraci vektor */

	inline Point_3<T> operator+(const Vector_3<T>& v1) const; /**< Kdyz k bodu prictu vektor tak se bod posune ve smeru vektoru a vrati se pozice noveho bodu. */
	inline Point_3<T> operator-(const Vector_3<T>& v1) const; /**< Kdyz k bodu prictu vektor tak se bod posune ve smeru vektoru a vrati se pozice noveho bodu. */

	inline void operator+=(const Vector_3<T>& v1); /**< Kdyz k bodu prictu vektor tak se bod posune ve smeru vektoru a ulozi se pozice noveho bodu. */
	inline void operator-=(const Vector_3<T>& v1); /**< Kdyz k bodu prictu vektor tak se bod posune ve smeru vektoru a ulozi se pozice noveho bodu. */

	inline bool operator ==(const Point_3<T> &p1) const; /**<Porovnava slozky 2 bodu jestli jsou stejne a pokud ano tak vraci True jinak False. */
	inline bool operator !=(const Point_3<T> &p1) const;	/**< Vraci opak toho co vraci operator==. */


	inline T get_distance(const Point_3<T> &p1) const;	/**< Vraci vzdalenost 2 bodu. (Odecte body od sebe a vrati delku tohoto odecteneho vektoru) */
	inline T get_distance_square(const Point_3<T> &p1) const;	/**< Vraci druhou mocninu vzdalenosti 2 bodu. (Odecte body od sebe a vrati 2. mocninu delky tohoto odecteneho vektoru) */

};




template<typename T> inline void Point_3<T>::rotate_point_arround_ray( const Ray_3<T> &normalize_Ray_3,const T& angle)
{
	Vector_3<T> tmp_v=*this-normalize_Ray_3.pos;		//spocitam vektor
	T length=tmp_v.get_length();			//ulozim si delku vektoru

	tmp_v.rotate_around_vector(normalize_Ray_3.dir,angle);	//vektor orotuju kolem jineho vektoru (tim dojde k denormalizaci)

	if ( tmp_v != Vector_3<T>(0,0,0) )
	{
		tmp_v.normalize();										//normalizace vektoru
	}

	tmp_v*=length;											//nastavim vektoru jeho puvodni delku
	*this=normalize_Ray_3.pos+tmp_v;									//bod posunu ve smeru orotovaneho vektoru 
}

template<typename T> inline void Point_3<T>::rotate_point( const Point_3<T> &pivot, const Vector_3<T> & normalize_axis,const T& angle)
{
	return this->rotate_point_arround_ray( MAKE_RAY<T>( pivot, normalize_axis ), angle );
}

template<typename T> inline Point_3<T> get_rotate_point_arround_ray( const Ray_3<T> &normalize_Ray_3, const T& angle)
{
	Point_3<T> tmp_point(this);
	tmp_point.rotate_point_arround_ray(normalize_Ray_3,angle);

	return tmp_point;
}

template<typename T> inline T Point_3<T>::get_distance(const Point_3<T> &p1) const //pocita vzdalenost 2 bodu v EUKLIDOVSKYM prostoru
{
	T x=this->arr[0]-p1.arr[0];
	T y=this->arr[1]-p1.arr[1];
	T z=this->arr[2]-p1.arr[2];

	return sqrt(x*x+y*y+z*z);
}
template<typename T> inline T Point_3<T>::get_distance_square(const Point_3<T> &p1) const //pocita vzdalenost 2 bodu v EUKLIDOVSKYM prostoru
{
	T x=this->arr[0]-p1.arr[0];
	T y=this->arr[1]-p1.arr[1];
	T z=this->arr[2]-p1.arr[2];

	return x*x+y*y+z*z;
}

template<typename T> inline bool Point_3<T>::operator ==(const Point_3<T> &p1) const
{
	if (abs(this->arr[0]-p1.arr[0])<=INTERVAL_TOLERANCE  &&  abs(this->arr[1]-p1.arr[1])<=INTERVAL_TOLERANCE  && abs(this->arr[2]-p1.arr[2])<=INTERVAL_TOLERANCE  )	//pokud se vsechny slozky rovnaji tak vraci true (=rovnost)
	{
		return true;
	}

	return false;
}
template<typename T> inline bool Point_3<T>::operator !=(const Point_3<T> &p1) const
{
	return !(this->operator ==(v1));	//vraci opacnou hodnotu nez ==
}

template<typename T> inline Point_3<T> Point_3<T>::operator+(const Vector_3<T>& v1) const
{
	Point_3<T> return_point( this->arr[0]+v1.arr[0], this->arr[1]+v1.arr[1], this->arr[2]+v1.arr[2] );
	return return_point;
}
template<typename T> inline Point_3<T> Point_3<T>::operator-(const Vector_3<T>& v1) const
{
	Point_3<T> return_point( this->arr[0]-v1.arr[0], this->arr[1]-v1.arr[1], this->arr[2]-v1.arr[2] );
	return return_point;
}
template<typename T> inline void Point_3<T>::operator+=(const Vector_3<T>& v1)
{
	this->arr[0] += v1.arr[0];
	this->arr[1] += v1.arr[1];
	this->arr[2] += v1.arr[2];
}
template<typename T> inline void Point_3<T>::operator-=(const Vector_3<T>& v1)
{
	this->arr[0] -= v1.arr[0];
	this->arr[1] -= v1.arr[1];
	this->arr[2] -= v1.arr[2];
}

template<typename T> inline Vector_3<T> Point_3<T>::operator -(const Point_3<T> &p1) const
{
	Vector_3<T> return_vector( this->arr[0]-p1.arr[0], this->arr[1]-p1.arr[1], this->arr[2]-p1.arr[2] );
	return return_vector;
}
template<typename T> Point_3<T>::Point_3(void):Base_3D_vector(0,0,0,1.0)	//konstruktor
{
}
template<typename T> Point_3<T>::Point_3(const T &x,const T &y,const T &z):Base_3D_vector(x,y,z, 1.0 )	//konstruktor
{
	//vola konstruktor predka ktery nastavi vse potrebne
}
template<typename T> Point_3<T>::~Point_3(void)
{
	//vola destruktor predka ktery znici a nastavi vse potrebne
}

/// Represents vector in 3d euclidian space.
/**
*	Trida reprezentujici vektor v prostoru s vetsinou matematickych operaci ktere lze s vektorem provadet.
*	Pro zvyseni rychlosti si trida udrzuje informaci o delce vektoru a delce 2. mocniny vektoru (v pripadech kdy uzivatel nekdy drive nejakou z techto metod uz volal a nedoslo ke zneplatneni techto delek).
*	Tim mohou nektere vypocty provadet rychleji (tyka se predevsim opakovanych volani get_distance a get_distance_square aniz by doslo ke zmene delek vektoru).
*/
template<typename T>
class Vector_3 :public Base_3D_vector<T>
{   
public:

	Vector_3(void);					/**< Konstruktor nenastavujici zadne hodnoty slozkam x,y,z. */
	Vector_3(const T& x,const T& y,const T& z);			/**< Konstruktor nastavujici hodnoty slozek x,y,z.  */
	//Vector_3(const Vector_3<T> &v);  neni potreba...defaultni staci
	~Vector_3(void);

	inline bool operator ==(const Vector_3<T> &v1) const; /**<Porovnava slozky 2 vektoru jestli jsou stejne a pokud ano tak vraci True jinak False. */
	inline bool operator !=(const Vector_3<T> &v1) const;	/**< Vraci opak toho co vraci operator==. */

	inline void clampLengthTo( const T from, const T to ); /**< Clamp vector so that the vector length is in interval <from,to>. Ratio of x,y,z components stay the same */


	//vektorove operace (operace VEKTOR VEKTOR = VEKTOR)
	inline Vector_3<T> operator +(const Vector_3<T> &v1) const;	/**< Operator scitani po slozkach, vysledek vraci jako novy Vector */
	inline Vector_3<T> operator -(const Vector_3<T> &v1) const;	/**< Operator odecitani po slozkach, vysledek vraci jako novy Vector */
	inline Vector_3<T> operator *(const Vector_3<T> &v1) const;	/**< Operator nasobeni po slozkach, vysledek vraci jako novy Vector */
	inline Vector_3<T> operator /(const Vector_3<T> &v1) const;	/**< Operator deleni po slozkach, vysledek vraci jako novy Vector */

	inline void operator +=(const Vector_3<T> &v1);	/**< Operator pricte po slozkach k 1.Vectoru  hodnoty x,y,z 2.Vectoru*/ 
	inline void operator -=(const Vector_3<T> &v1);	/**< Operator odecte po slozkach k 1.Vectoru  hodnoty x,y,z 2.Vectoru*/ 
	inline void operator /=(const Vector_3<T> &v1);	/**< Operator vynasobi po slozkach k 1.Vectoru  hodnoty x,y,z 2.Vectoru*/ 
	inline void operator *=(const Vector_3<T> &v1);	/**< Operator vydeli po slozkach k 1.Vectoru  hodnoty x,y,z 2.Vectoru*/ 


	//vektorovo-skalarni operace (operace VEKTOR SKALAR = VEKTOR)......pozor na spatne pretezovani (neplest SKALARy a VEKTORY)
	inline Vector_3<T> operator *(const T &s) const;		/**< Operator vynasobi slozky x,y,z hodnotou s, vysledek vraci jako novy Vector */
	inline Vector_3<T> operator /(const T &s) const;		/**< Operator vydeli slozky x,y,z hodnotou s, vysledek vraci jako novy Vector  */
	inline Vector_3<T> operator +(const T &s) const;		/**< Operator pricte ke slozkam x,y,z hodnotu s, vysledek vraci jako novy Vector */
	inline Vector_3<T> operator -(const T &s) const;		/**< Operator odecte odecte x,y,z hodnotu s, vysledek vraci jako novy Vector  */

	inline void operator +=(const T &s);					/**< Operator pricte ke slozkam x,y,z hodnotu s a temito hodnotami nahradi slozky x,y,z tohoto Vectoru.*/ 
	inline void operator -=(const T &s);					/**< Operator odecte od slozek x,y,z hodnotu s a temito hodnotami nahradi slozky x,y,z tohoto Vectoru*/	
	inline void operator *=(const T &s);					/**< Operator vynasobi slozky x,y,z hodnotou s a temito hodnotami nahradi slozky x,y,z tohoto Vectoru*/	
	inline void operator /=(const T &s);					/**< Operator vydeli slozky x,y,z hodnotou s a temito hodnotami nahradi slozky x,y,z tohoto Vectoru*/

	inline Vector_3<T> operator -(void);					/**< Unarni operator negace (vyrobi vektor s opacnymi slozkami puvodniho vektoru. */
	
	inline T get_angle_Rad( Vector_3<T> &v1);	/**< Vraci velikost uhlu (v RADIANECH), ktery sviraji vektory. */
	inline T get_angle_Ang( Vector_3<T> &v1);	/**< Vraci velikost uhlu (ve STUPNICH), ktery sviraji vektory. */
	
	inline T get_angle_Ang_xy(Vector_3<T> &v1);	/**< Vraci velikost uhlu (ve STUPNICH), ktery sviraji vektory v rovine xy. */
	inline T get_angle_Ang_xz(Vector_3<T> &v1);	/**< Vraci velikost uhlu (ve STUPNICH), ktery sviraji vektory v rovine xz. */
	inline T get_angle_Ang_yz(Vector_3<T> &v1);	/**< Vraci velikost uhlu (ve STUPNICH), ktery sviraji vektory v rovine yz. */

	inline Vector_3<T>& normalize(void);					/**< Prevede vektor na normalizovany vektor (tj. delka vektoru =1). */ 
	inline Vector_3<T> get_normalized_vector(void) const;	/**< Vrati normalizovany vektor puvodniho vektoru. */

	inline T get_length(void) const;						/**< Vraci delku vektoru. */ 
	inline T get_length_square(void) const;				/**< Vraci druhou mocninu delky vektoru. */ //vraci neodmocnenou vzdalenost (odmocnovani je casove narocne a pro porovnavani vzdalenosti staci 2. mocnina vzdalenosti)  */
	
	inline T get_dot_product(const Vector_3<T> &v1) const;		/**<Vraci vysledek po skalarnim soucinu 2 vektoru. */

	
	inline Vector_3<T> getPerpendicularVector(void) const;
	
	inline Vector_3<T> get_cross_product(const Vector_3<T> &v2) const;	/**< Spocte vektorovej soucin 2 vektoru, vysledek vraci zpet*/
	inline Vector_3<T>& calculate_cross_product(const Vector_3<T> &v1,const Vector_3<T> &v2);	/**< Spocte vektorovej soucin this a vstupniho vektoru, vysledkem nahradi this */

	Vector_3<T>& rotate_around_vector(const Vector_3<T> &normalize_axis,const T& angle); /**< Orotuje a ulozi vektor kolem osy axis o uhel angle (ve stupnich). POZOR axis musi byt NORMALIZOVANA!!! Vysledny vektor je pote denormalizovany!!! Tedy pokud uzivatel potrebuje i velikost vektoru a na jen smer tak je treba normalizovat jeste!!!*/ ////Je virtualni aby potomek P_Vector_2 (Performance Vektor mohl tuto metodu mit upravenou pro svoje potreby - tato operace totiz zneplatnuje predpocitane delky vektoru). 
	inline Vector_3<T> get_rotated_vector(const Vector_3<T> &normalize_axis,const T& angle) const;	/**< Orotuje vektor kolem osy axis o uhel angle (ve stupnich) a vraci orotovany novy vektor.  POZOR axis musi byt NORMALIZOVANA!!! */
};


template<typename T> inline Vector_3<T> Vector_3<T>::getPerpendicularVector(void) const
{
	Vector_3<T> tmpVec; // some deterministic vector which is not linear to this vector

	if ( this->arr[1] != 0 && this->arr[2] != 0 )
	{
		tmpVec.set_data(1,0,0);
	}
	else
	{
		tmpVec.set_data(0, 1, 1 );
	}

	return this->get_cross_product( tmpVec );
}

template<typename T> inline Vector_3<T> Vector_3<T>::get_rotated_vector(const Vector_3<T> &normalize_axis,const T& angle) const
{
	Vector_3<T> tmp_v(this); //okopiruju vektor
	tmp_v.rotate_around_vector(normalize_axis,angle); 
	return tmp_v;
}


template<typename T> Vector_3<T>& Vector_3<T>::rotate_around_vector(const Vector_3<T> &normalize_axis,const T&  angle) //POZOR axis musi byt NORMALIZOVANA!!!
{
	//http://www.blitzbasic.com/Community/posts.php?topic=57616

	double alpha_Rad=(angle/180.0)*(DT_PI);		//prevod ze STUPNU na RADIANY

	T uu=normalize_axis.arr[0] * normalize_axis.arr[0];
	T vv=normalize_axis.arr[1] * normalize_axis.arr[1];
	T ww=normalize_axis.arr[2] * normalize_axis.arr[2];

	T ux=normalize_axis.arr[0] * this->arr[0];
	T uy=normalize_axis.arr[0] * this->arr[1];
	T uz=normalize_axis.arr[0] * this->arr[2];

	T vx=normalize_axis.arr[1] * this->arr[0];
	T vy=normalize_axis.arr[1] * this->arr[1];
	T vz=normalize_axis.arr[1] * this->arr[2];

	T wx=normalize_axis.arr[2] * this->arr[0];
	T wy=normalize_axis.arr[2] * this->arr[1];
	T wz=normalize_axis.arr[2] * this->arr[2];

	T sa=sin(alpha_Rad);
	T ca=cos(alpha_Rad);

	T ux_vy_wz=ux+vy+wz;

	this->arr[0]	=	normalize_axis.arr[0]	*	ux_vy_wz	+	(this->arr[0]	*	(vv+ww)	-	normalize_axis.arr[0]	*	(vy+wz))	* ca	+	(-wy+vz)	*sa;
	this->arr[1]	=	normalize_axis.arr[1]	*	ux_vy_wz	+	(this->arr[1]	*	(uu+ww)	-	normalize_axis.arr[1]	*	(ux+wz))	* ca	+	(wx-uz)		*sa;
	this->arr[2]	=	normalize_axis.arr[2]	*	ux_vy_wz	+	(this->arr[2]	*	(uu+vv)	-	normalize_axis.arr[2]	*	(ux+vy))	* ca	+	(-vx+uy)	*sa;

	return *this;
}
template<typename T>  inline Vector_3<T> Vector_3<T>::get_cross_product(const Vector_3<T> &v2) const
{
	Vector_3<T> tmp_v;
	tmp_v.calculate_cross_product(*this,v2);
	return tmp_v;
}
template<typename T>  inline Vector_3<T>&  Vector_3<T>::calculate_cross_product(const Vector_3<T> &v1,const Vector_3<T> &v2) //pozor pokud tady budou 2 rovnobezne vektory tak mam asi problem
{
	//vzorecek sestrojen dle navodu: (1,2,3) × (4,5,6) = ((2×6 - 3×5), (3×4 - 1×6), (1×5 - 2×4)) = (-3,6,-3).

	this->arr[0] = v1.arr[1] * v2.arr[2] - v1.arr[2] * v2.arr[1];
	this->arr[1] = v1.arr[2] * v2.arr[0] - v1.arr[0] * v2.arr[2];
	this->arr[2] = v1.arr[0] * v2.arr[1] - v1.arr[1] * v2.arr[0];

	return *this;
}
/*
		x1=ax*vx+ Bx
		y1=ay*vy+ By
		z1=az*vz+ Bz
*/






/**
*	Trida reprezentujici paprsek v prostoru. Paprsek je reprezentovan smerovym vektorem a pocatecni bod parsku.
*/
template <typename T>
class Ray_3
{
public:	
	Point_3<T> pos;	/**< Pozice kterou paprsek prochazi.*/ // interne se snim pocita jako s pocatecnim bodem paprsku	
	Vector_3<T> dir;	/**< Smerovy vektor paprsku.		*/

	inline Point_3<T> get_intersection(const Ray_3<T> &Ray_3) const;		/**< Spocita prusecik 2 parsku a vraci pozici pruniku (v pripade ze prunik neni tak vysledna pozice ma nedefinovane slozky pozice X,Y,Z ) */
	inline void set_data(const Point_3<T> &pos,const Vector_3<T> &dir);		/**< Nastavuje bod kterym paprsek prochazi a smerovy vektor paprsku */
	
	inline T get_distance_from_point(const Point_3<T> &point);	/**< Vraci vzdalenost paprsku od bodu v prostoru */ //magicka formule viz http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
	
	Ray_3(const Point_3<T> &pos,const Vector_3<T> &dir);					/**< Konstruktor paprsku */
	Ray_3(void);															/**< Defaultni konstruktor paprsku */
};



template <typename T> Ray_3<T>::Ray_3(void)
{
	//defaultni konstruktor zavola defaultni konstruktory Point_3 a Vector_3 coz je to co chci
}


template <typename T> Ray_3<T>::Ray_3(const Point_3<T> &pos,const Vector_3<T> &dir)
{
	this->pos=pos;
	this->dir=dir;
}
template <typename T> inline T Ray_3<T>::get_distance_from_point(const Point_3<T> &point)
{
	assert(this->dir.get_length()); //kontrola na deleni 0
	return (this->dir.get_cross_product(this->pos-point)).get_length()/this->dir.get_length();	//http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
}
template <typename T> inline void Ray_3<T>::set_data(const Point_3<T> &pos, const Vector_3<T> &dir)
{
	this->pos=pos;
	this->dir=dir;
}

template<typename T>		//T je float, double,...
class Segment_3
{
public:
	Point_3<T> point1; /**< Prvni bod ohranicujici usecku. */
	Point_3<T> point2; /**< Druhy bod ohranicujici usecku. */
	
	Segment_3(const Point_3<T> &point1,const Point_3<T> &point2); /**< Nastavci point1 a point2. */
	Segment_3(void);

	inline Vector_3<T> get_dir(void) const; /**< Spocita a vrati direction vector (point1-point2)/*/

	inline Point_3<T> get_intersection(const Ray_3<T> &Ray_3) const;	//vraci pozici pruniku 2 paprsku (v pripade ze prunik neni tak vysledna pozice ma nedefinovane slozky pozice X,Y,Z
	inline Point_3<T> get_intersection(const Segment_3<T> &absc) const;
	inline T get_distance_from_point(const Point_3<T> &point) const; //peknej trik jak resit http://geometryalgorithms.com/Archive/algorithm_0102/algorithm_0102.htm#dist_Point_to_Segment_3()...ale zda ze nefunkcni :D

	inline void set_data(const Point_3<T> &point1,const Point_3<T> &point2); /**< Nastavi pozice krajnich bodu usecky. */
};
template <typename T> inline Vector_3<T> Segment_3<T>::get_dir(void) const
{
	return this->point1-this->point2;
}
template <typename T> inline T Segment_3<T>::get_distance_from_point(const Point_3<T> &point) const	
{

	/*VLASTNI POSTUP...mozna pomalejsi ale zase se zda ze funguje...dyztak casem optimalizuju, bude li treba
		-algoritmus zalozen na porovnani vektoru (zjistim tak jestli se point nachazi nad useckou nebo mimo ni)
		*/

	T ang1=this->get_dir().get_angle_Ang(this->point1-point);
	T ang2=this->get_dir().get_angle_Ang(point-this->point2);

	if (ang1<=90.0 && ang2<=90.0) //prumet je mezi body...vzdalenost se tedy rovna vzdalenosti obecneho parsku od bodu
	{
		Ray_3<T> tmp_Ray_3(this->point1,this->get_dir());
		return tmp_Ray_3.get_distance_from_point(point);
	}

	if (ang1>90.0)
	{
		return this->point1.get_distance(point);
	}
	
	return this->point2.get_distance(point);
}

template<typename T> inline void Segment_3<T>::set_data(const Point_3<T> &point1,const Point_3<T> &point2)
{
	this->point1=point1;
	this->point2=point2;
}

template<typename T> inline Point_3<T> Segment_3<T>::get_intersection(const Ray_3<T> &Ray_3) const
{	
	Ray_3<T> tmp_Ray_3(this->point1,this->get_dir());
	Point_3<T> col =tmp_Ray_3.get_intersection(Ray_3);
	
	
		
	//pokud vyslo nejaky cislo (tzn doslo k protnuti primky s paprskem) tak otestuju zda slo o protnuti mezi 2 body urcujicimi primku
	double dis1 = this->point1.get_distance_square(col); //pocitat s druhymi mocninami vzdalenosti muzu taky...je to rychlejsi o odmocnovani a vysledek to neovlivnuje
	double dis3 = this->point1.get_distance_square(point2);

	if (dis1>dis3+INTERVAL_TOLERANCE) return EMPTY_VECTOR<T>(); //pokud je vzdalenost pruseciku od 1. bodu usecky vetsi nez delka cele usecky tak netreba pokracovat...ke kolizi na useckach nedoslo
	double dis2 = this->point2.get_distance_square(col);
	
	if (dis2>dis3+INTERVAL_TOLERANCE) return EMPTY_VECTOR<T>(); //pokud je vzdalenost pruseciku od 2. bodu usecky vetsi nez delka cele usecky tak netreba pokracovat...ke kolizi na useckach nedoslo
		
	//pokud se dostanu az sem tak sem overil vsechny podminky ktere prusecik na usecce musi splnovat a zadna podminka nic nevyvratila, tedy bod lezi skutecne na usecce
	return col;
}
template<typename T> inline Point_3<T> Segment_3<T>::get_intersection(const Segment_3<T> &absc) const
{	
	Ray_3<T> Ray_3(this->pos,this->dir); //slo by i pretypovat ale takto se mi zda prehlednejsi
	Vector_3<T> col =absc.get_intersection(Ray_3); //nyni sem ziskal prusecik poloprimky s useckou a ted pokud byl prusecik uznan jako platny tak zjistim zda je platny i kdyz poloprimku prevedu taky na usecku

	
		
	//pokud vyslo nejaky cislo (tzn doslo k protnuti primky s paprskem) tak otestuju zda slo o protnuti mezi 2 body urcujicimi primku
	double dis1 = this->point1.get_distance_square(col); //pocitat s druhymi mocninami vzdalenosti muzu taky...je to rychlejsi o odmocnovani a vysledek to neovlivnuje
	double dis3 = this->point1.get_distance_square(point2);

	if (dis1>dis3+INTERVAL_TOLERANCE) return EMPTY_VECTOR(); //pokud je vzdalenost pruseciku od 1. bodu usecky vetsi nez delka cele usecky tak netreba pokracovat...ke kolizi na useckach nedoslo
	double dis2 = this->point2.get_distance_square(col);
	
	if (dis2>dis3+INTERVAL_TOLERANCE) return EMPTY_VECTOR(); //pokud je vzdalenost pruseciku od 2. bodu usecky vetsi nez delka cele usecky tak netreba pokracovat...ke kolizi na useckach nedoslo
		
	//pokud se dostanu az sem tak sem overil vsechny podminky ktere prusecik na usecce musi splnovat a zadna podminka nic nevyvratila, tedy bod lezi skutecne na usecce
	return col;
}


template<typename T> Segment_3<T>::Segment_3(void)
{
	//zavola se default konstruktor Vector_3() na objekty dir,pos,pos2
}
template<typename T> Segment_3<T>::Segment_3(const Point_3<T> &point1,const Point_3<T> &point2)
{
	this->point1=point1;
	this->point2=point2;
}

template<typename T> inline void Vector_3<T>::clampLengthTo( const T from, const T to )
{
	T len = this->get_length();
	*this /= len; //normalize vector

	T clampedLen = max( min( len, to), from ); 
	*this *= clampedLen; //extend vector into given interval
}


template<typename T> inline Vector_3<T> Vector_3<T>::operator +(const Vector_3<T> &v1) const
{ 
	Vector_3<T> tmp_v(*this);
	tmp_v+=v1;
	return tmp_v;
}
template<typename T> inline Vector_3<T> Vector_3<T>::operator -(const Vector_3<T> &v1) const
{
	Vector_3<T> tmp_v(*this);
	tmp_v-=v1;
	return tmp_v;
}
template<typename T> inline Vector_3<T> Vector_3<T>::operator *(const Vector_3<T> &v1) const //pozor nejde o skalarni soucin ale vyslovene nasobeni vektoru po slozkach kdy vracim VEKTOR
{
	Vector_3<T> tmp_v(*this);
	tmp_v*=v1;
	return tmp_v;
}
template<typename T> inline Vector_3<T> Vector_3<T>::operator /(const Vector_3<T> &v1) const	//nekontroluje deleni 0, musi osetrit uzivatel!!!
{
	Vector_3<T> tmp_v(*this);
	tmp_v/=v1;
	return tmp_v;
}
template<typename T> inline void Vector_3<T>::operator +=(const Vector_3<T> &v1)
{
	this->arr[0]+= v1.arr[0];
	this->arr[1]+= v1.arr[1];
	this->arr[2]+= v1.arr[2];
}
template<typename T> inline void Vector_3<T>::operator -=(const Vector_3<T> &v1)
{
	this->arr[0]-= v1.arr[0];
	this->arr[1]-= v1.arr[1];
	this->arr[2]-= v1.arr[2];
}
template<typename T> inline void Vector_3<T>::operator *=(const Vector_3<T> &v1)	//nejde o skalarni soucin, vysledkem je VEKTOR
{
	this->arr[0]*= v1.arr[0];
	this->arr[1]*= v1.arr[1];
	this->arr[2]*= v1.arr[2];
}
template<typename T> inline void Vector_3<T>::operator /=(const Vector_3<T> &v1)	//nekontroluje deleni 0 (pouze v DEBUG modu pomoci assert)
{
	assert(!(v1.arr[0]==0 || v1.arr[1]==0 || v1.arr[2]==0));

	this->arr[0]/= v1.arr[0];
	this->arr[1]/= v1.arr[1];
	this->arr[2]/= v1.arr[2];
}
template<typename T> inline bool Vector_3<T>::operator ==(const Vector_3<T> &v1) const
{
	if (abs(this->arr[0]-v1.arr[0] )<INTERVAL_TOLERANCE  &&  abs( this->arr[1]-v1.arr[1] )<INTERVAL_TOLERANCE && abs( this->arr[2]-v1.arr[2] )<INTERVAL_TOLERANCE )	//pokud se vsechny slozky rovnaji tak vraci true (=rovnost)
	{
		return true;
	}
	
	return false;
}
template<typename T>  inline Vector_3<T> Vector_3<T>::operator -(void)
{
	Vector_3<T> return_vector(-this->x(), -this->y(), -this->z());
	return return_vector;
}
template<typename T> inline bool Vector_3<T>::operator !=(const Vector_3<T> &v1) const
{
	return !(this->operator ==(v1));	//vraci opacnou hodnotu nez ==
}
template<typename T> inline Vector_3<T> Vector_3<T>::operator +(const T &s) const
{
	Vector_3<T> tmp_v(*this);
	tmp_v+=s;
	return tmp_v;
}
template<typename T> inline void Vector_3<T>::operator +=(const T &s)
{
	this->arr[0] += s;
	this->arr[1] += s;
	this->arr[2] += s;
}
template<typename T> inline Vector_3<T> Vector_3<T>::operator -(const T &s) const
{
	//Vector_3<T> tmp_v(this->x-s,this->y-s,this->z-s);
	//tato verze by mela stacit
	Vector_3<T> tmp_v(*this);
	tmp_v-=s;
	return tmp_v;
}
template<typename T> inline void Vector_3<T>::operator -=(const T &s)
{
	this->arr[0] -= s;
	this->arr[1] -= s;
	this->arr[2] -= s;
}
template<typename T> inline Vector_3<T> Vector_3<T>::operator *(const T &s) const
{
	//Vector_3<T> tmp_v(this->x*s,this->y*s,this->z*s);
	//tato verze by mela stacit
	Vector_3<T> tmp_v(*this);
	tmp_v*=s;
	return tmp_v;
}
template<typename T> inline void Vector_3<T>::operator *=(const T &s)
{
	this->arr[0] *= s;
	this->arr[1] *= s;
	this->arr[2] *= s;

}
template<typename T> inline Vector_3<T> Vector_3<T>::operator /(const T &s) const
{
	Vector_3<T> tmp_v(*this);
	tmp_v/=s;

	return tmp_v;
}
template<typename T> inline void Vector_3<T>::operator /=(const T &s) //nekontroluje deleni 0 v release modu
{
	assert( s!=0); // deleni 0

	this->arr[0] /= s;
	this->arr[1] /= s;
	this->arr[2] /= s;
}




template<typename T> inline T Vector_3<T>::get_dot_product(const Vector_3<T> &v1) const
{
	return this->arr[0] * v1.arr[0] + this->arr[1] * v1.arr[1] + this->arr[2] * v1.arr[2];
}

template<typename T> inline T Vector_3<T>::get_length(void) const //vrati delku vektoru
{
	return sqrt( this->arr[0] * this->arr[0] + this->arr[1] * this->arr[1] + this->arr[2] * this->arr[2] );
}
template<typename T> inline T Vector_3<T>::get_length_square(void) const //vrati delku vektoru^2
{
	return this->arr[0] * this->arr[0] + this->arr[1] * this->arr[1] + this->arr[2] * this->arr[2]; //spocitam 2. mocninu delky vektoru a rovnou si ji ulozim
}
template<typename T> inline Vector_3<T>& Vector_3<T>::normalize(void) //normalizuje vektor (jednotkova delka vektoru)
{
	T tmp_res=this->get_length();
	assert(tmp_res && "Deleni 0 pri normalizaci vektoru!!!");	//kontrola na deleni 0;
	*this/=(tmp_res);
	
	return *this;
}
template<typename T> inline Vector_3<T> Vector_3<T>::get_normalized_vector(void) const
{
	Vector_3<T> tmp_vec(*this);
	tmp_vec.normalize();

	return tmp_vec;
}

template<typename T> inline T Vector_3<T>::get_angle_Ang( Vector_3<T> &v1)
{
	T dot_p =this->get_angle_Rad(v1);
	return (dot_p/(2*DT_PI))*360.0; //prepocet z radianu na uhly
}

template<typename T> inline T Vector_3<T>::get_angle_Ang_xy(Vector_3<T> &v1)
{
	Vector_3<T> vec1_xy( v1.x(), v1.y(), 0);
	Vector_3<T> vec2_xy( this->x(), this->y(), 0);

	if ( vec1_xy == MAKE_VECTOR<T>(0,0,0) ||  vec2_xy == MAKE_VECTOR<T>(0,0,0) )
	{
		//cannot calculate the angle
		return 0;
	}

	T dot_p =vec1_xy.get_angle_Rad( vec2_xy );
	return (dot_p/(2*DT_PI))*360.0; //prepocet z radianu na uhly
}
template<typename T> inline T Vector_3<T>::get_angle_Ang_xz(Vector_3<T> &v1)
{
	Vector_3<T> vec1_xz( v1.x(), 0, v1.z() );
	Vector_3<T> vec2_xz( this->x(), 0, this->z() );

	if ( vec1_xz == MAKE_VECTOR<T>(0,0,0) ||  vec2_xz == MAKE_VECTOR<T>(0,0,0) )
	{
		//cannot calculate the angle
		return 0;
	}

	T dot_p =vec1_xz.get_angle_Rad( vec2_xz );
	return (dot_p/(2*DT_PI))*360.0; //prepocet z radianu na uhly
}
template<typename T> inline T Vector_3<T>::get_angle_Ang_yz(Vector_3<T> &v1)
{
	Vector_3<T> vec1_yz( 0, v1.y(), v1.z() );
	Vector_3<T> vec2_yz( 0, this->y(), this->z() );

	if ( vec1_yz == MAKE_VECTOR<T>(0,0,0) ||  vec2_yz == MAKE_VECTOR<T>(0,0,0) )
	{
		//cannot calculate the angle
		return 0;
	}

	T dot_p = vec1_yz.get_angle_Rad( vec2_yz );
	return (dot_p/(2*DT_PI))*360.0; //prepocet z radianu na uhly
}

template<typename T> inline T Vector_3<T>::get_angle_Rad( Vector_3<T> &v1)
{
	T dot_p=this->get_dot_product(v1);

	T val = dot_p/(this->get_length() * v1.get_length() );
	clamp<T> ( -1.0, 1.0, val ); 
	return acos( val );  
}
	


template<typename T> Vector_3<T>::Vector_3(void):Base_3D_vector(0,0,0,0)	//konstruktor
{
}
template<typename T> Vector_3<T>::Vector_3(const T &x,const T &y,const T &z):Base_3D_vector(x,y,z,0)	//konstruktor
{
}



template<typename T> Vector_3<T>::~Vector_3(void) //destruktor
{
}


template<typename T> inline Point_3<T> Ray_3<T>::get_intersection(const Ray_3<T> &Ray_3)  const
{
	/* 2 paprsky v tomto tvaru lehce spocitam (bod kolize museji mit rovny, tedy jedina neznama je parametr t,l (t v 1. paprsku a l v 2. paprsku)
	X=xt+Ax
	Y=yt+Ay
	Z=zt+Az
	*/

	assert(this->dir.arr[0] * Ray_3.dir.arr[1] - this->dir.arr[1] * Ray_3.dir.arr[0] ); //kontrola na deleni nulou

	T l=( this->dir.arr[1] * ( Ray_3.pos.arr[0] - this->pos.arr[0] ) + this->dir.arr[0] * ( Ray_3.pos.arr[1] - this->pos.arr[1] )) / ( this->dir.arr[0] * Ray_3.dir.arr[1] - this->dir.arr[1] * Ray_3.dir.arr[0] );
	T t=( Ray_3.dir.arr[0] * l + Ray_3.pos.arr[0] ) / this->dir.arr[0];

	Point_3<T> col[2];
	col[0] = this->pos + this->dir * t;
	col[1] = Ray_3.pos + Ray_3.dir * l;

	if (abs(col[0]-col[1])<INTERVAL_TOLERANCE) return col[0]; //je potreba overit zda jsou vysledne pruseciky stejne ( rovnobezne paprsky nemaji pruseciky)

	Point_3<T> tmp_p;
	return tmp_p;
}


//==========================POMOCNE FUNKCE MIMO TRIDU=======================================
namespace GEOMETRY
{

template<typename T> inline Point_3<T> get_rotate_point_arround_ray(const Point_3<T>& src_point, const Ray_3<T> &normalize_Ray_3, const T& angle)	/**< Vytvori novy bod, ktery orotuje bod kolem paprsku a vrati z funkce.  POZOR axis musi byt NORMALIZOVANA!!!*/
{
	return src_point.get_rotate_point_arround_ray(normalize_Ray_3,angle);
}
template<typename T> inline T get_distance(const Point_3<T> &p1, const Point_3<T> &p2) 	/**< Vraci vzdalenost 2 bodu. (Odecte body od sebe a vrati delku tohoto odecteneho vektoru) */
{
	return p1.get_distance(p2);
}
template<typename T> inline T get_distance_square(const Point_3<T> &p1, const Point_3<T> &p2)	/**< Vraci druhou mocninu vzdalenosti 2 bodu. (Odecte body od sebe a vrati 2. mocninu delky tohoto odecteneho vektoru) */
{
	return p1.get_distance_square(p2);
}

template<typename T> inline T get_angle_Rad(const Vector_3<T> &v1, const Vector_3<T> &v2)	/**< Vraci velikost uhlu (v RADIANECH), ktery sviraji vektory. */
{
	return v1.get_angle_Rad(v2);
}

template<typename T> inline T get_angle_Ang(const Vector_3<T> &v1,  const Vector_3<T> &v2)	/**< Vraci velikost uhlu (ve STUPNICH), ktery sviraji vektory. */
{
	return v1.get_angle_Ang(v2);
}
template<typename T> inline void normalize(Vector_3<T> &vec)
{
	vec.normalize();
}

template<typename T> inline Vector_3<T> get_normalized_vector(const Vector_3<T> &vec)				/**< Prevede vektor na normalizovany vektor (tj. delka vektoru =1). */ 
{
	return vec.get_normalized_vector();
}

template<typename T> inline T get_length(const Vector_3<T> &vec)						/**< Vraci delku vektoru. */
{
	return vec.get_length();
}

template<typename T> inline T get_length_square(const Vector_3<T> &vec)				/**< Vraci druhou mocninu delky vektoru. */ //vraci neodmocnenou vzdalenost (odmocnovani je casove narocne a pro porovnavani vzdalenosti staci 2. mocnina vzdalenosti)  */
{
	return vec.get_length_square();
}

template<typename T> inline T get_dot_product(const Vector_3<T> &v1)		/**<Vraci vysledek po skalarnim soucinu 2 vektoru. */
{
	return v1.get_dot_product();
}

template<typename T> inline Vector_3<T> get_cross_product(const Vector_3<T> &v1, const Vector_3<T> &v2) /**< Spocte vektorovej soucin 2 vektoru, vysledek vraci zpet*/
{
	return v1.get_cross_product(v2);
}

template<typename T> void rotate_around_vector(Vector_3<T> &vec,const Vector_3<T> &normalize_axis,const T& angle) /**< Orotuje a ulozi vektor kolem osy axis o uhel angle (ve stupnich). POZOR axis musi byt NORMALIZOVANA!!! Vysledny vektor je pote denormalizovany!!! Tedy pokud uzivatel potrebuje i velikost vektoru a na jen smer tak je treba normalizovat jeste!!!*/ ////Je virtualni aby potomek P_Vector_2 (Performance Vektor mohl tuto metodu mit upravenou pro svoje potreby - tato operace totiz zneplatnuje predpocitane delky vektoru). 
{
	vec.rotate_around_vector(normalize_axis,angle);
}

template<typename T> inline Vector_3<T> get_rotated_vector(const Vector_3<T> &vec, const Vector_3<T> &normalize_axis,const T& angle)	/**< Orotuje vektor kolem osy axis o uhel angle (ve stupnich) a vraci orotovany novy vektor.  POZOR axis musi byt NORMALIZOVANA!!! */
{
	return vec.get_rotated_vector(normalize_axis,angle);
}




template <typename T>
inline Point_3<T> MAKE_POINT(T x, T y, T z) //pomocna funkce pro snadnejsi definovani hodnot vektoru 
{
	Point_3<T> tmp_p(x,y,z);
	return tmp_p;
}
template <typename T>
inline Point_3<T> ADD_POINTS(Point_3<T> point1, Point_3<T> point2)
{
	Point_3<T> tmp_p(point1.arr[0] + point2.arr[0], point1.arr[1] + point2.arr[1], point1.arr[2] + point2.arr[2] );
	return tmp_p;
}
template <typename T>
inline Vector_3<T> CONVERT_TO_VECTOR(Point_3<T> point1)
{
	Vector_3<T> tmp_v( point1.arr[0], point1.arr[1], point1.arr[2] );
	return tmp_v;
}
template <typename T>
inline Point_3<T> CONVERT_TO_POINT(Vector_3<T> vector1)
{
	Point_3<T> tmp_p( vector1.arr[0], vector1.arr[1], vector1.arr[2] );
	return tmp_p;
}
template <typename T>
inline Vector_3<T> MAKE_VECTOR(T x, T y, T z) //pomocna funkce pro snadnejsi definovani hodnot vektoru 
{
	Vector_3<T> tmp_v( x,y,z );
	return tmp_v;
}
template <typename T>
inline Vector_3<T> EMPTY_VECTOR(void) //pomocna funkce pro snadnejsi vraceni cisla
{
	Vector_3<T> tmp_v;
	return tmp_v;
}

template <typename T>
inline Ray_3<T> MAKE_RAY(const Point_3<T> &pos,const Vector_3<T> &dir) //pomocna funkce pro snadnejsi definovani hodnot vektoru 
{
	Ray_3<T> tmp_r(pos,dir);
	return tmp_r;
}

template <typename T>
inline Segment_3<T> MAKE_Segment_3(const Point_3<T> &point1,const Point_3<T> &point2) //pomocna funkce pro snadnejsi definovani hodnot vektoru 
{
	Segment_3<T> tmp_a(point1,point2);
	return tmp_a;
}


template <typename T>
inline Vector_3<double> CONVERT_TO_DOUBLE( const Vector_3<T> &vec ) 
{
	Vector_3<double> tmp_v( vec.x(), vec.y(), vec.z() );
	return tmp_v;
}

template <typename T>
inline Point_3<double> CONVERT_TO_DOUBLE( const Point_3<T> &point ) 
{
	Point_3<double> tmp_p( point.x(), point.y(), point.z() );
	return tmp_p;
}

template <typename T>
inline Vector_3<float> CONVERT_TO_FLOAT( const Vector_3<T> &vec ) 
{
	Vector_3<float> tmp_v( vec.x(), vec.y(), vec.z() );
	return tmp_v;
}

template <typename T>
inline Point_3<float> CONVERT_TO_FLOAT( const Point_3<T> &point ) 
{
	Point_3<float> tmp_p( point.x(), point.y(), point.z() );
	return tmp_p;
}

template <typename T>
inline Vector_3<long> CONVERT_TO_LONG( const Vector_3<T> &vec ) 
{
	Vector_3<long> tmp_v( vec.x(), vec.y(), vec.z() );
	return tmp_v;
}

template <typename T>
inline Point_3<long> CONVERT_TO_LONG( const Point_3<T> &point ) 
{
	Point_3<long> tmp_p( point.x(), point.y(), point.z() );
	return tmp_p;
}

template <typename T>
inline Vector_3<long long> CONVERT_TO_LONG_LONG( const Vector_3<T> &vec ) 
{
	Vector_3<long long> tmp_v( vec.x(), vec.y(), vec.z() );
	return tmp_v;
}

template <typename T>
inline Point_3<long long> CONVERT_TO_LONG_LONG( const Point_3<T> &point ) 
{
	Point_3<long long> tmp_p( point.x(), point.y(), point.z() );
	return tmp_p;
}

}
}
#endif