/*********************************************************************//**
*	Collection of mathematic algebra objects and functions.
*       Tato kolekce obsahuje velke mnozstvi objektu a funkci, ktere
 *      se pouzivaji pri popisu geometrickeho prostoru vyjadreneho pomoci
 *      kartezkeho souradneho systemu. Tento balik obsahuje objekty
 *      jako je napriklad Bod, Vektor, Primka, Usecka, Quaternion,
 *      BoundingBox a Rovina. Dale implementuje telesa slozena z techto,
 *      elementu, ktere se deli na Aktivni a pasivni. Aktivni lze 
 *      vyjadrit jako graf, ktery svou strukturou predstavuje kvadr (osm
 *      bodu vzajemne pospojovanych) a Pasivni jako konvexni objekt slozeny
 *      z nekolika rovin. Implementovane funkci pak dokazi provadet
 *      ruzne pruniky a dohromady tak vytvaret komplexni kolekci, jejiz hlavni
 *      funkci je detekce kolizi.
*                                                                        
*
*	author: Michal Jirous
*	date: 16.12.2008
*	file: algebraic.cpp
**********************************************************************/   

#include "algebraic.h"
#include <time.h>
#include <SDL/SDL_opengl.h>
#include "mathematic.h"
#include "textureslib.h"

using namespace std;




/**************************************
*
* Definice objektu Bod (Point)
*	
**************************************/

alg::Point::Point()
{
	memset( this, 0, sizeof( alg::Point ) );
}

/** @param p Bod, jehoz souradnice se zkopiruji do tohoto objektu. */
alg::Point::Point(const alg::Point &p)
{
	memcpy( this, &p, sizeof( alg::Point ) );
}

/** @param x Vstupni souradnice x, ktera se ulozi tomuto objektu.
 *  @param y Vstupni souradnice x, ktera se ulozi tomuto objektu.
 *  @param z Vstupni souradnice x, ktera se ulozi tomuto objektu.
 **/
alg::Point::Point(float x, float y, float z)
{
	this->x = x;
	this->y = y;
	this->z = z;
}

/** @param values Pole tri hodnot typu float, ktere se ulozi jako souradnice x, y, z. */
alg::Point::Point( float *values )
{
	memcpy( this,values,sizeof( alg::Point ) );
}

/** @param p Bod, se kterym se tento porovna.
 *  @return true, pokud se nerovnaji, jinak false.
 */
bool alg::Point::operator !=( const alg::Point &p ) const
{
	return !( operator==( p ) );
}

/** @param p Bod, se kterym se tento porovna.
 *  @return true, pokud se rovnaji, jinak false.
 */
bool alg::Point::operator ==( const alg::Point &p ) const
{
	if( !compareFloats( x, p.x, FLOAT_COMPARE_ACCURACY_LOW ) && !compareFloats( y, p.y, FLOAT_COMPARE_ACCURACY_LOW ) & !compareFloats( z, p.z, FLOAT_COMPARE_ACCURACY_LOW ) )
		return true;
	return false;
}

/** @param p Bod, se kterym se tento soucet.
 *  @return Vysledny soucet bodu (this + parametr )
 */
alg::Point alg::Point::operator +( const alg::Point &p ) const
{
	return alg::Point( x + p.x, y + p.y, z + p.z );
}

/** @param p Bod, se ma k tomuto pricit.
 *  @return Tento objekt po secteni s parametrem p.
 */
alg::Point & alg::Point::operator +=( const alg::Point &p )
{
	x += p.x;
	y += p.y;
	z += p.z;
	return *this;
}

/** @param p Bod, ktery se ma od tohoto odecist.
 *  @return Vysledny rozdil bodu (this - parametr )
 */
alg::Point alg::Point::operator -( const alg::Point &p ) const
{
	return alg::Point( x - p.x, y - p.y, z - p.z );
}

/** @param p Bod, ktery se ma od tohoto odecist.
 *  @return Tento objekt po provedeni rozdilu s parametrem p.
 */
alg::Point & alg::Point::operator -=( const alg::Point &p )
{
	x -= p.x;
	y -= p.y;
	z -= p.z;
	return *this;
}

/** @param p Bod, jehoz souradnice se zkopiruji do tohoto objetku.
  * @return Referenci na tento objekt.
 */
alg::Point & alg::Point::operator <<( const alg::Point &p )
{
	memcpy( this,&p, sizeof( alg::Point ) );
	return *this;
}

/** @param p Bod, jehoz souradnice se zkopiruji do tohoto objetku.
  * @return Referenci na tento objekt.
 */
alg::Point & alg::Point::operator =( const alg::Point &p )
{
	return operator<<(p);
}

/** @param values Pole cisel typu float, ktere se zkopiruji do tohoto objektu jako souradnice x, y, z.
  * @return Referenci na tento objekt.
 */
alg::Point & alg::Point::operator=( const float *values )
{
	memcpy( this,values,sizeof( alg::Point ) );
	return *this;
}

/** @brief i Index souradnice ( 0=x, 1=y, 2=z ).
 *  @return Referenci na souradnici dle vstupniho indexu i.
 */
float& alg::Point::operator [](int i) const
{
	return *(((float*)(this))+i);
}

/** @param p Bod, se kterym se ma provest skalarni soucin.
 *  @return Vysledny skalarni soucin dvou bodu (jako vektoru).
 */
inline float alg::Point::Dot( const alg::Point &p ) const
{
	return x * p.x + y * p.y + z * p.z;
}

alg::Point alg::Point::operator-(  ) const
{
	return Point( -x, -y, -z );
}

void alg::Point::clear()
{
	memset( this, 0, sizeof( *this ) );
}





/**************************************
* \    / |-- | / ---  /\   |-\
*  \  /  |-- |<   |  |  |  |-/
*   \/   |-- | \  |   \/   | \
**************************************/

/*************************************
 *
 *  Vektor
 *
 ************************************/

/** @param a Cilovy bod.
 *  @param b Pocatecni bod.
 */
alg::Vector::Vector( const alg::Point &a, const alg::Point &b)
{
	x = a.x - b.x;
	y = a.y - b.y;
	z = a.z - b.z;
}

/** @param v Vektor, jehoz orientace se zkopiruje.
 *  @param iLength Cilova delka vektoru.
 */
alg::Vector::Vector( const alg::Vector &v, float iLength)
{
	float absol = v.absolute();
	if( absol > -0.000001 && absol < 0.000001 )
	{
		Vector();
		return;
	}
	float k = iLength / v.absolute();
	x = v.x * k;
	y = v.y * k;
	z = v.z * k;
}

/** @param sValue Retezec se tremi cisly, ktere jsou oddelene mezerou.
 */
alg::Vector::Vector( std::string sValue )
{
	clear();
	size_t blankChar = sValue.find_first_of(' ');
	if( blankChar == string::npos )
		return;
	
	string first = sValue.substr(0, blankChar);
	x = stringToFloat( first );
	sValue = sValue.substr( blankChar + 1 );

	blankChar = sValue.find_first_of(' ');
	if( blankChar == string::npos )
		return;

	first = sValue.substr(0, blankChar);
	y = stringToFloat( first );
	sValue = sValue.substr( blankChar + 1 );
	
	
	z = stringToFloat( sValue );
}

/** @v Druhy operand vektoroveho soucinu.
 *  @return vysledny vektorovy soucin.
 */
alg::Vector alg::Vector::operator *( const alg::Vector &v ) const
{
	return alg::Vector(y * v.z - z * v.y,z * v.x - x * v.z, x * v.y - y * v.x);
}

/** @param fValue Konstanta, kterou se vynasobi vsechny slozky vektoru.
 *  @return Vysledny vektor po nasobeni.
 */
alg::Vector alg::Vector::operator *( const float fValue ) const
{
	return alg::Vector( x * fValue, y * fValue, z * fValue );
}

/*alg::Vector alg::operator*( const float &f, const alg::Vector &v )
{ 
	return v*f; 
}*/

/** @param fValue Konstanta, kterou se vydeli vsechny slozky vektoru.
 *  @return Vysledny vektor po deleni.
 */
alg::Vector alg::Vector::operator /(const float fValue) const
{
	return alg::Vector( x / fValue, y / fValue, z / fValue );
}

/** @param v Vektor, ktery se zkopiruje do tohoto.
 *  @return Reference na tento vetkor.
 */
alg::Vector & alg::Vector::operator =( const alg::Vector &v)
{
	*this << v;
	return *this;
}

/** @param values Pole tri hodnot v poradi x, y, z, ktere se zkopiruji do tohoto vektoru.
 *  @return Referenci na tento vektor.
 */
alg::Vector & alg::Vector::operator=( const float *values )
{
	((alg::Point*)(this))->operator =( values );
	return *this;
}

/** @param p Druhy operand scitani dvou vektoru.
 *  @return Vysledny vektor po souctu.
 */
alg::Vector alg::Vector::operator +(const alg::Vector &p) const
{
	return alg::Vector( x + p.x, y + p.y, z + p.z );
}

/** @param p Vektor, ktery se k tomuto pricte.
 *  @return Referenci na tento vektor.
 */
alg::Vector & alg::Vector::operator +=(const alg::Vector &p)
{
	x += p.x;
	y += p.y;
	z += p.z;
	return *this;
}

/** @param p Druhy operand rozdilu dvou vektoru.
 *  @return Vysledny vektor po odecteni.
 */
alg::Vector alg::Vector::operator -( const alg::Vector &p ) const
{
	return alg::Vector( x - p.x, y - p.y, z - p.z );
}

/** @param p Vektor, ktery se od tohoto odecte.
 *  @return Referenci na tento vektor.
 */
alg::Vector & alg::Vector::operator -=( const alg::Vector &p )
{
	x -= p.x;
	y -= p.y;
	z -= p.z;
	return *this;
}

