#include "stdafx.h"

vec3 CCollisionDetection::GetPositionAfterWorldCollisions(vec3 pos0, vec3 pos1, CPlayer &player, vector<CSceneObject *> * objects, int step, CCollisionPolygon * exclude) {

	// Funkcja będzie wywoływana reukrencyjnie. Jeśli poziom zagnieżdżenia przekroczy 10,
	// to zwracamy po prostu pos1 jako naszą nową, "zmodyfikowaną" pozycję.
	// Także w sytuacji, gdy długość naszego przewidywanego ruchu jest równa 0 (gracz stoi w miejscu),
	// to nie ma sensu wykonywać obliczeń.
	if (step > 10 || (pos1 - pos0).Length() == 0.0f) {
		return pos1;
	}

	// Tu będą przechowywane dane o kolizji.
	SCollision collision;

	// Sprawdźmy, czy dochodzi do kolizji elipsoidy gracza z którymkolwiek z obiektów
	// spośród tych przekazanych jako "objects". W międzyczasie zostanie zapisana informacja
	// o najbliższej z kolizji (jeśli jakakolwiek zostanie wykryta).
	for (int i = 0; i < objects->size(); ++i) {

		// W naszym przykładzie chcemy sprawdzać kolizje jedynie z obiektami klasy CWall, bo z nimi
		// będziemy wiedzieli co zrobić. Ten fragment na pewno należy zmodyfikować zależnie
		// od własnych potrzeb.

		// Rzutujemy nasz wskaźnik na "jakiś" obiekt na wskaźnik na CWall.
		CWall * wall = reinterpret_cast<CWall *>(objects->at(i));

		// Jeśli użyliśmy reinterpret_cast zamiast zwykłego static_cast (lub
		// równoważnego zapisu (CWall*)(objects->at(i)), to w momencie gdy rzutowanie
		// nie jest możliwe, otrzymamy wartość NULL. Wtedy wiadomo, że nie jest to na pewno
		// CWall i pomijamy ten obiekt.
		// Pomijamy też obiekt jeśli jest to ten sam, co obiekt do którego adres został
		// przekazany w parametrze - zostanie to użyte do rozwiązania sytuacji gdy odbicie się
		// od jednego obiektu skutkuje odbiciem się od drugiego.
		if (wall == NULL || wall->collisionPolygon == exclude) {
			continue;
		}

		// Wywołujemy sprawdzanie kolizji elipsoidy gracza z polygonem danej "ściany", na odcinku pos0 => pos1.
		// Wyniki będą zapisane w "collision", pod warunkiem że wcześniej nie znajdowała się tam
		// bliższa kolizja.
		wall->collisionPolygon->CheckCollision(pos0, pos1, player.collisionEllipsoid, collision);
	}

	// Jeśli udało się wykryć jakąkolwiek kolizję (wtedy collision zawiera informacje o tej występującej najbliżej)...
	if (collision.hasCollided) {
		
		// Ustawienie informacji o punkcie kolizji na potrzeby ewentualnego narysowania (klawisz "K").
		Scene->LastCollisionPoint = collision.planeIntersectionPosition;
		Scene->MarkCollision = true;

		// Żądanie wyliczenia odpowiedzi (reakcji) na kolizję. Uaktualniona pozycja będzie tą, do której
		// gracz powinien się przemieścić po zaistnieniu tej kolizji.
		pos1 = CalculateReaction(pos0, pos1, collision);
		
		// Wywołujemy rekurencyjnie sprawdzanie i reakcję na kolizję, tym razem dla zaktualizowanej
		// ścieżki - zamiast pierwotnego pos1, mamy pozycję po uwzględnieniu reakcji na dotychczas
		// znalezioną kolizję. Dodatkowo w następnym rozważaniu pomijamy kolizje z polygonem, z którym
		// teraz ją wykryliśmy. Wynik zwracamy poziom wyżej.
		return GetPositionAfterWorldCollisions(pos0, pos1, player, objects, step + 1, collision.target);

	}

	// Jeśli nie było kolizji, idziemy śmiało tam gdzie mieliśmy iść.
	return pos1;
}

vec3 CCollisionDetection::CalculateReaction(vec3 pos0, vec3 pos1, SCollision &collision) {

	// Offset, o który odsuwamy się od ściany w celu uniknięcia tunelowania.
	// Jest to wartość dobrana eksperymentalnie. To ona odpowiada za "skakanie" przy ścianach,
	// ale podczas jej zmniejszania dochodzi czasem do przejścia przez ścianę.
	// Zachęcam do poszukania lepszego rozwiązania!
	float eps = .015f;

	vec3 vel = pos1 - pos0;
	vec3 destPoint = pos0 + vel;

	vec3 newVelocityVector;

	// Wyznaczenie płaszczyzny, wzdłuż której gracz będzie się poruszać po dotknięciu obiektu z którym
	// nastąpiła kolizja.
	CCollisionPlane slidingPlane(collision.reaction, collision.planeIntersectionPosition);

	// Wyznaczenie wektora przemieszczenia odpowiadającego temu fragmentowi ruchu, który odbyłby się
	// "za powierzchnią ściany".
	vec3 newDestPoint = destPoint - slidingPlane.N * (slidingPlane.SDistanceTo(destPoint));
	newVelocityVector = newDestPoint - collision.planeIntersectionPosition;

	// Wyznaczenie nowej pozycji:
	pos1 = pos0 + vel * collision.t; // Dociągnięcie najbliżej do ściany.
	pos1 = pos1 + newVelocityVector; // Przesunięcie wzdłuż płaszczyzny "slidingu".
	pos1 = pos1 + collision.reaction * eps; // Odsunięcie się od ściany (najbrzydsza część całego rozwiązania).

	// Zwracamy nową pozycję.
	return pos1;
}

float CCollisionDetection::GetSmallestPositiveQuadEqSolution(float a, float b, float c, float xMin, float &newXMin) {
	// Rozwiązanie równania kwadratowego i sprawdzenie, czy któreś z uzyskanych rozwiązań jest mniejsze niż
	// to dotychczas uznawane za najmniejsze (xMin), przy założeniu że nie interesują nas rozwiązania mniejsze od zera.
	float x1, x2;
	if (!SolveQuadEq(a, b, c, x1, x2) || (x1 < 0.0f && x2 < 0.0f) || (x1 > xMin && x2 > xMin)) {
		return false;
	}
	float x;
	if (x1 > x2) {
		float tmp = x1;
		x1 = x2;
		x2 = tmp;
	}
	if (x1 < 0.0f) {
		x = x2;
	}
	else {
		x = x1;
	}
	if (x < xMin) {
		newXMin = x;
		return true;
	}
	return false;
}

bool CCollisionDetection::SolveQuadEq(float a, float b, float c, float &x1, float &x2) {
	// Rozwiązanie równania kwadratowego metodą ze szkoły średniej (delta).
	float delta = b * b - 4.0f * a * c;
	if (delta < 0.0f) {
		return false;
	}
	float sqrDelta = sqrt(delta);
	x1 = (-b - sqrDelta) / (2.0f * a);
	x2 = (-b + sqrDelta) / (2.0f * a);
	return true;
}
