Tabelka liczona od pozycji pojazdu i przejechanej odległości

Driver.cpp

@@ -104,10 +104,57 @@ std::string StopReasonTable[] = {
     "Error", // stopError //z powodu błędu w obliczeniu drogi hamowania
 };
 
+double GetDistanceToEvent(TTrack* track, TEvent* event, double scan_dir, double start_dist, int iter = 0, bool back = false)
+{
+    std::shared_ptr<TSegment> segment = track->CurrentSegment();
+    vector3 pos_event = event->PositionGet();
+    double len1, len2;
+    double sd = scan_dir;
+    double seg_len = scan_dir > 0 ? 0.0 : 1.0;
+    double dzielnik = 1.0 / segment->GetLength();// rozdzielczosc mniej wiecej 1m
+    int krok = 0; // krok obliczeniowy do sprawdzania czy odwracamy
+    len2 = (pos_event - segment->FastGetPoint(seg_len)).Length();
+    do
+    {
+        len1 = len2;
+        seg_len += scan_dir > 0 ? dzielnik : -dzielnik;
+        len2 = (pos_event - segment->FastGetPoint(seg_len)).Length();
+        krok++;
+    } while ((len1 > len2) && (seg_len >= dzielnik && (seg_len <= (1 - dzielnik))));
+    //trzeba sprawdzić czy seg_len nie osiągnął skrajnych wartości, bo wtedy
+    // trzeba sprawdzić tor obok
+    if (1 == krok)
+        sd = -sd; // jeśli tylko jeden krok tzn, że event przy poprzednim sprawdzaym torze
+    if (((seg_len <= dzielnik) || (seg_len > (1 - dzielnik))) && (iter < 3))
+    { // przejście na inny tor
+        track = track->Neightbour(int(sd), sd);
+        start_dist += (1 == krok) ? 0 : back ? -segment->GetLength() : segment->GetLength();
+        return GetDistanceToEvent(track, event, sd, start_dist, ++iter, 1 == krok ? true : false);
+    }
+    else
+    { // obliczenie mojego toru
+        seg_len -= scan_dir > 0 ? dzielnik : -dzielnik; //trzeba wrócić do pozycji len1
+        seg_len = scan_dir < 0 ? 1 - seg_len : seg_len;
+        seg_len = back ? 1 - seg_len : seg_len; // odwracamy jeśli idzie do tyłu
+        start_dist -= back ? segment->GetLength() : 0;
+        return start_dist + (segment->GetLength() * seg_len);
+    }
+};
+
 //---------------------------------------------------------------------------
 //---------------------------------------------------------------------------
 //---------------------------------------------------------------------------
 
+inline TSpeedPos::TSpeedPos(TTrack *track, double dist, int flag)
+{
+    Set(track, dist, flag);
+};
+
+inline TSpeedPos::TSpeedPos(TEvent *event, double dist, TOrders order)
+{
+    Set(event, dist, order);
+};
+
 void TSpeedPos::Clear()
 {
     iFlags = 0; // brak flag to brak reakcji
@@ -185,64 +232,10 @@ void TSpeedPos::CommandCheck()
     }
 };
 
