Facebook
From Jittery Mockingbird, 3 Months ago, written in Plain Text.
Embed
Download Paste or View Raw
Hits: 55
  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.