/** @return invertovany vektor (vsechny slozky dostanou opacne znamenko.
 */
alg::Vector alg::Vector::operator-( void ) const
{
	return Vector( -x, -y, -z);
}

/** @return velikost vektoru. */
inline float alg::Vector::absolute() const
{
	return (static_cast<float>( sqrt((x*x) + (y*y) + (z*z))) );
}

/** @return Normalizovany vektor.
 */
alg::Vector & alg::Vector::normalize()
{
	float m = absolute();
	x /=m;
	y /=m;
	z /=m;
	return *this;
}

/** @param v Druhy operand skalarniho soucinu.
 *  @return Vysledny uhel, ktery oba vektory sviraji.
 */
float alg::Vector::scalarMultiply( const alg::Vector &v) const
{
	float m = absolute() * v.absolute();
	if( !compareFloats( m, 0.0f ) )
		return 0.0f;
	return myacos( multiply( v ) / m );
}

/** @param v Druhy operand skalarniho soucinu.
 *  @return Vysledny skalarni soucin.
 */
inline float alg::Vector::multiply( const alg::Point &v )  const
{
	return Dot( v );
}

/** @param x_axis_angle Uhel otoceni podle osy X (otoceni pri pohledu ve smeru nahoru-dolu).
 *  @param z_axis_angle Uhel otoceni podle osy Z (svisla) = otoceni pri pohledu do stran.
 *  @param distance Cilova velikost vektoru.
 *  @return Referenci na tento vektor.
 */
alg::Vector & alg::Vector::createFrom2Angles( float x_axis_angle, float z_axis_angle, float distance)
{
	const float size = distance * mycos(x_axis_angle);

	x = size * mycos(z_axis_angle);
	y = size * mysin(z_axis_angle);
	z = -distance * mysin(x_axis_angle);


	/*float t = distance * mycos(x_axis_angle);

	x = t * mycos(z_axis_angle);
	y = t * mysin(z_axis_angle);
	z = -distance * mysin(x_axis_angle);*/
	/*x = distance * mycos( z_axis_angle );
	y = distance * mysin( z_axis_angle );
	z = distance * mycos( x_axis_angle );
	z = z * mysin( x_axis_angle );*/

	return *this;
}

/** @param x_axis_angle Uhel otoceni podle osy X (otoceni pri pohledu ve smeru nahoru-dolu).
 *  @param distance Cilova velikost vektoru.
 *  @return Referenci na tento vektor.
 */
alg::Vector & alg::Vector::createFromAngle( float z_axis_angle, float distance )
{
	return createFrom2Angles( 0.0f, z_axis_angle, distance );
}







/**********************************************************
*
*	QUATERNION STRUCTURE
*
**********************************************************/

alg::Quaternion::Quaternion()
{
	memset( this, 0, sizeof( alg::Quaternion ) );
}

alg::Quaternion::Quaternion( const alg::Quaternion &q )
{
	memcpy( this, &q, sizeof( alg::Quaternion ) );
}

alg::Quaternion::Quaternion( float w, float x, float y, float z )
{
	this->w = w;
	this->x = x;
	this->y = y;
	this->z = z;
}

alg::Quaternion::Quaternion( float w, const alg::Vector &v )
{
	this->w = w;
	x = v.x;
	y = v.y;
	z = v.z;
}

void alg::Quaternion::normalize()
{
	float l = absolute();
	if( l > 0.0f )
	{
		x /= l;
		y /= l;
		z /= l;
		w /= l;
	}
}

void alg::Quaternion::create( float angle, const Vector &axis )
{
	create( angle, axis.x, axis.y, axis.z );
}

void alg::Quaternion::create( float angle, float axis_x, float axis_y, float axis_z )
{
	angle = angle * M_PI / 180.0f / 2.0f;
	float sin_a = sin( angle );
	
	x = sin_a * axis_x;
	y = sin_a * axis_y;
	z = sin_a * axis_z;
	w = cos( angle );
	normalize();
}

float alg::Quaternion::absolute() const
{
	return sqrt( w*w + x*x + y*y + z*z );
}

void alg::Quaternion::conjugate()
{
	x = -x;
	y = -y;
	z = -z;
}

void alg::Quaternion::invert()
{
	conjugate();
	float absolute = w*w + x*x + y*y + z*z;
	w /= absolute;
	x /= absolute;
	y /= absolute;
	z /= absolute;
}

Quaternion alg::Quaternion::operator+( const alg::Quaternion &q ) const
{
	Quaternion qn;
	qn.w = w + q.w;
	qn.x = x + q.x;
	qn.y = y + q.y;
	qn.z = z + q.z;
	return qn;
}

Quaternion alg::Quaternion::operator-( const alg::Quaternion &q ) const
{
	Quaternion qn;
	qn.w = w - q.w;
	qn.x = x - q.x;
	qn.y = y - q.y;
	qn.z = z - q.z;
	return qn;
}

Quaternion &alg::Quaternion::operator +=( const alg::Quaternion &q )
{
	w += q.w;
	x += q.x;
	y += q.y;
	z += q.z;
	return *this;
}

Quaternion &alg::Quaternion::operator -=( const alg::Quaternion &q )
{
	w -= q.w;
	x -= q.x;
	y -= q.y;
	z -= q.z;
	return *this;
}

Quaternion alg::Quaternion::operator*( float v ) const
{
	Quaternion q;
	q.w = w * v;
	q.x = x * v;
	q.y = y * v;
	q.z = z * v;
	return q;
}

Quaternion alg::Quaternion::operator/( float v ) const
{
	Quaternion q;
	q.w = w / v;
	q.x = x / v;
	q.y = y / v;
	q.z = z / v;
	return q;
}

Quaternion alg::Quaternion::operator*( const alg::Quaternion &q ) const
{
	//alg::Quaternion r;

	//r.w = w*q.w - x*q.x - y*q.y - z*q.z;
	//r.x = w*q.x + x*q.w + y*q.z - z*q.y;
	//r.y = w*q.y + y*q.w + z*q.x - x*q.z;
	//r.z = w*q.z + z*q.w + x*q.y - y*q.x;
	//return r;
	return Quaternion( w*q.w - x*q.x - y*q.y - z*q.z,
						w*q.x + x*q.w + y*q.z - z*q.y,
						w*q.y + y*q.w + z*q.x - x*q.z,
						w*q.z + z*q.w + x*q.y - y*q.x);
	
}

Quaternion alg::Quaternion::operator*( const Vector &q ) const
{
	alg::Quaternion r;

	r.w = -x*q.x - y*q.y - z*q.z;
	r.x = w*q.x + y*q.z - z*q.y;
	r.y = w*q.y + z*q.x - x*q.z;
	r.z = w*q.z + x*q.y - y*q.x;
	return r;
}


Quaternion alg::Quaternion::operator/( alg::Quaternion q ) const
{
	q.invert();
	return (operator*(q));
}



Vector alg::Quaternion::rotateVector( const Vector &v ) const
{
	alg::Quaternion q_c( w, -x, -y, -z );
	return Vector( &(( (*this) * v * q_c).x) );
}

void alg::Quaternion::getMatrix( float *matrix ) const
{
	float xx      = x * x;
    float xy      = x * y;
    float xz      = x * z;
    float xw      = x * w;

    float yy      = y * y;
    float yz      = y * z;
    float yw      = y * w;

    float zz      = z * z;
    float zw      = z * w;

    matrix[0]  = 1 - 2 * ( yy + zz );
    matrix[1]  =     2 * ( xy - zw );
    matrix[2]  =     2 * ( xz + yw );

    matrix[4]  =     2 * ( xy + zw );
    matrix[5]  = 1 - 2 * ( xx + zz );
    matrix[6]  =     2 * ( yz - xw );

    matrix[8]  =     2 * ( xz - yw );
    matrix[9]  =     2 * ( yz + xw );
    matrix[10] = 1 - 2 * ( xx + yy );

    matrix[3]  = matrix[7] = matrix[11] = matrix[12] = matrix[13] = matrix[14] = 0;
    matrix[15] = 1;
}









/*	Definition of class Line, which represents
*	single line in 3-dimensional space
*
*
*/

alg::Line::Line()
{
	alg::Point();
}

alg::Line::Line(const alg::Point &p, const alg::Vector &v)
{
	*this << p;
	m_vecDirection = v;
}

alg::Line::Line(alg::Point *p, const alg::Vector &v)
{
	*this << *p;
	m_vecDirection = v;
}

alg::Line::Line(const alg::Point &a, const alg::Point &b)
{
	*this << a;
	m_vecDirection = alg::Vector( a, b );
}

alg::Line::Line(alg::Point *a, alg::Point *b)
{
	*this << *a;
	m_vecDirection = alg::Vector( *a, *b );
}












/*	Definition of class BoundingBox
*	represents bounding box which is used in computer graphic
*	to rapidly accelerate collision detection algorithms
*	This object can represent any object like simple cube
*/

alg::BoundingBox::BoundingBox()
{
	clear();
}

alg::BoundingBox::BoundingBox( const alg::BoundingBox &b )
{
	setBounds( b );
}
		  
alg::BoundingBox::BoundingBox( const alg::Point &p )
{
	setBasicValues( p );
}

void alg::BoundingBox::clear()
{
	for( int i = 0; i < 6; i++ )
		m_fBounds[i] = 0.0f;
}

