siatka tajnych agentuw

#include <stdlib.h>
#include <time.h>

#include "agents.h"



AutoPilot::AutoPilot()
{
}
extern int cel_biere_se;
extern int cel_printf;
int cel = -1;
float odleglosc = -1;
float odleglosc_do_przedmiotu;

void AutoPilot::AutoControl(MovableObject *obj)
{
	Terrain *_terrain = obj->terrain;

	float pi = 3.1415;

	//podnoszenie_przedm = 1;
	Vector3 vect_local_forward = obj->state.qOrient.rotate_vector(Vector3(1, 0, 0));
	Vector3 vect_local_right = obj->state.qOrient.rotate_vector(Vector3(0, 0, 1));
	// TUTAJ NALEŻY UMIEŚCIĆ ALGORYTM AUTONOMICZNEGO STEROWANIA POJAZDEM
	// .................................................................
	// .................................................................
	if (cel == -1)
	{
		for (int i = 0; i < _terrain->number_of_items; i++)
		{
			if (_terrain->p[i].type == ITEM_COIN && _terrain->p[i].to_take)
			{
				odleglosc_do_przedmiotu = (_terrain->p[i].vPos - obj->state.vPos + Vector3(0, obj->state.vPos.y - _terrain->p[i].vPos.y, 0)).length();
				if (odleglosc == -1)
					odleglosc = odleglosc_do_przedmiotu;
				else
				if (odleglosc > odleglosc_do_przedmiotu &&  _terrain->p[i].to_take)
				{
					odleglosc = odleglosc_do_przedmiotu;
					cel = i;
					cel_printf = cel;

					// dobre biere se to pozdro
					cel_biere_se = cel;
				}
			}
		}
	}

	if (cel > -1)
	{
		Vector3 x = _terrain->p[cel].vPos - obj->state.vPos;
		Vector3 forward = obj->state.qOrient.rotate_vector(Vector3(1, 0, 0));// obroc_wektor(Wektor3(1, 0, 0));
		Vector3 right = obj->state.qOrient.rotate_vector(Vector3(0, 1, 0));

		float cos = (forward^x) / (x.length() * forward.length());
		float kat = acos(cos);
		odleglosc_do_przedmiotu = (_terrain->p[cel].vPos - obj->state.vPos + Vector3(0, obj->state.vPos.y - _terrain->p[cel].vPos.y, 0)).length();

		if (!_terrain->p[cel].to_take) {
			cel = -1;
			cel_printf = cel;
		}

		if (cos > 0.7)
		{
			obj->state.wheel_turn_angle = 0;
		}
		else
		{

			if (!obj->if_keep_steer_wheel)
			{
				if (kat > PI) {
					obj->breaking_degree = 0.7;
					obj->state.wheel_turn_angle = 0.1f;
				}
				else {
					obj->breaking_degree = 0.7;
					obj->state.wheel_turn_angle = -0.1f;
				}
			}
		}

		if (obj->state.vV.length() < 30.0) {
			obj->F = 5000.0;
		}
		else
			obj->F = 0.0;

		if (odleglosc_do_przedmiotu < 30)
		{
			if (obj->state.vV.length() > 1.0)
				obj->breaking_degree = 0.8;
		}else
		if (odleglosc_do_przedmiotu < 45)
		{
			if (obj->state.vV.length() > 2.0)
				obj->breaking_degree = 0.5;
		}
	}

}

void AutoPilot::ControlTest(MovableObject *_ob, float krok_czasowy, float czas_proby)
{
	bool koniec = false;
	float _czas = 0;               // czas liczony od początku testu
	//FILE *pl = fopen("test_sterowania.txt","w");
	while (!koniec)
	{
		_ob->Simulation(krok_czasowy);
		AutoControl(_ob);
		_czas += krok_czasowy;
		if (_czas >= czas_proby) koniec = true;
		//fprintf(pl,"czas %f, vPos[%f %f %f], got %d, pal %f, F %f, wheel_turn_angle %f, breaking_degree %f\n",_czas,_ob->vPos.x,_ob->vPos.y,_ob->vPos.z,_ob->money,_ob->amount_of_fuel,_ob->F,_ob->wheel_turn_angle,_ob->breaking_degree);
	}
	//fclose(pl);
}

// losowanie liczby z rozkladu normalnego o zadanej sredniej i wariancji
float Randn(float srednia, float wariancja, long liczba_iter)
{
	//long liczba_iter = 10;  // im wiecej iteracji tym rozklad lepiej przyblizony
	float suma = 0;
	for (long i = 0; i < liczba_iter; i++)
		suma += (float)rand() / RAND_MAX;
	return (suma - (float)liczba_iter / 2)*sqrt(12 * wariancja / liczba_iter) + srednia;
}