//Jacek Matulewski, e-mail: jacek@fizyka.umk.pl
//wersja 1.1

//pomoc w optymalizacji: Krzystof Chyziński, Michał Zieliński

#ifndef MACIERZGL_H
#define MACIERZGL_H

#include "Macierz.h"

#define _USE_MATH_DEFINES
#include <math.h>

#include <exception>

template<typename T>
T StopnieDoRadianów(T kątWStopniach)
{
	return (T)(M_PI * kątWStopniach / (T)180);
}

template<typename T>
T RadianyDoStopni(T kątWStopniach)
{
	return (T)(180 * kątWStopniach / M_PI);
}

#include "Wektor.h"

template<typename T>
struct TMacierzGrafika3D : public TMacierzKwadratowa<T, 4>
{
	/*
	wiersz,kolumna
	00 01 02 03
	10 11 12 13
	20 21 22 23
	30 31 32 33

	wiersz+kolumna*stopień (column-majored order)
	0 4 8  12
	1 5 9  13
	2 6 10 14
	3 7 11 15
	*/

public:
	//dlaczego nie widzi konstruktorów z szablonu bazowego - wolałbym tego nie mieć!!!!!!!!!!!!!!!
	TMacierzGrafika3D()
		: TMacierzKwadratowa<T, 4>()
	{
	}

	//dlaczego nie widzi konstruktorów z szablonu bazowego - wolałbym tego nie mieć!!!!!!!!!!!!!!!!
	TMacierzGrafika3D(const T elementy[16])
		: TMacierzKwadratowa<T, 4>(elementy)
	{
	}

	TMacierzGrafika3D(const TMacierzKwadratowa<T, 4>& m)
		: TMacierzKwadratowa<T, 4>(m)
	{
	}

	Wektor4 KopiaKolumny(const int indeksKolumny) const
	{
		T elementyKolumny[4];
		TMacierzKwadratowa<T,4>::KopiaKolumny(indeksKolumny, elementyKolumny);
		return Wektor4(elementyKolumny);
	}

	Wektor4 KopiaWiersza(const int indeksWiersza) const
	{
		T elementyWiersza[4];
		KopiaWiersza(indeksWiersza, elementyWiersza);
		return Wektor4(elementyWiersza);
	}

	void UstawSkalowanie(T sx, T sy, T sz)
	{
		ZerujElementy();
		UstawElement(0, 0, sx);
		UstawElement(1, 1, sy);
		UstawElement(2, 2, sz);
		UstawElement(3, 3, 1);
	}

	static TMacierzGrafika3D Skalowanie(T sx, T sy, T sz)
	{
		TMacierzGrafika3D m;
		m.UstawSkalowanie(sx, sy, sz);
		return m;
	}

	void UstawPrzesunięcie(T tx, T ty, T tz)
	{
		UstawJednostkową();
		UstawElement(0, 3, tx);
		UstawElement(1, 3, ty);
		UstawElement(2, 3, tz);
	}

	void UstawPrzesunięcie(Wektor3 t)
	{
		UstawPrzesunięcie(t[0], t[1], t[2]);
	}

	static TMacierzGrafika3D Przesunięcie(T tx, T ty, T tz)
	{
		TMacierzGrafika3D<T> m;
		m.UstawPrzesunięcie(tx, ty, tz);
		return m;
	}

	static TMacierzGrafika3D Przesunięcie(Wektor3 t)
	{
		return Przesunięcie(t[0], t[1], t[2]);
	}

	//w funkcjach obrotów kąty podawane są w stopniach
	void UstawObrótX(T kąt, bool kątWRadianach = false)
	{
		if (!kątWRadianach) kąt = StopnieDoRadianów(kąt);
		UstawJednostkową();
		T s = sin(kąt);
		T c = cos(kąt);
		UstawElement(1, 1, c);
		UstawElement(1, 2, -s);
		UstawElement(2, 1, s);
		UstawElement(2, 2, c);
	}

	static TMacierzGrafika3D ObrótX(T kąt, bool kątWRadianach = false)
	{
		TMacierzGrafika3D m;
		m.UstawObrótX(kąt, kątWRadianach);
		return m;
	}

	void UstawObrótY(T kąt, bool kątWRadianach = false)
	{
		if (!kątWRadianach) kąt = StopnieDoRadianów(kąt);
		UstawJednostkową();
		T s = sin(kąt);
		T c = cos(kąt);
		UstawElement(0, 0, c);
		UstawElement(0, 2, s);
		UstawElement(2, 0, -s);
		UstawElement(2, 2, c);
	}

	static TMacierzGrafika3D ObrótY(T kąt, bool kątWRadianach = false)
	{
		TMacierzGrafika3D m;
		m.UstawObrótY(kąt, kątWRadianach);
		return m;
	}

	void UstawObrótZ(T kąt, bool kątWRadianach = false)
	{
		if (!kątWRadianach) kąt = StopnieDoRadianów(kąt);
		UstawJednostkową();
		T s = sin(kąt);
		T c = cos(kąt);
		UstawElement(0, 0, c);
		UstawElement(0, 1, -s);
		UstawElement(1, 0, s);
		UstawElement(1, 1, c);
	}