bool alg::BoundingBox::compare( const alg::BoundingBox &b )
{
	/*	funkce porovna dve struktury, na zaklade vysledku se s timto objektem
			vykonava nebo nevykonava pocitani koliznich bodu	
			slouzi k velice uzitecne filtraci objektu, se kterymi se vlastni vypocet provadi
		*/
		
		/*	pokud je maximum ze vsech x-ovych souradnic prvniho vetsi
			nez maximum ze vsech x-ovych souradnic druheho x */
		if( m_fBounds[MAX_X] > b.m_fBounds[MAX_X] )	
		{
			if(b.m_fBounds[MAX_X] < m_fBounds[MIN_X])	//porovna zda je maximum druheho mensi nez minimum prvniho
				return false;	//doslo se k zaveru, ze objekty nemohou mit shodnou x souradnici a tak nemuze dojit ke kolizi
		}
		else if(m_fBounds[MAX_X] < b.m_fBounds[MIN_X])	//porovna zda je maximum prvniho mensi nez minimum druheho
				return false;	//doslo se k zaveru, ze objekty nemohou mit shodnou x souradnici a tak nemuze dojit ke kolizi

		//ostatni pripady jsou stejne..jen pro jine souradnice (y,z)
		if( m_fBounds[MAX_Y] > b.m_fBounds[MAX_Y] )
		{
			if(b.m_fBounds[MAX_Y] < m_fBounds[MIN_Y])
				return false;
		}
		else if(m_fBounds[MAX_Y] < b.m_fBounds[MIN_Y])
				return false;
			
		if( m_fBounds[MAX_Z] > b.m_fBounds[MAX_Z] )
		{
			if(b.m_fBounds[MAX_Z] < m_fBounds[MIN_Z])
				return false;
		}
		else if(m_fBounds[MAX_Z] < b.m_fBounds[MIN_Z])
				return false;

	/*for( int i = 0; i < 3; i++ )
	{
		if( m_fBounds[ i*2 + 1] > b.m_fBounds[ i*2 + 1] )	
		{
			if(b.m_fBounds[i*2 + 1] < m_fBounds[i*2])	//porovna zda je maximum druheho mensi nez minimum prvniho
				return false;	//doslo se k zaveru, ze objekty nemohou mit shodnou x souradnici a tak nemuze dojit ke kolizi
		}
		else if(m_fBounds[i*2 + 1] < b.m_fBounds[i*2])	//porovna zda je maximum prvniho mensi nez minimum druheho
				return false;	//doslo se k zaveru, ze objekty nemohou mit shodnou x souradnici a tak nemuze dojit ke kolizi
	}*/



		return true;	//tim, ze vraci true znamena pouze to, ze objekty spolu mohou kolidovat, ovsem nemusi

}

void alg::BoundingBox::setBounds( const alg::BoundingBox &b )
{
	memcpy( this, &b, sizeof( alg::BoundingBox ) );
	//for( int i = 0; i < 6; i++ )
	//	m_fBounds[i] = b.m_fBounds[i];
}

void alg::BoundingBox::updateData( const alg::Point &p )
{
	if( p.x < m_fBounds[MIN_X] ) m_fBounds[MIN_X] = p.x;
	if( p.x > m_fBounds[MAX_X] ) m_fBounds[MAX_X] = p.x;
	if( p.y < m_fBounds[MIN_Y] ) m_fBounds[MIN_Y] = p.y;
	if( p.y > m_fBounds[MAX_Y] ) m_fBounds[MAX_Y] = p.y;
	if( p.z < m_fBounds[MIN_Z] ) m_fBounds[MIN_Z] = p.z;
	if( p.z > m_fBounds[MAX_Z] ) m_fBounds[MAX_Z] = p.z;
}

void alg::BoundingBox::updateData( const alg::BoundingBox &b)
{
	if( b.m_fBounds[MIN_X] < m_fBounds[MIN_X] ) m_fBounds[MIN_X] = b.m_fBounds[MIN_X];
	if( b.m_fBounds[MAX_X] > m_fBounds[MAX_X] ) m_fBounds[MAX_X] = b.m_fBounds[MAX_X];
	if( b.m_fBounds[MIN_Y] < m_fBounds[MIN_Y] ) m_fBounds[MIN_Y] = b.m_fBounds[MIN_Y];
	if( b.m_fBounds[MAX_Y] > m_fBounds[MAX_Y] ) m_fBounds[MAX_Y] = b.m_fBounds[MAX_Y];
	if( b.m_fBounds[MIN_Z] < m_fBounds[MIN_Z] ) m_fBounds[MIN_Z] = b.m_fBounds[MIN_Z];
	if( b.m_fBounds[MAX_Z] > m_fBounds[MAX_Z] ) m_fBounds[MAX_Z] = b.m_fBounds[MAX_Z];


}


void alg::BoundingBox::setRange( const float range )
{
	m_fBounds[MIN_X] -= range;
	m_fBounds[MAX_X] += range;
	m_fBounds[MIN_Y] -= range;
	m_fBounds[MAX_Y] += range;
	m_fBounds[MIN_Z] -= range;
	m_fBounds[MAX_Z] += range;
}

void alg::BoundingBox::setBasicValues( const alg::Point &p)
{
	m_fBounds[ alg::MIN_X ] = p.x;
	m_fBounds[ alg::MAX_X ] = p.x;
	m_fBounds[ alg::MIN_Y ] = p.y;
	m_fBounds[ alg::MAX_Y ] = p.y;
	m_fBounds[ alg::MIN_Z ] = p.z;
	m_fBounds[ alg::MAX_Z ] = p.z;
}

bool alg::BoundingBox::isPointInBoundingBox( const alg::Point &p) const
{
	if( m_fBounds[ alg::MIN_X ] > p.x + FLOAT_COMPARE_ACCURACY_AVERAGE
		|| m_fBounds[ alg::MAX_X ] < p.x - FLOAT_COMPARE_ACCURACY_AVERAGE )	return false;
	if( m_fBounds[ alg::MIN_Y ] > p.y + FLOAT_COMPARE_ACCURACY_AVERAGE
		|| m_fBounds[ alg::MAX_Y ] < p.y - FLOAT_COMPARE_ACCURACY_AVERAGE )	return false;
	if( m_fBounds[ alg::MIN_Z ] > p.z + FLOAT_COMPARE_ACCURACY_AVERAGE
		|| m_fBounds[ alg::MAX_Z ] < p.z - FLOAT_COMPARE_ACCURACY_AVERAGE )	return false;
	return true;
}

alg::Point alg::BoundingBox::getCenterPoint() const
{
	return alg::Point( (m_fBounds[ alg::MAX_X ] + m_fBounds[ alg::MIN_X ]) / 2.0f,
					(m_fBounds[ alg::MAX_Y ] + m_fBounds[ alg::MIN_Y ]) / 2.0f,
					(m_fBounds[ alg::MAX_Z ] + m_fBounds[ alg::MIN_Z ]) / 2.0f );
}

float alg::BoundingBox::getDiameter()
{
	return sqrt( pow( m_fBounds[ alg::MAX_X ] - m_fBounds[ alg::MIN_X ], 2.0f)
		+ pow( m_fBounds[ alg::MAX_Y ] - m_fBounds[ alg::MIN_Y ], 2.0f)
		+ pow( m_fBounds[ alg::MAX_Z ] - m_fBounds[ alg::MIN_Z ], 2.0f) );
		/*(alg::Vector( m_fBounds[ alg::BoundingBox::MAX_X ] - m_fBounds[ alg::BoundingBox::MIN_X ],
					m_fBounds[ alg::BoundingBox::MAX_Y ] - m_fBounds[ alg::BoundingBox::MIN_Y ],
					m_fBounds[ alg::BoundingBox::MAX_Z ] - m_fBounds[ alg::BoundingBox::MIN_Z ])).absolute();*/
}

void alg::BoundingBox::translate( const Vector &v )
{
	m_fBounds[MIN_X] += v.x;
	m_fBounds[MAX_X] += v.x;
	m_fBounds[MIN_Y] += v.y;
	m_fBounds[MAX_Y] += v.y;
	m_fBounds[MIN_Z] += v.z;
	m_fBounds[MAX_Z] += v.z;
}

bool alg::BoundingBox::lineIntersect(  const Vector &v, const Point &p, Vector &normal, float &distance )
{
	float Tn = MIN_FLOAT, Tf = MAX_FLOAT;

	if( v.x > 0.0f )
	{
		Tn = (m_fBounds[ MIN_X ] - p.x) / v.x;
		Tf = (m_fBounds[ MAX_X ] - p.x ) / v.x;
		normal.x = -1.0f;
	}
	else if( v.x < 0.0f )
	{
		Tn = (m_fBounds[ MAX_X ] - p.x) / v.x;
		Tf = (m_fBounds[ MIN_X ] - p.x ) / v.x;
		normal.x = 1.0f;
	}
	else if( m_fBounds[ MIN_X ] > p.x || m_fBounds[ MAX_X ] < p.x )//v.z == 0 
		return false;



	if( v.y > 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MIN_Y ] - p.y) / v.y );
		Tf = min( Tf, (m_fBounds[ MAX_Y ] - p.y ) / v.y );
		normal.y = -1.0f;
	}
	else if( v.y < 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MAX_Y ] - p.y) / v.y );
		Tf = min( Tf, (m_fBounds[ MIN_Y ] - p.y ) / v.y );
		normal.y = 1.0f;
	}
	else if( m_fBounds[ MIN_Y ] > p.y || m_fBounds[ MAX_Y ] < p.y )//v.z == 0 
		return false;



	
	if( v.z > 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MIN_Z ] - p.z) / v.z );
		Tf = min( Tf, (m_fBounds[ MAX_Z ] - p.z ) / v.z );
		normal.z = -1.0f;
	}
	else if( v.z < 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MAX_Z ] - p.z) / v.z );
		Tf = min( Tf, (m_fBounds[ MIN_Z ] - p.z ) / v.z );
		normal.z = 1.0f;
	}
	else if( m_fBounds[ MIN_Z ] > p.z || m_fBounds[ MAX_Z ] < p.z )//v.z == 0 
		return false;


		
	if( compareFloats(Tn, Tf ) == 1 )
		return false;
	else
	{
		distance = Tn;
		return true;
	}
}

