/*
Zaawansowana kamera 3D
Autor: Adam Sawicki - sawickiap@poczta.onet.pl - http://regedit.gamedev.pl/

Kodowanie Windows-1250, koniec wiersza CR+LF, test: Zażółć gęślą jaźń
Licencja: Public Domain.
*/
#include "Camera.hpp"
#include <limits>

#undef min
#undef max

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Kod pomocniczy

static const float MINFLOAT = std::numeric_limits<float>::min();
static const float MAXFLOAT = std::numeric_limits<float>::max();

static const float PI_2 = D3DX_PI * 0.5f;
static const float PI_X_2 = D3DX_PI * 2.0f;

const BOX BOX::ZERO = BOX(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);

// Uniwersalny, brakujący w C++ operator dosłownego rzutowania (reintepretacji)
template <typename destT, typename srcT>
destT &absolute_cast(srcT &v)
{
	return reinterpret_cast<destT&>(v);
}
template <typename destT, typename srcT>
const destT &absolute_cast(const srcT &v)
{
	return reinterpret_cast<const destT&>(v);
}

// Liczba zmiennoprzecinkowa bliska zeru
// O dziwo to makro zamienione na funkcję inline działa wolniej - nigdy nie ufaj
// optymalizacji kompilatora!
#define FLOAT_ALMOST_ZERO(F) ((absolute_cast<unsigned>(F) & 0x7f800000L) == 0)

// Zwraca część całkowitą liczby wraz ze znakiem
inline float trunc(float x)
{
	return (x < 0.0f) ? ceilf(x) : floorf(x);
}
inline double trunc(double x)
{
	return (x < 0.0) ? ceil(x) : floor(x);
}

// Zwraca część ułamkową liczby wraz ze znakiem
inline float frac(float x)
{
	return x - trunc(x);
}

void SphericalToCartesian(D3DXVECTOR3 *Out, float Yaw, float Pitch, float R)
{
	float sy = sinf(Yaw), cy = cosf(Yaw);
	float sp = sinf(Pitch), cp = cosf(Pitch);

	Out->x = R * cp * cy;
	Out->y = R * sp;
	Out->z = - R * cp * sy;
}

float NormalizeAngle(float angle)
{
	angle /= PI_X_2;
	angle = frac(angle);
	if (angle < 0.0f)
		angle += 1.0f;
	else if (angle >= 1.0f)
		angle -= 1.0f;
	return angle * PI_X_2;
}

// Get the intersection point of three planes.
//                    -1
// |x|     | n1.x n1.y n1.z |     |d1|
// |y|  =  | n2.x n2.y n2.z |  X  |d2|
// |z|     | n3.x n3.y n3.z |     |d3|
bool Intersect3Planes(const D3DXPLANE &P1, const D3DXPLANE &P2, const D3DXPLANE &P3, D3DXVECTOR3 *OutP)
{
	// Inny algorytm (albo inaczej zapisany) jest też w książce "3D Math Primer for Graphics and Game Development", str. 286.

	float fDet;
    float MN[9]  = { P1.a, P1.b, P1.c, P2.a, P2.b, P2.c, P3.a, P3.b, P3.c };
    float IMN[9] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
    float MD[3]  = { -P1.d, -P2.d , -P3.d };

    IMN[0] =   MN[4] * MN[8] - MN[5] * MN[7];
    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(FLOAT_ALMOST_ZERO(fDet))
        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;

    OutP->x = IMN[0] * MD[0] + IMN[1] * MD[1] + IMN[2] * MD[2];
    OutP->y = IMN[3] * MD[0] + IMN[4] * MD[1] + IMN[5] * MD[2];
    OutP->z = IMN[6] * MD[0] + IMN[7] * MD[1] + IMN[8] * MD[2];

    return true;
}

inline void Min(D3DXVECTOR3 *Out, const D3DXVECTOR3 &v1, const D3DXVECTOR3 &v2)
{
	Out->x = Min(v1.x, v2.x);
	Out->y = Min(v1.y, v2.y);
	Out->z = Min(v1.z, v2.z);
}

inline void Max(D3DXVECTOR3 *Out, const D3DXVECTOR3 &v1, const D3DXVECTOR3 &v2)
{
	Out->x = Max(v1.x, v2.x);
	Out->y = Max(v1.y, v2.y);
	Out->z = Max(v1.z, v2.z);
}

inline float LengthSq(const D3DXVECTOR3 &v)
{
	return v.x*v.x + v.y*v.y + v.z*v.z;
}

inline float Length(const D3DXVECTOR3 &v)
{
	return sqrtf(LengthSq(v));
}

inline float Distance(const D3DXVECTOR3 &p1, const D3DXVECTOR3 &p2)
{
	return Length(p2-p1);
}


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// FRUSTUM

void FRUSTUM_PLANES::Set(const D3DXMATRIX &WorldViewProj)
{
	// Left clipping plane
	Planes[0].a = WorldViewProj._14 + WorldViewProj._11;
	Planes[0].b = WorldViewProj._24 + WorldViewProj._21;
	Planes[0].c = WorldViewProj._34 + WorldViewProj._31;
	Planes[0].d = WorldViewProj._44 + WorldViewProj._41;
	// Right clipping plane
	Planes[1].a = WorldViewProj._14 - WorldViewProj._11;
	Planes[1].b = WorldViewProj._24 - WorldViewProj._21;
	Planes[1].c = WorldViewProj._34 - WorldViewProj._31;
	Planes[1].d = WorldViewProj._44 - WorldViewProj._41;
	// Top clipping plane
	Planes[2].a = WorldViewProj._14 - WorldViewProj._12;
	Planes[2].b = WorldViewProj._24 - WorldViewProj._22;
	Planes[2].c = WorldViewProj._34 - WorldViewProj._32;
	Planes[2].d = WorldViewProj._44 - WorldViewProj._42;
	// Bottom clipping plane
	Planes[3].a = WorldViewProj._14 + WorldViewProj._12;
	Planes[3].b = WorldViewProj._24 + WorldViewProj._22;
	Planes[3].c = WorldViewProj._34 + WorldViewProj._32;
	Planes[3].d = WorldViewProj._44 + WorldViewProj._42;
	// Near clipping plane
	Planes[4].a = WorldViewProj._13;
	Planes[4].b = WorldViewProj._23;
	Planes[4].c = WorldViewProj._33;
	Planes[4].d = WorldViewProj._43;
	// Far clipping plane
	Planes[5].a = WorldViewProj._14 - WorldViewProj._13;
	Planes[5].b = WorldViewProj._24 - WorldViewProj._23;
	Planes[5].c = WorldViewProj._34 - WorldViewProj._33;
	Planes[5].d = WorldViewProj._44 - WorldViewProj._43;
}

