chodzi taki wenszy nawet gdy nic sie nei dzieje

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

#include "agents.h"



AutoPilot::AutoPilot()
{
}
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].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;
                    }
            }
        }
    }

    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;
        }

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

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

        if (odleglosc_do_przedmiotu < 30)
            if (obj->state.vV.length() < 4.0)
                obj->F = 5000.0;
            else
                obj->F = 0.0;
        else if (odleglosc_do_przedmiotu < 200)
            if (obj->state.vV.length() < 10.0)
                obj->F = 5000.0;
            else
                obj->F = 0.0;
        else
            if (obj->state.vV.length() < 25.0)
                obj->F = 5000.0;
            else
                obj->F = 0.0;

    }

}

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;
}