bool alg::BoundingBox::lineIntersectDetectNormal( const Vector &v, const Point &p, Point &result, Vector &normal, int &hrana )
{
	float Tn = MIN_FLOAT, Tf = MAX_FLOAT;

	int ff = 0;
	if( v.x > 0.0f )
	{
		Tn = (m_fBounds[ MIN_X ] - p.x) / v.x;
		Tf = (m_fBounds[ MAX_X ] - p.x ) / v.x;
		normal = Vector( -1.0f, 0.0f, 0.0f);
		hrana = 1;
		ff++;
	}
	else if( v.x < 0.0f )
	{
		Tn = (m_fBounds[ MAX_X ] - p.x) / v.x;
		Tf = (m_fBounds[ MIN_X ] - p.x ) / v.x;
		normal = Vector( 1.0f, 0.0f, 0.0f);
		hrana = 1;
		ff++;
	}
	else if( m_fBounds[ MIN_X ] > p.x || m_fBounds[ MAX_X ] < p.x )//v.z == 0 
		return false;

	if( v.y > 0.0f )
	{
		float newTn = (m_fBounds[ MIN_Y ] - p.y) / v.y ;
		if( compareFloats( newTn, Tn ) == 0 )
		{
			ff++;	
			hrana |= 2;
		}
		else if( newTn > Tn )
		{
			Tn = newTn;
			hrana = 2;
			normal = Vector( 0.0f, -1.0f, 0.0f);
		}
		Tf = min( Tf, (m_fBounds[ MAX_Y ] - p.y ) / v.y );
		
	}
	else if( v.y < 0.0f )
	{
		float newTn = (m_fBounds[ MAX_Y ] - p.y) / v.y ;
		if( compareFloats( newTn, Tn ) == 0 )
		{
			ff++;
			hrana |= 2;
		}
		else if( newTn > Tn )
		{
			hrana = 2;
			Tn = newTn;
			normal = Vector( 0.0f, 1.0f, 0.0f);
		}
		Tf = min( Tf, (m_fBounds[ MIN_Y ] - p.y ) / v.y );
	}
	else if( m_fBounds[ MIN_Y ] > p.y || m_fBounds[ MAX_Y ] < p.y )//v.z == 0 
		return false;

	if( v.z > 0.0f )
	{
		float newTn = (m_fBounds[ MIN_Z ] - p.z) / v.z ;
		if( compareFloats( newTn, Tn ) == 0 )
		{
			ff++;
			hrana |= 4;
		}
		if( newTn > Tn )
		{
			Tn = newTn;
			hrana = 4;
			normal = Vector( 0.0f, 0.0f, -1.0f);
		}
		Tf = min( Tf, (m_fBounds[ MAX_Z ] - p.z ) / v.z );
	}
	else if( v.z < 0.0f )
	{
		float newTn = (m_fBounds[ MAX_Z ] - p.z) / v.z ;
		if( compareFloats( newTn, Tn ) == 0 )
		{
			ff++;
			hrana |= 4;
		}
		if( newTn > Tn )
		{
			Tn = newTn;
			hrana = 4;
			normal = Vector( 0.0f, 0.0f, 1.0f);
		}
		Tf = min( Tf, (m_fBounds[ MIN_Z ] - p.z ) / v.z );
		//normal.z = 1.0f;
	}
	else if( m_fBounds[ MIN_Z ] > p.z || m_fBounds[ MAX_Z ] < p.z )//v.z == 0 
		return false;
	
	//hrana = ff > 1;
		
	if( compareFloats(Tn, Tf ) == 1 )
		return false;
	else
	{
		result = p + v * Tn;
		return true;
	}

}

bool alg::BoundingBox::lineIntersect( const Vector &v, const Point &p, Point &result, float accuracy )
{//point.x = (line.x) + line.m_vecDirection[0]*t;

	float Tn = MIN_FLOAT, Tf = MAX_FLOAT;


	if( v.x > 0.0f )
	{
		Tn = (m_fBounds[ MIN_X ] - p.x) / v.x;
		Tf = (m_fBounds[ MAX_X ] - p.x ) / v.x;
	}
	else if( v.x < 0.0f )
	{
		Tn = (m_fBounds[ MAX_X ] - p.x) / v.x;
		Tf = (m_fBounds[ MIN_X ] - p.x ) / v.x;
	}
	else if( m_fBounds[ MIN_X ] > p.x || m_fBounds[ MAX_X ] < p.x )//v.z == 0 
		return false;

	if( v.y > 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MIN_Y ] - p.y) / v.y );
		Tf = min( Tf, (m_fBounds[ MAX_Y ] - p.y ) / v.y );
	}
	else if( v.y < 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MAX_Y ] - p.y) / v.y );
		Tf = min( Tf, (m_fBounds[ MIN_Y ] - p.y ) / v.y );
	}
	else if( m_fBounds[ MIN_Y ] > p.y || m_fBounds[ MAX_Y ] < p.y )//v.z == 0 
		return false;

	if( v.z > 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MIN_Z ] - p.z) / v.z );
		Tf = min( Tf, (m_fBounds[ MAX_Z ] - p.z ) / v.z );
	}
	else if( v.z < 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MAX_Z ] - p.z) / v.z );
		Tf = min( Tf, (m_fBounds[ MIN_Z ] - p.z ) / v.z );
	}
	else if( m_fBounds[ MIN_Z ] > p.z || m_fBounds[ MAX_Z ] < p.z )//v.z == 0 
		return false;
	
		
	if( compareFloats(Tn, Tf, accuracy ) == 1 )
		return false;
	else
	{
		if( this->isPointInBoundingBox( p ) )
			result = p + v * Tf;
		else
			result = p + v * Tn;
		return true;
	}
}




bool alg::BoundingBox::lineIntersectDistance( const Vector &v, const Point &p, Point &result, float &distance, float accuracy )
{
	float Tn = MIN_FLOAT, Tf = MAX_FLOAT;


	if( v.x > 0.0f )
	{
		Tn = (m_fBounds[ MIN_X ] - p.x) / v.x;
		Tf = (m_fBounds[ MAX_X ] - p.x ) / v.x;
	}
	else if( v.x < 0.0f )
	{
		Tn = (m_fBounds[ MAX_X ] - p.x) / v.x;
		Tf = (m_fBounds[ MIN_X ] - p.x ) / v.x;
	}
	else if( m_fBounds[ MIN_X ] > p.x || m_fBounds[ MAX_X ] < p.x )//v.z == 0 
		return false;

	if( v.y > 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MIN_Y ] - p.y) / v.y );
		Tf = min( Tf, (m_fBounds[ MAX_Y ] - p.y ) / v.y );
	}
	else if( v.y < 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MAX_Y ] - p.y) / v.y );
		Tf = min( Tf, (m_fBounds[ MIN_Y ] - p.y ) / v.y );
	}
	else if( m_fBounds[ MIN_Y ] > p.y || m_fBounds[ MAX_Y ] < p.y )//v.z == 0 
		return false;

	if( v.z > 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MIN_Z ] - p.z) / v.z );
		Tf = min( Tf, (m_fBounds[ MAX_Z ] - p.z ) / v.z );
	}
	else if( v.z < 0.0f )
	{
		Tn = max( Tn, (m_fBounds[ MAX_Z ] - p.z) / v.z );
		Tf = min( Tf, (m_fBounds[ MIN_Z ] - p.z ) / v.z );
	}
	else if( m_fBounds[ MIN_Z ] > p.z || m_fBounds[ MAX_Z ] < p.z )//v.z == 0 
		return false;
	
		
	if( compareFloats(Tn, Tf, accuracy ) == 1 )
		return false;
	else
	{
		if( this->isPointInBoundingBox( p ) )
		{
			distance = Tf;
			result = p + v * Tf;
		}
		else
		{
			distance = Tn;
			result = p + v * Tn;
		}


		return true;
	}





}




alg::LinkedPoint::LinkedPoint()
{
	m_iStatus = alg::LinkedPoint::UNUSED,
	m_bAllowed = false;
}

alg::LinkedPoint &alg::LinkedPoint::operator=( const alg::LinkedPoint &l)
{
	*this << l;
	return *this;
}










alg::Abscissa::Abscissa()
{
	m_iUsageIndex = 0;
	A = B = NULL;
}

alg::Abscissa::Abscissa(alg::Point *a, alg::Point *b)
{
	m_iUsageIndex = 0;
	A = a;
	B = b;
}











alg::Plane::Plane()
{
	m_fDistance = 0.0f;
}

alg::Plane::Plane(alg::Vector &v)
{
	m_fDistance = 0.0f;
	m_vecNormal = v;
}

alg::Plane::Plane(alg::Vector &v, float distance)
{
	m_fDistance = distance;
	m_vecNormal = v;
}

alg::Plane::Plane( alg::Vector &v, alg::Point &p )
{
	m_vecNormal = v;
	determineDistance(p);
}

void alg::Plane::determineDistance( const alg::Point &p)
{
	m_fDistance = ( -(p.x * m_vecNormal[0] + p.y * m_vecNormal[1] + p.z * m_vecNormal[2]));
}

//alg::LoadingPlane::LoadingPlane()
//{
//	m_fShiftX = m_fShiftY = m_fRotate = m_fScaleX = m_fScaleY = 0.0f;
//}
//
//
//alg::PasiveModelPlane::~PasiveModelPlane()
//{
//	if( m_iDlistID )
//		glDeleteLists( m_iDlistID, 1 );
//	if( m_pTexture )
//		textureLibrary.unloadTexture( m_pTexture->key );
//}

void alg::ActiveModelPlane::determineDistance()
{
	alg::Plane::determineDistance( *m_Points[0] );
}