void FRUSTUM_PLANES::Set(const FRUSTUM_POINTS &FrustumPoints)
{
	D3DXPlaneFromPoints(&Planes[PLANE_NEAR],
		&FrustumPoints[FRUSTUM_POINTS::NEAR_RIGHT_BOTTOM],
		&FrustumPoints[FRUSTUM_POINTS::NEAR_RIGHT_TOP],
		&FrustumPoints[FRUSTUM_POINTS::NEAR_LEFT_TOP]);
	D3DXPlaneFromPoints(&Planes[PLANE_FAR],
		&FrustumPoints[FRUSTUM_POINTS::FAR_LEFT_BOTTOM],
		&FrustumPoints[FRUSTUM_POINTS::FAR_LEFT_TOP],
		&FrustumPoints[FRUSTUM_POINTS::FAR_RIGHT_TOP]);
	D3DXPlaneFromPoints(&Planes[PLANE_LEFT],
		&FrustumPoints[FRUSTUM_POINTS::NEAR_LEFT_BOTTOM],
		&FrustumPoints[FRUSTUM_POINTS::NEAR_LEFT_TOP],
		&FrustumPoints[FRUSTUM_POINTS::FAR_LEFT_TOP]);
	D3DXPlaneFromPoints(&Planes[PLANE_RIGHT],
		&FrustumPoints[FRUSTUM_POINTS::FAR_RIGHT_BOTTOM],
		&FrustumPoints[FRUSTUM_POINTS::FAR_RIGHT_TOP],
		&FrustumPoints[FRUSTUM_POINTS::NEAR_RIGHT_TOP]);
	D3DXPlaneFromPoints(&Planes[PLANE_BOTTOM],
		&FrustumPoints[FRUSTUM_POINTS::NEAR_LEFT_BOTTOM],
		&FrustumPoints[FRUSTUM_POINTS::FAR_LEFT_BOTTOM],
		&FrustumPoints[FRUSTUM_POINTS::FAR_RIGHT_BOTTOM]);
	D3DXPlaneFromPoints(&Planes[PLANE_TOP],
		&FrustumPoints[FRUSTUM_POINTS::FAR_LEFT_TOP],
		&FrustumPoints[FRUSTUM_POINTS::NEAR_LEFT_TOP],
		&FrustumPoints[FRUSTUM_POINTS::NEAR_RIGHT_TOP]);
}

void FRUSTUM_PLANES::Normalize()
{
	D3DXPlaneNormalize(&Planes[0], &Planes[0]);
	D3DXPlaneNormalize(&Planes[1], &Planes[1]);
	D3DXPlaneNormalize(&Planes[2], &Planes[2]);
	D3DXPlaneNormalize(&Planes[3], &Planes[3]);
	D3DXPlaneNormalize(&Planes[4], &Planes[4]);
	D3DXPlaneNormalize(&Planes[5], &Planes[5]);
}

void FRUSTUM_POINTS::Set(const FRUSTUM_PLANES &FrustumPlanes)
{
	Intersect3Planes(FrustumPlanes.Planes[4], FrustumPlanes.Planes[0], FrustumPlanes.Planes[3], &Points[0]);
	Intersect3Planes(FrustumPlanes.Planes[4], FrustumPlanes.Planes[1], FrustumPlanes.Planes[3], &Points[1]);
	Intersect3Planes(FrustumPlanes.Planes[4], FrustumPlanes.Planes[0], FrustumPlanes.Planes[2], &Points[2]);
	Intersect3Planes(FrustumPlanes.Planes[4], FrustumPlanes.Planes[1], FrustumPlanes.Planes[2], &Points[3]);
	Intersect3Planes(FrustumPlanes.Planes[5], FrustumPlanes.Planes[0], FrustumPlanes.Planes[3], &Points[4]);
	Intersect3Planes(FrustumPlanes.Planes[5], FrustumPlanes.Planes[1], FrustumPlanes.Planes[3], &Points[5]);
	Intersect3Planes(FrustumPlanes.Planes[5], FrustumPlanes.Planes[0], FrustumPlanes.Planes[2], &Points[6]);
	Intersect3Planes(FrustumPlanes.Planes[5], FrustumPlanes.Planes[1], FrustumPlanes.Planes[2], &Points[7]);
}

void FRUSTUM_POINTS::Set(const D3DXMATRIX &WorldViewProjInv)
{
	D3DXVECTOR3 P[] = {
		D3DXVECTOR3(-1.f, -1.f, 0.f), D3DXVECTOR3(+1.f, -1.f, 0.f),
		D3DXVECTOR3(-1.f, +1.f, 0.f), D3DXVECTOR3(+1.f, +1.f, 0.f),
		D3DXVECTOR3(-1.f, -1.f, 1.f), D3DXVECTOR3(+1.f, -1.f, 1.f),
		D3DXVECTOR3(-1.f, +1.f, 1.f), D3DXVECTOR3(+1.f, +1.f, 1.f) };

	D3DXVec3TransformCoord(&Points[0], &P[0], &WorldViewProjInv);
	D3DXVec3TransformCoord(&Points[1], &P[1], &WorldViewProjInv);
	D3DXVec3TransformCoord(&Points[2], &P[2], &WorldViewProjInv);
	D3DXVec3TransformCoord(&Points[3], &P[3], &WorldViewProjInv);
	D3DXVec3TransformCoord(&Points[4], &P[4], &WorldViewProjInv);
	D3DXVec3TransformCoord(&Points[5], &P[5], &WorldViewProjInv);
	D3DXVec3TransformCoord(&Points[6], &P[6], &WorldViewProjInv);
	D3DXVec3TransformCoord(&Points[7], &P[7], &WorldViewProjInv);
}

void FRUSTUM_POINTS::CalcCenter(D3DXVECTOR3 *Out) const
{
	// Środek near clipping plane
	D3DXVECTOR3 NearCenter = (Points[NEAR_LEFT_TOP] + Points[NEAR_RIGHT_BOTTOM]) * 0.5f;
	// Środek far clipping plane
	D3DXVECTOR3 FarCenter = (Points[FAR_LEFT_BOTTOM] + Points[FAR_RIGHT_TOP]) * 0.5f;
	// Punkt w połowie odległości między nimi to środek sfery
	*Out = (NearCenter + FarCenter) * 0.5f;
}

void FRUSTUM_POINTS::CalcBoundingBox(BOX *Box) const
{
	Box->p1 = Box->p2 = Points[0];

	for (int i = 1; i < 8; i++)
	{
		Min(&Box->p1, Box->p1, Points[i]);
		Max(&Box->p2, Box->p2, Points[i]);
	}
}

void FRUSTUM_POINTS::CalcBoundingSphere(D3DXVECTOR3 *SphereCenter, float *SphereRadius) const
{
	// Środek frustuma to środek sfery
	CalcCenter(SphereCenter);
	// Promień sfery to odległość tego punktu od jakiegoś punktu z rogu far clipping plane
	*SphereRadius = Distance(*SphereCenter, Points[FAR_LEFT_TOP]);
}