	static TMacierzGrafika3D ObrótZ(T kąt, bool kątWRadianach = false)
	{
		TMacierzGrafika3D m;
		m.UstawObrótZ(kąt, kątWRadianach);
		return m;
	}

	//yaw, pitch, roll
	void UstawObrótXYZ(T kątX, T kątY, T kątZ, bool kątWRadianach = false)
	{
		if (!kątWRadianach)
		{
			kątX = StopnieDoRadianów(kątX);
			kątY = StopnieDoRadianów(kątY);
			kątZ = StopnieDoRadianów(kątZ);
		}
		T sx = sin(kątX); T cx = cos(kątX);
		T sy = sin(kątY); T cy = cos(kątY);
		T sz = sin(kątZ); T cz = cos(kątZ);
		ZerujElementy();
		UstawElement(0, 0, cy*cz);
		UstawElement(0, 1, -cy*sz);
		UstawElement(0, 2, sy);
		UstawElement(1, 0, sx*sy*cz + cx*sz);
		UstawElement(1, 1, -sx*sy*sz + cx*cz);
		UstawElement(1, 2, -sx*cy);
		UstawElement(2, 0, -cx*sy*cz + sx*sz);
		UstawElement(2, 1, cx*sy*sz + sx*cz);
		UstawElement(2, 2, cx*cy);
		UstawElement(3, 3, 1);
	}

	static TMacierzGrafika3D ObrótXYZ(T kątX, T kątY, T kątZ, bool kątWRadianach = false)
	{
		TMacierzGrafika3D m;
		m.UstawObrótXYZ(kątX, kątY, kątZ, kątWRadianach);
		return m;
	}

	//kąty Eulera
	void UstawObrótZXZ(T kątZ2, T kątX, T kątZ1, bool kątWRadianach = false)
	{
		if (!kątWRadianach)
		{
			kątZ2 = StopnieDoRadianów(kątZ2);
			kątX = StopnieDoRadianów(kątX);
			kątZ1 = StopnieDoRadianów(kątZ1);
		}
		T sz2 = sin(kątZ2); T cz2 = cos(kątZ2);
		T sx = sin(kątX); T cx = cos(kątX);
		T sz1 = sin(kątZ1); T cz1 = cos(kątZ1);
		ZerujElementy();
		UstawElement(0, 0, cz2*cz1 - sz2*cx*sz1);
		UstawElement(0, 1, -cz2*sz1 - sz2*cx*cz1);
		UstawElement(0, 2, sz2*sx);
		UstawElement(1, 0, sz2*cz1 + cz2*cx*sz1);
		UstawElement(1, 1, -sz2*sz1 + cz2*cx*cz1);
		UstawElement(1, 2, -cz2*sx);
		UstawElement(2, 0, sx*sz1);
		UstawElement(2, 1, sx*cz1);
		UstawElement(2, 2, cx);
		UstawElement(3, 3, 1);
	}

	static TMacierzGrafika3D ObrótZXZ(T kątZ2, T kątX, T kątZ1, bool kątWRadianach = false)
	{
		TMacierzGrafika3D m;
		m.UstawObrótZXZ(kątZ2, kątX, kątZ1, kątWRadianach);
		return m;
	}

	void UstawObrótWokółOsi(T kąt, T ux, T uy, T uz, bool kątWRadianach = false)
	{
		if (!kątWRadianach) kąt = StopnieDoRadianów(kąt);
		T s = sin(kąt); 
		T c = cos(kąt);
		ZerujElementy();
		UstawElement(0, 0, c + (1 - c)*ux*ux);
		UstawElement(0, 1, (1 - c)*ux*uy - s*uz);
		UstawElement(0, 2, (1 - c)*uz*ux + s*uy);
		UstawElement(1, 0, (1 - c)*ux*uy + s*uz);
		UstawElement(1, 1, c + (1 - c)*uy*uy);
		UstawElement(1, 2, (1 - c)*uy*uz - s*ux);
		UstawElement(2, 0, (1 - c)*uz*ux - s*uy);
		UstawElement(2, 1, (1 - c)*uy*uz + s*ux);
		UstawElement(2, 2, c + (1 - c)*uz*uz);
		UstawElement(3, 3, 1);
	}

	void UstawObrótWokółOsi(T kąt, Wektor3 u, bool kątWRadianach = false)
	{
		UstawObrótWokółOsi(kąt, u[0], u[1], u[2], kątWRadianach);
	}

	static TMacierzGrafika3D ObrótWokółOsi(T kąt, T ux, T uy, T uz, bool kątWRadianach = false)
	{
		TMacierzGrafika3D m;
		m.UstawObrótWokółOsi(kąt, ux, uy, uz, kątWRadianach);
		return m;
	}

	static TMacierzGrafika3D ObrótWokółOsi(T kąt, Wektor3 u, bool kątWRadianach = false)
	{
		return ObrótWokółOsi(kąt, u[0], u[1], u[2], kątWRadianach);
	}