//alg::LoadingBrush::~LoadingBrush()
//{
//	m_planes.clear();
////	m_lplanes.clear();
//}
//
//
//void alg::AbstractBrush::recalculateBoundingBox()
//{
//	list<alg::LinkedPoint>::iterator iter = m_points.begin();
//	//BasicLinkedList<alg::LinkedPoint>::BasicLink *tmpLink = m_points.root;
//	m_Bounds.clear();
//	if( iter != m_points.end() )
//		m_Bounds.setBasicValues( (*iter) );
//	while( iter != m_points.end() )
//	{
//		m_Bounds.updateData( (*iter) );
//		iter++;
//	}
//
//}
//
//bool alg::PasiveModelBrush::isPointInBrush( const alg::Point &point )
//{
//	for( list<alg::PasiveModelPlane>::iterator iter = m_polygons.begin(); iter != m_polygons.end(); iter++ )
//	{
//		if( !alg::isPointBelowOrOnPlane( *iter, point ) )
//			return false;
//	}
//
//	/*BasicLinkedList<alg::PasiveModelPlane>::BasicLink *tmpLink = m_polygons.root;
//	while( tmpLink != NULL )
//	{
//		if( !alg::isPointBelowOrOnPlane( tmpLink->pointer, point ) )
//			return false;
//		tmpLink = tmpLink->next;
//	}*/
//	return true;
//}

//bool alg::isPointInBrushLoadingVersion( alg::PasiveModelBrush &brush, alg::Point &point )
//{
//	for( list<alg::PasiveModelPlane>::iterator iter = brush.m_polygons.begin(); iter != brush.m_polygons.end(); iter++ )
//	{
//		if( !alg::isPointBelowOrOnPlaneLoadingVersion( *iter, point ) )
//			return false;
//	}
//
//
//	/*BasicLinkedList<alg::PasiveModelPlane>::BasicLink *tmpLink = brush.m_polygons.root;
//	while( tmpLink != NULL )
//	{
//		if( !alg::isPointBelowOrOnPlaneLoadingVersion( tmpLink->pointer, point ) )
//			return false;
//		tmpLink = tmpLink->next;
//	}*/
//	return true;
//}


//void alg::AbstractBrush::basicTranslate(alg::Vector &v)	//v je opacny smer pohybu
//{
//	m_Bounds.translate( -v );
//	//BasicLinkedList<alg::LinkedPoint>::BasicLink *tmpLink = m_points.root;
//
//	for( list<alg::LinkedPoint>::iterator iter = m_points.begin(); iter != m_points.end(); iter++ )
//	{
//		(*iter) << (*iter) - v;
//	}
//
//
//	/*while( tmpLink != NULL )
//	{
//		tmpLink->pointer << tmpLink->pointer - v;
//		tmpLink = tmpLink->next;
//	}*/
//}
//
//void alg::PasiveModelBrush::translate(alg::Vector &v)		//v je opacny smer pohybu
//{	
//	basicTranslate( v );
//		
//	//BasicLinkedList<alg::PasiveModelPlane>::BasicLink *tmpPlane = m_polygons.root;
//
//	for( list<alg::PasiveModelPlane>::iterator iter = m_polygons.begin(); iter != m_polygons.end(); iter++ )
//	{
//		(*iter).determineDistance();
//		(*iter).m_Bounds.translate( -v );
//	}
//
//
//	//while( tmpPlane != NULL )
//	//{
//	//	
//	//	tmpPlane = tmpPlane->next;
//	//}
//}







/*===========================================================
*
*	DYNAMIC MODEL
*
*==========================================================*/

DynamicModel::DynamicModel()
{
	memset( this, 0, sizeof(*this) );
}


void DynamicModel::updateStaticBoundingBox()
{
	Point *p = (Point*)&m_StaticBounds.m_fBounds;
	(p[0]) = m_Points[BLN];
	(p[1]) = m_Points[TRF];
}

void DynamicModel::updateAllBoundingBoxes()
{
	updateStaticBoundingBox();
	setDynamicBounds();
}

void DynamicModel::setDynamicBounds()
{
	//TODO
	m_DynamicBounds = m_StaticBounds;
	for( int i = 0; i < POINT_COUNT; ++i )
		m_DynamicBounds.updateData( m_Points[i] + m_vecMovement );
	m_DynamicBounds.setRange( 1 );
}

void DynamicModel::translateStatic( const Vector &movement )
{
	for( int i = 0; i < POINT_COUNT; ++i )
		m_Points[i] += movement;

	m_bottomCenterPoint += movement;
	m_middleCenterPoint += movement;
	m_topCenterPoint += movement;

	updateStaticBoundingBox();
}

void DynamicModel::translateAll( const Vector& movement )
{
	translateStatic( movement );
	for( int i = 0; i < m_PlanesCount; ++i )	//omg..
		m_Planes[i].determineDistance();	//body roviny sou nula...fix it ^^ = o radek vyse :D

	setDynamicBounds();
}

void DynamicModel::createLinkedBlock( const alg::Point &centerPoint, float height, float width )
{
	removeMovedModel();
//	m_MovedBrush.filteredStaticPlanes.deleteAll();

	//vytvorime novy collision model, 8 bodu na sebe navzajem ukazujicich
	LinkedPoint *A = &m_Points[BLN];
	LinkedPoint *B = &m_Points[BRN];
	LinkedPoint *C = &m_Points[BRF];
	LinkedPoint *D = &m_Points[BLF];
	
	LinkedPoint *E = &m_Points[TLN];
	LinkedPoint *F = &m_Points[TRN];
	LinkedPoint *G = &m_Points[TRF];
	LinkedPoint *H = &m_Points[TLF];


	A->m_Neightbours[0] = B;	B->m_Neightbours[0] = A;	C->m_Neightbours[0] = B;	D->m_Neightbours[0] = A;
	A->m_Neightbours[1] = D;	B->m_Neightbours[1] = C;	C->m_Neightbours[1] = D;	D->m_Neightbours[1] = C;
	A->m_Neightbours[2] = E;	B->m_Neightbours[2] = F;	C->m_Neightbours[2] = G;	D->m_Neightbours[2] = H;
	
	E->m_Neightbours[0] = F;	F->m_Neightbours[0] = E;	G->m_Neightbours[0] = F;	H->m_Neightbours[0] = E;
	E->m_Neightbours[1] = H;	F->m_Neightbours[1] = G;	G->m_Neightbours[1] = H;	H->m_Neightbours[1] = G;
	E->m_Neightbours[2] = A;	F->m_Neightbours[2] = B;	G->m_Neightbours[2] = C;	H->m_Neightbours[2] = D;
	
	A->x = centerPoint.x - width;	A->y = centerPoint.y - width;		A->z = centerPoint.z - height;
	B->x = centerPoint.x + width;	B->y = centerPoint.y - width;		B->z = centerPoint.z - height;
	C->x = centerPoint.x + width;	C->y = centerPoint.y + width;		C->z = centerPoint.z - height;
	D->x = centerPoint.x - width;	D->y = centerPoint.y + width;		D->z = centerPoint.z - height;
	
	E->x = centerPoint.x - width;	E->y = centerPoint.y - width;		E->z = centerPoint.z;
	F->x = centerPoint.x + width;	F->y = centerPoint.y - width;		F->z = centerPoint.z;
	G->x = centerPoint.x + width;	G->y = centerPoint.y + width;		G->z = centerPoint.z;
	H->x = centerPoint.x - width;	H->y = centerPoint.y + width;		H->z = centerPoint.z;
	
	A->direction = Vector(-1.0f, -1.0f, -1.0f);
	B->direction = Vector(1.0f, -1.0f, -1.0f);
	C->direction = Vector(1.0f,1.0f,-1.0f);
	D->direction = Vector(-1.0f, 1.0f, -1.0f);

	E->direction = Vector(-1.0f, -1.0f, 1.0f);
	F->direction = Vector(1.0f, -1.0f, 1.0f);
	G->direction = Vector(1.0f,1.0f,1.0f);
	H->direction = Vector(-1.0f, 1.0f, 1.0f);


	updateStaticBoundingBox();
	

	//inicializace funkcnich bodu
	m_bottomCenterPoint = centerPoint;
	m_bottomCenterPoint.z -= height;

	m_middleCenterPoint = centerPoint;
	m_middleCenterPoint.z -= height / 2.0f;
	
	m_topCenterPoint = centerPoint;
	
}

bool alg::DynamicModel::isPointIn( const alg::Point &point ) const
{
	return m_StaticBounds.isPointInBoundingBox( point );

}

void alg::DynamicModel::removeMovedModel()
{
	m_PlanesCount = 0;
}



