diff --git a/utilities/Classes.h b/utilities/Classes.h index 6a5ca167e..ee4442982 100644 --- a/utilities/Classes.h +++ b/utilities/Classes.h @@ -85,6 +85,7 @@ enum class TCommandType cm_SectionVelocity, //ograniczenie prędkości na odcinku cm_ShuntVelocity, // prędkość manewrowa na semaforze cm_SetProximityVelocity, // informacja wstępna o ograniczeniu + cm_PointVelocity, // ograniczenie dla czoła w punkcie podaną odległość za komórką, odczytane przy jej minięciu cm_ChangeDirection, cm_PassengerStopPoint, cm_OutsideStation, diff --git a/vehicle/Driver.cpp b/vehicle/Driver.cpp index 4aa09683c..2bd05baab 100644 --- a/vehicle/Driver.cpp +++ b/vehicle/Driver.cpp @@ -202,6 +202,7 @@ void TSpeedPos::Clear() iFlags = 0; // brak flag to brak reakcji fVelNext = -1.0; // prędkość bez ograniczeń fSectionVelocityDist = 0.0; //brak długości + fPointDist = 0.0; fDist = 0.0; vPos = glm::dvec3(0, 0, 0); trTrack = nullptr; // brak wskaźnika @@ -212,6 +213,9 @@ void TSpeedPos::CommandCheck() TCommandType command = evEvent->input_command(); double value1 = evEvent->input_value(1); double value2 = evEvent->input_value(2); + // the point of a PointVelocity limit is taken anew from the current command + iFlags &= ~spPointVelocity; + fPointDist = 0.0; switch (command) { case TCommandType::cm_ShuntVelocity: @@ -266,6 +270,15 @@ void TSpeedPos::CommandCheck() iFlags |= spProximityVelocity; // fSectionVelocityDist = value2; break; + case TCommandType::cm_PointVelocity: + // PointVelocity v d: the head has to be at v (-1: no limit) at the point d metres beyond the cell, + // the limit ends when the head passes that point. the cell is read until the head passes the cell, + // what it says then holds for the point (the warning disc of a level crossing, Osp1/Osp2) + fVelNext = value1; + fPointDist = std::max( value2, 0.0 ); + iFlags &= ~(spShuntSemaphor | spPassengerStopPoint | spStopOnSBL | spSemaphor); // not a signal, even if the cell was one before + iFlags |= spPointVelocity; + break; case TCommandType::cm_OutsideStation: // w trybie manewrowym: skanować od niej wstecz i stanąć po wyjechaniu za sygnalizator i // zmienić kierunek @@ -339,6 +352,7 @@ bool TSpeedPos::Update() else if (iFlags & spEvent && ((iFlags & spElapsed) == 0 || fVelNext == 0.0)) { // jeśli event // ignore already passed signals, but keep an eye on overrun stops + // (a PointVelocity cell is latched when the head passes it, except at 0, which has to be released by the cell) // odczyt komórki pamięci najlepiej by było zrobić jako notyfikację, // czyli zmiana komórki wywoła jakąś podaną funkcję CommandCheck(); // sprawdzenie typu komendy w evencie i określenie prędkości @@ -809,7 +823,7 @@ void TController::TableCheck(double fDistance) sSpeedTable[i].iFlags = 0; // to nie ma go po co trzymać (odtykacz usunie ze środka) } } - else if (sSpeedTable[i].iFlags & spEvent && sSpeedTable[i].fDist < (typeid(*sSpeedTable[i].evEvent) == typeid(putvalues_event) ? -fLength : 0) && mvOccupied->CategoryFlag == 2 && + else if (sSpeedTable[i].iFlags & spEvent && (sSpeedTable[i].iFlags & spPointVelocity) == 0 && sSpeedTable[i].fDist < (typeid(*sSpeedTable[i].evEvent) == typeid(putvalues_event) ? -fLength : 0) && mvOccupied->CategoryFlag == 2 && sSpeedTable[i].fDist < -0.75) // jeśli event { // jeśli jest z tyłu @@ -918,6 +932,7 @@ TCommandType TController::TableUpdate(double &fVelDes, double &fDist, double &fN mvOccupied->SecuritySystem.set_cabsignal_lock(false); // te flagi są ustawiane tutaj, w razie potrzeby iDrivigFlags &= ~(moveTrackEnd | moveSwitchFound | moveSignalFound | /*moveSpeedLimitFound*/ moveStopPointFound ); + update_point_limits(); for( std::size_t idx = 0; idx < sSpeedTable.size(); ++idx ) { // o ile dana pozycja tabelki jest istotna @@ -943,12 +958,14 @@ TCommandType TController::TableUpdate(double &fVelDes, double &fDist, double &fN if( v >= 0.0 // pozycje z prędkością -1 można spokojnie pomijać || railwaytrackend ) { - d = point.fDist; + // a PointVelocity limit lies fPointDist beyond its entry and stays ahead of us until the head passes it + auto const ispointlimit { TestFlag( point.iFlags, spPointVelocity ) }; + d = point.fDist + point.fPointDist; if( v >= 0.0 ) { // points located in front of us can potentially affect our acceleration and target speed if( d > 0.0 - && false == TestFlag(point.iFlags, spElapsed) ) { + && ( false == TestFlag(point.iFlags, spElapsed) || ispointlimit ) ) { // sygnał lub ograniczenie z przodu (+32=przejechane) // 2014-02: jeśli stoi, a ma do przejechania kawałek, to niech jedzie if( mvOccupied->Vel < 0.01 @@ -971,7 +988,7 @@ TCommandType TController::TableUpdate(double &fVelDes, double &fDist, double &fN a = std::lerp( a, AccPreferred, std::clamp( ( d - brakingdistance ) / brakingdistance, 0.0, 1.0 ) ); } } - if( d < fMinProximityDist + if( d < ( ispointlimit && v > 0.0 ? point_limit_approach() : fMinProximityDist ) && v < fVelDes ) { // jak jest już blisko, ograniczenie aktualnej prędkości fVelDes = v; @@ -1051,7 +1068,9 @@ TCommandType TController::TableUpdate(double &fVelDes, double &fDist, double &fN bStopTargetIsPassengerStop = ( v == 0.0 ) && ( ( point.iFlags & spPassengerStopPoint ) != 0 ); } // potentially update our current speed limit - if( v < VelLimitLastDist.first + // a point limit binds only the head at its point, it neither extends nor breaks a limit over a stretch of the line + if( false == ispointlimit + && v < VelLimitLastDist.first && ( d < 0 // the point counts as part of last speed limit either if it's behind us || VelLimitLastDist.second > 0 ) ) { // or if we already have the last limit ongoing VelLimitLastDist.second = d + fLength; @@ -1059,23 +1078,22 @@ TCommandType TController::TableUpdate(double &fVelDes, double &fDist, double &fN VelLimitLastDist.second += point.trTrack->Length(); } } - else { + else if( ( point.iFlags & ( spSwitch | spPassengerStopPoint | spPointVelocity ) ) == 0 ) { // if we found a point which isn't part of the current speed limit mark the limit as broken // NOTE: we exclude switches from this rule, as speed limits generally extend through these - if( ( point.iFlags & ( spSwitch | spPassengerStopPoint ) ) == 0 ) { - speedlimitiscontinuous = false; - } + speedlimitiscontinuous = false; } } // if (v>=0.0) // finding a point which doesn't impose speed limit means any potential active speed limit can't extend through entire scan table // NOTE: we test actual speed value for the given point, as events may receive (v = -1) as a way to disable them in irrelevant modes - if( point.fVelNext < 0 && (point.iFlags & (spSwitch | spPassengerStopPoint)) == 0 ) { + if( point.fVelNext < 0 && (point.iFlags & (spSwitch | spPassengerStopPoint | spPointVelocity)) == 0 ) { // NOTE: we exclude switches from this rule, as speed limits generally extend through these + // and point limits, which say nothing of the line they lie on speedlimitiscontinuous = false; } if (fNext >= 0.0) { // jeśli ograniczenie - if( (point.iFlags & (spEnabled | spEvent)) == (spEnabled | spEvent) && eSignNext == nullptr ) { // tylko sygnał przypisujemy + if( (point.iFlags & (spEnabled | spEvent)) == (spEnabled | spEvent) && (point.iFlags & spPointVelocity) == 0 && eSignNext == nullptr ) { // tylko sygnał przypisujemy (a point limit isn't one) // jeśli jeszcze nic nie zapisane tam eSignNext = point.evEvent; // dla informacji } @@ -1085,6 +1103,9 @@ TCommandType TController::TableUpdate(double &fVelDes, double &fDist, double &fN } } + // the point limits latched before, which aren't in the table any more + apply_point_limits( fVelDes, fDist, fNext, fAcc ); + // jeśli mieliśmy ograniczenie z semafora i nie ma przed nami if( VelSignalLast >= 0.0 && (iDrivigFlags & (moveSignalFound | moveSwitchFound | moveStopPointFound)) == 0 @@ -1422,9 +1443,208 @@ TController::TableUpdateStopPoint( TCommandType &Command, TSpeedPos &Point, doub return false; // } +bool TController::point_limit_latched( basic_event const *Cell ) const { + + return std::any_of( + m_pointlimits.begin(), m_pointlimits.end(), + [&]( point_limit const &Limit ) { return Limit.source == Cell; } ); +} + +double TController::point_limit_end( glm::dvec3 const &Heading ) const { + + auto const origin { pVehicle->GetPosition() }; + auto result { 0.0 }; + auto first { true }; + for( auto const *vehicle : { pVehicles[ end::front ], pVehicles[ end::rear ] } ) { + if( vehicle == nullptr ) { continue; } + for( auto const side : { end::front, end::rear } ) { + auto const along { glm::dot( vehicle->CouplerPosition( side ) - origin, Heading ) }; + result = ( first ? along : std::max( result, along ) ); + first = false; + } + } + return result; +} + +void TController::latch_point_limit( TSpeedPos const &Point ) { + + if( point_limit_latched( Point.evEvent ) ) { return; } + m_pointlimits.push_back( + point_limit { Point.evEvent, Point.fVelNext, Point.fDist + Point.fPointDist, Point.fPointDist, travel_direction() } ); + if( Global.iWriteLogEnabled & 8 ) { + WriteLog( "Speed table update for " + OwnerName() + ", latched point limit of " + Point.evEvent->m_name + + ": " + to_string( Point.fVelNext, 0 ) + " in " + to_string( m_pointlimits.back().distance, 0 ) + " m" ); + } +} + +void TController::update_point_limits() { + + if( pVehicle == nullptr ) { return; } + auto const position { pVehicle->GetPosition() }; + if( m_pointlimitsvehicle == pVehicle ) { + auto const movement { position - m_pointlimitsposition }; + auto const length { glm::length( movement ) }; + if( length >= 200.0 ) { + // a longer jump between two updates isn't driving: the consist was moved elsewhere + m_pointlimits.clear(); + } + else if( length > 0.0 ) { + auto const step { movement / length }; + for( auto &limit : m_pointlimits ) { + if( glm::length( limit.heading ) < 0.5 ) { + // the way wasn't known when the limit was latched, the head was passing the cell + limit.heading = step; + } + auto const along { glm::dot( step, limit.heading ) }; + limit.distance += ( along >= 0.0 ? -length : length ); + // the way towards the point follows the line, step by step; a step far off it + // (a jolt of a standing vehicle) doesn't turn it + if( std::abs( along ) > 0.5 ) { + limit.heading = ( along >= 0.0 ? step : -step ); + } + } + } + } + // a change of the vehicle (another cab) starts the measure from its place + auto const vehiclechanged { m_pointlimitsvehicle != pVehicle }; + auto const previousposition { m_pointlimitsposition }; + m_pointlimitsvehicle = pVehicle; + m_pointlimitsposition = position; + // the consist, as its end vehicles and its length + auto const *front { pVehicles[ end::front ] }; + auto const *rear { pVehicles[ end::rear ] }; + auto const consistchanged { + false == ( ( front == m_pointlimitsends[ 0 ] && rear == m_pointlimitsends[ 1 ] ) + || ( front == m_pointlimitsends[ 1 ] && rear == m_pointlimitsends[ 0 ] ) ) + || std::abs( fLength - m_pointlimitslength ) > 0.5 }; + m_pointlimitsends[ 0 ] = front; + m_pointlimitsends[ 1 ] = rear; + m_pointlimitslength = fLength; + // vehicles coupled or uncoupled at the end towards the point move the head towards it or away. only a change of + // the consist counts, as the end measured from the vehicle shifts on curves. another vehicle measured from (a cab + // change, which may come with a coupling in the same order) moves the end measured before to its place + for( auto &limit : m_pointlimits ) { + if( glm::length( limit.heading ) < 0.5 ) { continue; } + auto const consistend { point_limit_end( limit.heading ) }; + if( limit.endknown && consistchanged ) { + auto const previousend { + vehiclechanged ? + limit.end + glm::dot( previousposition - position, limit.heading ) : + limit.end }; + limit.distance -= consistend - previousend; + } + limit.end = consistend; + limit.endknown = true; + } + + auto const travel { travel_direction() }; + std::erase_if( + m_pointlimits, + [&]( point_limit const &Limit ) { + // passed by the head + if( Limit.distance < 0.0 ) { + if( Global.iWriteLogEnabled & 8 ) { + WriteLog( "Speed table update for " + OwnerName() + ", passed point limit of " + Limit.source->m_name ); + } + return true; + } + // the head backed up behind the cell: the speed table reads it again if it is ahead + if( Limit.distance > Limit.celldistance + 1.0 ) { + if( Global.iWriteLogEnabled & 8 ) { + WriteLog( "Speed table update for " + OwnerName() + ", left behind point limit of " + Limit.source->m_name ); + } + return true; + } + // the cell ahead of the head again, going towards the point after backing up past the cell: it is read anew + // (not while backing up: the cell may as well be scanned from the other side) + return glm::dot( travel, Limit.heading ) > 0.0 + && std::any_of( + sSpeedTable.begin(), sSpeedTable.end(), + [&]( TSpeedPos const &Point ) { + return Point.evEvent == Limit.source + && TestFlag( Point.iFlags, spEnabled | spEvent | spPointVelocity ) + && Point.fDist > 0.0; } ); } ); +} + +void TController::apply_point_limits( double &Velocity, double &Distance, double &Next, double &Acceleration ) { + + auto const travel { travel_direction() }; + for( auto const &limit : m_pointlimits ) { + // latched with no limit, or passed (dropped at the next update) + if( limit.velocity < 0.0 || limit.distance <= 0.0 ) { continue; } + // going away from the point, or the way isn't known + if( glm::dot( travel, limit.heading ) <= 0.0 ) { continue; } + // the same as for a point of the speed table ahead + auto const v { limit.velocity }; + auto const d { limit.distance }; + auto a { ( v * v - mvOccupied->Vel * mvOccupied->Vel ) / ( 25.92 * d ) }; + if( mvOccupied->Vel < v ) { + auto const brakingdistance { 1.2 * fBrakeDist * braking_distance_multiplier( v ) }; + if( brakingdistance > 0.0 ) { + a = std::lerp( a, AccPreferred, std::clamp( ( d - brakingdistance ) / brakingdistance, 0.0, 1.0 ) ); + } + } + if( d < point_limit_approach() && v < Velocity ) { + Velocity = v; + } + if( a <= Acceleration + && ( v < Next || Next < 0 ) ) { + Acceleration = a; + Next = v; + Distance = d; + bStopTargetIsPassengerStop = false; + } + } +} + +double TController::point_limit_approach() const { + // a point limit holds at its point and not a metre beyond, so the braking curve alone leaves a slow consist + // coasting above it (the deceleration asked for is below what makes the driver brake). within the final + // approach, and five seconds of the present speed before it, the speed is held down to it already + return fMaxProximityDist + mvOccupied->Vel / 3.6 * 5.0; +} + +glm::dvec3 TController::travel_direction() const { + + if( pVehicle == nullptr || iDirection == 0 ) { return glm::dvec3 { 0.0 }; } + // the direction is given against the couplers of the vehicle the driver is in, which is the vehicle measured; + // along its own length it follows the track, where the chord of a long consist on a curve doesn't + auto const along { pVehicle->CouplerPosition( end::front ) - pVehicle->CouplerPosition( end::rear ) }; + auto const length { glm::length( along ) }; + if( length < 0.001 ) { return glm::dvec3 { 0.0 }; } + return ( iDirection > 0 ? along : -along ) / length; +} + bool TController::TableUpdateEvent( double &Velocity, TCommandType &Command, TSpeedPos &Point, double &Signaldistance, int const Pointindex ) { + if( TestFlag( Point.iFlags, spPointVelocity ) ) { + // PointVelocity: no signal, no command, only the limit at its point + // done once the head passes the point + if( Point.fDist + Point.fPointDist < 0.0 ) { + if( Global.iWriteLogEnabled & 8 ) { + WriteLog( "Speed table update for " + OwnerName() + ", passed point limit of " + Point.GetName() ); + } + Point.Clear(); + return true; + } + if( Point.fDist < 0.0 ) { + if( point_limit_latched( Point.evEvent ) ) { + // a table built anew found under the consist a cell whose state holds already + Point.Clear(); + return true; + } + if( Point.fVelNext != 0.0 ) { + // the head passed the cell: what it says now holds at the point, whatever happens to the table. + // a stop at 0 stays in the table, where the cell can still release it + latch_point_limit( Point ); + Point.Clear(); + return true; + } + } + return false; + } + // sprawdzanie eventów pasywnych miniętych if( Point.fDist < 0.0 ) { if( SemNextIndex == Pointindex ) { @@ -4643,6 +4863,11 @@ bool TController::PutCommand( std::string NewCommand, double NewValue1, double N return true; } + if( NewCommand == "PointVelocity" ) { + // read by the speed table scan only; when the cell's event runs, there is nothing to do + return true; + } + if (NewCommand == "SetProximityVelocity") { /* diff --git a/vehicle/Driver.h b/vehicle/Driver.h index 512d4b8cf..063c844f9 100644 --- a/vehicle/Driver.h +++ b/vehicle/Driver.h @@ -145,7 +145,8 @@ enum TSpeedPosFlag spSemaphor = 0x4000, // semafor pociągowy spRoadVel = 0x8000, // zadanie prędkości drogowej spSectionVel = 0x20000, // odcinek z ograniczeniem - spProximityVelocity = 0x40000 // odcinek z ograniczeniem i podaną jego długościa + spProximityVelocity = 0x40000, // odcinek z ograniczeniem i podaną jego długościa + spPointVelocity = 0x80000 // ograniczenie dla czoła w punkcie fPointDist za wpisem (PointVelocity) // spDontApplySpeedLimit = 0x10000 // this point won't apply its speed limit. potentially set by the scanning vehicle }; @@ -155,6 +156,7 @@ class TSpeedPos double fDist{ 0.0 }; // aktualna odległość (ujemna gdy minięte) double fVelNext{ -1.0 }; // prędkość obowiązująca od tego miejsca double fSectionVelocityDist{ 0.0 }; // długość ograniczenia prędkości + double fPointDist{ 0.0 }; // PointVelocity: o ile dalej od wpisu leży punkt, w którym czoło ma mieć fVelNext int iFlags{ spNone }; // flagi typu wpisu do tabelki bool bMoved{ false }; // czy przesunięty (dotyczy punktu zatrzymania w peronie) double fMoved{ 0.0 }; // ile przesunięty (dotyczy punktu zatrzymania w peronie) @@ -553,6 +555,20 @@ private: TCommandType TableUpdate( double &fVelDes, double &fDist, double &fNext, double &fAcc ); bool TableUpdateStopPoint( TCommandType &Command, TSpeedPos &Point, double const Signaldistance ); bool TableUpdateEvent( double &Velocity, TCommandType &Command, TSpeedPos &Point, double &Signaldistance, int const Pointindex ); + // keeps the PointVelocity limit of a speed table entry whose cell the head has just passed + void latch_point_limit( TSpeedPos const &Point ); + // moves the latched point limits by the movement of the consist, drops those passed and those read anew + void update_point_limits(); + // the latched point limits ahead in the direction of travel, taken as the points of the speed table are + void apply_point_limits( double &Velocity, double &Distance, double &Next, double &Acceleration ); + // the way the consist runs, along the track under the vehicle whose movement is measured; zero when it isn't known + glm::dvec3 travel_direction() const; + // how far the end of the consist lying the farthest along the heading is ahead of the measured vehicle + double point_limit_end( glm::dvec3 const &Heading ) const; + // whether the state of the cell is latched already + bool point_limit_latched( basic_event const *Cell ) const; + // how far before a point limit the speed is held down to it + double point_limit_approach() const; // returns most recently calculated distance to potential obstacle ahead double TrackObstacle() const; void TablePurger(); @@ -587,6 +603,23 @@ private: std::size_t SemNextIndex{ npos }; std::size_t SemNextStopIndex{ npos }; double dMoveLen = 0.0; // odległość przejechana od ostatniego sprawdzenia tabelki + // a PointVelocity limit latched when the head passed its cell. kept apart from the speed table, which is built + // anew on a change of direction, a change of the consist or a takeover, and measured by the movement in the world, + // so stopping, backing up and going on between the cell and the point keeps it + struct point_limit { + basic_event const *source { nullptr }; // the cell, as the head passed it + double velocity { -1.0 }; // what the cell said then + double distance { 0.0 }; // left for the head to the point, along the line + double celldistance { 0.0 }; // from the cell to the point; the head farther than that from the point is behind the cell + glm::dvec3 heading { 0.0 }; // the way towards the point, in the world + double end { 0.0 }; // point_limit_end() at the last update, to follow vehicles coupled or uncoupled at that end + bool endknown { false }; + }; + std::vector m_pointlimits; + TDynamicObject const *m_pointlimitsvehicle { nullptr }; // the vehicle whose movement moves the points + glm::dvec3 m_pointlimitsposition { 0.0 }; // where it was at the last update + TDynamicObject const *m_pointlimitsends[ 2 ] { nullptr, nullptr }; // the end vehicles of the consist at the last update + double m_pointlimitslength { 0.0 }; // and its length basic_event *eSignNext = nullptr; // sygnał zmieniający prędkość, do pokazania na [F2] neighbour_data Obstacle; // nearest vehicle detected ahead on current route diff --git a/world/Event.cpp b/world/Event.cpp index 624cdd714..533328152 100644 --- a/world/Event.cpp +++ b/world/Event.cpp @@ -733,6 +733,10 @@ putvalues_event::deserialize_( cParser &Input, scene::scratch_data &Scratchpad ) m_input.command_type = TCommandType::cm_SectionVelocity; m_passive = true; } + else if( token == "PointVelocity" ) { + m_input.command_type = TCommandType::cm_PointVelocity; + m_passive = true; + } else if( token == "ShuntVelocity" ) { m_input.command_type = TCommandType::cm_ShuntVelocity; m_passive = true; diff --git a/world/MemCell.cpp b/world/MemCell.cpp index dd2fa2418..ef7261210 100644 --- a/world/MemCell.cpp +++ b/world/MemCell.cpp @@ -82,6 +82,11 @@ TCommandType TMemCell::CommandCheck() eCommand = cm_SetProximityVelocity; bCommand = false; // ta komenda nie jest wysyłana } + else if( szText == "PointVelocity" ) // tarcze ostrzegawcze przejazdowe: v1 prędkość, v2 odległość + { + eCommand = cm_PointVelocity; + bCommand = false; // odczytywana skanowaniem, nie wysyłana + } else if( szText == "Emergency_brake" ) { eCommand = cm_EmergencyBrake; @@ -193,6 +198,8 @@ bool TMemCell::IsVelocity() const return true; if (eCommand == cm_ShuntVelocity) return true; + if (eCommand == cm_PointVelocity) + return true; return eCommand == cm_SetProximityVelocity; };