	void UstawRzutNaPłaszczyznę(Wektor3 położenieŹródłaŚwiatła, Wektor3 normalnaDoPłaszczyznyRzutowania, float przesunięciePłaszczyznyRzutowania)
	{		
		Wektor4 L = Wektor4(położenieŹródłaŚwiatła[0], położenieŹródłaŚwiatła[1], położenieŹródłaŚwiatła[2], 1);
		Wektor4 N = Wektor4(normalnaDoPłaszczyznyRzutowania[0], normalnaDoPłaszczyznyRzutowania[1], normalnaDoPłaszczyznyRzutowania[2], przesunięciePłaszczyznyRzutowania);
		float alfa = Wektor4::IloczynSkalarny(N, L);
		//
		UstawElement(0, 0, alfa - N[0] * L[0]);
		UstawElement(0, 1, -N[1] * L[0]);
		UstawElement(0, 2, -N[2] * L[0]);
		UstawElement(0, 3, -N[3] * L[0]);
		//
		UstawElement(1, 0, -N[0] * L[1]);
		UstawElement(1, 1, alfa - N[1] * L[1]);		
		UstawElement(1, 2, -N[2] * L[1]);
		UstawElement(1, 3, -N[3] * L[1]);
		//
		UstawElement(2, 0, -N[0] * L[2]);
		UstawElement(2, 1, -N[1] * L[2]);
		UstawElement(2, 2, alfa - N[2] * L[2]);
		UstawElement(2, 3, -N[3] * L[2]);
		//
		UstawElement(3, 0, -N[0] * L[3]);
		UstawElement(3, 1, -N[1] * L[3]);
		UstawElement(3, 2, -N[2] * L[3]);
		UstawElement(3, 3, alfa - N[3] * L[3]);
	}

	static TMacierzGrafika3D RzutNaPłaszczyznę(Wektor3 położenieŹródłaŚwiatła, Wektor3 normalnaDoPłaszczyznyRzutowania, float przesunięciePłaszczyznyRzutowania)
	{
		TMacierzGrafika3D m;
		m.UstawRzutNaPłaszczyznę(położenieŹródłaŚwiatła, normalnaDoPłaszczyznyRzutowania, przesunięciePłaszczyznyRzutowania);
		return m;
	}

	void UstawRzutIzometryczny(T l, T r, T b, T t, T n, T f)
	{
		T w = r - l;
		T h = t - b;
		T d = f - n;
		UstawSkalowanie(2 / w, 2 / h, -2 / d);
		UstawElement(0, 3, -(r + l) / w);
		UstawElement(1, 3, -(t + b) / h);
		UstawElement(2, 3, -(f + n) / d);
	}

	static TMacierzGrafika3D RzutIzometryczny(T l, T r, T b, T t, T n, T f)
	{
		TMacierzGrafika3D m;
		m.UstawRzutIzometryczny(l, r, b, t, n, f);
		return m;
	}

	void UstawRzutPerspektywiczny(T l, T r, T b, T t, T n, T f)
	{
		T w = r - l;
		T h = t - b;
		T d = f - n;
		UstawSkalowanie(2 * n / w, 2 * n / h, -(f + n) / d);
		UstawElement(0, 2, (r + l) / w);
		UstawElement(1, 2, (t + b) / h);
		UstawElement(3, 2, -1);
		UstawElement(2, 3, -2 * n * f / d);
		UstawElement(3, 3, 0);
	}

	static TMacierzGrafika3D RzutPerspektywiczny(T l, T r, T b, T t, T n, T f)
	{
		TMacierzGrafika3D m;
		m.UstawRzutPerspektywiczny(l, r, b, t, n, f);
		return m;
	}

	static TMacierzGrafika3D UstawRzutPerspektywiczny2(T kątPionowegoPolaWidzeniaWStopniach, T proporcjaEkranu, T n, T f)
	{
		T kątPionowegoPolaWidzenia = StopnieDoRadianów(kątPionowegoPolaWidzeniaWStopniach);
		T h = 2 * n * tan(kątPionowegoPolaWidzenia / 2);
		T w = proporcjaEkranu * h;
		UstawRzutPerspektywiczny(-w / 2, w / 2, -h / 2, h / 2, n, f);
	}

	static TMacierzGrafika3D RzutPerspektywiczny2(T kątPionowegoPolaWidzeniaWStopniach, T proporcjaEkranu, T n, T f)
	{
		TMacierzGrafika3D m;
		m.UstawRzutPerspektywiczny2(l, r, b, t, n, f);
		return m;
	}

	void UstawWidokPatrzNa(
		Wektor3 kamera,
		Wektor3 centrum,
		Wektor3 polaryzacja)
	{
		Wektor3 E = kamera;
		Wektor3 C = centrum;
		Wektor3 U = polaryzacja;

		Wektor3 F = C - E;
		Wektor3 Fp = F.Unormowany();

		Wektor3 R = Wektor3::IloczynWektorowy(Fp, U);
		R.Normuj();

		Wektor3 Up = Wektor3::IloczynWektorowy(R, Fp);

		ZerujElementy();
		for (int kolumna = 0; kolumna < 3; kolumna++)
		{
			UstawElement(0, kolumna, R[kolumna]);
			UstawElement(1, kolumna, Up[kolumna]);
			UstawElement(2, kolumna, -Fp[kolumna]);
		}
		UstawElement(0, 3, -Wektor3::IloczynSkalarny(R, E));
		UstawElement(1, 3, -Wektor3::IloczynSkalarny(Up, E));
		UstawElement(2, 3, Wektor3::IloczynSkalarny(Fp, E));
		UstawElement(3, 3, 1);
	}