bool alg::boundingBoxCollisionDetection( const BoundingBox &bounds, CollisionData &collData )
{

	Vector firstContact(0.0f, 0.0f, 0.0f);
	Vector lastContact(1.0f, 1.0f, 1.0f);

	//if( !collData.model.m_DynamicBounds.compare(bounds ) )
	//	return false;
	bool f = false;

	for( int i = 0; i < 3; ++i )
	{
		if( collData.model.m_vecMovement[i] < 0.0f && bounds.m_fBounds[i+3] < collData.model.m_StaticBounds.m_fBounds[i]+0.0001f )
		{
			firstContact[i] = (bounds.m_fBounds[i+3] - collData.model.m_StaticBounds.m_fBounds[i]) / collData.model.m_vecMovement[i];
			f = true;
		}
		else if( collData.model.m_vecMovement[i] > 0.0f && collData.model.m_StaticBounds.m_fBounds[i+3] < bounds.m_fBounds[i]+0.0001f )
		{
			firstContact[i] = (bounds.m_fBounds[i] - collData.model.m_StaticBounds.m_fBounds[i+3]) / collData.model.m_vecMovement[i];
			f = true;
		}
		else if( collData.model.m_StaticBounds.m_fBounds[i+3] < bounds.m_fBounds[i] || bounds.m_fBounds[i+3] < collData.model.m_StaticBounds.m_fBounds[i])
			return false;

		if( collData.model.m_vecMovement[i] < 0.0f && collData.model.m_StaticBounds.m_fBounds[i+3] > bounds.m_fBounds[i]  )
			lastContact[i] = (bounds.m_fBounds[i] - collData.model.m_StaticBounds.m_fBounds[i+3]) / collData.model.m_vecMovement[i];
		else if(collData.model.m_vecMovement[i] > 0.0f && bounds.m_fBounds[i+3] > collData.model.m_StaticBounds.m_fBounds[i])
			lastContact[i] = (bounds.m_fBounds[i+3] - collData.model.m_StaticBounds.m_fBounds[i]) / collData.model.m_vecMovement[i];
	}

	int maxEntry = 0;
	if( firstContact.y > firstContact.x )
		maxEntry = 1;
	if( firstContact.z > firstContact[maxEntry] )
		maxEntry = 2;


	float exit = min( lastContact.x, min(lastContact.y, lastContact.z) );


	if(!f || firstContact[maxEntry] > exit)
		return false;

	//entry je ten cas

	Vector newNormal;	//vypocitame normalu

	//nastavime spravny smer normaly
	if( collData.model.m_vecMovement[maxEntry] > 0 )
		newNormal[maxEntry] = -1;
	else
		newNormal[maxEntry] = 1;
	
	float dist = (collData.model.m_vecMovement * firstContact[maxEntry]).absolute();


	if( dist < collData.maxdistance )
			collData.highestPoint = max( collData.highestPoint, bounds.m_fBounds[MAX_Z] );

	if( dist < collData.distance )
	{
		collData.normal = newNormal;
		collData.distance = dist;
		return true;
	}

	return false;
}



bool alg::boundingBoxCollisionDetection2( const BoundingBox &bounds, CollisionData &collData )
{
	alg::BoundingBox futureBox = collData.model.m_StaticBounds;
	futureBox.translate( collData.model.m_vecMovement );	//vytvorime cilovou pozici bounding boxu pomoci pohyboveho vektoru

	//nyni vlastni algoritmus vypoctu
	//je treba detekovat, zda vubec dojde ke kolizi a za druhe zjistit normalovy vektor a kolizni bod

	//zjistime, zda dojde ke kolizi
	if( futureBox.compare( bounds ) )
	{
		Vector difference, intersect;

		//je dulezite pocitat se spravnymi stenami kvadru
		if( collData.model.m_vecMovement.x < 0 )	//zaporna hodnota pohybu v ose X
			difference.x = futureBox.m_fBounds[MIN_X] - bounds.m_fBounds[MAX_X];
		else
			difference.x = futureBox.m_fBounds[MAX_X] - bounds.m_fBounds[MIN_X];

		if( collData.model.m_vecMovement.y < 0 )	//zaporna hodnota pohybu v ose Y
			difference.y = futureBox.m_fBounds[MIN_Y] - bounds.m_fBounds[MAX_Y];
		else
			difference.y = futureBox.m_fBounds[MAX_Y] - bounds.m_fBounds[MIN_Y];

		if( collData.model.m_vecMovement.z < 0 )	//zaporna hodnota pohybu v ose Y
			difference.z = futureBox.m_fBounds[MIN_Z] - bounds.m_fBounds[MAX_Z];
		else
			difference.z = futureBox.m_fBounds[MAX_Z] - bounds.m_fBounds[MIN_Z];

		//nyni zname presne vzdalenosti hran mezi obema boxy a muzeme zjistit, kterou stenou pronikl dovnitr
		for( int i = 0; i < 3; i++ )
			intersect[i] = fabs( collData.model.m_vecMovement[i] / difference[i] );
		
		int maxIndex = 0;
		for( int i = 1; i < 3; i++ )
			if( intersect[i] > intersect[maxIndex] )
				maxIndex = i;

		Vector newNormal;	//vypocitame normalu

		//nastavime spravny smer normaly
		if( collData.model.m_vecMovement[maxIndex] > 0 )
			newNormal[maxIndex] = -1;
		else
			newNormal[maxIndex] = 1;

		float fCurrDistance = fabs( collData.model.m_vecMovement[maxIndex] ) - fabs(difference[maxIndex] );

		if( fCurrDistance < collData.maxdistance )
			collData.highestPoint = max( collData.highestPoint, bounds.m_fBounds[MAX_Z] );

		if( fCurrDistance < collData.distance )
		{
			collData.normal = newNormal;
			collData.distance = fCurrDistance;
			return true;
		}

	}

	return false;
}

bool alg::DynamicModel::collisionDetection(  CollisionData &collData  ) const
{
	return boundingBoxCollisionDetection( m_StaticBounds, collData );
}

void alg::DynamicModel::createMovedModel()
{
	createMovedModel( m_vecMovement );
}


Vector tmpMovement;
void alg::DynamicModel::createMovedModel( const alg::Vector &movement )
{
	tmpMovement = movement;
	removeMovedModel();
	
	setDynamicBounds();

	bool g_ALL_Z = false;
	bool g_ALL_X = false;
	bool g_ALL_Y = false;
	

	int index = 0;
	if( tmpMovement.z > 0.0f )
		index = 0;
	else if( tmpMovement.z < 0.0f )
		index = 4;	//prvni z bottom bodu
	else
	{
		index = 0;
		g_ALL_Z = true;
	}

	if( tmpMovement.y > 0.0f )
		index += 0;
	else if( tmpMovement.y < 0.0f )
		index += 2;	//pocet near/far
	else
		g_ALL_Y = true;

	if( tmpMovement.x > 0.0f )
		index += 0;
	else if( tmpMovement.x < 0.0f )
		index += 1;
	else
		g_ALL_X = true;

	//m_Points[index].m_bAllowed = false;

	LinkedPoint *pFirstAllowed = NULL;
	for( int i = 0; i < POINT_COUNT; ++i )
	{
		/* Nyni vyresetujeme hodnoty status kazdeho bodu na UNUSED */
		m_Points[i].m_iStatus = alg::LinkedPoint::UNUSED;
		m_Points[i].m_bAllowed = true;
	//			pFirstAllowed = &m_Points[i];
	}

	m_Points[index].m_bAllowed = false;
	if( g_ALL_Z )
	{
		m_Points[index+4].m_bAllowed = false;
		if( g_ALL_Y )
		{
			m_Points[index+2].m_bAllowed = false;
			m_Points[index+2+4].m_bAllowed = false;
		}
		else if( g_ALL_X )
		{
			m_Points[index+1].m_bAllowed = false;
			m_Points[index+1+4].m_bAllowed = false;
		}
	}
	else if( g_ALL_Y )
	{
		m_Points[index+2].m_bAllowed = false;
		if( g_ALL_X )
		{
			m_Points[index+1].m_bAllowed = false;
			m_Points[index+1+2].m_bAllowed = false;
		}
	}
	else if( g_ALL_X )
	{
		m_Points[index+1].m_bAllowed = false;
	}

	for( int i = 0; i < 3; ++i )
		if( m_Points[index].m_Neightbours[i]->m_bAllowed  )
		{
			pFirstAllowed = m_Points[index].m_Neightbours[i];
			break;
		}

	//	/* Zjistime, ktere bodu mohou tvorit se sousedy roviny. */
	//	float u = fabs( m_vecMovement.scalarMultiply( m_Points[i].direction ) );

	//	if( u < 144.73561 /*u < 125.26f*/ )	//uhel za kterym uz bod neni videt opacnym smerem pohybu
	//	{
	//		m_Points[i].m_bAllowed = true;
	//		if( !pFirstAllowed )
	//			pFirstAllowed = &m_Points[i];
	//	}
	//	else
	//		m_Points[i].m_bAllowed = false;
	//}

	/* Nyni vytvorime nove roviny protazeneho modelu */
	createNewPlanes( *pFirstAllowed );
}
#include <iostream>
void alg::DynamicModel::createNewPlanes( alg::LinkedPoint &point )
{
	if(point.m_iStatus != alg::LinkedPoint::UNUSED)
		return;
	
	if(!point.m_bAllowed)
	{
		point.m_iStatus = alg::LinkedPoint::CLOSED;
		return;
	}
	else
		point.m_iStatus = alg::LinkedPoint::OPEN;
	

	//ted musime projit vsechny sousedy

	for( int i = 0; i < MAX_NEIGHTBOURS; ++i )
	{
		LinkedPoint *neightbour = point.m_Neightbours[i];
		createNewPlanes( *neightbour );
		if( neightbour->m_iStatus != alg::LinkedPoint::CLOSED && point.m_iStatus != alg::LinkedPoint::CLOSED )	//nakonec vytvarime roviny z dvojice bodu, ktere nejsou CLOSED
		{
			ActiveModelPlane &plane = m_Planes[m_PlanesCount];
			//normalovy vektor vznikne vektorovym soucinem vektoru z 2 bodu a vektorem pohybu
			plane.m_vecNormal = tmpMovement * Vector( point, *neightbour );
			
			plane.m_Points[0] = &point;
			plane.m_Points[1] = neightbour;
			plane.determineDistance();			

				
			if( !compareFloats( plane.m_vecNormal[0], 0.0f) && !compareFloats( plane.m_vecNormal[1], 0.0f) && !compareFloats( plane.m_vecNormal[2], 0.0f ) )
				continue;

			m_PlanesCount++;
		}
	}

	point.m_iStatus = alg::LinkedPoint::CLOSED;
}


















/*	Set of algebraic funcions
*
*
*/


bool alg::isPointBelowOrOnPlane( alg::Plane &plane, const alg::Point &point )
{
	float x = plane.m_vecNormal[0]*point.x + plane.m_vecNormal[1]*point.y + plane.m_vecNormal[2]*point.z + plane.m_fDistance;
	if(x < 0.001f)
		return true;
	return false;
}