-bool TSpeedPos::Update(vector3 *p, vector3 *dir, double &len)
-{ // przeliczenie odległości od punktu (*p), w kierunku (*dir), zaczynając od pojazdu
-    // dla kolejnych pozycji podawane są współrzędne poprzedniego obiektu w (*p)
-    vector3 v = vPos - *p; // wektor od poprzedniego obiektu (albo pojazdu) do punktu zmiany
-    fDist = v.Length(); // długość wektora to odległość pomiędzy czołem a sygnałem albo początkiem toru
-    if (len == 0.0)
-    { // jeżeli liczymy względem pojazdu
-        double angle;
-        if( dir ) {
-            angle =
-                glm::dot(
-                    glm::normalize( glm::make_vec3( &v.x ) ), // towards target
-                    glm::normalize( glm::make_vec3( &dir->x ) ) ); // orientation at scan point
-        }
-        else {
-            angle = fDist;
-        }
-        if (angle < 0.0) // iloczyn skalarny jest ujemny, gdy punkt jest z tyłu
-        { // jeśli coś jest z tyłu, to dokładna odległość nie ma już większego znaczenia
-            fDist = -fDist; // potrzebne do badania wyjechania składem poza ograniczenie
-            if (iFlags & spElapsed) {
-                // jeśli minięty (musi być minięty również przez końcówkę składu)
-                // NOTE: empty branch, why?
-            }
-            else
-            {
-                // minięty - będziemy liczyć odległość względem przeciwnego końca
-                // toru (nadal może być z przodu i ograniczać)
-                iFlags ^= spElapsed;
-                if ((iFlags & (spEnd | spTrack | spEnabled)) == (spTrack | spEnabled)) {
-                    // tylko jeśli (istotny) tor, bo eventy są punktowe
-                    if (trTrack) {
-                        // może być NULL, jeśli koniec toru (????)
-                        vPos =
-                            (iFlags & spReverse) ?
-                                trTrack->CurrentSegment()->FastGetPoint_0() :
-                                trTrack->CurrentSegment()->FastGetPoint_1(); // drugi koniec istotny
-                    }
-                }
-            }
-        }
-        else {
-            if( fDist < 50.0 ) {
-                // old sceneries use trick of placing 'helper' semaphores underground, which can lead to vehicles running over them instead of stopping in front of them
-                // to account for it at short distances we redo distance calculation on 2d plane
-                fDist = glm::length( glm::vec3( v.x, 0.0, v.z ) );
-            }
-        }
-    }
-
-    if (fDist > 0.0) // nie może być 0.0, a przypadkiem mogło by się trafić i było by źle
-        if ((iFlags & spElapsed) == 0) // 32 ustawione, gdy obiekt już został minięty
-        { // jeśli obiekt nie został minięty, można od niego zliczać narastająco (inaczej może być
-            // problem z wektorem kierunku)
-            len = fDist = len + fDist; // zliczanie dlugości narastająco
-            *p = vPos; // nowy punkt odniesienia
-            *dir = Normalize(v); // nowy wektor kierunku od poprzedniego obiektu do aktualnego
-        }
+bool TSpeedPos::Update()
+{
+    if (fDist < 0.0)
+        iFlags |= spElapsed; // trzeba zazanaczyć, że minięty
     if (iFlags & spTrack) // jeśli tor
     {
         if (trTrack) // może być NULL, jeśli koniec toru (???)
@@ -298,6 +291,11 @@ bool TSpeedPos::Update(vector3 *p, vector3 *dir, double &len)
     return false;
 };
 
+inline void TSpeedPos::UpdateDistance(double dist)
+{ // aktualizuje odległość we wpisie
+    fDist -= dist;
+}
+
 std::string TSpeedPos::GetName()
 {
 	if (iFlags & spTrack) // jeśli tor
@@ -456,16 +454,21 @@ void TController::TableTraceRoute(double fDistance, TDynamicObject *pVehicle)
         pTrack = pVehicle->RaTrackGet(); // odcinek, na którym stoi
         fTrackLength = pVehicle->RaTranslationGet(); // pozycja na tym torze (odległość od Point1)
         fLastDir = pVehicle->DirectionGet() * pVehicle->RaDirectionGet(); // ustalenie kierunku skanowania na torze
-        if( fLastDir > 0.0 ) {
+        double odl_czola_od_wozka = (pVehicle->AxlePositionGet() - pVehicle->RearPosition()).Length();
+        if( fLastDir < 0.0 ) {
             // jeśli w kierunku Point2 toru
             fTrackLength = pTrack->Length() - fTrackLength; // przeskanowana zostanie odległość do Point2
         }
+        fTrackLength -= odl_czola_od_wozka;
+        fCurrentDistance = -fLength - fTrackLength; // aktualna odległość ma być ujemna gdyż jesteśmy na końcu składu
         fLastVel = pTrack->VelocityGet(); // aktualna prędkość
         sSpeedTable.clear();
         iLast = -1;
         tLast = nullptr; //żaden nie sprawdzony
         SemNextIndex = -1;
         SemNextStopIndex = -1;
+        fTrackLength = pTrack->Length(); //skasowanie zmian w zmiennej żeby poprawnie liczyło w dalszych krokach
+        MoveDistanceReset(); // AI startuje 1s po zaczęciu jazdy i mógł już coś przejechać
     }
     else {
         if( iTableDirection == 0 ) { return; }
@@ -495,21 +498,14 @@ void TController::TableTraceRoute(double fDistance, TDynamicObject *pVehicle)
                 }
             }
         }
+
         auto const &lastspeedpoint = sSpeedTable[ iLast ];
         pTrack = lastspeedpoint.trTrack;
         assert( pTrack != nullptr );
         // flaga ustawiona, gdy Point2 toru jest blizej
-        fLastDir = (
-            TestFlag( lastspeedpoint.iFlags, spReverse ) ?
-                -1.0 :
-                 1.0 );
-        // aktualna odleglosc do jego Point1
-        fCurrentDistance = lastspeedpoint.fDist;
-        // nie doliczac dlugosci gdy: miniety początek lub jazda do konca toru
-        fTrackLength = (
-            ( lastspeedpoint.iFlags & ( spElapsed | spEnd ) != 0 ) ?
-                0.0 :
-                pTrack->Length() );
+        fLastDir = lastspeedpoint.iFlags & spReverse  ? -1.0 : 1.0;
+        fCurrentDistance = lastspeedpoint.fDist; // aktualna odleglosc do jego Point1
+        fTrackLength = pTrack->Length();
     }
 
     if( iTableDirection == 0 ) {
@@ -517,11 +513,6 @@ void TController::TableTraceRoute(double fDistance, TDynamicObject *pVehicle)
         return;
     }
 
