Unit test.

ZeroMQTest/Code/ActiveOV.cs

@@ -96,7 +96,7 @@ namespace WanderOV
                 {
                     vehicle = new Vehicle();
                     vehicle.owner = owner;
-                    vehicle.speedMtrPerSec = 10/3.6f; // 30kmph to m/s
+                    vehicle.speedMtrPerSec = 45/3.6f; // 30kmph to m/s
                     vehicle.vehicleNumber  = vehicleNumb;
                     vehicles.Add( vehicleKey, vehicle );
                 }

ZeroMQTest/Code/OVParsers/Netex.cs

@@ -4,7 +4,6 @@ using System.Numerics;
 using System.Reflection;
 using System.Xml;
 using WanderOV;
-using static OVAPI.Code.OVParsers.KV1;
 
 namespace OVAPI.Code.OVParsers
 {
@@ -482,12 +481,12 @@ namespace OVAPI.Code.OVParsers
             }
         }
 
-        static bool Sane(float f)
+        public static bool Sane(float f)
         {
             return f >= -1e10 && f < 1e10;
         }
 
-        static bool Sane(Vector2 v)
+        public static bool Sane(Vector2 v)
         {
             return Sane( v.X ) && Sane( v.Y );
         }

ZeroMQTest/Code/OVParsers/Network.cs

@@ -67,7 +67,8 @@ namespace WanderOV
     {
         public IJourney FindJourney( string journeyNumber );
 
-        // Given a vehicle's position and journey interpolate the postion along the route in the journey.
+        // Given a vehicle's position and journey interpolate the postion along the route in the journey.
+        // Returns false, if at end of journey.
         public bool FindPositionInJourney(
             IJourney journey, 
             ref int prevPositionIdx, 
@@ -76,84 +77,79 @@ namespace WanderOV
             out (Vector2 pnt, float dst, Vector2 dir) interpolant
             )
         {
-            // If last point was known, continue from there.
-            int startOffset = 0;
-            if (prevPositionIdx >= 0)
-            {
-               startOffset = prevPositionIdx;
-            }
-
             // If already at end, just return last point.
             var route = journey.Route;
-            if ( startOffset == route.NumPoints-1)
+            if (prevPositionIdx == route.NumPoints-1)
             {
                 interpolant = route.Point(route.NumPoints-1);
-                return true;
+                return false;
             }
-            
+
             // Find closest point on route.
-            float closestDist  = 1e30f;
-            int cloestPointIdx = -1;
-            
-            for (int i = startOffset;i< route.NumPoints; i++)
+            if (prevPositionIdx < 0)
             {
-                var p0 = route.Point(i);
-                var distSq = Vector2.DistanceSquared(oldPosition, p0.point);
-                if (distSq < closestDist)
+                float closestDist  = 1e30f;
+
+                for (int i = 0;i< route.NumPoints;i++)
                 {
-                    closestDist  = distSq;
-                    cloestPointIdx = i;
+                    var p0 = route.Point(i);
+                    var distSq = Vector2.DistanceSquared(oldPosition, p0.point);
+                    if (distSq < closestDist)
+                    {
+                        closestDist  = distSq;
+                        prevPositionIdx = i;
+                    }
                 }
+                Debug.Assert( prevPositionIdx >= 0 );
             }
-            
-            Debug.Assert(cloestPointIdx >= 0);
 
             // Find point in between two points (interpolant/startPoint).
             Vector2 startPoint;
             int nextPointIdx;
-            if ( cloestPointIdx == 0)
+            if (prevPositionIdx == 0)
             {
                 nextPointIdx = 1;
                 GetPointInSegment( oldPosition, route.Point(0).point, route.Point(1).point, out startPoint );
             }
-            else if (cloestPointIdx == route.NumPoints-1)
+            else if (prevPositionIdx == route.NumPoints-1)
             {
                 nextPointIdx = route.NumPoints-1;
                 GetPointInSegment( oldPosition, route.Point( route.NumPoints-2 ).point, route.Point( route.NumPoints-1 ).point, out startPoint );
             }
             else
             {
-                nextPointIdx = cloestPointIdx;
+                nextPointIdx = prevPositionIdx;
                 if (GetPointInSegment( oldPosition, route.Point( nextPointIdx-1 ).point, route.Point( nextPointIdx ).point, out startPoint ) == 1)
                 {
-                    nextPointIdx = cloestPointIdx+1;
+                    nextPointIdx = prevPositionIdx+1;
                     GetPointInSegment( oldPosition, route.Point( nextPointIdx-1 ).point, route.Point( nextPointIdx ).point, out startPoint );
                 }
             }
 
             var prevPoint = startPoint;
             float distToNextPoint = (route.Point( nextPointIdx ).point-startPoint).Length();
-            while(advanceDistance > distToNextPoint )
+            float movedDistance = 0;
+            while (advanceDistance > distToNextPoint )
             {
                 if (nextPointIdx == route.NumPoints-1)
                 {
                     // We also know that (advanceDistance is > distToNextPoint), so we set interpolant to last point.
+                    prevPositionIdx = nextPointIdx;
                     interpolant = route.Point( nextPointIdx ); // Last point.dir is copied from second last point.
-                    return true;
+                    interpolant.dst = movedDistance+distToNextPoint;
+                    return false;
                 }
                 advanceDistance -= distToNextPoint;
+                movedDistance  += distToNextPoint;
+                prevPoint       = route.Point( nextPointIdx ).point;
+                distToNextPoint = route.Point( nextPointIdx ).dist;
                 nextPointIdx++;
-                distToNextPoint = route.Point( nextPointIdx-1 ).dist;
-                prevPoint = route.Point( nextPointIdx-1 ).point;
             }
-            var advDist  = MathF.Min(advanceDistance,distToNextPoint);
-            var endPoint = prevPoint + route.Point(nextPointIdx-1).dir*advDist;
-            interpolant  = (endPoint, advDist, route.Point(nextPointIdx-1).dir );
-
-            var prevPointIdx = prevPositionIdx;
-            prevPositionIdx = nextPointIdx-1;
-            Debug.Assert( (prevPointIdx-1) <= prevPositionIdx );
+            Debug.Assert(advanceDistance <= distToNextPoint);
+            var endPoint = prevPoint + route.Point(nextPointIdx-1).dir*advanceDistance;
+            interpolant  = (endPoint, movedDistance+advanceDistance, route.Point(nextPointIdx-1).dir );
 
+            prevPositionIdx  = nextPointIdx-1;
             return true;
         }
 

ZeroMQTest/Code/Program.cs

-using OVAPI.Code.OVParsers;
+using OVApI;
+using OVAPI.Code.OVParsers;
 using System.Diagnostics;
 using WanderOV;
 
@@ -10,13 +11,13 @@ while (true)
         App p = new App();
         p.Run();
     }