bool alg::isPointOnPlane( alg::Plane &plane, alg::Point &point )
{
	//rozliseni porovnavani je 2.0f -> je vysoke z duvodu velke nerpesnosti, ktera se zde muze vyskytnout
	if( !compareFloats( plane.m_vecNormal[0]*point.x + plane.m_vecNormal[1]*point.y + plane.m_vecNormal[2]*point.z + plane.m_fDistance, 0.0f, 2.0f) )
		return true;
	return false;
}

bool alg::isPointBelowPlane( alg::Plane &plane, alg::Point &point )
{
	float x = plane.m_vecNormal[0]*point.x + plane.m_vecNormal[1]*point.y + plane.m_vecNormal[2]*point.z + plane.m_fDistance;
	if(x < -0.2f)
		return true;
	return false;
}



bool alg::planePlanesIntersect( alg::Plane &a, alg::Plane &b, alg::Plane &c, alg::Point &point)
{
	//prevzaty kod
	float fDet;
	float MN[9] = { a.m_vecNormal[0], a.m_vecNormal[1], a.m_vecNormal[2], b.m_vecNormal[0], b.m_vecNormal[1], b.m_vecNormal[2], c.m_vecNormal[0], c.m_vecNormal[1], c.m_vecNormal[2] };
	float IMN[9] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
	float MD[3] = { a.m_fDistance, b.m_fDistance , c.m_fDistance };

	  IMN[0] = MN[4] * MN[8] - MN[5] * MN[7];	// ???? pocitame determinant? :))))
	  IMN[3] = -(MN[3] * MN[8] - MN[5] * MN[6]);
	  IMN[6] = MN[3] * MN[7] - MN[4] * MN[6];

	  fDet = MN[0] * IMN[0] + MN[1] * IMN[3] + MN[2] *IMN[6];

	  if(fDet < 0.00001f && fDet > -0.00001)
		   return false;

	  IMN[1] = -(MN[1] * MN[8] - MN[2] * MN[7]);
	  IMN[4] = MN[0] * MN[8] - MN[2] * MN[6];
	  IMN[7] = -(MN[0] * MN[7] - MN[1] * MN[6]);
	  IMN[2] = MN[1] * MN[5] - MN[2] * MN[4];
	  IMN[5] = -(MN[0] * MN[5] - MN[2] * MN[3]);
	  IMN[8] = MN[0] * MN[4] - MN[1] * MN[3];

	  fDet = 1.0f / fDet;

	  IMN[0] *= fDet;
	  IMN[1] *= fDet;
	  IMN[2] *= fDet;
	  IMN[3] *= fDet;
	  IMN[4] *= fDet;
	  IMN[5] *= fDet;
	  IMN[6] *= fDet;
	  IMN[7] *= fDet;
	  IMN[8] *= fDet;
	
	


	  point.x = IMN[0] * MD[0] + IMN[1] * MD[1] + IMN[2] * MD[2];
	  point.y = IMN[3] * MD[0] + IMN[4] * MD[1] + IMN[5] * MD[2];
	  point.z = IMN[6] * MD[0] + IMN[7] * MD[1] + IMN[8] * MD[2];
		
	  point.x *= -1;
	  point.y *= -1;
	  point.z *= -1;
	  
	  return true;
}


bool alg::planeLineIntersect( const alg::Line &line, const alg::Plane &plane, alg::Point &point )
{
	float angle = line.m_vecDirection.scalarMultiply( plane.m_vecNormal );
	if( compareFloats( angle, 90.0f, FLOAT_COMPARE_ACCURACY_PRECISION ) == 0 )//pokud jsou vektory na sebe kolme, tak k pruniku nedojde
		return false;

	float t = - ( plane.m_vecNormal[0]*(line.x) + plane.m_vecNormal[1]*(line.y) + plane.m_vecNormal[2]*(line.z) + plane.m_fDistance);
	t /= plane.m_vecNormal[0]*line.m_vecDirection[0] + plane.m_vecNormal[1]*line.m_vecDirection[1] + plane.m_vecNormal[2]*line.m_vecDirection[2];
		

		point.x = ((line.x) + line.m_vecDirection[0]*t);
		point.y = ((line.y) + line.m_vecDirection[1]*t);
		point.z = ((line.z) + line.m_vecDirection[2]*t);
	return true;
}

float alg::pointFromPlaneDistance( alg::Plane &plane, Point &point)
{
	return (plane.m_vecNormal[0]*point.x + plane.m_vecNormal[1]*point.y + plane.m_vecNormal[2]*point.z + plane.m_fDistance) / plane.m_vecNormal.absolute();
}








