- Main Page
- Modules
- Classes
- Files
- Related Pages

- File List
- File Members

# src/vectorMath/CarTrace.h

Go to the documentation of this file.

    00001 #ifndef CARTRACE_H_
    00002 #define CARTRACE_H_
    00003
    00004 #include "../config.h"
    00005 #include <ode/ode.h>
    00006 #include <typeinfo>
    00007 #include "../drawstuff/drawstuff.h"
    00008 #include "Vector3.h"
    00009 #include "VectorMath.h"
    00010 #include "../Simulator.h"
    00011 #include "../Logger.h"
    00012 #include "../sim/Sector.h"
    00013 #include "../sim/Car.h"
    00014 #include "../sim/Semaphore.h"
    00015
    00021 class CarTrace
    00022 {
    00023 public:
    00024     CarTrace();
    00031     static void Trace(Car * thisCar, dReal &steer,dReal &speed, int nCarInitRotation)
    00032     {
    00033         //nastavi ze auto nebrzdi, pokud spadneme do break procedury, nastavi se na 1
    00034         thisCar->SetIsBreaking(0);
    00035         //DrawVectorInDS(car_position,car_direction);
    00036         const dReal * car_position = thisCar->getCarPosition();
    00037         const dReal * car_direction = thisCar->getCarDirection();
    00038         const dReal * left_wheel_position = thisCar->getFrontRightWheelPosition();
    00039         const dReal * right_wheel_position = thisCar->getFrontLeftWheelPosition();
    00040         Vector3 car_pos = DReal2TVector(car_position);
    00041         Vector3 car_dir;
    00042         if (nCarInitRotation == 90 || nCarInitRotation == 270)
    00043             car_dir = DRealRotation2TVector(car_direction);
    00044         else
    00045             car_dir = DRealRotation2TVector2(car_direction);
    00046         Vector3 left_wheel_pos = DReal2TVector(left_wheel_position);
    00047         Vector3 right_wheel_pos = DReal2TVector(right_wheel_position);
    00048         car_dir *= 20;
    00049
    00050         Vector3 vektor_p10_stupnu = Vector3();
    00051         Vector3 vektor_m10_stupnu = Vector3();
    00052         if (thisCar->getSpeed() > 0.3)
    00053         {
    00054             vektor_p10_stupnu = Vector3::getVectorAngleMoved(car_dir, 15);
    00055             vektor_m10_stupnu = Vector3::getVectorAngleMoved(car_dir, -15);
    00056         }
    00057         else
    00058         {
    00059             vektor_p10_stupnu = Vector3::getVectorAngleMoved(car_dir, 18);
    00060             vektor_m10_stupnu = Vector3::getVectorAngleMoved(car_dir, -18);
    00061         }
    00062
    00063         int color_violet [3] = {1,0,1};
    00064         int color_red [3] = {1,0,0};
    00065         int color_green [3] = {0,1,0};
    00066         int color_blue [3] = {0,0,1};
    00067         int color_yellow [3] = {1,1,0};
    00068         int color_lightblue [3] = {0,1,1};
    00069
    00070         //draw vectors for tracing
    00074
    00075         Vector3 return_cross_point;
    00076
    00077         Simulator* simulator = Simulator::getInstance();
    00078
    00079         double meetDistances [20];
    00080         Vector3 meetPoints [20];
    00081         int meetAngles [20];
    00082         int myMeetIndex = 0;
    00083         for (int n=0; n<20; n++)
    00084         {
    00085             meetDistances [n] = TRACE_INFINITY;
    00086             meetPoints [n] = Vector3();
    00087             meetAngles [n] = 0;
    00088         }
    00089
    00090         SectorMap sectors = simulator->getSectors();
    00091
    00092         //*******************************************************************************
    00093         //zjistovani sektoru, path planning
    00094         //get sector, car is in
    00095         Sector* mySector = findCarCurrentSector(sectors, left_wheel_pos, car_dir, simulator);
    00096         if (mySector != NULL)
    00097         {
    00098 //          if (mySector->id == 11)
    00099 //              _DEBUG("mySector->id " << mySector->id << "thisCar->getCurrentSectorID() " << thisCar->getCurrentSectorID()); // << " car " << thisCar->_id << " next_s " << mySector->getNextSectorID(0));
    00100             // pokud sem v novym sektoru, nastav ten co v nem bylo auto predtim jako predchozi
    00101
    00102             if (mySector->id != thisCar->getCurrentSectorID())
    00103             {
    00104                 thisCar->setLastSectorID(thisCar->getCurrentSectorID());
    00105                 //pokud m�me naplanovanou trasu, ridme se podle ni
    00106                 if (thisCar->getNextNextSectorID() > 0)
    00107                 {
    00108                     //_DEBUG("transferring next next to next");
    00109                     thisCar->setNextSectorID(thisCar->getNextNextSectorID());
    00110                     thisCar->setNextNextSectorID(0);
    00111                 }
    00112             }
    00113             //nastav nynejsi sektor do auta
    00114             thisCar->setCurrentSectorID(mySector->id);
    00115             //nastav nasledujici sektor do auta, posli ve kterem byl predtim, pro krizovatku
    00116             int arrNextExits[20];
    00117             int nextIsCrossing = 0;
    00118             mySector->getNextSectorID(thisCar->getCurrentSectorID(), arrNextExits);
    00119             //set next sector
    00120             //if ( !(thisCar->getLastSectorID() == 2 || thisCar->getLastSectorID() == 12 || thisCar->getLastSectorID() == 20 || thisCar->getLastSectorID() == 23 || thisCar->getLastSectorID() == 8 || thisCar->getLastSectorID() == 5) ) // TODO
    00121             if ( !(thisCar->getLastSectorID() == 3 || thisCar->getLastSectorID() == 8 || thisCar->getLastSectorID() == 10) ) // || thisCar->getLastSectorID() == 12 || thisCar->getLastSectorID() == 20 || thisCar->getLastSectorID() == 23 || thisCar->getLastSectorID() == 8 || thisCar->getLastSectorID() == 5) ) // TODO
    00122                 thisCar->setNextSectorID(arrNextExits[1]);
    00123
    00124             //_DEBUG("arrNextExits[1] " << arrNextExits[1]);
    00125
    00126             Sector *myNextSector = simulator->getSectorByID(thisCar->getNextSectorID());
    00127             if (myNextSector != NULL)
    00128                 nextIsCrossing = myNextSector->isCrossing();
    00129             //_DEBUG("nextIsCrossing " << nextIsCrossing);
    00130
    00131             // pokud je dalsi sektor krizovatka, zjisti i dalsi usek
    00132             if (nextIsCrossing == 1 && thisCar->getNextNextSectorID() < 1)
    00133             {
    00134                 Sector *myNextSector = simulator->getSectorByID(thisCar->getNextSectorID());
    00135                 //_DEBUG("nextSector " << myNextSector->id);
    00136                 if (myNextSector != NULL)
    00137                 {
    00138                     //_DEBUG("goooo");
    00139                     int arrNextNextExits[20];
    00140                     int nextNextIsCrossing = 0;
    00141                     myNextSector->getNextSectorID(thisCar->getCurrentSectorID(), arrNextNextExits);
    00142                     thisCar->setNextNextSectorID( arrNextNextExits[VectorMath::randomIntInRange(1,arrNextNextExits[0]-1)] );
    00143                 }
    00144             }
    00145
    00146         }
    00147 //      if (thisCar->getCurrentSectorID() != NULL && thisCar->getCurrentSectorID() == 11)
    00148 //          _DEBUG("cs " << thisCar->getCurrentSectorID() << " ns " << thisCar->getNextSectorID() << " nns " << thisCar->getNextNextSectorID() << " ls "  << thisCar->getLastSectorID());
    00149 //      else
    00150 //          thisCar->brake(2, 2); // zabrzdi, kdyz vyjedes ze sektoru
    00151
    00152         //*******************************************************************************
    00153         //davani prednosti
    00154
    00155         // pokud je auto v sektoru s nizsi prednosti a v casti kde my brzit
    00156         AOContainer::iterator it;
    00157         AOContainer AO = simulator->GetAObjects();
    00158         if (
    00159             thisCar->getCurrentSectorID() == 3
    00160             && car_pos.X() < 20.5 //car in [2]
    00161             && car_pos.Y() < 0.5 //car in [2]
    00162             && car_pos.X() > 17 //car in [2]
    00163             && car_pos.Y() > -1.5 //car in [2]
    00164             )
    00165             {
    00166                 int isSomeCarInMainRoad = 0;
    00167                 //projdi vsechny auta
    00168                 for (it = AO.begin(); it != AO.end(); it++)
    00169                 {
    00170                     Car* car = dynamic_cast<Car*>( (*it).second );
    00171                     double distance_to_some_car_mag;
    00172                     if (car != 0)
    00173                     {
    00174                         const dReal * s_c_p = car->getPosition(0);
    00175                         Vector3 some_car_position = Vector3(s_c_p[0],s_c_p[1],s_c_p[2]); //pozice nejakeho auta
    00176                         if
    00177                         (
    00178                         car->getCurrentSectorID() == 5
    00179                         && some_car_position.X() < 22 //car in [1]
    00180                         && some_car_position.Y() < 4 //car in [1]
    00181                         && some_car_position.X() > 18 //car in [1]
    00182                         && some_car_position.Y() > 2 //car in [1]
    00183                         )
    00184                             isSomeCarInMainRoad = 1;
    00185                     }
    00186                     if (isSomeCarInMainRoad == 1)
    00187                         thisCar->brake(40, 2);
    00188                     else
    00189                     {
    00190                         thisCar->accelerate(3,2);
    00191                     }
    00192                 }
    00193
    00194             }
    00195
    00196         //*******************************************************************************
    00197         //udrz se na silnici
    00198         //projdi sektory a zjisti vzdalenost od okraju silnice
    00199         for (SectorMap::iterator sit = sectors.begin(); sit != sectors.end(); sit++)
    00200         {
    00201             //BorderContainer
    00202             int nPolygonID = 0;
    00203             int nVectorID = 0;
    00204             //trasuj jen sektory, kde auto je a bude
    00205             if ( (*sit).first == thisCar->getCurrentSectorID() || (*sit).first == thisCar->getNextSectorID()) // trace only current sector
    00206                 ;
    00207             else
    00208                 continue;
    00209             std::vector<Border> bc = (*sit).second->getBorders();
    00210             for (BorderContainer::iterator bit = bc.begin(); bit != bc.end(); bit++)
    00211             {
    00212                 //jump out if it is start or end of sector
    00213                 if ( (*bit)._side == 0 || (*bit)._side == 2)
    00214                     continue;
    00215
    00216                 //vyhodit ty sub useky z krizovatky, ktere nevedou tam kam chci jet
    00217                 int xx = -1;
    00218                 if (
    00219                      ( (*bit)._endSector == thisCar->getNextSectorID() && (*bit)._startSector == thisCar->getLastSectorID() )
    00220                     || ( (*bit)._endSector == thisCar->getNextNextSectorID() && (*bit)._startSector == thisCar->getCurrentSectorID() )
    00221                     || (*bit)._endSector == -1
    00222                    )
    00223                     ;//ok
    00224                 else
    00225                     continue;
    00226
    00227                 int nPolygonSize = (*bit)._polygon->_size;
    00228                 for (int i = 0; i < nPolygonSize; i++)
    00229                 {
    00230                     Vector3 startPoint = Vector3
    00231                         (
    00232                         (*bit)._polygon->_vertices[i][0],
    00233                         (*bit)._polygon->_vertices[i][1],
    00234                         (*bit)._polygon->_vertices[i][2]
    00235                         );
    00236                     if ( i + 1 < nPolygonSize)
    00237                     {
    00238                         Vector3 endPoint = Vector3
    00239                             (
    00240                             (*bit)._polygon->_vertices[i+1][0],
    00241                             (*bit)._polygon->_vertices[i+1][1],
    00242                             (*bit)._polygon->_vertices[i+1][2]
    00243                             );
    00244
    00245                         //call trace function and fill array
    00246                         GetMeetPointVectorAndUsecka(startPoint,endPoint,left_wheel_pos,vektor_m10_stupnu,color_yellow,meetDistances,meetPoints,myMeetIndex,meetAngles,1);
    00247                         GetMeetPointVectorAndUsecka(startPoint,endPoint,right_wheel_pos,vektor_p10_stupnu,color_blue,meetDistances,meetPoints,myMeetIndex,meetAngles,2);
    00248                     }
    00249                 }
    00250             }
    00251             nPolygonID ++;
    00252         }
    00253
    00254         //vybere pouze nejblizsi body
    00255         double minDistance1 = TRACE_INFINITY;
    00256         int minDistanceIndex1 = -1;
    00257         for (int j = 0; j < myMeetIndex; j++)
    00258         {
    00259             if (meetAngles[j] == 1 &&  meetDistances[j] < minDistance1 && meetDistances[j] > 0.001)
    00260             {
    00261                 minDistance1 = meetDistances[j];
    00262                 minDistanceIndex1 = j;
    00263             }
    00264         }
    00265
    00266         double minDistance2 = TRACE_INFINITY;
    00267         int minDistanceIndex2 = -1;
    00268         for (int k = 0; k < myMeetIndex; k++)
    00269         {
    00270             if (meetAngles[k] == 2 && meetDistances[k] < minDistance2 && meetDistances[k] > 0.001)
    00271             {
    00272                 minDistance2 = meetDistances[k];
    00273                 minDistanceIndex2 = k;
    00274             }
    00275         }
    00276         //DrawUseckaInDS(meetPoints[minDistanceIndex1],meetPoints[minDistanceIndex2],color_green);
    00277
    00278         //vybere pouze vzdalenosti, co me zajimaji a priradi je do leve a prave
    00279         double right_distance = TRACE_INFINITY;
    00280         double left_distance = TRACE_INFINITY;
    00281         for (int n = 0; n < myMeetIndex; n++)
    00282         {
    00283             //_DEBUG("");
    00284             //_DEBUG("1Ang:" << meetAngles[n] << ":Dist:" << meetDistances[n]);
    00285             //_DEBUG("MDI1:" << minDistanceIndex1 << ":MDI2:" << minDistanceIndex2);
    00286             if (meetDistances[n] > 0.001 && (n == minDistanceIndex1 || n == minDistanceIndex2))
    00287             {
    00288                 if (meetAngles[n] == 1)
    00289                     left_distance = meetDistances[n];
    00290                 if (meetAngles[n] == 2)
    00291                     right_distance = meetDistances[n];
    00292                 //_DEBUG("2Ang:" << meetAngles[n] << ":Dist:" << meetDistances[n]);
    00293                 //trace cross point
    00294                 //DrawUseckaInDS(Vector3(0,0,0),meetPoints[n],color_green);
    00295             }
    00296         }
    00297
    00298         //trace informace o tom na kterem tykadle je bliz ke krajnici
    00299         //      if (left_distance < 2)
    00300         //          DrawUseckaInDS(Vector3(0,0,0),Vector3(0,1,0),color_red);
    00301         //      if (right_distance < 2)
    00302         //          DrawUseckaInDS(Vector3(0,0,0),Vector3(0,1,1),color_blue);
    00303
    00304         //kdyz je prave tykadlo bliz, zatoc doleva a naopak
    00305         //std::cout << "left:" << left_distance << ":right:" << right_distance << "\n";
    00306         steer = 0;
    00307         //_DEBUG("left:" << left_distance << ":right:" << right_distance);
    00308         if (left_distance < 2 || right_distance < 2)
    00309         {
    00310             //normalni polomer kdyz nejsem moc blizko krajnice
    00311             if (left_distance > right_distance)
    00312                 steer = 0.2;
    00313             if (left_distance < right_distance)
    00314                 steer = -0.2;
    00315             //kdyz si hodne blizko, zatoc poradne
    00316             if (left_distance < 0.13)
    00317                 steer = -1.1;
    00318             if (right_distance < 0.13)
    00319                 steer = 1.1;
    00320             if (left_distance < 0.08)
    00321                 steer = -1.9;
    00322             if (right_distance < 0.08)
    00323                 steer = 1.9;
    00324
    00325         }
    00326
    00327         //*******************************************************************************
    00328         //trasovani ostatnich aut
    00329         //AOContainer::iterator it;
    00330         //AOContainer AO = simulator->GetAObjects();
    00331         //projdi vsechny auta
    00332         for (it = AO.begin(); it != AO.end(); it++)
    00333         {
    00334             Car* car = dynamic_cast<Car*>( (*it).second );
    00335             double distance_to_some_car_mag;
    00336             if (car != 0)
    00337             {
    00338                 const dReal * s_c_p = car->getPosition(0);
    00339                 Vector3 some_car_position = Vector3(s_c_p[0],s_c_p[1],s_c_p[2]); //pozice nejakeho auta
    00340                 Vector3 distance_to_some_car = some_car_position - car_pos;//vektor vzdalenosti auta od meho auta
    00341                 distance_to_some_car_mag = distance_to_some_car.mag();
    00342                 //_DEBUG("distance_to_some_car_mag:" << distance_to_some_car_mag);
    00343                 if (
    00344                     distance_to_some_car_mag > 0
    00345                     && distance_to_some_car_mag < 2
    00346                     && thisCar->getSpeed() > car->getSpeed()
    00347                     )
    00348                     {
    00349                         const dReal * some_car_left_back_wheel_position = car->getBackRightWheelPosition();
    00350                         const dReal * some_car_right_back_wheel_position = car->getBackLeftWheelPosition();
    00351                         Vector3 sCLBWP = DReal2TVector(some_car_left_back_wheel_position);
    00352                         Vector3 sCRBWP = DReal2TVector(some_car_right_back_wheel_position);
    00353                         if ( GetMeetPointVectorAndUsecka(sCLBWP,sCRBWP,car_pos,car_dir) )
    00354                             thisCar->brake(2, 2);
    00355                         //DrawVectorInDS(car_pos,car_dir);
    00356                     }
    00357
    00358                     //speed = 0.3;
    00359                 //DrawUseckaInDS(Vector3(0,0,0),some_car_position,color_green);
    00360             }
    00361             //(*it).second->;
    00362         }
    00363
    00364         //*******************************************************************************
    00365         //semafory
    00392
    00393         //*******************************************************************************
    00394         //rozjizdeni, pokud stojim
    00395         if (thisCar->GetIsBreaking() == 0)
    00396             thisCar->accelerate(3,500);
    00397
    00398     }
    00403     static Sector* findCarCurrentSector(SectorMap sectors, Vector3 car_pos, Vector3 car_dir, Simulator* simulator)
    00404     {
    00405         int sector_id = 0;
    00406
    00407         //oprava bugu v sektoru 11 v kruhovem objezdu
    00408 //      Sector* sector4 = simulator->getSectorByID(4);
    00409 //      sector_id = sector4->id;
    00410 //      std::vector<Border> bc4 = sector4->getBorders();
    00411 //      if ( isCarInBorders(bc4, car_pos, car_dir, sector_id) == 1 )
    00412 //          return sector4;
    00413
    00414         //oprava bugu v sektoru 11 v kruhovem objezdu
    00415         Sector* sector11 = simulator->getSectorByID(11);
    00416         sector_id = sector11->id;
    00417         std::vector<Border> bc11 = sector11->getBorders();
    00418         if ( isCarInBorders(bc11, car_pos, car_dir, sector_id) == 1 )
    00419             return sector11;
    00420
    00421 //      Sector* sector8 = simulator->getSectorByID(8);
    00422 //      sector_id = sector8->id;
    00423 //      std::vector<Border> bc8 = sector8->getBorders();
    00424 //      if ( isCarInBorders(bc8, car_pos, car_dir, sector_id) == 1 )
    00425 //          return sector8;
    00426
    00427
    00428
    00429         for (SectorMap::iterator sit = sectors.begin(); sit != sectors.end(); sit++)
    00430         {
    00431             //BorderContainer
    00432             sector_id = (*sit).second->id;
    00433             std::vector<Border> bc = (*sit).second->getBorders();
    00434             if ( isCarInBorders(bc, car_pos, car_dir, sector_id) == 1 )
    00435                 return (*sit).second;
    00436         }
    00437         //_DEBUG("carOutOfSector car_pos " << car_pos);
    00438         return NULL; //car in no sector
    00439     }
    00444     static int isCarInGivenSector(SectorMap sectors, Vector3 car_pos, Vector3 car_dir, int givenSectorID)
    00445     {
    00446         for (SectorMap::iterator sit = sectors.begin(); sit != sectors.end(); sit++)
    00447         {
    00448             //BorderContainer
    00449             std::vector<Border> bc = (*sit).second->getBorders();
    00450             if ((*sit).first == givenSectorID )
    00451                 return isCarInBorders(bc, car_pos, car_dir, (*sit).second->id);
    00452             //_DEBUG("car_position_in_sector " << IsCarInBorders(bc, car_pos, car_dir) << " sector " << (*sit).first);
    00453         }
    00454         return -1; // sector not found
    00455     }
    00460     static int isCarInBorders(std::vector<Border> bc, Vector3 car_pos, Vector3 car_dir, int sector_id)
    00461     {
    00462         int nCountCrossings = 0;
    00463         dReal minX = 100000;
    00464         dReal minY = 100000;
    00465         dReal maxX = -100000;
    00466         dReal maxY = -100000;
    00467         //first filter only sectors, where point is within biggest rectangle
    00468         for (BorderContainer::iterator bit = bc.begin(); bit != bc.end(); bit++)
    00469         {
    00470             //std::cout << "side:" << (*bit)._side;
    00471             if ( (*bit)._side == 4) //jump out if it is internal path
    00472                 continue;
    00473             int nPolygonSize = (*bit)._polygon->_size;
    00474             for (int i = 0; i < nPolygonSize; i++)
    00475             {
    00476                 Vector3 startPoint = Vector3
    00477                     (
    00478                     (*bit)._polygon->_vertices[i][0],
    00479                     (*bit)._polygon->_vertices[i][1],
    00480                     (*bit)._polygon->_vertices[i][2]
    00481                     );
    00482
    00483                 if ((*bit)._polygon->_vertices[i][0] < minX) minX = (*bit)._polygon->_vertices[i][0];
    00484                 if ((*bit)._polygon->_vertices[i][1] < minY) minY = (*bit)._polygon->_vertices[i][1];
    00485                 if ((*bit)._polygon->_vertices[i][0] > maxX) maxX = (*bit)._polygon->_vertices[i][0];
    00486                 if ((*bit)._polygon->_vertices[i][1] > maxY) maxY = (*bit)._polygon->_vertices[i][1];
    00487             }
    00488         }
    00489 //      if ( sector_id == 4 || sector_id == 5 )
    00490 //          _DEBUG(":(( " << car_pos << " sector " << sector_id);
    00491         // point has to be in the quarter made by max and min values
    00492 //      if ( sector_id == 4 )
    00493 //          _DEBUG(car_pos.X() << " " << car_pos.Y() << " " << minX << " " << minY << " " << maxX << " " << maxY << "sector" << sector_id);
    00494         if (
    00495             car_pos.X() <= maxX
    00496             && car_pos.X() >= minX
    00497             && car_pos.Y() <= maxY
    00498             && car_pos.Y() >= minY
    00499             )
    00500             {
    00501 //              if ( sector_id == 14  )
    00502 //                  _DEBUG(":)");
    00503                 //number of crossings must not be dividable by 2
    00504                 for (BorderContainer::iterator bit = bc.begin(); bit != bc.end(); bit++)
    00505                 {
    00506                     //std::cout << "side:" << (*bit)._side;
    00507                     if ( (*bit)._side == 4) //jump out if it crossing internal path
    00508                         continue;
    00509                     int nPolygonSize = (*bit)._polygon->_size;
    00510                     for (int i = 0; i < nPolygonSize; i++)
    00511                     {
    00512                         Vector3 startPoint = Vector3
    00513                             (
    00514                             (*bit)._polygon->_vertices[i][0],
    00515                             (*bit)._polygon->_vertices[i][1],
    00516                             (*bit)._polygon->_vertices[i][2]
    00517                             );
    00518                         if ( i + 1 < nPolygonSize)
    00519                         {
    00520                             Vector3 endPoint = Vector3
    00521                                 (
    00522                                 (*bit)._polygon->_vertices[i+1][0],
    00523                                 (*bit)._polygon->_vertices[i+1][1],
    00524                                 (*bit)._polygon->_vertices[i+1][2]
    00525                                 );
    00526
    00527                             //call trace function and fill array
    00528                             //GetMeetPointVectorAndUsecka(startPoint,endPoint,car_pos,car_dir,color_yellow,meetDistances,meetPoints,myMeetIndex,meetAngles,1);
    00529                             VectorMath wm = VectorMath();
    00530                             Vector3 return_cross_point = Vector3();
    00531                             //_DEBUG(startPoint << " " << endPoint);
    00532                             double dResult = wm.GetDistanceFromUsecka(startPoint, endPoint, car_pos, car_dir, return_cross_point);
    00533                             if (dResult != TRACE_INFINITY && dResult > 0.01)
    00534                                 nCountCrossings ++;
    00535                         }
    00536                     }
    00537                 }
    00538 //              if ( sector_id == 4 )
    00539 //                  _DEBUG("nCountCrossings " << nCountCrossings);
    00540                 if (nCountCrossings % 2 == 1)
    00541                     return 1;
    00542             }
    00543
    00544         return 0;
    00545     }
    00551     static int IsCarInSector2(std::vector<Border> bc, Vector3 carPosition, Vector3 carSpeed)
    00552     {
    00553         int nIsFirstBorder = 1;
    00554         int nVerticlesIndex = 0;
    00555         Vector3 vertices[40];
    00556
    00557         for (BorderContainer::iterator bit = bc.begin(); bit != bc.end(); bit++)
    00558         {
    00559             //std::cout << "side:" << (*bit)._side;
    00560             if ( (*bit)._side == 0 || (*bit)._side == 2) //jump out if it is start or end of sector
    00561                 continue;
    00562             int nPolygonSize = (*bit)._polygon->_size;
    00563             if (nIsFirstBorder == 1)
    00564             {
    00565                 for (int i = 0; i < nPolygonSize; i++)
    00566                 {
    00567                     Vector3 startPoint = Vector3
    00568                         (
    00569                         (*bit)._polygon->_vertices[i][0],
    00570                         (*bit)._polygon->_vertices[i][1],
    00571                         (*bit)._polygon->_vertices[i][2]
    00572                         );
    00573                         vertices[nVerticlesIndex] = startPoint;
    00574                         nVerticlesIndex ++;
    00575                 }
    00576                 nIsFirstBorder = 0;
    00577             }
    00578             else
    00579             {
    00580                 for (int i = nPolygonSize-1; i > -1; i--)
    00581                 {
    00582                     Vector3 startPoint = Vector3
    00583                         (
    00584                         (*bit)._polygon->_vertices[i][0],
    00585                         (*bit)._polygon->_vertices[i][1],
    00586                         (*bit)._polygon->_vertices[i][2]
    00587                         );
    00588                     vertices[nVerticlesIndex] = startPoint;
    00589                     nVerticlesIndex ++;
    00590                 }
    00591             }
    00592
    00593         }
    00594         VectorMath wm = VectorMath();
    00595 //      for (int ii = 0; ii < nVerticlesIndex; ii++)
    00596 //          _DEBUG("verticles" << vertices[ii]);
    00597 //      _DEBUG("car_pos" << carPosition);
    00598         return wm.pointInPolygon(carPosition, vertices, nVerticlesIndex-1);
    00599     }
    00603     static Vector3 GetMeetPointVectorAndUsecka(Vector3 startPoint, Vector3 endPoint, Vector3 car_pos, Vector3 car_dir, int * aColor, double * meetDistances, Vector3 * meetPoints, int &meetIndex, int * meetAngles, int angleWithVelocity)
    00604     {
    00605         //      double meetDistances [20];
    00606         //      Vector3 meetPoints [20];
    00607
    00608         VectorMath wm = VectorMath();
    00609         Vector3 return_cross_point = Vector3();
    00610         double dResult = wm.GetDistanceFromUsecka(startPoint, endPoint, car_pos, car_dir, return_cross_point);
    00611         if (dResult != TRACE_INFINITY && dResult > 0.01)
    00612         {
    00613             meetDistances[meetIndex] = dResult;
    00614             meetPoints[meetIndex] = return_cross_point;
    00615             meetAngles[meetIndex] = angleWithVelocity;
    00616             meetIndex++;
    00617             DrawBoxInDs(return_cross_point,aColor);
    00618         }
    00619         return return_cross_point;
    00620     }
    00624     static bool GetMeetPointVectorAndUsecka(Vector3 startPoint, Vector3 endPoint, Vector3 car_pos, Vector3 car_dir)
    00625     {
    00626         //      double meetDistances [20];
    00627         //      Vector3 meetPoints [20];
    00628
    00629         VectorMath wm = VectorMath();
    00630         Vector3 return_cross_point = Vector3();
    00631         double dResult = wm.GetDistanceFromUsecka(startPoint, endPoint, car_pos, car_dir, return_cross_point);
    00632         if (dResult != TRACE_INFINITY && dResult > 0.01)
    00633         {
    00634             return true;
    00635         }
    00636         return false;
    00637     }
    00638
    00642     static Vector3 DReal2TVector(const dReal * position)
    00643     {
    00644         return Vector3(position[0],position[1],position[2]);
    00645     }
    00649     static Vector3 DRealRotation2TVector(const dReal * position)
    00650     {
    00651         return Vector3(-position[0],position[1],position[2]);
    00652     }
    00656     static Vector3 DRealRotation2TVector2(const dReal * position)
    00657     {
    00658         return Vector3(position[0],position[1],position[2]);
    00659     }
    00660     virtual ~CarTrace();
    00661 };
    00662
    00663 #endif /*CARTRACE_H_*/