-    catch ( Exception e )
+    catch (Exception e )
     {
-        Console.WriteLine(e.Message);
-        Console.WriteLine(e.StackTrace);
+        Console.WriteLine( e.Message );
+        Console.WriteLine( e.StackTrace );
         Console.WriteLine( "Restarting.." );
     }
-           
+
 }
 
 class App
@@ -28,10 +29,6 @@ class App
 
     public App()
     {
-        //KV1 kV1 = new KV1();
-        //ov = kV1.LoadAll( "_database" );
-        //ov.PrintStats();
-
         Netex netex = new Netex();
         netex.LoadAll( "_database" );
         netex.PrintStats();

ZeroMQTest/OVAPI.csproj

@@ -20,6 +20,7 @@
   </ItemGroup>
 
   <ItemGroup>
+    <PackageReference Include="MSTest.TestFramework" Version="3.1.1" />
     <PackageReference Include="NetMQ" Version="4.0.1.13" />
     <PackageReference Include="Sylvan.Data.Csv" Version="1.3.3" />
   </ItemGroup>

ZeroMQTest/Tests/UnitTests1.cs

+using Microsoft.VisualStudio.TestTools.UnitTesting;
+using OVAPI.Code.OVParsers;
+using System.Diagnostics;
+using System.Numerics;
+using WanderOV;
+
+namespace OVApI
+{
+    [TestClass]
+    public class UnitTests
+    {
+        [TestMethod]
+        public void TesetExtrapolation()
+        {
+
+            Netex netex = new Netex();
+            netex.LoadAll( "_database" );
+            netex.PrintStats();
+
+            var journey = netex.FindJourney( "36005" );
+
+            INetwork network = netex;
+            int prevPos = -1;
+            var prevPoint = journey.Route.Point( 0 ).point;
+            prevPoint.X -= 10;
+            float routeTotDist = 0;
+            for (int i = 0; i < journey.Route.NumPoints; i++)
+            {
+                routeTotDist += journey.Route.Point( i ).dist;
+            }
+
+            float advDist = 7200; // 1 mtr;
+            int kIters = 0;
+            float totDist = 0;
+
+            (Vector2 pnt, float dst, Vector2 dir) interpolant;
+            while ( network.FindPositionInJourney( journey, ref prevPos, prevPoint, advDist, out interpolant) )
+            {
+                var len = interpolant.dir.Length();
+                Debug.Assert( Netex.Sane( len ) );
+                Debug.Assert( Netex.Sane( interpolant.dir ) );
+                kIters++;
+                totDist += interpolant.dst;
+                prevPoint = interpolant.pnt;
+            }
+            totDist += interpolant.dst;
+
+            var delta =  MathF.Abs( totDist-routeTotDist );
+            Assert.IsTrue( delta < 0.01f );
+        }
+    }
+}
\ No newline at end of file