diff --git a/Driver.cpp b/Driver.cpp index 98709cc7..290f5b06 100644 --- a/Driver.cpp +++ b/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 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 diff --git a/Driver.h b/Driver.h index 512e1d23..33009584 100644 --- a/Driver.h +++ b/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; } diff --git a/DynObj.cpp b/DynObj.cpp index a47db564..81a44c3a 100644 --- a/DynObj.cpp +++ b/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 diff --git a/Ground.cpp b/Ground.cpp index a131644e..8feabd04 100644 --- a/Ground.cpp +++ b/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() )