void FRUSTUM_RADAR::SetFovAndAspect(float FovY, float Aspect)
{
	float FovY_half = FovY * 0.5f;
	float FovY_half_tan = tanf(FovY_half);
	float FovY_half_tan_aspect = FovY_half_tan * Aspect;

	m_UFactor = FovY_half_tan;
	m_RFactor = FovY_half_tan_aspect;

	m_USphereFactor = 1.f / cosf(FovY_half);
	m_RSphereFactor = 1.f / cosf(atanf(FovY_half_tan_aspect));
}

void FRUSTUM_RADAR::SetProjection(float FovY, float Aspect, float ZNear, float ZFar)
{
	m_ZNear = ZNear;
	m_ZFar  = ZFar;
	
	SetFovAndAspect(FovY, Aspect);
}

void FRUSTUM_RADAR::SetView(const D3DXVECTOR3 &Eye, const D3DXVECTOR3 &Forward, const D3DXVECTOR3 &Up, const D3DXVECTOR3 &Right)
{
	m_Eye = Eye;
	m_Forward = Forward;
	m_Up = Up;
	m_Right = Right;
}


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Klasa MatrixCamera

const D3DXMATRIX & MatrixCamera::GetViewInv() const
{
	if (!m_ViewInvIs)
	{
		D3DXMatrixInverse(&m_ViewInv, NULL, &GetView());
		m_ViewInvIs = true;
	}
	return m_ViewInv;
}

const D3DXMATRIX & MatrixCamera::GetProjInv() const
{
	if (!m_ProjInvIs)
	{
		D3DXMatrixInverse(&m_ProjInv, NULL, &GetProj());
		m_ProjInvIs = true;
	}
	return m_ProjInv;
}

const D3DXMATRIX & MatrixCamera::GetViewProj() const
{
	if (!m_ViewProjIs)
	{
		m_ViewProj = GetView() * GetProj();
		m_ViewProjIs = true;
	}
	return m_ViewProj;
}

const D3DXMATRIX & MatrixCamera::GetViewProjInv() const
{
	if (!m_ViewProjInvIs)
	{
		D3DXMatrixInverse(&m_ViewProjInv, NULL, &GetViewProj());
		m_ViewProjInvIs = true;
	}
	return m_ViewProjInv;
}

const FRUSTUM_PLANES & MatrixCamera::GetFrustumPlanes() const
{
	if (!m_FrustumPlanesIs)
	{
		m_FrustumPlanes.Set(GetViewProj()); // Tutaj nie ma INV!
		m_FrustumPlanes.Normalize();
		m_FrustumPlanesIs = true;
	}
	return m_FrustumPlanes;
}

const FRUSTUM_POINTS & MatrixCamera::GetFrustumPoints() const
{
	if (!m_FrustumPointsIs)
	{
		// Metoda 1
		m_FrustumPoints.Set(GetViewProjInv()); // Tutaj jest INV!

		// Metoda 2
		//m_FrustumPoints.Create(GetFrustumPlanes());

		m_FrustumPointsIs = true;
	}
	return m_FrustumPoints;
}

const BOX & MatrixCamera::GetFrustumBox() const
{
	if (!m_FrustumBoxIs)
	{
		GetFrustumPoints().CalcBoundingBox(&m_FrustumBox);
		m_FrustumBoxIs = true;
	}
	return m_FrustumBox;
}


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Klasa ParamsCamera

ParamsCamera::ParamsCamera(const D3DXVECTOR3 &EyePos, const D3DXVECTOR3 &ForwardDir, const D3DXVECTOR3 &UpDir, float FovY, float Aspect, float ZNear, float ZFar) :
m_EyePos(EyePos),
m_ForwardDir(ForwardDir),
m_UpDir(UpDir),
m_FovY(FovY),
m_Aspect(Aspect),
m_ZNear(ZNear),
m_ZFar(ZFar)
{
	Changed();
}

void ParamsCamera::Set(const D3DXVECTOR3 &EyePos, const D3DXVECTOR3 &ForwardDir, const D3DXVECTOR3 &UpDir, float FovY, float Aspect, float ZNear, float ZFar)
{
	m_EyePos = EyePos;
	m_ForwardDir = ForwardDir;
	m_UpDir = UpDir;
	m_FovY = FovY;
	m_Aspect = Aspect;
	m_ZNear = ZNear;
	m_ZFar = ZFar;
	Changed();
}

const MatrixCamera & ParamsCamera::GetMatrices() const
{
	if (!m_MatricesIs)
	{
		D3DXMATRIX View, Proj;

		D3DXMatrixLookAtLH(&View, &GetEyePos(), &(GetEyePos()+GetForwardDir()), &GetUpDir());
		D3DXMatrixPerspectiveFovLH(&Proj, GetFovY(), GetAspect(), GetZNear(), GetZFar());

		m_Matrices.Set(View, Proj);
		m_MatricesIs = true;
	}
	return m_Matrices;
}

const D3DXVECTOR3 & ParamsCamera::GetRightDir() const
{
	if (!m_RightDirIs)
	{
		// WERSJA 1
		D3DXVec3Cross(&m_RightDir, &GetUpDir(), &GetForwardDir());
		D3DXVec3Normalize(&m_RightDir, &m_RightDir);

		// WERSJA 2
		//m_RightDir = (const D3DXVECTOR3 &)GetMatrices().GetViewInv()._11;

		m_RightDirIs = true;
	}
	return m_RightDir;
}

const D3DXVECTOR3 & ParamsCamera::GetRealUpDir() const
{
	return (const D3DXVECTOR3 &)GetMatrices().GetViewInv()._21;
}


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Klasa Camera

Camera::Camera()
{
	Changed();
}

Camera::Camera(const D3DXVECTOR3 &Pos, float AngleX, float AngleY, float CameraDist, float ZNear, float ZFar, float FovY, float Aspect) :
m_Pos(Pos),
m_ZNear(ZNear),
m_ZFar(ZFar),
m_FovY(FovY),
m_Aspect(Aspect),
m_Mode(MODE_CHARACTER),
m_AngleX(AngleX),
m_AngleY(AngleY),
m_CameraDist(CameraDist),
m_Orientation(D3DXQUATERNION(0.0f, 0.0f, 0.0f, 1.0f))
{
	Changed();
}

Camera::Camera(const D3DXVECTOR3 &Pos, const D3DXQUATERNION &Orientation, float ZNear, float ZFar, float FovY, float Aspect) :
m_Pos(Pos),
m_ZNear(ZNear),
m_ZFar(ZFar),
m_FovY(FovY),
m_Aspect(Aspect),
m_Mode(MODE_QUATERNION),
m_AngleX(0.f),
m_AngleY(0.f),
m_CameraDist(0.f),
m_Orientation(Orientation)
{
}

Camera::~Camera()
{
}