-    if( fCurrentDistance >= fDistance ) {
-        // all done
-        return;
-    }
-
     while (fCurrentDistance < fDistance)
     {
         if (pTrack != tLast) // ostatni zapisany w tabelce nie był jeszcze sprawdzony
@@ -533,70 +524,69 @@ void TController::TableTraceRoute(double fDistance, TDynamicObject *pVehicle)
             if( ( pEvent = CheckTrackEvent( fLastDir, pTrack ) ) != nullptr ) // jeśli jest semafor na tym torze
             { // trzeba sprawdzić tabelkę, bo dodawanie drugi raz tego samego przystanku nie jest korzystne
                 if (TableNotFound(pEvent)) // jeśli nie ma
-                    if( TableAddNew() )
-                    {
-                        if( Global::iWriteLogEnabled & 8 ) {
-                            WriteLog( "Speed table for " + OwnerName() + " found new event, " + pEvent->asName );
-                        }
-                        auto &newspeedpoint = sSpeedTable[ iLast ];
-                        if( newspeedpoint.Set( pEvent, fCurrentDistance, OrderCurrentGet() ) ) {
+                {
+                    TableAddNew(); // zawsze jest true
 
-                            fDistance = fCurrentDistance; // jeśli sygnał stop, to nie ma potrzeby dalej skanować
-                            SemNextStopIndex = iLast;
-                            if( SemNextIndex == -1 ) {
-                                SemNextIndex = iLast;
-                            }
-                            if( Global::iWriteLogEnabled & 8 ) {
-                                WriteLog( "(stop signal from "
-                                    + ( SemNextStopIndex != -1 ? sSpeedTable[SemNextStopIndex].GetName() : "unknown semaphor" )
-                                    + ")" );
-                            }
+                    if (Global::iWriteLogEnabled & 8) {
+                        WriteLog("Speed table for " + OwnerName() + " found new event, " + pEvent->asName);
+                    }
+                    auto &newspeedpoint = sSpeedTable[iLast];
+                    if (newspeedpoint.Set(pEvent, GetDistanceToEvent(pTrack, pEvent, fLastDir, fCurrentDistance), OrderCurrentGet())) {
+
+                        fDistance = newspeedpoint.fDist; // jeśli sygnał stop, to nie ma potrzeby dalej skanować
+                        SemNextStopIndex = iLast;
+                        if (SemNextIndex == -1) {
+                            SemNextIndex = iLast;
                         }
-                        else {
-                            if( ( true == newspeedpoint.IsProperSemaphor( OrderCurrentGet() ) )
-                             && ( SemNextIndex == -1 ) ) {
-                                SemNextIndex = iLast; // sprawdzamy czy pierwszy na drodze
-                            }
-                            if( Global::iWriteLogEnabled & 8 ) {
-                                WriteLog( "(forward signal for "
-                                    +  ( SemNextIndex != -1 ? sSpeedTable[SemNextIndex].GetName() : "unknown semaphor" )
-                                    + ")" );
-                            }
+                        if (Global::iWriteLogEnabled & 8) {
+                            WriteLog("(stop signal from "
+                                + (SemNextStopIndex != -1 ? sSpeedTable[SemNextStopIndex].GetName() : "unknown semaphor")
+                                + ")");
                         }
                     }
+                    else {
+                        if ((true == newspeedpoint.IsProperSemaphor(OrderCurrentGet()))
+                            && (SemNextIndex == -1)) {
+                            SemNextIndex = iLast; // sprawdzamy czy pierwszy na drodze
+                        }
+                        if (Global::iWriteLogEnabled & 8) {
+                            WriteLog("(forward signal for "
+                                + (SemNextIndex != -1 ? sSpeedTable[SemNextIndex].GetName() : "unknown semaphor")
+                                + ")");
+                        }
+                    }
+                }
             } // event dodajemy najpierw, żeby móc sprawdzić, czy tor został dodany po odczytaniu prędkości następnego
 
             if( ( pTrack->VelocityGet() == 0.0 ) // zatrzymanie
              || ( pTrack->iAction ) // jeśli tor ma własności istotne dla skanowania
              || ( pTrack->VelocityGet() != fLastVel ) ) // następuje zmiana prędkości
             { // odcinek dodajemy do tabelki, gdy jest istotny dla ruchu
-                if (TableAddNew())
-                { // teraz dodatkowo zapamiętanie wybranego segmentu dla skrzyżowania
-                    sSpeedTable[ iLast ].Set(
-                        pTrack, fCurrentDistance,
-                        ( fLastDir < 0 ?
-                            spEnabled | spReverse :
-                            spEnabled ) ); // dodanie odcinka do tabelki z flagą kierunku wejścia
-                    if (pTrack->eType == tt_Cross) {
-                        // na skrzyżowaniach trzeba wybrać segment, po którym pojedzie pojazd
-                        // dopiero tutaj jest ustalany kierunek segmentu na skrzyżowaniu
-                        sSpeedTable[iLast].iFlags |=
-                            ( ( pTrack->CrossSegment(
-                                    ( fLastDir < 0 ?
-                                        tLast->iPrevDirection :
-                                        tLast->iNextDirection ),
-                                        iRouteWanted ) & 0xf ) << 28 ); // ostatnie 4 bity pola flag
-                        sSpeedTable[iLast].iFlags &= ~spReverse; // usunięcie flagi kierunku, bo może być błędna
-                        if( sSpeedTable[ iLast ].iFlags < 0 ) {
-                            sSpeedTable[ iLast ].iFlags |= spReverse; // ustawienie flagi kierunku na podstawie wybranego segmentu
-                        }
-                        if( int( fLastDir ) * sSpeedTable[ iLast ].iFlags < 0 ) {
-                            fLastDir = -fLastDir;
-                        }
-                        if( AIControllFlag ) {
-                            // dla AI na razie losujemy kierunek na kolejnym skrzyżowaniu
-                            iRouteWanted = 1 + Random( 3 );
-                        }
+                TableAddNew();
+                sSpeedTable[ iLast ].Set(
+                    pTrack, fCurrentDistance,
+                    ( fLastDir < 0 ?
+                        spEnabled | spReverse :
+                        spEnabled ) ); // dodanie odcinka do tabelki z flagą kierunku wejścia
+                if (pTrack->eType == tt_Cross) {
+                    // na skrzyżowaniach trzeba wybrać segment, po którym pojedzie pojazd
+                    // dopiero tutaj jest ustalany kierunek segmentu na skrzyżowaniu
+                    sSpeedTable[iLast].iFlags |=
+                        ((pTrack->CrossSegment(
+                        (fLastDir < 0 ?
+                            tLast->iPrevDirection :
+                            tLast->iNextDirection),
+                            iRouteWanted) & 0xf) << 28); // ostatnie 4 bity pola flag
+                    sSpeedTable[iLast].iFlags &= ~spReverse; // usunięcie flagi kierunku, bo może być błędna
+                    if (sSpeedTable[iLast].iFlags < 0) {
+                        sSpeedTable[iLast].iFlags |= spReverse; // ustawienie flagi kierunku na podstawie wybranego segmentu
+                    }
+                    if (int(fLastDir) * sSpeedTable[iLast].iFlags < 0) {
+                        fLastDir = -fLastDir;
+                    }
+                    if (AIControllFlag) {
+                        // dla AI na razie losujemy kierunek na kolejnym skrzyżowaniu
+                        iRouteWanted = 1 + Random(3);
                     }
                 }
             }
@@ -688,14 +678,16 @@ void TController::TableCheck(double fDistance)
     }
     else if (iTableDirection)
     { // trzeba sprawdzić, czy coś się zmieniło
-        vector3 dir = pVehicles[0]->VectorFront() * pVehicles[0]->DirectionGet(); // wektor kierunku jazdy
-        vector3 pos = pVehicles[0]->HeadPosition(); // zaczynamy od pozycji pojazdu
-        double len = 0.0; // odległość będziemy zliczać narastająco
+        for (auto &sp : sSpeedTable)
+        {
+            sp.UpdateDistance(MoveDistanceGet()); // aktualizacja odległości dla wszystkich pozycji tabeli
+        }
+        MoveDistanceReset(); // kasowanie odległości po aktualizacji tabelki
         for( int i = 0; i <= iLast; ++i )
         { // aktualizacja rekordów z wyjątkiem ostatniego
             if (sSpeedTable[i].iFlags & spEnabled) // jeśli pozycja istotna
             {
-                if (sSpeedTable[i].Update(&pos, &dir, len))
+                if (sSpeedTable[i].Update())
                 {
                     if( Global::iWriteLogEnabled & 8 ) {
                         WriteLog( "Speed table for " + OwnerName() + " detected switch change at " + sSpeedTable[ i ].trTrack->NameGet() + " (generating fresh trace)" );
@@ -738,7 +730,7 @@ void TController::TableCheck(double fDistance)
                 }
             }
         }
-        sSpeedTable[iLast].Update(&pos, &dir, len); // aktualizacja ostatniego
+        sSpeedTable[iLast].Update(); // aktualizacja ostatniego
         // WriteLog("TableCheck: Upate last track. Dist=" + AnsiString(sSpeedTable[iLast].fDist));
         if( sSpeedTable[ iLast ].fDist < fDistance ) {
             TableTraceRoute( fDistance, pVehicles[ 1 ] ); // doskanowanie dalszego odcinka

Driver.h

@@ -145,8 +145,12 @@ class TSpeedPos
     void CommandCheck();
 
   public:
+    TSpeedPos(TTrack *track, double dist, int flag);
+    TSpeedPos(TEvent *event, double dist, TOrders order);
+    TSpeedPos() {};
     void Clear();
-    bool Update(vector3 *p, vector3 *dir, double &len);
+    bool Update();
+    void UpdateDistance(double dist);
     bool Set(TEvent *e, double d, TOrders order = Wait_for_orders);
     void Set(TTrack *t, double d, int f);
     std::string TableText();
@@ -175,7 +179,8 @@ class TController
     TEvent *eSignSkip = nullptr; // można pominąć ten SBL po zatrzymaniu
     std::size_t SemNextIndex{ std::size_t(-1) };
     std::size_t SemNextStopIndex{ std::size_t( -1 ) };
-  private: // parametry aktualnego składu
+    double dMoveLen = 0.0; // odległość przejechana od ostatniego sprawdzenia tabelki
+private: // parametry aktualnego składu
     double fLength = 0.0; // długość składu (do wyciągania z ograniczeń)
     double fMass = 0.0; // całkowita masa do liczenia stycznej składowej grawitacji
     double fAccGravity = 0.0; // przyspieszenie składowej stycznej grawitacji
@@ -366,7 +371,20 @@ class TController
     void TableCheck(double fDistance);
     TCommandType TableUpdate(double &fVelDes, double &fDist, double &fNext, double &fAcc);
     void TablePurger();
-public:
+    inline double MoveDistanceGet()
+    {
+        return dMoveLen;
+    }
+    inline void MoveDistanceReset()
+    {
+        dMoveLen = 0.0;
+    }
+  public:
+    inline void MoveDistanceAdd(double distance)
+    {
+        dMoveLen += distance * iDirection; //jak jedzie do tyłu to trzeba uwzględniać, że distance jest ujemna
+    }
+  public:
     std::size_t TableSize() const { return sSpeedTable.size(); }
     void TableClear();
     int TableDirection() { return iTableDirection; }

DynObj.cpp

@@ -3026,6 +3026,8 @@ bool TDynamicObject::Update(double dt, double dt1)
     MoverParameters->AccN *= -ABuGetDirection();
 */
     // if (dDOMoveLen!=0.0) //Ra: nie może być, bo blokuje Event0
+    if (Mechanik)
+        Mechanik->MoveDistanceAdd(dDOMoveLen); // dodanie aktualnego przemieszczenia
     Move(dDOMoveLen);
     if (!bEnabled) // usuwane pojazdy nie mają toru
     { // pojazd do usunięcia

Ground.cpp

@@ -208,7 +208,8 @@ void TGroundNode::InitNormals()
         {
             v1 = Piece->vertices[i + 0].position - Piece->vertices[i + 1].position;
             v2 = Piece->vertices[i + 1].position - Piece->vertices[i + 2].position;
-            n1 = glm::normalize(glm::cross(v1, v2));
+            auto c = glm::cross(v1, v2);
+            n1 = glm::length(c) != 0 ? glm::normalize(c) : glm::vec3();
             if( Piece->vertices[i + 0].normal == glm::vec3() )
                 Piece->vertices[i + 0].normal = (n1);
             if( Piece->vertices[i + 1].normal == glm::vec3() )