	void UstawWidokPatrzNa(
		T kameraX, T kameraY, T kameraZ,
		T centrumX, T centrumY, T centrumZ,
		T polaryzacjaX, T polaryzacjaY, T polaryzacjaZ)
	{
		Wektor3 kamera(kameraX, kameraY, kameraZ);
		Wektor3 centrum(centrumX, centrumY, centrumZ);
		Wektor3 polaryzacja(polaryzacjaX, polaryzacjaY, polaryzacjaZ);
		UstawWidokPatrzNa(kamera, centrum, polaryzacja);
		
		/*
		T E[3] = { kameraX, kameraY, kameraZ };
		T C[3] = { centrumX, centrumY, centrumZ };
		T U[3] = { polaryzacjaX, polaryzacjaY, polaryzacjaZ };

		T F[3], Fp[3];
		RóżnicaWektorów<T, 3>(C, E, F);
		WektorUnormowany<T, 3>(F, Fp);

		T R[3];
		IloczynWektorowy<T>(Fp, U, R);
		NormujWektor<T, 3>(R);

		T Up[3];
		IloczynWektorowy<T>(R, Fp, Up);

		ZerujElementy();
		for (int kolumna = 0; kolumna < 3; kolumna++)
		{
			UstawElement(0, kolumna, R[kolumna]);
			UstawElement(1, kolumna, Up[kolumna]);
			UstawElement(2, kolumna, -Fp[kolumna]);
		}
		UstawElement(0, 3, -IloczynSkalarny<T, 3>(R, E));
		UstawElement(1, 3, -IloczynSkalarny<T, 3>(Up, E));
		UstawElement(2, 3, IloczynSkalarny<T, 3>(Fp, E));
		UstawElement(3, 3, 1);
		*/
	}

	static TMacierzGrafika3D WidokPatrzNa(
		Wektor3 kamera,
		Wektor3 centrum,
		Wektor3 polaryzacja)
	{
		TMacierzGrafika3D m;
		m.UstawWidokPatrzNa(kamera, centrum, polaryzacja);
		return m;
	}

	static TMacierzGrafika3D WidokPatrzNa(
		T kameraX, T kameraY, T kameraZ,
		T centrumX, T centrumY, T centrumZ,
		T polaryzacjaX, T polaryzacjaY, T polaryzacjaZ)
	{
		TMacierzGrafika3D m;
		m.UstawWidokPatrzNa(
			kameraX, kameraY, kameraZ,
			centrumX, centrumY, centrumZ,
			polaryzacjaX, polaryzacjaY, polaryzacjaZ);
		return m;
	}

	//PRZETESTOWAC!!!!!!!!!!!!!!!!!!!!!!
	static TMacierzGrafika3D OperatorGwiazdki(T x, T y, T z)
	{
		TMacierzGrafika3D m;
		m.UstawElement(0, 1, -z);
		m.UstawElement(0, 2, y);
		m.UstawElement(1, 0, z);
		m.UstawElement(1, 2, -x);
		m.UstawElement(2, 0, -y);
		m.UstawElement(2, 1, x);
		m.UstawElement(3, 3, 1);
		return m;
	}