void Camera::Set(const D3DXVECTOR3 &Pos, float AngleX, float AngleY, float CameraDist, float ZNear, float ZFar, float FovY, float Aspect)
{
	m_Pos = Pos;
	m_ZNear = ZNear;
	m_ZFar = ZFar;
	m_FovY = FovY;
	m_Aspect = Aspect;
	m_Mode = MODE_CHARACTER;
	m_AngleX = AngleX;
	m_AngleY = AngleY;
	m_CameraDist = CameraDist;
	Changed();
}

void Camera::Set(const D3DXVECTOR3 &Pos, const D3DXQUATERNION &Orientation, float ZNear, float ZFar, float FovY, float Aspect)
{
	m_Pos = Pos;
	m_ZNear = ZNear;
	m_ZFar = ZFar;
	m_FovY = FovY;
	m_Aspect = Aspect;
	m_Mode = MODE_CHARACTER;
	m_Orientation = Orientation;
	Changed();
}

const ParamsCamera & Camera::GetParams() const
{
	if (!m_ParamsIs)
	{
		if (m_Mode == MODE_CHARACTER)
		{
			D3DXVECTOR3 EyePos, ForwardDir;
			D3DXVECTOR3 UpDir = D3DXVECTOR3(0.0f, 1.0f, 0.0f);

			// Wersja 1 lub 2
			//D3DXMATRIX Rot;

			// Wersja 1
			//D3DXMatrixRotationYawPitchRoll(&Rot, GetAngleY(), GetAngleX(), 0.f);

			// Wersja 2 - Rozpisane dla optymalizacji.
			//float sy = sinf(GetAngleY()), cy = cosf(GetAngleY());
			//float sp = sinf(GetAngleX()), cp = cosf(GetAngleX());
			//Rot._11 = cy;    Rot._12 = 0.0f; Rot._13 = -sy;   Rot._14 = 0.0f;
			//Rot._21 = sy*sp; Rot._22 = cp;   Rot._23 = cy*sp; Rot._24 = 0.0f;
			//Rot._31 = sy*cp; Rot._32 = -sp;  Rot._33 = cy*cp; Rot._34 = 0.0f;
			//Rot._41 = 0.0f;  Rot._42 = 0.0f; Rot._43 = 0.0f;  Rot._44 = 1.0f;
			//D3DXVECTOR3 v = D3DXVECTOR3(0.0f, 0.0f, 1.0f);

			// Wersja 1 lub 2
			//D3DXVec3TransformNormal(&ForwardDir, v, Rot);

			// Wersja 3
			SphericalToCartesian(&ForwardDir, GetAngleY() - PI_2, -GetAngleX(), 1.f);

			// FPP
			if (GetCameraDist() == 0.f)
				EyePos = GetPos();
			// TPP
			else
				EyePos = GetPos() - ForwardDir * GetCameraDist();

			m_Params.Set(EyePos, ForwardDir, UpDir, GetFovY(), GetAspect(), GetZNear(), GetZFar());
		}
		else // m_Mode == MODE_QUATERNION
		{
			D3DXMATRIX RotMat;
			D3DXMatrixRotationQuaternion(&RotMat, &m_Orientation);
			D3DXVECTOR3 Forward, Up;
			D3DXVec3TransformNormal(&Forward, &D3DXVECTOR3(0.0f, 0.0f, 1.0f), &RotMat);
			D3DXVec3TransformNormal(&Up,      &D3DXVECTOR3(0.0f, 1.0f, 0.0f), &RotMat);
			m_Params.Set(GetPos(), Forward, Up, GetFovY(), GetAspect(), GetZNear(), GetZFar());
		}

		m_ParamsIs = true;
	}
	return m_Params;
}

const MatrixCamera & Camera::GetMatrices() const
{
	return GetParams().GetMatrices();
}

void Camera::CalcMouseRay(D3DXVECTOR3 *OutRayOrig, D3DXVECTOR3 *OutRayDir, float MouseX, float MouseY) const
{
	D3DXVECTOR3 PtNear_Proj = D3DXVECTOR3(MouseX * 2.f - 1.f, (1.f - MouseY) * 2.f - 1.f, 0.f);
	D3DXVECTOR3 PtFar_Proj = D3DXVECTOR3(PtNear_Proj.x, PtNear_Proj.y, 1.f);

	const D3DXMATRIX & ViewProjInv = GetMatrices().GetViewProjInv();

	D3DXVECTOR3 PtNear_World, PtFar_World;
	D3DXVec3TransformCoord(&PtNear_World, &PtNear_Proj, &ViewProjInv);
	D3DXVec3TransformCoord(&PtFar_World,  &PtFar_Proj,  &ViewProjInv);

	*OutRayOrig = PtNear_World;
	*OutRayDir = PtFar_World - PtNear_World;
	D3DXVec3Normalize(OutRayDir, OutRayDir);
}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Funkcje do kolizji frustuma

bool PointInFrustum(const D3DXVECTOR3 &p, const FRUSTUM_PLANES &Frustum)
{
	if (D3DXPlaneDotCoord(&Frustum.Planes[0], &p) <= 0.0f) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[1], &p) <= 0.0f) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[2], &p) <= 0.0f) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[3], &p) <= 0.0f) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[4], &p) <= 0.0f) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[5], &p) <= 0.0f) return false;

	return true;
}

bool PointInFrustum(const D3DXVECTOR3 &p, const FRUSTUM_RADAR &Frustum)
{
	D3DXVECTOR3 OP = p - Frustum.GetEye();

	float f = D3DXVec3Dot(&OP, &Frustum.GetForward());
	if (f < Frustum.GetZNear() || f > Frustum.GetZFar())
		return false;

	float r = D3DXVec3Dot(&OP, &Frustum.GetRight());
	float RLimit = Frustum.GetRFactor() * f;
	if (r < -RLimit || r > RLimit)
		return false;

	float u = D3DXVec3Dot(&OP, &Frustum.GetUp());
	float ULimit = Frustum.GetUFactor() * f;
	if (u < -ULimit || u > ULimit)
		return false;

	return true;
}

bool BoxToFrustum_Fast(const BOX &Box, const FRUSTUM_PLANES &Frustum)
{
	// Na podstawie książki "3D Game Engine Programming", Stefan Zerbst with Oliver Duvel

	D3DXVECTOR3 vmin;
	for (int i = 0; i < 6; i++)
	{
		if (Frustum.Planes[i].a <= 0.0f)
			vmin.x = Box.p1.x;
		else
			vmin.x = Box.p2.x;

		if (Frustum.Planes[i].b <= 0.0f)
			vmin.y = Box.p1.y;
		else
			vmin.y = Box.p2.y;

		if (Frustum.Planes[i].c <= 0.0f)
			vmin.z = Box.p1.z;
		else
			vmin.z = Box.p2.z;

		if (D3DXPlaneDotCoord(&Frustum.Planes[i], &vmin) < 0.0f)
			return false;
	}
	return true;
}

