diff --git a/ZeroMQTest/Code/ActiveOV.cs b/ZeroMQTest/Code/ActiveOV.cs index 9743869923a4203eb6fe6c8c9b797f61d4a7df82..0f228417764bdbd85d8c14f903f8131ee483670d 100644 --- a/ZeroMQTest/Code/ActiveOV.cs +++ b/ZeroMQTest/Code/ActiveOV.cs @@ -16,12 +16,15 @@ namespace WanderOV internal long lastUpdate; internal long lastAdvanceTime; internal bool reachedLastStop; + internal bool mustTeleport; internal IJourney journey; internal float speedMtrPerSec; internal float interpolatedX; internal float interpolatedY; + internal float catmullX; + internal float catmullY; internal float angle; internal int prevPointIdxInJourney=-1; @@ -55,6 +58,7 @@ namespace WanderOV { var timeNow = stopwatch.ElapsedMilliseconds; + foreach( var vkp in vehicles ) { var vehicle = vkp.Value; @@ -69,26 +73,30 @@ namespace WanderOV float advanceDistanceMtr = elapsedSeconds*vehicle.speedMtrPerSec; if ( network.FindPositionInJourney( - vehicle.journey, - ref vehicle.prevInterpolatedPointIdxInJourney, - new Vector2( vehicle.interpolatedX, vehicle.interpolatedY ), - advanceDistanceMtr, - out (Vector2 point, float dist, Vector2 dir) interpolant + vehicle.journey, + ref vehicle.prevInterpolatedPointIdxInJourney, + new Vector2( vehicle.interpolatedX, vehicle.interpolatedY ), + advanceDistanceMtr, + out (Vector2 point, float dist, Vector2 dir) interpolant, + out Vector2 catmullPnt )) { vehicle.interpolatedX = interpolant.point.X; vehicle.interpolatedY = interpolant.point.Y; + vehicle.catmullX = catmullPnt.X; + vehicle.catmullY = catmullPnt.Y; Debug.Assert( MathF.Abs( interpolant.dir.Length()-1) < 0.001f ); vehicle.angle = MathF.Atan2( interpolant.dir.X, interpolant.dir.Y ) * 57.29578f; // To Degrees. // Check if is at last stop. float distToLastStop = Vector2.Distance( vehicle.journey.Stop( vehicle.journey.NumStops-1 ).stop.Point, new Vector2( vehicle.interpolatedX, vehicle.interpolatedY ) ); - if ( distToLastStop < 5 ) + if ( distToLastStop < 1.5f ) { vehicle.reachedLastStop = true; } } + // break; } } @@ -112,7 +120,7 @@ namespace WanderOV vehicles.Add( vehicleKey, vehicle ); } - var prevLineNumber = vehicle.linePrivateNumber; + var prevJourneyNumb = vehicle.journeyNumber; vehicle.punctuality = v.infos[(int)VehicleInfoType.Punctuality]; vehicle.journeyNumber = v.infos[(int)VehicleInfoType.JourneyNumber]; vehicle.lastUpdate = lastTime; @@ -121,44 +129,53 @@ namespace WanderOV vehicle.rdx = float.Parse( v.infos[(int)VehicleInfoType.Rdx] ); vehicle.rdy = float.Parse( v.infos[(int)VehicleInfoType.Rdy] ); - // If journey changes, reset position to new journey. - if (vehicle.linePrivateNumber != prevLineNumber) + if (vehicle.rdx == -1 && vehicle.rdy == -1) { - vehicle.journey = network.FindJourney( owner, vehicle.linePrivateNumber ); - vehicle.prevPointIdxInJourney = -1; - vehicle.prevInterpolatedPointIdxInJourney = -1; - vehicle.lastAdvanceTime = lastTime; - vehicle.speedMtrPerSec = 25/3.6f; // initial speed - // Sync the interpolated X/Y with the actual X/Y as the journey has changed. - vehicle.interpolatedX = vehicle.rdx; - vehicle.interpolatedY = vehicle.rdy; - vehicle.reachedLastStop = false; + vehicles.Remove( vehicleKey ); } - - // Find the actual position in journey and compare it with interpolated position, if ahead, speedup movement, otherwise slowdown. - if (vehicle.Valid && - network.FindPositionInJourney( - vehicle.journey, - ref vehicle.prevPointIdxInJourney, - new Vector2( vehicle.rdx, vehicle.rdy ), - 0, - out (Vector2 point, float dist, Vector2 dir) interpolant - )) + else { - if (vehicle.prevPointIdxInJourney != -1 && vehicle.prevInterpolatedPointIdxInJourney != -1) + // If journey changes, reset position to new journey. + if (vehicle.journeyNumber != prevJourneyNumb) { - int segmentDelta = vehicle.prevPointIdxInJourney - vehicle.prevInterpolatedPointIdxInJourney; - if (segmentDelta>0) - { - vehicle.speedMtrPerSec *= (1 + 0.2f*segmentDelta); - } - else if ( segmentDelta < 0) + vehicle.mustTeleport = true; + vehicle.journey = network.FindJourney( owner, vehicle.journeyNumber, vehicle.linePrivateNumber ); + vehicle.prevPointIdxInJourney = -1; + vehicle.prevInterpolatedPointIdxInJourney = -1; + vehicle.lastAdvanceTime = lastTime; + vehicle.speedMtrPerSec = 25/3.6f; // initial speed + // Sync the interpolated X/Y with the actual X/Y as the journey has changed. + vehicle.interpolatedX = vehicle.rdx; + vehicle.interpolatedY = vehicle.rdy; + vehicle.reachedLastStop = false; + } + + // Find the actual position in journey and compare it with interpolated position, if ahead, speedup movement, otherwise slowdown. + if (vehicle.Valid && + network.FindPositionInJourney( + vehicle.journey, + ref vehicle.prevPointIdxInJourney, + new Vector2( vehicle.rdx, vehicle.rdy ), + 0, + out _, + out _ + )) + { + if (vehicle.prevPointIdxInJourney != -1 && vehicle.prevInterpolatedPointIdxInJourney != -1) { - vehicle.speedMtrPerSec /= (1 + 0.2f*-segmentDelta); + int segmentDelta = vehicle.prevPointIdxInJourney - vehicle.prevInterpolatedPointIdxInJourney; + if (segmentDelta>0) + { + vehicle.speedMtrPerSec *= (1 + 0.2f*segmentDelta); + } + else if (segmentDelta < 0) + { + vehicle.speedMtrPerSec /= (1 + 0.2f*-segmentDelta); + } + + if (vehicle.speedMtrPerSec < 5) vehicle.speedMtrPerSec = 5; + if (vehicle.speedMtrPerSec > 20) vehicle.speedMtrPerSec = 20; } - - if (vehicle.speedMtrPerSec < 3) vehicle.speedMtrPerSec = 3; - if (vehicle.speedMtrPerSec > 17) vehicle.speedMtrPerSec= 17; } } } ); @@ -170,7 +187,7 @@ namespace WanderOV var pendingRemoves = new List<string>(); foreach (var v in vehicles) { - if (timeNow - v.Value.lastUpdate > 1000 * 60*2) + if (timeNow - v.Value.lastUpdate > 1000 * 60*1.5f) { pendingRemoves.Add( v.Key ); } @@ -183,14 +200,15 @@ namespace WanderOV if (vehicles==null||vehicles.Count==0) return null; StringBuilder sb = new StringBuilder(); - sb.AppendLine( $"owner,displayLineNumb,privLineNumb,journyNum,vehicleNum,punctuality,rdx,rdy,interpolated-rdx,interpolated-rdy,angleDeg,speedMtrps" ); + sb.AppendLine( $"mustTeleport,owner,displayLineNumb,privLineNumb,journyNum,vehicleNum,punctuality,rdx,rdy,interpolated-rdx,interpolated-rdy,angleDeg,speedMtrps" ); foreach (var kvp in vehicles) { var v = kvp.Value; if (v.Valid) { var pubNumber = v.journey.Line.LineDisplayNumber; - sb.AppendLine( $"{v.owner},{pubNumber},{v.linePrivateNumber},{v.journeyNumber},{v.vehicleNumber},{v.punctuality},{v.rdx},{v.rdy},{v.interpolatedX},{v.interpolatedY},{v.angle},{v.speedMtrPerSec}" ); + sb.AppendLine( $"{v.mustTeleport},{v.owner},{pubNumber},{v.linePrivateNumber},{v.journeyNumber},{v.vehicleNumber},{v.punctuality},{v.rdx},{v.rdy},{v.interpolatedX},{v.interpolatedY},{v.angle},{v.speedMtrPerSec}" ); + v.mustTeleport=false; } } return sb.ToString(); diff --git a/ZeroMQTest/Code/OVParsers/Netex.cs b/ZeroMQTest/Code/OVParsers/Netex.cs index c8a2130442d67380a31a1fc2399b2931c437f911..92b5fd5ba50a5b904fb28d848dc22869fab63ca6 100644 --- a/ZeroMQTest/Code/OVParsers/Netex.cs +++ b/ZeroMQTest/Code/OVParsers/Netex.cs @@ -2,6 +2,7 @@ using System.IO.Compression; using System.Numerics; using System.Reflection; +using System.Security.Cryptography; using System.Xml; using WanderOV; @@ -41,7 +42,6 @@ namespace OVAPI.Code.OVParsers class Route : IRoute { internal Line line; - internal JourneyPattern journey; internal List<RouteLink> links; internal List<(Vector2 point, float dst, Vector2 dir)> points; internal bool valid; @@ -111,8 +111,7 @@ namespace OVAPI.Code.OVParsers Dictionary<string, Destination> destinations = new Dictionary<string, Destination>(); Dictionary<string, Stop> stops = new Dictionary<string, Stop>(); Dictionary<string, JourneyPattern> journeyPatterns = new Dictionary<string, JourneyPattern>(); - Dictionary<string, JourneyPattern> journeyNumbersToPattern = new Dictionary<string, JourneyPattern>(); - Dictionary<string, Line> privLineNumberToLine = new Dictionary<string, Line>(); + Dictionary<string, Dictionary<string, List<JourneyPattern>>> journeyNumbersToPattern = new Dictionary<string, Dictionary<string, List<JourneyPattern>>>(); public void PrintStats() { @@ -148,7 +147,7 @@ namespace OVAPI.Code.OVParsers ParseElementList( doc, "DestinationDisplay", LoadDestination ); ParseElementList( doc, "ScheduledStopPoint", LoadStop ); ParseElementList( doc, "ServiceJourneyPattern", LoadJourneyPattern ); - /// ParseElementList( doc, "ServiceJourney", LoadJourneyCode ); + ParseElementList( doc, "ServiceJourney", LoadJourneyCode ); } void LoadRoutePoint( XmlElement elem ) @@ -245,7 +244,6 @@ namespace OVAPI.Code.OVParsers line.transportType = elem.GetChild( "TransportMode" ).InnerText; line.linePlanningNumber = elem.GetChild( "PrivateCode" ).InnerText; lines.Add( id, line ); - privLineNumberToLine.Add( string.Join( ':', activeOwner, line.linePlanningNumber ), line ); } catch (Exception e) { Console.WriteLine( e.Message ); } } @@ -312,7 +310,6 @@ namespace OVAPI.Code.OVParsers { var journey = new JourneyPattern(); journey.route = routes[elem.GetChild( "RouteRef" ).GetAttribute( "ref" )]; - journey.route.journey = journey; var destElem = elem.GetChild( "DestinationDisplayRef" ); if (destElem != null) { @@ -360,7 +357,19 @@ namespace OVAPI.Code.OVParsers string journeyNumb = elem.GetChild( "PrivateCode" ).InnerText; string journeyRef = elem.GetChild( "ServiceJourneyPatternRef" ).GetAttribute("ref"); var key = string.Join( ':', activeOwner, journeyNumb ); - journeyNumbersToPattern.TryAdd( key, journeyPatterns[journeyRef] ); + if (!journeyNumbersToPattern.TryGetValue( key, out var mapOfLinesToPatterns )) + { + mapOfLinesToPatterns = new Dictionary<string, List<JourneyPattern>>(); + journeyNumbersToPattern.Add( key, mapOfLinesToPatterns ); + } + var journeyPattern = journeyPatterns[journeyRef]; + var pviLineNumb = journeyPattern.line.linePlanningNumber; + if (!mapOfLinesToPatterns.TryGetValue( pviLineNumb , out var listOfPatterns)) + { + listOfPatterns = new List<JourneyPattern>(); + mapOfLinesToPatterns.Add( pviLineNumb, listOfPatterns ); + } + listOfPatterns.Add( journeyPattern ); } public void LoadAll( string folder ) @@ -405,8 +414,8 @@ namespace OVAPI.Code.OVParsers if (link.points.Count < 2) { route.points.Add( (new Vector2( link.from.rdx, link.from.rdy ), 0, Vector2.Zero) ); - if (i == route.links.Count-1) - route.points.Add( (new Vector2( link.to.rdx, link.from.rdy ), 0, Vector2.Zero) ); + if (i == route.links.Count-1) // 'To' of a link is same as the 'from' from previous link, so only on last link, add the 'to'. + route.points.Add( (new Vector2( link.to.rdx, link.to.rdy ), 0, Vector2.Zero) ); } else { @@ -433,21 +442,22 @@ namespace OVAPI.Code.OVParsers var p1 = route.points[j]; var dir = p1.point-p0.point; var len = dir.Length(); - if (len < 0.2f) + if (len <= 0.5f) { route.points.RemoveAt( j ); j--; } else { - Debug.Assert( dir.Length() > 0.3f ); - Debug.Assert( MathF.Abs( (dir / dir.Length()).Length() - 1 )< 0.1f ); - route.points[i] = (p0.point, len, dir/dir.Length()); + Debug.Assert( len > 0.5f ); + dir /= len; + Debug.Assert( MathF.Abs( Vector2.Dot(dir, dir)-1) < 0.01f ); // Ensure normalized. + route.points[i] = (p0.point, len, dir); break; } } } - if (route.points.Count>=2) + if (route.points.Count>=2) // Set last point direction to previous. { var lastElem = route.points[route.points.Count-1]; lastElem.dir = route.points[route.points.Count-2].dir; @@ -456,7 +466,6 @@ namespace OVAPI.Code.OVParsers } } - // Compute dist and direction from point to point in route. foreach (var kvp in routes) { var route = kvp.Value; @@ -475,13 +484,15 @@ namespace OVAPI.Code.OVParsers } // Network overrides - public IJourney FindJourney( string owner, string privLineNumber ) + public IJourney FindJourney( string owner, string journeyNumb, string privLineNumber ) { - string key = string.Join(':', owner, privLineNumber); - if(privLineNumberToLine.TryGetValue( key, out var line ) ) + string key = string.Join(':', owner, journeyNumb); + if (journeyNumbersToPattern.TryGetValue( key, out var lineToJourneys ) ) { - if (line.route.valid) - return line.route.journey; + if (lineToJourneys.TryGetValue( privLineNumber, out var listOfJourneys )) + { + return listOfJourneys[0]; + } } return null; } diff --git a/ZeroMQTest/Code/OVParsers/Network.cs b/ZeroMQTest/Code/OVParsers/Network.cs index e5f5d785ac0a848d52dbbdd5ac4460014688e178..cb719be70d8ec035a0fe3239b70b8906befdc636 100644 --- a/ZeroMQTest/Code/OVParsers/Network.cs +++ b/ZeroMQTest/Code/OVParsers/Network.cs @@ -67,77 +67,129 @@ namespace WanderOV public interface INetwork { - public IJourney FindJourney( string owner, string privLineNumber ); - + public IJourney FindJourney( string owner, string journeyNumber, string privLineNumber ); + + float GetT( float t, float alpha, Vector2 p0, Vector2 p1 ) + { + var d = p1 - p0; + float a = Vector2.Dot(d, d); + float b = MathF.Pow( a, alpha*0.5f ); + return (b + t); + } + + Vector2 CatmullRom( Vector2 p0, Vector2 p1, Vector2 p2, Vector2 p3, float t /* between 0 and 1 */, float alpha = .5f /* between 0 and 1 */ ) + { + float t0 = 0.0f; + float t1 = GetT( t0, alpha, p0, p1 ); + float t2 = GetT( t1, alpha, p1, p2 ); + float t3 = GetT( t2, alpha, p2, p3 ); + t = Vector2.Lerp( new Vector2( t1, 0 ), new Vector2( t2, 0 ), t ).X; + var A1 = ( t1-t )/( t1-t0 )*p0 + ( t-t0 )/( t1-t0 )*p1; + var A2 = ( t2-t )/( t2-t1 )*p1 + ( t-t1 )/( t2-t1 )*p2; + var A3 = ( t3-t )/( t3-t2 )*p2 + ( t-t2 )/( t3-t2 )*p3; + var B1 = ( t2-t )/( t2-t0 )*A1 + ( t-t0 )/( t2-t0 )*A2; + var B2 = ( t3-t )/( t3-t1 )*A2 + ( t-t1 )/( t3-t1 )*A3; + var C = ( t2-t )/( t2-t1 )*B1 + ( t-t1 )/( t2-t1 )*B2; + return C; + } + // Given a vehicle's position and journey interpolate the postion along the route in the journey. - // Returns false, if at end of journey. + // Returns false, if at end of journey. + // The returned interpolant.point should be the input for the next request. While the pntCatMull, + // countains a more smoothish output of the request. The angle of the interpolant is also based on the catMull tangent. + // If advanceDistance == 0, then it only finds the point inside the interpolant. Dist and direction rare not valid. public bool FindPositionInJourney( IJourney journey, - ref int prevPositionIdx, + ref int prevPointIndex, Vector2 oldPosition, float advanceDistance, - out (Vector2 pnt, float dst, Vector2 dir) interpolant + out (Vector2 pnt, float dst, Vector2 dir) interpolant, + out Vector2 pntCatMull ) { // If already at end, just return last point. var route = journey.Route; - if (prevPositionIdx == route.NumPoints-1) + if (prevPointIndex == route.NumPoints-1) { interpolant = route.Point(route.NumPoints-1); + pntCatMull = interpolant.pnt; return false; } // Find closest point on route. - if (prevPositionIdx < 0) + if (prevPointIndex < 0) { float closestDist = 1e30f; for (int i = 0;i< route.NumPoints;i++) { - var p0 = route.Point(i); - var distSq = Vector2.DistanceSquared(oldPosition, p0.point); + var p00 = route.Point(i); + var distSq = Vector2.DistanceSquared(oldPosition, p00.point); if (distSq < closestDist) { closestDist = distSq; - prevPositionIdx = i; + prevPointIndex = i; } } - Debug.Assert( prevPositionIdx >= 0 ); + Debug.Assert( prevPointIndex >= 0 ); } // Find point in between two points (interpolant/startPoint). Vector2 startPoint; int nextPointIdx; - if (prevPositionIdx == 0) + if (prevPointIndex == 0) { nextPointIdx = 1; GetPointInSegment( oldPosition, route.Point(0).point, route.Point(1).point, out startPoint ); } - else if (prevPositionIdx == route.NumPoints-1) + else if (prevPointIndex == 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 = prevPositionIdx; - if (GetPointInSegment( oldPosition, route.Point( nextPointIdx-1 ).point, route.Point( nextPointIdx ).point, out startPoint ) == 1) + nextPointIdx = prevPointIndex+1; + while (GetPointInSegment( oldPosition, route.Point( prevPointIndex ).point, route.Point( nextPointIdx ).point, out _ ) == 1) { - nextPointIdx = prevPositionIdx+1; - GetPointInSegment( oldPosition, route.Point( nextPointIdx-1 ).point, route.Point( nextPointIdx ).point, out startPoint ); + prevPointIndex++; + nextPointIdx++; + if ( prevPointIndex >= route.NumPoints-1 ) + { + interpolant = route.Point( route.NumPoints-1 ); + pntCatMull = interpolant.pnt; + return false; + } + + } + while (GetPointInSegment( oldPosition, route.Point( prevPointIndex ).point, route.Point( nextPointIdx ).point, out startPoint ) == -1) + { + prevPointIndex--; + nextPointIdx--; + if (prevPointIndex == 0) + { + break; + } } } + if ( advanceDistance == 0 ) + { + interpolant = (startPoint, 0, route.Point( prevPointIndex ).dir); + } + var prevPoint = startPoint; float distToNextPoint = (route.Point( nextPointIdx ).point-startPoint).Length(); 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; + prevPointIndex = nextPointIdx; interpolant = route.Point( nextPointIdx ); // Last point.dir is copied from second last point. + pntCatMull = interpolant.pnt; interpolant.dst = movedDistance+distToNextPoint; return false; } @@ -148,10 +200,30 @@ namespace WanderOV nextPointIdx++; } 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; + prevPointIndex = nextPointIdx-1; + var endPoint = prevPoint + route.Point(prevPointIndex).dir*advanceDistance; + float t = (endPoint-route.Point(prevPointIndex).point).Length() / route.Point(prevPointIndex).dist; + Vector2 p0, p3; + if (nextPointIdx-2<0) + { + p0 = route.Point( 0 ).point - route.Point( 0 ).dir; + } + else + { + p0 = route.Point( nextPointIdx-2 ).point; + } + if (nextPointIdx+1==route.NumPoints) + { + p3 = route.Point( route.NumPoints -1 ).point + route.Point( route.NumPoints -1 ).dir; + } + else + { + p3 = route.Point( nextPointIdx+1 ).point; + } + pntCatMull = CatmullRom( p0, route.Point(prevPointIndex).point, route.Point(nextPointIdx).point, p3, t, 0.1f ); + var ctmPnt2 = CatmullRom( p0, route.Point(prevPointIndex).point, route.Point(nextPointIdx).point, p3, t+0.1f, 0.1f ); + interpolant = (endPoint, movedDistance+advanceDistance, Vector2.Normalize ( ctmPnt2-pntCatMull ) ); + Debug.Assert( MathF.Abs( interpolant.dir.Length()-1 ) < 0.001f ); return true; } diff --git a/ZeroMQTest/Code/Program.cs b/ZeroMQTest/Code/Program.cs index 4f20c6ce6e68657025e4ab7561440170b1a9011c..2134107e86212a196c8326d9b27b6452bfca1073 100644 --- a/ZeroMQTest/Code/Program.cs +++ b/ZeroMQTest/Code/Program.cs @@ -79,7 +79,7 @@ class App break; case "journey": // Expects owner, journeyNumber and vehicleNumber. - var journey = network.FindJourney( msg.fields[1], msg.fields[4] ); + var journey = network.FindJourney( msg.fields[1], msg.fields[2], msg.fields[4] ); if (journey!=null) { // field[1] is owner, field[2] is journeyNumber, field[3] is vehicleNumber diff --git a/ZeroMQTest/Tests/UnitTests.cs b/ZeroMQTest/Tests/UnitTests.cs index 46d5716f9511fca5acce29a3a19472e0cfd67527..53ca9dbe2be16c8fe904ccb9fe3da8d8e27c8d6d 100644 --- a/ZeroMQTest/Tests/UnitTests.cs +++ b/ZeroMQTest/Tests/UnitTests.cs @@ -17,7 +17,7 @@ namespace OVApI netex.LoadAll( "_database" ); netex.PrintStats(); - var journey = netex.FindJourney( "HTM", "36005" ); + var journey = netex.FindJourney( "HTM", "36005", "5" ); INetwork network = netex; int prevPos = -1; @@ -34,7 +34,8 @@ namespace OVApI float totDist = 0; (Vector2 pnt, float dst, Vector2 dir) interpolant; - while ( network.FindPositionInJourney( journey, ref prevPos, prevPoint, advDist, out interpolant) ) + Vector2 catmullPnt; + while ( network.FindPositionInJourney( journey, ref prevPos, prevPoint, advDist, out interpolant, out catmullPnt) ) { var len = interpolant.dir.Length(); Debug.Assert( Netex.Sane( len ) );