	//http://stackoverflow.com/questions/1148309/inverting-a-4x4-matrix
	TMacierzGrafika3D Odwrotna() const
	{
		TMacierzGrafika3D m = *this;
		T elementyMacierzyOdwrotnej[16];

		elementyMacierzyOdwrotnej[0] = 
			m[5] * m[10] * m[15] -
			m[5] * m[11] * m[14] -
			m[9] * m[6] * m[15] +
			m[9] * m[7] * m[14] +
			m[13] * m[6] * m[11] -
			m[13] * m[7] * m[10];

		elementyMacierzyOdwrotnej[4] = 
		    -m[4] * m[10] * m[15] +
			m[4] * m[11] * m[14] +
			m[8] * m[6] * m[15] -
			m[8] * m[7] * m[14] -
			m[12] * m[6] * m[11] +
			m[12] * m[7] * m[10];

		elementyMacierzyOdwrotnej[8] = 
			m[4] * m[9] * m[15] -
			m[4] * m[11] * m[13] -
			m[8] * m[5] * m[15] +
			m[8] * m[7] * m[13] +
			m[12] * m[5] * m[11] -
			m[12] * m[7] * m[9];

		elementyMacierzyOdwrotnej[12] = 
		    -m[4] * m[9] * m[14] +
			m[4] * m[10] * m[13] +
			m[8] * m[5] * m[14] -
			m[8] * m[6] * m[13] -
			m[12] * m[5] * m[10] +
			m[12] * m[6] * m[9];

		elementyMacierzyOdwrotnej[1] = 
			-m[1] * m[10] * m[15] +
			m[1] * m[11] * m[14] +
			m[9] * m[2] * m[15] -
			m[9] * m[3] * m[14] -
			m[13] * m[2] * m[11] +
			m[13] * m[3] * m[10];

		elementyMacierzyOdwrotnej[5] = 
			m[0] * m[10] * m[15] -
			m[0] * m[11] * m[14] -
			m[8] * m[2] * m[15] +
			m[8] * m[3] * m[14] +
			m[12] * m[2] * m[11] -
			m[12] * m[3] * m[10];

		elementyMacierzyOdwrotnej[9] = 
			-m[0] * m[9] * m[15] +
			m[0] * m[11] * m[13] +
			m[8] * m[1] * m[15] -
			m[8] * m[3] * m[13] -
			m[12] * m[1] * m[11] +
			m[12] * m[3] * m[9];

		elementyMacierzyOdwrotnej[13] = 
			m[0] * m[9] * m[14] -
			m[0] * m[10] * m[13] -
			m[8] * m[1] * m[14] +
			m[8] * m[2] * m[13] +
			m[12] * m[1] * m[10] -
			m[12] * m[2] * m[9];

		elementyMacierzyOdwrotnej[2] = 
			m[1] * m[6] * m[15] -
			m[1] * m[7] * m[14] -
			m[5] * m[2] * m[15] +
			m[5] * m[3] * m[14] +
			m[13] * m[2] * m[7] -
			m[13] * m[3] * m[6];

		elementyMacierzyOdwrotnej[6] = 
			-m[0] * m[6] * m[15] +
			m[0] * m[7] * m[14] +
			m[4] * m[2] * m[15] -
			m[4] * m[3] * m[14] -
			m[12] * m[2] * m[7] +
			m[12] * m[3] * m[6];

		elementyMacierzyOdwrotnej[10] = 
			m[0] * m[5] * m[15] -
			m[0] * m[7] * m[13] -
			m[4] * m[1] * m[15] +
			m[4] * m[3] * m[13] +
			m[12] * m[1] * m[7] -
			m[12] * m[3] * m[5];

		elementyMacierzyOdwrotnej[14] = 
			-m[0] * m[5] * m[14] +
			m[0] * m[6] * m[13] +
			m[4] * m[1] * m[14] -
			m[4] * m[2] * m[13] -
			m[12] * m[1] * m[6] +
			m[12] * m[2] * m[5];

		elementyMacierzyOdwrotnej[3] = 
			-m[1] * m[6] * m[11] +
			m[1] * m[7] * m[10] +
			m[5] * m[2] * m[11] -
			m[5] * m[3] * m[10] -
			m[9] * m[2] * m[7] +
			m[9] * m[3] * m[6];

		elementyMacierzyOdwrotnej[7] = 
			m[0] * m[6] * m[11] -
			m[0] * m[7] * m[10] -
			m[4] * m[2] * m[11] +
			m[4] * m[3] * m[10] +
			m[8] * m[2] * m[7] -
			m[8] * m[3] * m[6];

		elementyMacierzyOdwrotnej[11] = 
			-m[0] * m[5] * m[11] +
			m[0] * m[7] * m[9] +
			m[4] * m[1] * m[11] -
			m[4] * m[3] * m[9] -
			m[8] * m[1] * m[7] +
			m[8] * m[3] * m[5];

		elementyMacierzyOdwrotnej[15] = 
			m[0] * m[5] * m[10] -
			m[0] * m[6] * m[9] -
			m[4] * m[1] * m[10] +
			m[4] * m[2] * m[9] +
			m[8] * m[1] * m[6] -
			m[8] * m[2] * m[5];

		T wyznacznik = m[0] * elementyMacierzyOdwrotnej[0] + m[1] * elementyMacierzyOdwrotnej[4] + m[2] * elementyMacierzyOdwrotnej[8] + m[3] * elementyMacierzyOdwrotnej[12];
		if (wyznacznik == 0) throw std::exception("Macierz osobliwa");
		for (int i = 0; i < 16; i++) elementyMacierzyOdwrotnej[i] /= wyznacznik;
		return TMacierzGrafika3D(elementyMacierzyOdwrotnej);
	}
};

/* --------------------------------------------------------------------------------------- */

#include "glew.h"
#include "wglew.h"

struct MacierzOpenGL : public TMacierzGrafika3D<float>
{
private:
	GLint identyfikatorMacierzy; //uniform location

public:
	//rzutowanie na tablicę T[Rozmiar] - łamie zasady enkapsulacji, ale może być wygodne w OpenGL
	/*
	operator float*() const
	{
	return (float*)elementy;
	}
	*/

	MacierzOpenGL()
		: TMacierzGrafika3D<float>(), identyfikatorMacierzy(-1)
	{
	}

	MacierzOpenGL(const TMacierzKwadratowa<float, 4>& m)
		: TMacierzGrafika3D<float>(m), identyfikatorMacierzy(-1)
	{
	}

	MacierzOpenGL(const TMacierzGrafika3D<float>& m)
		: TMacierzGrafika3D<float>(m), identyfikatorMacierzy(-1)
	{
	}