bool BoxToFrustum(const BOX &Box, const FRUSTUM_PLANES &FrustumPlanes, const FRUSTUM_POINTS *OptFrustumPoints)
{
	// Najpierw test podstawowy
	if (!BoxToFrustum_Fast(Box, FrustumPlanes))
		return false;

	// Wylicz Frustum Points jeśli trzeba
	FRUSTUM_POINTS MyFrustumPoints;
	const FRUSTUM_POINTS *FrustumPointsPtr = OptFrustumPoints;
	if (FrustumPointsPtr == NULL)
	{
		MyFrustumPoints.Set(FrustumPlanes);
		FrustumPointsPtr = &MyFrustumPoints;
	}

	// Dodatkowy test wg Separating Axis Theorem

	D3DXVECTOR3 BoxCenter; Box.CalcCenter(&BoxCenter);
	D3DXVECTOR3 FrustumCenter; FrustumPointsPtr->CalcCenter(&FrustumCenter);
	D3DXVECTOR3 BoxCorner;
	BoxCorner.x = (FrustumCenter.x > BoxCenter.x ? Box.p2.x : Box.p1.x);
	BoxCorner.y = (FrustumCenter.y > BoxCenter.y ? Box.p2.y : Box.p1.y);
	BoxCorner.z = (FrustumCenter.z > BoxCenter.z ? Box.p2.z : Box.p1.z);

	D3DXVECTOR3 BoxToFrustumVec = FrustumCenter - BoxCenter;
	D3DXVECTOR3 BoxToFrustumDir; D3DXVec3Normalize(&BoxToFrustumDir, &BoxToFrustumVec);

	D3DXPLANE SeparatingPlane;
	D3DXPlaneFromPointNormal(&SeparatingPlane, &BoxCorner, &BoxToFrustumDir);

	if (ClassifyFrustumToPlane(*FrustumPointsPtr, SeparatingPlane) > 0)
		return false;

	return true;
}

bool BoxToFrustum(const BOX &Box, const FRUSTUM_RADAR &Frustum)
{
	// Algorytm na podstawie kodu z książki "Game Programming Gems", tom 5, rozdz. 1.6
	// "Improved Frustum Culling", Frank Puig Placeres

	D3DXVECTOR3 P;
	int OutOfLeft = 0, OutOfRight = 0, OutOfFar = 0, OutOfNear = 0, OutOfTop = 0, OutOfBottom = 0;
	bool IsInRightTest, IsInUpTest, IsInFrontTest;

	D3DXVECTOR3 Corners[2];
	Corners[0] = Box.p1 - Frustum.GetEye();
	Corners[1] = Box.p2 - Frustum.GetEye();

	for (int i = 0; i < 8; i++)
	{
		IsInRightTest = IsInUpTest = IsInFrontTest = false;
		P.x = Corners[ i        & 1 ].x;
		P.y = Corners[ (i >> 2) & 1 ].y;
		P.z = Corners[ (i >> 1) & 1 ].z;

		float r = D3DXVec3Dot(&P, &Frustum.GetRight());
		float u = D3DXVec3Dot(&P, &Frustum.GetUp());
		float f = D3DXVec3Dot(&P, &Frustum.GetForward());

		if (r < -Frustum.GetRFactor() * f)
			OutOfLeft++;
		else if (r > Frustum.GetRFactor() * f)
			OutOfRight++;
		else
			IsInRightTest = true;

		if (u < -Frustum.GetUFactor() * f)
			OutOfBottom++;
		else if (u > Frustum.GetUFactor() * f)
			OutOfTop++;
		else
			IsInUpTest = true;

		if (f < -Frustum.GetZNear())
			OutOfNear++;
		else if (f > Frustum.GetZFar())
			OutOfFar++;
		else
			IsInUpTest = true;

		if (IsInRightTest && IsInFrontTest && IsInUpTest)
			return true;
	}

	if (OutOfLeft == 8 || OutOfRight == 8 || OutOfFar == 8 || OutOfNear == 8 || OutOfTop == 8 || OutOfBottom == 8)
		return false;
	return true;
}

bool BoxInFrustum(const BOX &Box, const FRUSTUM_PLANES &Frustum)
{
	// Każdy punkt AABB musi leżeć w jego wnętrzu
	if (!PointInFrustum(Box.p1, Frustum)) return false;
	if (!PointInFrustum(D3DXVECTOR3(Box.p2.x, Box.p1.y, Box.p1.z), Frustum)) return false;
	if (!PointInFrustum(D3DXVECTOR3(Box.p1.x, Box.p2.y, Box.p1.z), Frustum)) return false;
	if (!PointInFrustum(D3DXVECTOR3(Box.p2.x, Box.p2.y, Box.p1.z), Frustum)) return false;
	if (!PointInFrustum(D3DXVECTOR3(Box.p1.x, Box.p1.y, Box.p2.z), Frustum)) return false;
	if (!PointInFrustum(D3DXVECTOR3(Box.p2.x, Box.p1.y, Box.p2.z), Frustum)) return false;
	if (!PointInFrustum(D3DXVECTOR3(Box.p1.x, Box.p2.y, Box.p2.z), Frustum)) return false;
	if (!PointInFrustum(Box.p2, Frustum)) return false;
	return true;
}

bool SphereToFrustum_Fast(const D3DXVECTOR3 &SphereCenter, float SphereRadius, const FRUSTUM_PLANES &Frustum)
{
	SphereRadius = -SphereRadius;
	if (D3DXPlaneDotCoord(&Frustum.Planes[0], &SphereCenter) <= SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[1], &SphereCenter) <= SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[2], &SphereCenter) <= SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[3], &SphereCenter) <= SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[4], &SphereCenter) <= SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[5], &SphereCenter) <= SphereRadius) return false;

	return true;
}

bool SphereToFrustum(
	const D3DXVECTOR3 &SphereCenter, float SphereRadius,
	const FRUSTUM_PLANES &FrustumPlanes, const FRUSTUM_POINTS *OptFrustumPoints)
{
	if (!SphereToFrustum_Fast(SphereCenter, SphereRadius, FrustumPlanes))
		return false;

	// Wylicz Frustum Points jeśli trzeba
	FRUSTUM_POINTS MyFrustumPoints;
	const FRUSTUM_POINTS *FrustumPointsPtr = OptFrustumPoints;
	if (FrustumPointsPtr == NULL)
	{
		MyFrustumPoints.Set(FrustumPlanes);
		FrustumPointsPtr = &MyFrustumPoints;
	}

	// Dodatkowy test wg Separating Axis Theorem

	D3DXVECTOR3 FrustumCenter; FrustumPointsPtr->CalcCenter(&FrustumCenter);
	D3DXVECTOR3 SphereToFrustumVec = FrustumCenter - SphereCenter;
	D3DXVECTOR3 SphereToFrustumDir; D3DXVec3Normalize(&SphereToFrustumDir, &SphereToFrustumVec);

	D3DXPLANE SeparatingPlane;
	D3DXPlaneFromPointNormal(&SeparatingPlane, &(SphereCenter + SphereToFrustumDir * SphereRadius), &SphereToFrustumDir);

	if (ClassifyFrustumToPlane(*FrustumPointsPtr, SeparatingPlane) > 0)
		return false;

	return true;
}

