#include "stdafx.h"

void CCollisionPolygon::CheckCollision(vec3 pos0, vec3 pos1, CCollisionEllipsoid * e, SCollision &result) {
	// Sprawdzenie, czy na odcinku pos0 => pos1 dochodzi do kolizji elipsoidy e z naszym polygonem.

	// Pierwszy krok to transformacja do przestrzeni "elipsoidalnej". Przechodzimy do takiej przestrzeni,
	// w której nasza elipsoida jest kulą o r=1.0. Dzięki temu znacznie uprościmy obliczenia.
	// Aby tego dokonać, skalujemy każdą współrzędną świata przez odwrotność długości odpowiadającego promienia elipsoidy.
	// Na koniec będziemy musieli powrócić do pierwotnej przestrzeni, poprzez przemnożenie współrzędnych
	// przez NIEodwrócone długości promieni.

	// Transformujemy położenia wierzchołków polygonu, "vv" będzie zawierać pozycje w nowej przestrzeni.
	vec3 * vv = new vec3[vNum];
	for (int i = 0; i < vNum; ++i) {
		vv[i] = v[i];
		vv[i].Scale(e->rInv);
	}
	// Trzeba pamiętać, żeby przed wyjściem z funkcji zwolnić pamięć po vv!

	// Utworzenie płaszczyzny przechodzącej przez punkty "vv".
	CCollisionPlane plane(vv);

	// Transformacja początku i końca naszego ruchu (pos0 i pos1).
	vec3 basePoint = pos0;
	vec3 destinationPoint = pos1;
	basePoint.Scale(e->rInv);
	destinationPoint.Scale(e->rInv);
	
	// Obliczenie wektora przemieszczenia w nowej przestrzeni, oraz znormalizowanego wektora kierunku.
	vec3 vel = destinationPoint - basePoint;
	vec3 velNormalized = vel;
	velNormalized.Normalize();

	// Później w razie wystąpienia kolizji zapiszemy pod tą zmienną wektor kierunku,
	// w jakim powinna nastąpić reakcja na kolizję (odbicie się).
	vec3 reaction;

	#pragma region Sprawdzenie kolizji z płaszczyzną na której leży polygon.

		// Nie interesują nas kolizje zachodzące podczas poruszania się "zza" płaszczyzny.
		// Inaczej może dojść do nieprzyjemnego blokowania się.
		if (!plane.IsFrontFacingTo(velNormalized)) {
			delete[] vv;
			return;
		}

		float t0, t1; // Punkty pierwszego i drugiego przecięcia powierzchni kuli z płaszczyzną (wejście i wyjście).
		bool embeddedInPlane = false;

		// Odległość środka kuli (dawnej elipsy) od najbliższego punktu płaszczyny.
		float distance = plane.SDistanceTo(basePoint);

		float normalDotVel = plane.N.Dot(vel);

		// Jeśli poruszamy się równolegle do płaszczyzny...
		if (normalDotVel == 0.0f) {
			if (abs(distance) >= 1.0f) {
				// Środek kuli jest dalej od płaszczyzny, niż jej długość promienia => nie ma kolizji.
				delete[] vv;
				return;
			}
			// W innym wypadku wnioskujemy, że płaszczyzna przecina kulę na całej długości ruchu.
			embeddedInPlane = true;
			t0 = 0.0f;
			t1 = 1.0f;
		}

		// Jeśli NIE poruszamy się równolegle do płaszczyzny...
		else {
			// Punkty przecięcia dwóch punktów na powierzchni kuli z płaszczyzną.
			t0 = (-1.0f - distance) / normalDotVel;
			t1 = ( 1.0f - distance) / normalDotVel;

			// Uporządkowanie, aby t0 było zawsze mniejszą z wartości.
			if (t0 > t1) {
				float tmp = t1;
				t1 = t0;
				t0 = tmp;
			}

			// Sprawdzenie, czy zachodzi kolizja z płaszczyzną na rozpatrywanym odcinku.
			if (t0 > 1.0f || t1 < 0.0f) {
				// t0 i t1 są poza zakresem 0..1 => nie ma kolizji.
				delete[] vv;
				return;
			}

			// Clamping do przedziału 0..1.
			t0 = __max(0.0f, __min(1.0f, t0));
			t1 = __max(0.0f, __min(1.0f, t1));
		}

	#pragma endregion

	// Wiemy już, że gdzieś dochodzi do przecięcia/zetknięcia kuli z płaszczyzną.
	// Pozostaje "tylko" sprawdzić, czy to miejsce znajduje się wewnątrz, na wierzchołku
	// lub krawędzi polygonu, czy też jest zupełnie gdzie indziej i wtedy nie zachodzi kolizja.

	vec3 collisionPoint;
	float tc = 1.0f;
	bool collided = false;

	#pragma region Sprawdzenie kolizji z wnętrzem polygonu.
	
		if (!embeddedInPlane) {
			
			// Obliczamy punkt na płaszczyźnie z bliższym (wcześniejszym) miejscem przecięcia kuli.
			vec3 planeIntersectionPoint = (basePoint - plane.N) + vel * t0;

			// Czy punkt znajduje się wewnątrz polygonu?
			if (CheckPointInPolygon(planeIntersectionPoint, vv)) {
				// Mamy kolizję!
				tc = t0;
				collided = true;
				collisionPoint = planeIntersectionPoint; // Punktem kolizji będzie punkt na płaszczyźnie.
				reaction = plane.N; // Kierunkiem reakcji/odbicia będzie wektor normalny płaszczyzny.
				
				//printf("%s: interior\n", parent->Name);
			}

		}
	
	#pragma endregion
		
	if (!collided) {

		float t = 1.0f; // Nasz najlepszy do tej pory wynik - jeśli znajdziemy kolizję z wierzchołkiem lub krawędzią, to t się zmniejszy.
		vec3 velocity = vel;
		vec3 base = basePoint;
		float velocityL2 = vel.Length2(); // Często będziemy używać kwadratu długości prędkości, więc wyliczamy go raz.
		float a, b, c; // Współczynniki równania kwadratowego.
		float newT; // Miejsce, gdzie będzie wpisywana wartość nowowyliczonego "t".

		#pragma region Sprawdzenie kolizji z wierzchołkami polygonu.

			// Wymaga rozwiązania równania kwadratowego dla każdego z wierzchołków polygonu.
			// Wytłumaczenie współczynników a, b, c znaleźć można w sugerowanej literaturze.

			a = velocityL2; // "a" jest wspólne dla wszystkich wierzchołków.

			// Dla każdego wierzchołka rozwiążemy równanie z innymi "b" i "c".
			for (int i = 0; i < vNum; ++i) {
				b = 2.0f * (velocity.Dot(base - vv[i]));
				c = (vv[i] - base).Length2() - 1.0f;
				if (CCollisionDetection::GetSmallestPositiveQuadEqSolution(a, b, c, t, newT)) {
					// Udało się znaleźć lepsze od poprzedniego rozwiązanie. Mamy kolizję!
					t = newT;
					tc = t;
					collided = true;
					collisionPoint = vv[i]; // Miejscem kolizji będzie wierzchołek.
					reaction = basePoint - collisionPoint; // Kierunkiem reakcji będzie kierunek pomiędzy środkiem kuli a wierzchołkiem.
					
					//printf("%s: vertex\n", parent->Name);
				}
			}

		#pragma endregion

		#pragma region Sprawdzenie kolizji z krawędziami polygonu.

			// Wymaga rozwiązania równania kwadratowego dla każdej z krawędzi polygonu.
			// Wytłumaczenie współczynników a, b, c znaleźć można w sugerowanej literaturze.

			for (int i = 0; i < vNum; ++i) {
				vec3 e = vv[(i + 1) % vNum] - vv[i];
				vec3 btv = vv[i] - base;
				float eL2 = e.Length2();
				float edv = e.Dot(velocity);
				float edbtv = e.Dot(btv);
				a = -eL2 * velocityL2 + edv * edv;
				b = eL2 * 2.0f * velocity.Dot(btv) - 2.0f * edv * edbtv;
				c = eL2 * (1.0f - btv.Length2()) + edbtv * edbtv;
			
				if (CCollisionDetection::GetSmallestPositiveQuadEqSolution(a, b, c, t, newT)) {
					
					// Obliczamy współczynnik określający odległość od jednego wierzchołka krawędzi do drugiego,
					// odpowiadający odległości po której jest miejsce wystąpienia kolizji.
					float f = (edv * newT - edbtv) / eL2;

					// Jeśli jesteśmy w obrębie krawędzi (f=0..1)...
					if (f >= 0.0f && f <= 1.0f) {
						//...to mamy kolizję!
						t = newT;
						tc = t;
						collided = true;
						collisionPoint = vv[i] + e * f; // Miejscem kolizji jest punkt oddalony o "f" od pierwszego z wierzchołków krawędzi, w stronę drugiego z wierzchołków.
						reaction = basePoint - collisionPoint; // Kierunkiem reakcji będzie kierunek pomiędzy środkiem kuli a miejscem kolizji.
						//printf("%s: edge\n", parent->Name);
					}
				}
			}

		#pragma endregion

	}

	#pragma region Ustawienie odpowiedzi o kolizji.
		
		if (collided) {

			// Odległość od punktu startowego, po której następuje znaleziona kolizja (w pierwotnej przestrzeni!).
			float distToCollision = tc * (pos1 - pos0).Length();

			// Czy znaleziona kolizja jest bliższa od poprzednio uznanej za najbliższą?
			// (lub czy do tej pory jeszcze nie znaleziono żadnej?)
			if (!result.hasCollided || result.distance > distToCollision) {
				
				// Powracamy do pierwotnej przestrzeni.
				reaction.Scale(e->r);
				reaction.Normalize();
				collisionPoint.Scale(e->r);

				result.hasCollided = true; // Doszło do kolizji!
				result.target = this; // Z czym doszło do kolizji?
				result.t = tc; // Współczynnik 0..1 przemieszczenia po jakim następuje kolizja.
				result.distance = distToCollision; // Odległość którą można przebyć do wystąpienia kolizji.
				result.planeIntersectionPosition = collisionPoint; // Punkt przecięcia z polygonem - miejsce kolizji.
				result.reaction = reaction; // Kierunek reakcji na kolizję.

			}
		}

	#pragma endregion

	delete[] vv;

}

bool CCollisionPolygon::CheckPointInPolygon(vec3 p, vec3 *vv) {
	// Test "Point In Polygon", czyli sprawdzenie czy zadany punkt leży wewnątrz wielokąta.
	// Metoda dość brzydka, ale do bólu prosta. Sprawdzenie, czy suma kątów pomiędzy
	// zadanym punktem a kolejnymi parami wierzchołków jest równa 2*PI. Jeśli punkt będzie
	// poza wielokątem, suma kątów będzie inna. Żeby zrozumieć, wystarczy zrobić sobie prosty rysunek.
	float angle = 0.0f;
	for (int i = 0; i < vNum; ++i) {
		vec3 a = vv[i] - p;
		vec3 b = vv[(i + 1) % vNum] - p;
		float albl = a.Length() * b.Length();
		angle += acos(a.Dot(b) / albl);
	}
	if (abs(angle - PI * 2.0f) > .01f) {
		return false;
	}
	return true;
}