	void ZwiążZIdentyfikatorem(GLint identyfikatorMacierzy, bool prześlijWartość = false)
	{
		this->identyfikatorMacierzy = identyfikatorMacierzy;
		if (prześlijWartość) PrześlijWartość();
	}

	GLint PobierzIdentyfikator()
	{
		return identyfikatorMacierzy;
	}

	void PrześlijWartość(bool zgłaszajBłądPrzyBrakuWiązania = false)
	{
		if (identyfikatorMacierzy < 0)
		{
			if (zgłaszajBłądPrzyBrakuWiązania) throw std::exception("Aby przesłać macierz do programu shaderów ustaw wpierw jej identyfikator (uniform location)");
			else return;
		}
		glUniformMatrix4fv(identyfikatorMacierzy, 1, false, elementy);
	}

	static const MacierzOpenGL Jednostkowa;
	static const MacierzOpenGL Zerowa;
};

//const MacierzOpenGL MacierzOpenGL::Jednostkowa = MacierzOpenGL::TwórzJednostkową();
//const MacierzOpenGL MacierzOpenGL::Zerowa = MacierzOpenGL();

typedef MacierzOpenGL Macierz4;

/* --------------------------------------------------------------------------------------- */

#include <stdlib.h>
#define _USE_MATH_DEFINES
#include <math.h>