bool SphereToFrustum(const D3DXVECTOR3 &SphereCenter, float SphereRadius, const FRUSTUM_RADAR &Frustum)
{
	D3DXVECTOR3 OP = SphereCenter - Frustum.GetEye();

	float f = D3DXVec3Dot(&OP, &Frustum.GetForward());
	if (f < Frustum.GetZNear() - SphereRadius || f > Frustum.GetZFar() + SphereRadius)
		return false;

	// dr i du to jest poprawka, której nie uwzględnia oryginalny artykuł o reprezentacji radarowej
	// w książce "Game Programming Gems 5", tom 5, rozdz. 1.6 - "Improved Frustum Culling",
	// Frank Puig Placeres, ale uwzględnia ją autor "View Frustum Culling Tutorial",
	// António Ramires Fernandes, http://www.lighthouse3d.com/opengl/viewfrustum/index.php

	float dr = SphereRadius * Frustum.GetRSphereFactor();
	float r = D3DXVec3Dot(&OP, &Frustum.GetRight());
	float RLimit = Frustum.GetRFactor() * f;
	if (r < -RLimit - dr || r > RLimit + dr)
		return false;

	float du = SphereRadius * Frustum.GetUSphereFactor();
	float u = D3DXVec3Dot(&OP, &Frustum.GetUp());
	float ULimit = Frustum.GetUFactor() * f;
	if (u < -ULimit - du || u > ULimit + du)
		return false;

	return true;
}

bool SphereInFrustum(const D3DXVECTOR3 &SphereCenter, float SphereRadius, const FRUSTUM_PLANES &Frustum)
{
	if (D3DXPlaneDotCoord(&Frustum.Planes[0], &SphereCenter) < SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[1], &SphereCenter) < SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[2], &SphereCenter) < SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[3], &SphereCenter) < SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[4], &SphereCenter) < SphereRadius) return false;
	if (D3DXPlaneDotCoord(&Frustum.Planes[5], &SphereCenter) < SphereRadius) return false;

	return true;
}

int ClassifyFrustumToPlane(const FRUSTUM_POINTS &Frustum, const D3DXPLANE &Plane)
{
	float a = D3DXPlaneDotCoord(&Plane, &Frustum.Points[0]); if (a == 0.0f) return 0;
	float b = D3DXPlaneDotCoord(&Plane, &Frustum.Points[1]); if (b*a <= 0.0f) return 0;
	b = D3DXPlaneDotCoord(&Plane, &Frustum.Points[2]); if (b*a <= 0.0f) return 0;
	b = D3DXPlaneDotCoord(&Plane, &Frustum.Points[3]); if (b*a <= 0.0f) return 0;
	b = D3DXPlaneDotCoord(&Plane, &Frustum.Points[4]); if (b*a <= 0.0f) return 0;
	b = D3DXPlaneDotCoord(&Plane, &Frustum.Points[5]); if (b*a <= 0.0f) return 0;
	b = D3DXPlaneDotCoord(&Plane, &Frustum.Points[6]); if (b*a <= 0.0f) return 0;
	b = D3DXPlaneDotCoord(&Plane, &Frustum.Points[7]); if (b*a <= 0.0f) return 0;
	return (a > 0.0f) ? +1 : -1;
}

bool TriangleInFrustum(const D3DXVECTOR3 &t1, const D3DXVECTOR3 &t2, const D3DXVECTOR3 &t3, const FRUSTUM_PLANES &Frustum)
{
	return
		PointInFrustum(t1, Frustum) &&
		PointInFrustum(t2, Frustum) &&
		PointInFrustum(t3, Frustum);
}

bool TriangleToFrustum(
	const D3DXVECTOR3 &t1, const D3DXVECTOR3 &t2, const D3DXVECTOR3 &t3, const D3DXPLANE *OptTrianglePlane,
	const FRUSTUM_PLANES &FrustumPlanes, const FRUSTUM_POINTS *OptFrustumPoints)
{
	// Algorytm wymyśliłem sam na podstawie Separating Axis Theorem.

	// Etap 1

	// Cały trójkąt leży po ujemnej stronie jednej z płaszczyzn frustuma - odrzuć
	for (unsigned i = 0; i < 6; i++)
	{
		if (
			D3DXPlaneDotCoord(&FrustumPlanes[i], &t1) < 0.f &&
			D3DXPlaneDotCoord(&FrustumPlanes[i], &t2) < 0.f &&
			D3DXPlaneDotCoord(&FrustumPlanes[i], &t3) < 0.f)
		{
			return false;
		}
	}

	// Etap 2

	// Wylicz TrianglePlane
	const D3DXPLANE *TrianglePlanePtr = OptTrianglePlane;
	D3DXPLANE TrianglePlane;
	if (TrianglePlanePtr == NULL)
	{
		D3DXPlaneFromPoints(&TrianglePlane, &t1, &t2, &t3);
		TrianglePlanePtr = &TrianglePlane;
	}

	// Wylicz FrustumPoints
	const FRUSTUM_POINTS *FrustumPointsPtr = OptFrustumPoints;
	FRUSTUM_POINTS FrustumPoints;
	if (FrustumPointsPtr == NULL)
	{
		FrustumPoints.Set(FrustumPlanes);
		FrustumPointsPtr = &FrustumPoints;
	}

	// Frustum nie przecina płaszczyzny trójkąta - odrzuć
	if (ClassifyFrustumToPlane(*FrustumPointsPtr, *TrianglePlanePtr) != 0)
		return false;

	return true;
}

bool FrustumToFrustum(
	const FRUSTUM_PLANES &Frustum1_Planes, const FRUSTUM_POINTS &Frustum1_Points,
	const FRUSTUM_PLANES &Frustum2_Planes, const FRUSTUM_POINTS &Frustum2_Points)
{
	// Na podstawie Separating Axis Theorem:
	// Dwa frustumy nie zachodzą na siebie wtedy i tylko wtedy, gdy wszystkie punkty
	// jednego z nich leżą po ujemnej stronie jednej z płaszczyzn drugiego.

	for (unsigned i = 0; i < 6; i++)
		if (ClassifyFrustumToPlane(Frustum2_Points, Frustum1_Planes[i]) < 0)
			return false;
	for (unsigned i = 0; i < 6; i++)
		if (ClassifyFrustumToPlane(Frustum1_Points, Frustum2_Planes[i]) < 0)
			return false;

	return true;

	// Inny, ale niekoniecznie szybszy algorytm znajduje się w książce ShaderX, tom 3, rozdz. 5.4, str. 437.
}

