fix(roads): an axle getting on a crossroads takes the way the other axle took, one starting where it gets on

world/TrkFoll.cpp

@@ -22,6 +22,31 @@ http://mozilla.org/MPL/2.0/.
 #include "vehicle/Driver.h"
 #include "utilities/Logs.h"
 
+namespace {
+
+// how far the start of a way across a crossroads can be from the end of the path an axle leaves, to be where the axle gets on
+constexpr double cross_reach { 1.0 };
+
+// number of the ways across a crossroads; each can be taken either way
+int cross_count( TTrack const &Cross ) {
+
+    return Cross.SwitchExtension->iRoads == 4 ? 6 : 3;
+}
+
+// where the way of specified code starts, and the direction it leads in there. the code is the number of the way, 1..6,
+// negative if the way is taken against its points
+bool cross_start( TTrack const &Cross, int const Code, glm::dvec3 &Point, glm::dvec3 &Direction ) {
+
+    if( Code == 0 || std::abs( Code ) > cross_count( Cross ) ) { return false; }
+    auto const &segment { Cross.SwitchExtension->Segments[ std::abs( Code ) - 1 ] };
+    if( segment == nullptr ) { return false; }
+    Point = ( Code > 0 ? segment->FastGetPoint_0() : segment->FastGetPoint_1() );
+    Direction = ( Code > 0 ? segment->FastGetPoint( 0.05 ) : segment->FastGetPoint( 0.95 ) ) - Point;
+    return true;
+}
+
+} // namespace
+
 TTrackFollower::~TTrackFollower()
 {
 }
@@ -30,6 +55,8 @@ bool TTrackFollower::Init(TTrack *pTrack, TDynamicObject *NewOwner, double fDir)
 {
     fDirection = fDir;
     Owner = NewOwner;
+    // the axle is put on the track, it doesn't come from the one it was on before
+    pCurrentSegment = nullptr;
     SetCurrentTrack(pTrack, 0);
     iEventFlag = 3; // na torze startowym również wykonać eventy 1/2
     iEventallFlag = 3;
@@ -92,6 +119,9 @@ TTrack * TTrackFollower::SetCurrentTrack(TTrack *pTrack, int end)
                 // kiedy powstaje!
                 iSegment = 1; // doraźna poprawka
             }
+            // the way has to start where the axle gets on. the one the driver picked may not: the driver may have looked
+            // again since the other axle got on, the vehicle may have no driver of its own, or it may be going backwards
+            iSegment = CrossEntry( *pTrack, iSegment, end );
             if ((end ? iSegment : -iSegment) < 0)
                 fDirection = -fDirection; // wtórna zmiana
             pTrack->SwitchForced(abs(iSegment) - 1, nullptr); // wybór zapamiętanego segmentu
@@ -123,6 +153,67 @@ TTrack * TTrackFollower::SetCurrentTrack(TTrack *pTrack, int end)
     return pCurrentTrack;
 };
 
+// picks the way across the crossroads (Cross) for the axle which gets on it by specified end of the path it's on. it's the way
+// taken by another axle of the vehicle, or of a vehicle coupled to it, which is on the crossroads already; or the one the driver
+// picked (Wish); or, if neither starts where the axle gets on, the one leading the most straight on of the ways which start there
+int TTrackFollower::CrossEntry( TTrack const &Cross, int const Wish, int const End ) const {
+
+    if( pCurrentSegment == nullptr || Cross.SwitchExtension == nullptr ) { return Wish; }
+
+    auto const joint { End ? pCurrentSegment->FastGetPoint_1() : pCurrentSegment->FastGetPoint_0() };
+    auto const starts_here = [&]( int const Code ) {
+        glm::dvec3 point, direction;
+        return cross_start( Cross, Code, point, direction ) && glm::distance( point, joint ) < cross_reach; };
+
+    if( Owner != nullptr ) {
+        for( auto const *vehicle : { Owner, Owner->Prev(), Owner->Next() } ) {
+            if( vehicle == nullptr ) { continue; }
+            for( auto const *axle : { &vehicle->Axle0, &vehicle->Axle1 } ) {
+                if( axle == this || axle->pCurrentTrack != &Cross || axle->iSegment == 0 ) { continue; }
+                if( starts_here( axle->iSegment ) ) { return axle->iSegment; }
+                // a vehicle going back over the crossroads gets on the way at its other end
+                if( starts_here( -axle->iSegment ) ) { return -axle->iSegment; }
+            }
+        }
+    }
+    if( starts_here( Wish ) ) { return Wish; }
+
+    auto const incoming { joint - ( End ? pCurrentSegment->FastGetPoint( 0.95 ) : pCurrentSegment->FastGetPoint( 0.05 ) ) };
+    auto nearest { cross_reach };
+    for( int way = 1; way <= cross_count( Cross ); ++way ) {
+        for( auto const code : { way, -way } ) {
+            glm::dvec3 point, direction;
+            if( cross_start( Cross, code, point, direction ) ) {
+                nearest = std::min( nearest, glm::distance( point, joint ) );
+            }
+        }
+    }
+    if( nearest >= cross_reach ) {
+        // no way starts there, the crossroads isn't joined with the path; it's left the way it was
+        return Wish;
+    }
+    auto result { Wish };
+    auto straightest { -2.0 };
+    for( int way = 1; way <= cross_count( Cross ); ++way ) {
+        for( auto const code : { way, -way } ) {
+            glm::dvec3 point, direction;
+            if( false == cross_start( Cross, code, point, direction )
+             || glm::distance( point, joint ) > nearest + 0.05 ) {
+                continue;
+            }
+            auto const straight {
+                glm::length2( incoming ) > 0.0 && glm::length2( direction ) > 0.0 ?
+                    glm::dot( glm::normalize( incoming ), glm::normalize( direction ) ) :
+                    -1.0 };
+            if( straight > straightest ) {
+                straightest = straight;
+                result = code;
+            }
+        }
+    }
+    return result;
+}
+
 bool TTrackFollower::Move(double fDistance, bool bPrimary)
 { // przesuwanie wózka po torach o odległość (fDistance), z wyzwoleniem eventów
     // bPrimary=true - jest pierwszą osią w pojeździe, czyli generuje eventy i przepisuje pojazd