1. #include <stdlib.h>
2. #include <time.h>
3.  
4. #include "agents.h"
5.  
6.  
7. int agent_target = -1;
8. float odleglosc = 10000000.0;
9. float odlegloscNowa = 0.0;
10. int index = -1;
11. int typ = 0;
12.  
13. AutoPilot::AutoPilot()
14. {
15. }
16.  
17. void AutoPilot::AutoControl(MovableObject *obj)
18. {
19.         Terrain *_terrain = obj->terrain;
20.  
21.         float pi = 3.1415;
22.         //podnoszenie_przedm = 1;
23.         Vector3 vect_local_forward = obj->state.qOrient.rotate_vector(Vector3(1, 0, 0));
24.         Vector3 vect_local_right = obj->state.qOrient.rotate_vector(Vector3(0, 0, 1));
25.         // TUTAJ NALEŻY UMIEŚCIĆ ALGORYTM AUTONOMICZNEGO STEROWANIA POJAZDEM
26.         // .................................................................
27.         // .................................................................
28.         if (agent_target == -1) {
29.                 odleglosc = 10000000.0;
30.                 odlegloscNowa = 0.0;
31.                 index = -1;
32.                 for (int i = 0; i < _terrain->number_of_items; i++)
33.                 {
34.                         if (obj->state.amount_of_fuel < 150.0)
35.                                 typ = ITEM_BARREL;
36.                         else
37.                                 typ = ITEM_COIN;
38.                         if (_terrain->p[i].type == typ && _terrain->p[i].to_take)
39.                         {
40.                                 odlegloscNowa = (_terrain->p[i].vPos - obj->state.vPos + Vector3(0, obj->state.vPos.y - _terrain->p[i].vPos.y, 0)).length();
41.                                 if (odleglosc > odlegloscNowa)
42.                                 {
43.                                         odleglosc = odlegloscNowa;
44.                                         index = i;
45.                                 }
46.                         }
47.                 }
48.                 agent_target = index;
49.         }
50.         else if (agent_target >= 0 && _terrain->p[agent_target].to_take)
51.         {
52.  
53.                 Vector3 t = _terrain->p[agent_target].vPos - obj->state.vPos;
54.                 Vector3 w_przod = obj->state.qOrient.rotate_vector(Vector3(1, 0, 0));// obroc_wektor(Wektor3(1, 0, 0));
55.  
56.                 float cos = (t^w_przod) / (t.length()*w_przod.length ());
57.                 float kat = acos(cos);
58.  
59.                 if (cos > 0.95)
60.                 {
61.                         obj->state.wheel_turn_angle= 0;
62.                 }
63.                 else
64.                 {
65.                         if (kat < PI)
66.                                 obj->state.wheel_turn_angle = -PI / 2 - .1f;
67.                         else
68.                                 obj->state.wheel_turn_angle = +PI / 2 - .1f;
69.                 }
70.  
71.                 if (obj->state.vV.length() < 8.0){
72.                         obj->F = 5000.0;
73.                 }
74.                 else
75.                         obj->F = 0.0;
76.  
77.                 if ((_terrain->p[agent_target].vPos - obj->state.vPos + Vector3(0, obj->state.vPos.y - _terrain->p[agent_target].vPos.y, 0)).length() < 5.0*obj->radius)
78.                 {
79.                        
80.                         if (obj->state.vV.length() > 3.0)
81.                                 obj->F = -1500.0;
82.  
83.                 }
84.         }
85.         else {
86.                 agent_target = -1;
87.         }
88. }
89.  
90. void AutoPilot::ControlTest(MovableObject *_ob, float krok_czasowy, float czas_proby)
91. {
92.         bool koniec = false;
93.         float _czas = 0;               // czas liczony od początku testu
94.         //FILE *pl = fopen("test_sterowania.txt","w");
95.         while (!koniec)
96.         {
97.                 _ob->Simulation(krok_czasowy);
98.                 AutoControl(_ob);
99.                 _czas += krok_czasowy;
100.                 if (_czas >= czas_proby) koniec = true;
101.                 //fprintf(pl,"czas %f, vPos[%f %f %f], got %d, pal %f, F %f, wheel_turn_angle %f, breaking_degree %fn",_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);
102.         }
103.         //fclose(pl);
104. }
105.  
106. // losowanie liczby z rozkladu normalnego o zadanej sredniej i wariancji
107. float Randn(float srednia, float wariancja, long liczba_iter)
108. {
109.         //long liczba_iter = 10;  // im wiecej iteracji tym rozklad lepiej przyblizony
110.         float suma = 0;
111.         for (long i = 0; i < liczba_iter; i++)
112.                 suma += (float)rand() / RAND_MAX;
113.         return (suma - (float)liczba_iter / 2)*sqrt(12 * wariancja / liczba_iter) + srednia;
114. }
115.