template<typename T>
bool TestyMacierzyGrafika3D(unsigned int ziarno, T tolerancjaBledu = (T)1E-9)
{
	bool wynik = true;
	const T zakres = 10;	
	//T tolerancjaBledu = (T)1E-9; //dla double
	//if (typeif(T) == typeid(float)) tolerancjaBledu = (T)1E-5; //dla float

	//zerowa macierz
	TMacierzGrafika3D<T> m;
	for (int i = 0; i < TMacierzGrafika3D<T>::Rozmiar; ++i) if (m[i] != 0) wynik = false;

	//skalowanie
	TWektor<T,4> wektor;
	T współczynnikiSkalowania[3];
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
		współczynnikiSkalowania[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	TMacierzGrafika3D<T> macierzSkalowania = TMacierzGrafika3D<T>::Skalowanie(współczynnikiSkalowania[0], współczynnikiSkalowania[1], współczynnikiSkalowania[2]);
	TWektor<T,4> wynikWektor = macierzSkalowania.PrzetransformowanyWektor(wektor);
	for (int i = 0; i < 3; ++i) if (wynikWektor[i] != współczynnikiSkalowania[i] * wektor[i]) wynik = false;

	//translacja
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	T przesunięcie[3];
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
		przesunięcie[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	TMacierzGrafika3D<T> macierzTranslacji = TMacierzGrafika3D<T>::Przesunięcie(przesunięcie[0], przesunięcie[1], przesunięcie[2]);
	wynikWektor = macierzTranslacji.PrzetransformowanyWektor(wektor);
	for (int i = 0; i < 3; ++i) if (wynikWektor[i] != wektor[i] + przesunięcie[i]) wynik = false;

	//obrót OZ o 0 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	TMacierzGrafika3D<T> macierzObrotuZ = TMacierzGrafika3D<T>::ObrótZ(0);
	wynikWektor = macierzObrotuZ.PrzetransformowanyWektor(wektor);
	for (int i = 0; i < 3; ++i) if (wynikWektor[i] != wektor[i]) wynik = false;

	//obrót OZ o 360 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuZ = TMacierzGrafika3D<T>::ObrótZ(360);
	wynikWektor = macierzObrotuZ.PrzetransformowanyWektor(wektor);
	for (int i = 0; i < 3; ++i) if (fabs(wynikWektor[i] - wektor[i])>tolerancjaBledu) wynik = false;

	//obrót OZ o 180 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuZ = TMacierzGrafika3D<T>::ObrótZ(180);
	wynikWektor = macierzObrotuZ.PrzetransformowanyWektor(wektor);
	if (wynikWektor[2] != wektor[2]) wynik = false;
	if (fabs(wynikWektor[1] + wektor[1])>tolerancjaBledu) wynik = false;
	if (fabs(wynikWektor[0] + wektor[0])>tolerancjaBledu) wynik = false;

	//obrót OZ o 90 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuZ = TMacierzGrafika3D<T>::ObrótZ(90);
	wynikWektor = macierzObrotuZ.PrzetransformowanyWektor(wektor);
	if (wynikWektor[2] != wektor[2]) wynik = false;
	if (fabs(wynikWektor[1] - wektor[0])>tolerancjaBledu) wynik = false;
	if (fabs(wynikWektor[0] + wektor[1])>tolerancjaBledu) wynik = false;

	//obrót OZ o -90 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuZ = TMacierzGrafika3D<T>::ObrótZ(-90);
	wynikWektor = macierzObrotuZ.PrzetransformowanyWektor(wektor);
	if (wynikWektor[2] != wektor[2]) wynik = false;
	if (fabs(wynikWektor[1] + wektor[0])>tolerancjaBledu) wynik = false;
	if (fabs(wynikWektor[0] - wektor[1])>tolerancjaBledu) wynik = false;


	//obrót OX o 0 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	TMacierzGrafika3D<T> macierzObrotuX = TMacierzGrafika3D<T>::ObrótX(0);
	wynikWektor = macierzObrotuX.PrzetransformowanyWektor(wektor);
	for (int i = 0; i < 3; ++i) if (wynikWektor[i] != wektor[i]) wynik = false;

	//obrót OX o 360 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuX = TMacierzGrafika3D<T>::ObrótX(360);
	wynikWektor = macierzObrotuX.PrzetransformowanyWektor(wektor);
	for (int i = 0; i < 3; ++i) if (fabs(wynikWektor[i] - wektor[i])>tolerancjaBledu) wynik = false;

	//obrót OX o 180 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuX = TMacierzGrafika3D<T>::ObrótX(180);
	wynikWektor = macierzObrotuX.PrzetransformowanyWektor(wektor);
	if (wynikWektor[0] != wektor[0]) wynik = false;
	if (fabs(wynikWektor[1] + wektor[1])>tolerancjaBledu) wynik = false;
	if (fabs(wynikWektor[2] + wektor[2])>tolerancjaBledu) wynik = false;

	//obrót OX o 90 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuX = TMacierzGrafika3D<T>::ObrótX(90);
	wynikWektor = macierzObrotuX.PrzetransformowanyWektor(wektor);
	if (wynikWektor[0] != wektor[0]) wynik = false;
	if (fabs(wynikWektor[2] - wektor[1])>tolerancjaBledu) wynik = false;
	if (fabs(wynikWektor[1] + wektor[2])>tolerancjaBledu) wynik = false;

	//obrót OX o -90 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuX = TMacierzGrafika3D<T>::ObrótX(-90);
	wynikWektor = macierzObrotuX.PrzetransformowanyWektor(wektor);
	if (wynikWektor[0] != wektor[0]) wynik = false;
	if (fabs(wynikWektor[2] + wektor[1])>tolerancjaBledu) wynik = false;
	if (fabs(wynikWektor[1] - wektor[2])>tolerancjaBledu) wynik = false;

	//obrót OY o 0 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	TMacierzGrafika3D<T> macierzObrotuY = TMacierzGrafika3D<T>::ObrótY(0);
	wynikWektor = macierzObrotuY.PrzetransformowanyWektor(wektor);
	for (int i = 0; i < 3; ++i) if (wynikWektor[i] != wektor[i]) wynik = false;

	//obrót OY o 360 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuY = TMacierzGrafika3D<T>::ObrótY(360);
	wynikWektor = macierzObrotuY.PrzetransformowanyWektor(wektor);
	for (int i = 0; i < 3; ++i) if (fabs(wynikWektor[i] - wektor[i])>tolerancjaBledu) wynik = false;

	//obrót OY o 180 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuY = TMacierzGrafika3D<T>::ObrótY(180);
	wynikWektor = macierzObrotuY.PrzetransformowanyWektor(wektor);
	if (wynikWektor[1] != wektor[1]) wynik = false;
	if (fabs(wynikWektor[0] + wektor[0])>tolerancjaBledu) wynik = false;
	if (fabs(wynikWektor[2] + wektor[2])>tolerancjaBledu) wynik = false;

	//obrót OY o 90 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuY = TMacierzGrafika3D<T>::ObrótY(90);
	wynikWektor = macierzObrotuY.PrzetransformowanyWektor(wektor);
	if (wynikWektor[1] != wektor[1]) wynik = false;
	if (fabs(wynikWektor[0] - wektor[2])>tolerancjaBledu) wynik = false;
	if (fabs(wynikWektor[2] + wektor[0])>tolerancjaBledu) wynik = false;

	//obrót OY o -90 stopni
	for (int i = 0; i < 4; ++i)
	{
		wektor[i] = 0;
		wynikWektor[i] = 0;
	}
	for (int i = 0; i < 3; ++i)
	{
		wektor[i] = zakres*rand() / RAND_MAX;
	}
	wektor[3] = 1;
	macierzObrotuY = TMacierzGrafika3D<T>::ObrótY(-90);
	wynikWektor = macierzObrotuY.PrzetransformowanyWektor(wektor);
	if (wynikWektor[1] != wektor[1]) wynik = false;
	if (fabs(wynikWektor[0] + wektor[2])>tolerancjaBledu) wynik = false;
	if (fabs(wynikWektor[2] - wektor[0])>tolerancjaBledu) wynik = false;

	//zgodność obrotów OX
	T kąt = 15;
	TMacierzGrafika3D<T> wzorzec = TMacierzGrafika3D<T>::ObrótX(kąt);
	TMacierzGrafika3D<T> porównywana = TMacierzGrafika3D<T>::ObrótXYZ(kąt, 0, 0); if (!wzorzec.Równa(porównywana)) wynik = false;
	porównywana = TMacierzGrafika3D<T>::ObrótZXZ(0, kąt, 0); if (!wzorzec.Równa(porównywana)) wynik = false;
	porównywana = TMacierzGrafika3D<T>::ObrótWokółOsi(kąt, 1, 0, 0); if (!wzorzec.Równa(porównywana)) wynik = false;
	
	//zgodność obrotów OY
	wzorzec = TMacierzGrafika3D<T>::ObrótY(kąt);
	porównywana = TMacierzGrafika3D<T>::ObrótXYZ(0, kąt, 0); if (!wzorzec.Równa(porównywana)) wynik = false;
	porównywana = TMacierzGrafika3D<T>::ObrótWokółOsi(kąt, 0, 1, 0); if (!wzorzec.Równa(porównywana)) wynik = false;
	
	//zgodność obrotów OZ
	wzorzec = TMacierzGrafika3D<T>::ObrótZ(kąt);
	porównywana = TMacierzGrafika3D<T>::ObrótXYZ(0, 0, kąt); if (!wzorzec.Równa(porównywana)) wynik = false;
	porównywana = TMacierzGrafika3D<T>::ObrótZXZ(kąt, 0, 0); if (!wzorzec.Równa(porównywana)) wynik = false;
	porównywana = TMacierzGrafika3D<T>::ObrótZXZ(0, 0, kąt); if (!wzorzec.Równa(porównywana)) wynik = false;
	porównywana = TMacierzGrafika3D<T>::ObrótWokółOsi(kąt, 0, 0, 1); if (!wzorzec.Równa(porównywana)) wynik = false;


	//rzut izometryczny
	T l = -1;
	T r = 1;
	T b = (T)-0.71;
	T t = (T)0.71;
	T n = 1;
	T f = 10;
	TMacierzGrafika3D<T> macierzRzutu = TMacierzGrafika3D<T>::RzutIzometryczny(l, r, b, t, n, f);
	T elementyMacierzyWzorcowej[16];
	for (int i = 0; i < 16; ++i) elementyMacierzyWzorcowej[i] = 0;
	elementyMacierzyWzorcowej[0] = 2 / (r - l);
	elementyMacierzyWzorcowej[5] = 2 / (t - b);
	elementyMacierzyWzorcowej[10] = -2 / (f - n);
	elementyMacierzyWzorcowej[12] = -(r + l) / (r - l);
	elementyMacierzyWzorcowej[13] = -(t + b) / (t - b);
	elementyMacierzyWzorcowej[14] = -(f + n) / (f - n);
	elementyMacierzyWzorcowej[15] = 1;
	TMacierzGrafika3D<T> macierzWzorcowa(elementyMacierzyWzorcowej);
	for (int i = 0; i<TMacierzGrafika3D<T>::Rozmiar; ++i)
	{
		if (fabs(macierzRzutu[i] - macierzWzorcowa[i]) > tolerancjaBledu) wynik = false;
		if (fabs(macierzRzutu[i] - elementyMacierzyWzorcowej[i]) > tolerancjaBledu) wynik = false;
	}

	//rzut perspektywiczny
	macierzRzutu = TMacierzGrafika3D<T>::RzutPerspektywiczny(l, r, b, t, n, f);
	for (int i = 0; i < 16; ++i) elementyMacierzyWzorcowej[i] = 0;
	elementyMacierzyWzorcowej[0] = 2 * n / (r - l);
	elementyMacierzyWzorcowej[5] = 2 * n / (t - b);
	elementyMacierzyWzorcowej[8] = (r + l) / (r - l);
	elementyMacierzyWzorcowej[9] = (t + b) / (t - b);
	elementyMacierzyWzorcowej[10] = -(f + n) / (f - n);
	elementyMacierzyWzorcowej[11] = -1;
	elementyMacierzyWzorcowej[14] = -2 * n * f / (f - n);
	macierzWzorcowa.UstawElementy(elementyMacierzyWzorcowej);
	for (int i = 0; i<TMacierzGrafika3D<T>::Rozmiar; ++i)
	{
		if (fabs(macierzRzutu[i] - macierzWzorcowa[i]) > tolerancjaBledu) wynik = false;
		if (fabs(macierzRzutu[i] - elementyMacierzyWzorcowej[i]) > tolerancjaBledu) wynik = false;
	}

	//TMacierzGrafika3D<T> ig = TMacierzGrafika3D<T>::Jednostkowa*TMacierzGrafika3D<T>::OperatorGwiazdki(1, 0, 0);
	//if (ig.KopiaKolumny(0,))

	TMacierzGrafika3D<T> macierz = TMacierzGrafika3D<T>::ObrótXYZ(10, 15, 20);
	TMacierzGrafika3D<T> macierzOdwrotna = macierz.Odwrotna();
	TMacierzGrafika3D<T> macierzTransponowana = macierz.Transponowana();
	TMacierzGrafika3D<T> roznica = macierzOdwrotna - macierzTransponowana;
	if (!macierzTransponowana.Równa(macierzOdwrotna, tolerancjaBledu)) wynik = false; //dla ortonormalnych odwrotna = transponowana
	TMacierzGrafika3D<T> macierzOdwrotna2 = macierzOdwrotna.Odwrotna();
	wynik = true;
	if (!macierzOdwrotna2.Równa(macierz, tolerancjaBledu)) wynik = false;

	return wynik;
}

#endif