bool SweptSphereToPlane(const D3DXVECTOR3 &SphereCenter, float SphereRadius, const D3DXVECTOR3 &SphereSweepDir, const D3DXPLANE &Plane, float *OutT0, float *OutT1)
{
	float b_dot_n = D3DXPlaneDotCoord(&Plane, &SphereCenter);
	float d_dot_n = D3DXPlaneDotNormal(&Plane, &SphereSweepDir);

	if (d_dot_n == 0.f)
	{
		if (fabsf(b_dot_n) <= SphereRadius)
		{
			if (OutT0) *OutT0 = 0.f;
			if (OutT1) *OutT1 = MAXFLOAT;
			return true;
		}
		else
			return false;
	}
	else
	{
		if (OutT0 || OutT1)
		{
			d_dot_n = 1.f / d_dot_n;
			float Tmp0 = ( SphereRadius - b_dot_n) * d_dot_n;
			float Tmp1 = (-SphereRadius - b_dot_n) * d_dot_n;
			if (OutT0) *OutT0 = Min(Tmp0, Tmp1);
			if (OutT1) *OutT1 = Max(Tmp0, Tmp1);
		}
		return true;
	}
}

bool SweptSphereToFrustum(const D3DXVECTOR3 &SphereCenter, float SphereRadius, const D3DXVECTOR3 &SphereSweepDir, const FRUSTUM_PLANES &Frustum)
{
	// Funkcja na podstawie: PracticalPSM - przykładowy program autorstwa nVidia Coroporation.
	// Ale znacznie ulepszona, bo tam była napisana beznadziejnie!

    // algorithm -- get all 12 intersection points of the swept sphere with the view frustum
    // for all points >0, displace sphere along the sweep driection.  if the displaced sphere
    // is inside the frustum, return TRUE.  else, return FALSE

	float a, b;
	D3DXVECTOR3 DisplacedSphereCenter;
	float DisplacedSphereRadius = SphereRadius; // * 1.1f; // <- tak było w oryginale, ale WTF???

	for (size_t i = 0; i < 6; i++)
	{
		if (SweptSphereToPlane(SphereCenter, SphereRadius, SphereSweepDir, Frustum[i], &a, &b))
		{
			if (a >= 0.f)
			{
				DisplacedSphereCenter = SphereCenter + SphereSweepDir * a;
				if (SphereToFrustum_Fast(DisplacedSphereCenter, DisplacedSphereRadius, Frustum))
					return true;
			}
			if (b >= 0.f)
			{
				DisplacedSphereCenter = SphereCenter + SphereSweepDir * b;
				if (SphereToFrustum_Fast(DisplacedSphereCenter, DisplacedSphereRadius, Frustum))
					return true;
			}
		}
	}

	return false;
}


//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Funkcje do kolizji promienia

bool RayToBox(float *OutT, const D3DXVECTOR3 &RayOrig, const D3DXVECTOR3 &RayDir, const BOX &Box)
{
	bool inside = true;
	float xt, xn;

	if (RayOrig.x < Box.p1.x)
	{
		xt = Box.p1.x - RayOrig.x;
		xt /= RayDir.x;
		xn = -1.0f;
		inside = false;
	}
	else if (RayOrig.x > Box.p2.x)
	{
		xt = Box.p2.x - RayOrig.x;
		xt /= RayDir.x;
		xn = 1.0f;
		inside = false;
	}
	else
		xt = -1.0f;

	float yt, yn;

	if (RayOrig.y < Box.p1.y)
	{
		yt = Box.p1.y - RayOrig.y;
		yt /= RayDir.y;
		yn = -1.0f;
		inside = false;
	}
	else if (RayOrig.y > Box.p2.y)
	{
		yt = Box.p2.y - RayOrig.y;
		yt /= RayDir.y;
		yn = 1.0f;
		inside = false;
	}
	else
		yt = -1.0f;

	float zt, zn;

	if (RayOrig.z < Box.p1.z)
	{
		zt = Box.p1.z - RayOrig.z;
		zt /= RayDir.z;
		zn = -1.0f;
		inside = false;
	}
	else if (RayOrig.z > Box.p2.z)
	{
		zt = Box.p2.z - RayOrig.z;
		zt /= RayDir.z;
		zn = 1.0f;
		inside = false;
	}
	else
		zt = -1.0f;

	if (inside)
	{
		*OutT = 0.0f;
		return true;
	}

	// Select the farthest plane - this is the plane of intersection
	int plane = 0;

	float t = xt;
	if (yt > t)
	{
		plane = 1;
		t = yt;
	}

	if (zt > t)
	{
		plane = 2;
		t = zt;
	}

	// Check if the point of intersection lays within the box face

	switch(plane)
	{
	case 0: // ray intersects with yz plane
		{
			float y = RayOrig.y + RayDir.y * t;
			if (y < Box.p1.y || y > Box.p2.y) return false;
			float z = RayOrig.z + RayDir.z * t;
			if (z < Box.p1.z || z > Box.p2.z) return false;
		}
		break;
	case 1: // ray intersects with xz plane
		{
			float x = RayOrig.x + RayDir.x * t;
			if (x < Box.p1.x || x > Box.p2.x) return false;
			float z = RayOrig.z + RayDir.z * t;
			if (z < Box.p1.z || z > Box.p2.z) return false;
		}
		break;
	default:
		case 2: // ray intersects with xy plane
		{
			float x = RayOrig.x + RayDir.x * t;
			if (x < Box.p1.x || x > Box.p2.x) return false;
			float y = RayOrig.y + RayDir.y * t;
			if (y < Box.p1.y || y > Box.p2.y) return false;
		}
		break;
	}

	*OutT = t;
	return true;
}

bool RayToSphere(const D3DXVECTOR3 &RayOrig, const D3DXVECTOR3 &RayDir, const D3DXVECTOR3 &SphereCenter, float SphereRadius, float *OutT)
{
	// Równanie kwadratowe.
	// Napisane samodzielnie z małą pomocą:
	// http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter1.htm
	// link znaleziony na:
	// http://www.realtimerendering.com/int/

	D3DXVECTOR3 RayOrig_minus_SphereCenter = RayOrig - SphereCenter;
	float a = D3DXVec3Dot(&RayDir, &RayDir);
	float b = 2.f * D3DXVec3Dot(&RayDir, &RayOrig_minus_SphereCenter);
	float c = D3DXVec3Dot(&RayOrig_minus_SphereCenter, &RayOrig_minus_SphereCenter) - (SphereRadius * SphereRadius);
	float Delta = b * b - 4.f * a * c;

	if (Delta < 0.f)
		return false;

	float a_2 = 2.f * a;
	float minus_b = - b;
	float sqrt_Delta = sqrtf(Delta);

	// Pierwszy pierwiastek - ten mniejszy
	*OutT = (minus_b - sqrt_Delta) / a_2;
	// Przypadek że cała sfera jest przed RayOrig - pierwiastek mniejszy to wynik
	if (*OutT >= 0.f)
		return true;
	// Drugi pierwiastek - ten większy
	*OutT = (minus_b + sqrt_Delta) / a_2;
	// Przypadek że poczatek promienia jest wewnątrz sfery
	if (*OutT >= 0.f)
	{
		*OutT = 0.f;
		return true;
	}
	// Przypadek że sfera jest z tyłu za promieniem
	return true;
}