//short int g_iAbscissaUsageNumber = 0;
//
//bool alg::PasiveModelBrush::collisionDetection(alg::ActiveModelBrush &brush, alg::Vector &resultNormal, float &fDistance, float &fStairsMax)
//{
//	g_iAbscissaUsageNumber++;
//	//BasicLinkedList<alg::PasiveModelPlane*> m_polygonList;	//polygony, ktere projdou testem na 90 stupnu
//	alg::Vector vecMovement = brush.m_vecMove;
//	alg::Point tmpIntersectedPoint;
//	bool bIsColliding = false;
//
//	//Vector *move = NULL;
//	//list<point*> pointList;
////	Vector *finishedNormal = NULL;
////	float finishedLenght = m_fNormalLength;
//	bool isPointAllowed = true;
//	
//	float fMaxVectorSize = vecMovement.absolute()+1.0f;
//	
////	straightLine *pLine = NULL;	//pomocna primka
////	simplePoint *pIntersectedPoint = NULL;
////	plPoint *pPlayerPoint = NULL;
////	polygon *tmp = firstOfModel;	//zjistime polygony objektu
//	
//	//prochazime vsechny polygonu tohoto brushe
//	for( list<alg::PasiveModelPlane>::iterator iter_polygon = m_polygons.begin(); iter_polygon != m_polygons.end(); iter_polygon++ )
//	//for( BasicLinkedList<alg::PasiveModelPlane>::BasicLink *tmpPolygon = m_polygons.root; tmpPolygon != NULL; tmpPolygon = tmpPolygon->next )
//	{
//		PasiveModelPlane &tmpPolygon = (*iter_polygon);
//		//a musime otestovat, zda z timto polygonem lze kolidovat (test 90 stupnu )
//		if( fabs( (-vecMovement).scalarMultiply( tmpPolygon.m_vecNormal ) ) > 90.f ||
//			!tmpPolygon.m_Bounds.compare( brush.m_MovedBrush.m_Bounds ) )
//			continue;
//		
//		//nyni prochazime vsechny body brushe, pro ktery se pocita detekce kolizi ( hlavne hrac )
//		for( list<alg::LinkedPoint>::iterator iter_linkedPoint = brush.m_points.begin(); iter_linkedPoint != brush.m_points.end(); iter_linkedPoint++ )
//		//for( BasicLinkedList<alg::LinkedPoint>::BasicLink *tmpLinkedPoint = brush.m_points.root; tmpLinkedPoint != NULL; tmpLinkedPoint = tmpLinkedPoint->next )
//		{
//			LinkedPoint &tmpLinkedPoint = (*iter_linkedPoint);
//			//spocitame prunik primky (vznikle z bodu brushe a vektoru pohybu brushe) s polygonem tohoto Pasive brushe
//			if( planePointIntersect(alg::Line( tmpLinkedPoint, vecMovement), tmpPolygon, tmpIntersectedPoint ) )
//			{
//				//pokud doslo k pruniku, tak otestujeme, zda bod lezi v  brushy
//				if( //!tmpPolygon->pointer.m_Bounds.isPointInBoundingBox( tmpIntersectedPoint ) ||
//					!isPointInBrush( tmpIntersectedPoint ) )
//					continue;
//
//				float fTmpDistance = alg::Vector( tmpIntersectedPoint, tmpLinkedPoint).absolute();
//
//				if( fTmpDistance < fDistance )	//mame nove reseni
//				{
//					bIsColliding = true;
//					fDistance = fTmpDistance;
//					resultNormal = tmpPolygon.m_vecNormal;
//				}
//			}
//		}
//		//nyni nastane druha cast detekce kolizi = roviny posunuteho modelu s hranama tohoto pasive brushe
//
//		//takze prochazime vsechny usecky polygonu
//		//for( BasicLinkedList<alg::Abscissa*>::BasicLink *tmpAbscissa = tmpPolygon->pointer.m_abscissas.root; tmpAbscissa != NULL; tmpAbscissa = tmpAbscissa->next)
//		for( list<alg::Abscissa*>::iterator iter_abscissa = tmpPolygon.m_abscissas.begin(); iter_abscissa != tmpPolygon.m_abscissas.end(); iter_abscissa++ )
//		{
//			Abscissa *tmpAbscissa = (*iter_abscissa);
//			if( tmpAbscissa->m_iUsageIndex == g_iAbscissaUsageNumber )	//kazdou jen jednou
//				continue;
//
//			tmpAbscissa->m_iUsageIndex = g_iAbscissaUsageNumber;
//		
//			//for( BasicLinkedList<alg::ActiveModelPlane>::BasicLink *tmpActivePlane = brush.m_MovedBrush.m_planes.root; tmpActivePlane != NULL; tmpActivePlane = tmpActivePlane->next )
//			for( list<alg::ActiveModelPlane>::iterator iter_activePlane = brush.m_MovedBrush.m_planes.begin(); iter_activePlane != brush.m_MovedBrush.m_planes.end(); iter_activePlane++ )
//			{
//				ActiveModelPlane &tmpActivePlane = (*iter_activePlane);
//				alg::Line tmpAbscissaLine( tmpAbscissa->A, tmpAbscissa->B );
//				if( alg::planePointIntersect( tmpAbscissaLine, tmpActivePlane, tmpIntersectedPoint ) )
//				{
//					//a pokud dojde k pruniku
//					//tak otestujeme, zda lezi mezi body a tim padem v polygonu
//
//					alg::BoundingBox tmpBounds( *tmpAbscissa->A );
//					tmpBounds.updateData( *tmpAbscissa->B );
//					if( tmpBounds.isPointInBoundingBox( tmpIntersectedPoint ) )
//					{
//						Point collisionPoint;
//
//						
//	
//						alg::BoundingBox test = brush.m_Bounds;
//						test.setRange( ALG_COLLISION_DETECT_2ND_PHASE_BOUNDS_RANGE );
//						
//						bool isColliding = false;
//						if( test.isPointInBoundingBox( tmpIntersectedPoint ) )
//						{
//							isColliding = true;
//							collisionPoint = tmpIntersectedPoint;
//						}
//						else if( test.lineIntersect( -vecMovement, tmpIntersectedPoint, collisionPoint ) )
//							isColliding = true;
//						
//						
//						
//						if( isColliding )
//						{
//							/*float maximum = max( abs(vecMovement.x), max( abs(vecMovement.y), abs(vecMovement.z) ) );
//							Vector additionalDistance = vecMovement * (ALG_COLLISION_DETECT_2ND_PHASE_BOUNDS_RANGE / maximum);*/
//
//							float fTmpDistance = Vector( collisionPoint - tmpIntersectedPoint).absolute() /*+ ALG_COLLISION_DETECT_2ND_PHASE_BOUNDS_RANGE*2.0f*/;
//
//							float angle = alg::Vector( tmpAbscissaLine.m_vecDirection[0], tmpAbscissaLine.m_vecDirection[1], 0.0f ).scalarMultiply(tmpAbscissaLine.m_vecDirection );
//							
//							if( fabs( angle ) < ALG_STAIRS_DETECT_ANGLE && fTmpDistance < brush.m_vecMove.absolute()   )
//							{
//								fStairsMax = max( fStairsMax, tmpIntersectedPoint.z );
//							}
//							angle = alg::Vector(tmpAbscissaLine.m_vecDirection[0], tmpAbscissaLine.m_vecDirection[1], 0.0f ).scalarMultiply(tmpAbscissaLine.m_vecDirection );
//							if( fTmpDistance < fDistance )	//mame nove reseni
//							{
//								
//
//									//Z obou bodu usecky vytvorime bounding box
//								alg::BoundingBox b1( *tmpActivePlane.m_points.front() );
//								alg::BoundingBox b2( *tmpActivePlane.m_points.back() );
//								
//								b1.setRange( 0.2f );	//nastavime jim prislusnou velikost
//								b2.setRange( 0.2f );
//								
//								b1.updateData( b2 );	//a spojime dohromady
//
//								//takze...sou body, ktere vzniknou pri pruniku a nemusi to byt z hrany, takze
//								//to musime otestovat
//								if( b1.isPointInBoundingBox( collisionPoint ) )
//								{
//									fDistance = fTmpDistance;
//									bIsColliding = true;
//									alg::Vector planeLineVector(*tmpActivePlane.m_points.front(), *tmpActivePlane.m_points.back() );	//vector primky hrany nalezici brushy
//									alg::Vector normalVector = planeLineVector * tmpAbscissaLine.m_vecDirection;
//								
//									if( fabs(normalVector.scalarMultiply(vecMovement)) < 90.0f)
//										normalVector= -normalVector;
//									normalVector.normalize();
//									resultNormal = normalVector;
//								}
//
//								
//							}
//							//angle = alg::Vector(tmpAbscissaLine.m_vecDirection[0], tmpAbscissaLine.m_vecDirection[1], 0.0f ).scalarMultiply(tmpAbscissaLine.m_vecDirection );
//						}
//						//brush.m_Bounds.lineIntersect( -vecMovement, tmpIntersectedPoint, collisionPoint );
//					}
//				}
//	
//			}
//		}
//	}
//
//	//posledni cast, kdy se testuje prunik primky z tohoto pasive brushe s brushem
//	/*for( BasicLinkedList<ActiveModelPlane*>::BasicLink *tmpActivePlane = brush.m_MovedBrush.filteredStaticPlanes.root;tmpActivePlane != NULL; tmpActivePlane = tmpActivePlane->next )
//	{*/
//		
//		int hrana = 0;
//		Vector normalVector;
//		
//		//pro vsechny body pasivniho brushe
//		//for( BasicLinkedList<alg::LinkedPoint>::BasicLink *tmpLinkedPoint = m_points.root; tmpLinkedPoint != NULL; tmpLinkedPoint = tmpLinkedPoint->next )
//		for( list<alg::LinkedPoint>::iterator iter_linkedPoint = m_points.begin(); iter_linkedPoint != m_points.end(); iter_linkedPoint++ )
//		{
//			//udelame prunik s activnim modelem
//			LinkedPoint &tmpLinkedPoint = (*iter_linkedPoint);
//			if( brush.m_Bounds.lineIntersectDetectNormal( -vecMovement, tmpLinkedPoint, tmpIntersectedPoint, normalVector, hrana )
//				/*alg::planePointIntersect( alg::Line(tmpLinkedPoint->pointer,vecMovement),*tmpActivePlane->pointer,tmpIntersectedPoint )*/ )		
//			{
//				/*if( brush.isPointInBrush( tmpIntersectedPoint ) )
//				{*/
//					
//					float fTmpDistance = alg::Vector( tmpLinkedPoint, tmpIntersectedPoint).absolute();
//					//brush.m_Bounds.lineIntersect( vecMovement, tmpLinkedPoint->pointer, tmpIntersectedPoint );
//					
//					if( fTmpDistance < brush.m_vecMove.absolute() )
//					{
//						fStairsMax = max( fStairsMax, tmpIntersectedPoint.z );
//					}
//
//					if( fTmpDistance < fDistance )	//mame nove reseni
//					{
//						//brush.m_Bounds.lineIntersectDetectNormal( -vecMovement, tmpLinkedPoint->pointer, tmpIntersectedPoint, normalVector, hrana );
//						bIsColliding = true;
//						//brush.m_Bounds.lineIntersectDetectNormal( -vecMovement, tmpLinkedPoint->pointer, tmpIntersectedPoint, normalVector, hrana );
//						fDistance = fTmpDistance;
//						resultNormal = -normalVector;
//					}
//				/*}*/
//			}
//		}
//
//
//
//	/*}*/
//	return bIsColliding;
//}

alg::Vector alg::mirrorVectorByPlane( alg::Point &target, alg::Vector &planeNormal )
{
	float distance = planeNormal.multiply( target ) / planeNormal.absolute();
	return ( target - Vector( planeNormal, distance ) * 2 );
}

float alg::get360VectorsAngle( Vector &start, Vector &target )
{
	Vector plane;
	if( start == Vector( 0,0,1) )
		plane = Vector( 1, 0, 0) * start;
	else
		plane = Vector( 0,0,1) * start;
	plane = start * plane;

	float angle = start.scalarMultiply( target );

	float x = plane[0]*target.x + plane[1]*target.y + plane[2]*target.z;
	if( x < 0 )	//znamena, ze je pod = zaporne
		angle = 360 -  angle;
	return angle;
}






Vector alg::rotateVector( float angle, float axis_x, float axis_y, float axis_z, Vector &q )
{
	Quaternion qt;
	qt.create( angle, axis_x, axis_y, axis_z );
	return qt.rotateVector( q );
}


int alg::isPointInPolygon( const Point &target, const Point *points, const Plane &plane, const int point_count )
{
	int max = fabs(plane.m_vecNormal.x) < fabs(plane.m_vecNormal.y) ? 1 : 0; 
    max = fabs(plane.m_vecNormal[max]) < fabs(plane.m_vecNormal.z) ? 2 : max;

	//souradnice Z je maximalni ( projekce X,Y )
    if( max == 2 )
    {
        int i, j, c = 0;
        for (i = 0, j = point_count - 1; i < point_count; j = i++)
        {
             if ( ((points[i].y > target.y) != ( points[j].y > target.y )) &&
                 (target.x < (points[j].x-points[i].x) * (target.y-points[i].y) / (points[j].y-points[i].y) + points[i].x) )
            c = !c;
        }
        return c;
    }
	else if( max == 1 )	//Y je maximalni ( projekce X,Z )
	{
		int i, j, c = 0;
        for (i = 0, j = point_count - 1; i < point_count; j = i++)
        {
             if ( ((points[i].z > target.z) != ( points[j].z > target.z )) &&
                 (target.x < (points[j].x-points[i].x) * (target.z-points[i].z) / (points[j].z-points[i].z) + points[i].x) )
            c = !c;
        }
        return c;
	}
	else	//X je maximalni ( projekce Y,Z )
	{
		int i, j, c = 0;
        for (i = 0, j = point_count - 1; i < point_count; j = i++)
        {
             if ( ((points[i].z > target.z) != ( points[j].z > target.z )) &&
                 (target.y < (points[j].y-points[i].y) * (target.z-points[i].z) / (points[j].z-points[i].z) + points[i].y) )
            c = !c;
        }
        return c;
	}

}


/***************************************************
 *  UNIT TESTS
 **************************************************/    

bool AlgUnitTest::completeTest()
{
    bool b = AlgPointUnitTest();
	return b;
}

bool AlgUnitTest::AlgPointUnitTest()
{
    alg::Point a = Point( 1,2,3 );
    alg::Point b = a;
    alg::Point c(b);
    alg::Point d( Point(2,4,5) );
    alg::Point e = d << b;
    if( e != b )
        return false;
    e -= Point( 2,5,6);
    if( e != Point( -1,-3,-3) )
        return false;
    return true;
}