/*
Jakiś algorytm na to jest też w książce "3D Math Primer for Graphics and Game Development", str. 288.
*/

bool RayToPlane(const D3DXVECTOR3 &RayOrig, const D3DXVECTOR3 &RayDir, const D3DXPLANE &Plane, float *OutT, float *OutVD)
{
	// Napisane na podstawie:
	// http://www.siggraph.org/education/materials/HyperGraph/raytrace/rayplane_intersection.htm

	//       A xo + B yo + C zo + D
	// t = - ----------------------
	//         A xd + B yd + C zd

	// Ten sam wzór jest w książce "3D Math Primer for Graphics and Game Development", str. 284.
	// Inna wersja dostępna jest w książce: "3D Game Engine Programming", Stefan Zerbst with Oliver Duvel, str. 136.

	*OutVD = Plane.a * RayDir.x + Plane.b * RayDir.y + Plane.c * RayDir.z;
	if (*OutVD == 0.0f)
		return false;
	
	*OutT = - (Plane.a * RayOrig.x + Plane.b * RayOrig.y + Plane.c * RayOrig.z + Plane.d) / *OutVD;
	
	return true;
}

bool RayToTriangle(
	const D3DXVECTOR3 &RayOrig, const D3DXVECTOR3 &RayDir,
	const D3DXVECTOR3 &p0, const D3DXVECTOR3 &p1, const D3DXVECTOR3 &p2,
	bool BackfaceCulling,
	float *OutT)
{
	////// Nowy, piękny algorytm :)
	// Znaleziony w Google Code Search:
	// http://www.google.com/codesearch?hl=en&q=+RayTriangleIntersect+show:M63-4o6bYUI:fUr9QIwtaKY:Dw059DARM5E&sa=N&cd=1&ct=rc&cs_p=http://www.angstrom-distribution.org/unstable/sources/ode-snapshot-20060210.tar.bz2&cs_f=ode-snapshot-20060210/ode/src/collision_trimesh_trimesh.cpp#first
	// Dopisałem do niego Backface Culling, działa bez problemu :D

	D3DXVECTOR3 tvec, pvec, qvec;

	// find vectors for two edges sharing vert0
	D3DXVECTOR3 edge1 = p1 - p0;
	D3DXVECTOR3 edge2 = p2 - p0;

	// begin calculating determinant - also used to calculate U parameter
	D3DXVec3Cross(&pvec, &RayDir, &edge2);

	// if determinant is near zero, ray lies in plane of triangle
	float det = D3DXVec3Dot(&edge1, &pvec);
	if (BackfaceCulling && det < 0.0f)
		return false;
	if (FLOAT_ALMOST_ZERO(det))
		return false;
	float inv_det = 1.0f / det;

	// calculate distance from vert0 to ray origin
	tvec = RayOrig - p0;

	// calculate U parameter and test bounds
	float u = D3DXVec3Dot(&tvec, &pvec) * inv_det;
	if (u < 0.0f || u > 1.0f)
		return false;

	// prepare to test V parameter
	D3DXVec3Cross(&qvec, &tvec, &edge1);

	// calculate V parameter and test bounds
	float v = D3DXVec3Dot(&RayDir, &qvec) * inv_det;
	if (v < 0.0f || u + v > 1.0f)
		return false;

	// calculate t, ray intersects triangle
	if (OutT != NULL)
		*OutT = D3DXVec3Dot(&edge2, &qvec) * inv_det;
	return true;

	////// Stary algorytm: Moller i Trumbore
	// Na podstawie książki: "3D Game Engine Programming", Stefan Zerbst with Oliver Duvel, str. 134.
	// Dobry, ale niestety nie obsługuje on Backface Culling i nijak nie udało mi się tego do niego wprowadzić.

/*	D3DXVECTOR3 pvec, tvec, qvec;

	D3DXVECTOR3 edge1 = p1 - p0;
	D3DXVECTOR3 edge2 = p2 - p0;

	Cross(&pvec, RayDir, edge2);

	// If close to 0 ray os parallel
	float det = D3DXVec3Dot(&edge1, &pvec);

	//if (Cull && det < 0.0f)
	if (det < 0.0f)
		return false;
	if (FLOAT_ALMOST_ZERO(det))
		return false;

	// Distance to plane, < 0 means aray behind the plane
	tvec = RayOrig - p0;
	float u = D3DXVec3Dot(&tvec, &pvec);
	if (u < 0.0f || u > det)
		return false;

	Cross(&qvec, tvec, edge1);
	float v = D3DXVec3Dot(&RayDir, &qvec);
	if (v < 0.0f || u+v > det)
		return false;

	if (OutT)
	{
		*OutT = D3DXVec3Dot(&edge2, &qvec);
		float InvDet = 1.0f / det;
		*OutT *= InvDet;
	}
	return true;*/

	// Jakiś algorytm, dużo dłuższy, jest też w książce "3D Math Primer for Graphics and Game Development", str. 294.
}

bool RayToFrustum(const D3DXVECTOR3 &RayOrig, const D3DXVECTOR3 &RayDir, const FRUSTUM_PLANES &Frustum, float *t_near, float *t_far)
{
	// To jest uniwersalny algorytm na kolizję promienia/prostej/odcinka z wielościanem wypukłym.
	// Na podstawie Graphics Gems II, rozdział V.1.

	*t_near = MINFLOAT;
	*t_far = MAXFLOAT;
	float vn, vd, t;

	for (int i = 0; i < 6; i++)
	{
		vn = D3DXPlaneDotCoord(&Frustum.Planes[i], &RayOrig);
		vd = D3DXPlaneDotNormal(&Frustum.Planes[i], &RayDir);

		// Nie ma przecięcia z płaszczyzną
		if (FLOAT_ALMOST_ZERO(vd))
		{
			// Promień na zewnątrz - na pewno nie ma przecięcia
			if (vn < 0.0f)
				return false;
		}
		else
		{
			t = -vn / vd;

			if (vd > 0.0f)
				*t_near = Max(*t_near, t);
			else
				*t_far = Min(*t_far, t);

			if (*t_near > *t_far)
				return false;
		}
	}
	return (*t_near != MINFLOAT && *t_far != MAXFLOAT);
}