diff --git a/ZeroMQTest/Code/ActiveOV.cs b/ZeroMQTest/Code/ActiveOV.cs index 97b3728a55a9e44e96c8cd698a4303189ab4937e..f2cdc34f8c0bcbbf8206f2f3e2cd1f2f1db59931 100644 --- a/ZeroMQTest/Code/ActiveOV.cs +++ b/ZeroMQTest/Code/ActiveOV.cs @@ -59,7 +59,6 @@ namespace WanderOV { var timeNow = stopwatch.ElapsedMilliseconds; - foreach (var vkp in vehicles) { var vehicle = vkp.Value; @@ -88,115 +87,121 @@ namespace WanderOV 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 < 1.5f) - { - vehicle.reachedLastStop = true; - } } - - // break; + else + { + vehicle.reachedLastStop = true; + } } } - public void UpdateVehicles( List<VehicelInfo> _vehicles ) + public void UpdateVehicles( Dictionary<string, VehicleInfo> updatedVehicles ) { - if (_vehicles == null) + if (updatedVehicles == null) return; long lastTime = stopwatch.ElapsedMilliseconds; - _vehicles.ForEach( v => + foreach (var kvp in updatedVehicles) { - var owner = v.infos[(int)VehicleInfoType.Owner]; - var vehicleNumb = v.infos[(int)VehicleInfoType.VehicleNumber]; - var vehicleKey = string.Join(':', owner, vehicleNumb); + var updatedVehicle = kvp.Value; - if (!vehicles.TryGetValue( vehicleKey, out var vehicle )) + if (!vehicles.TryGetValue( updatedVehicle.ID, out var vehicle )) { - vehicle = new Vehicle(); - vehicle.owner = owner; - vehicle.vehicleNumber = vehicleNumb; - vehicles.Add( vehicleKey, vehicle ); + vehicle = new() + { + owner = updatedVehicle.Owner, + vehicleNumber = updatedVehicle.Number + }; + vehicles.Add( updatedVehicle.ID, vehicle ); } - var prevJourneyNumb = vehicle.journeyNumber; - vehicle.punctuality = v.infos[(int)VehicleInfoType.Punctuality]; - vehicle.journeyNumber = v.infos[(int)VehicleInfoType.JourneyNumber]; - vehicle.lastUpdate = lastTime; - vehicle.linePrivateNumber = v.infos[(int)VehicleInfoType.LinePrivateNumber]; + UpdateVehicleInfo( vehicle, updatedVehicle, lastTime ); + UpdateVehiclePosition( vehicle, updatedVehicle ); + UpdateVehicleJourney( vehicle, updatedVehicle, lastTime ); + } + } - if (v.infos[(int)VehicleInfoType.Rdx] == null) - { - // At stops, GPS-coordinates are not always listed, get from userstopcode. - if (v.infos[(int)VehicleInfoType.StopCode] != null) - { - var stop = network.FindStop( owner, v.infos[(int)VehicleInfoType.StopCode] ); - if (stop != null) - { - vehicle.rdx = stop.Point.X; - vehicle.rdy = stop.Point.Y; - } - } - } - else - { - if (float.TryParse( v.infos[(int)VehicleInfoType.Rdx], out var newRdx ) && - float.TryParse( v.infos[(int)VehicleInfoType.Rdy], out var newRdy )) - { - vehicle.rdx = newRdx; - vehicle.rdy = newRdy; - } - } + private void UpdateVehicleInfo( Vehicle vehicle, VehicleInfo updatedVehicle, long lastTime ) + { + vehicle.punctuality = updatedVehicle.infos[(int)VehicleInfoType.Punctuality]; + vehicle.linePrivateNumber = updatedVehicle.infos[(int)VehicleInfoType.LinePrivateNumber]; + vehicle.lastUpdate = lastTime; + } - // If journey changes, reset position to new journey. - if (vehicle.journeyNumber != prevJourneyNumb) + private void UpdateVehiclePosition( Vehicle vehicle, VehicleInfo updatedVehicle ) + { + if (updatedVehicle.infos[(int)VehicleInfoType.Rdx] == null) + { + IStop stop; + if (updatedVehicle.infos[(int)VehicleInfoType.StopCode] != null && + (stop = network.FindStop( vehicle.owner, updatedVehicle.infos[(int)VehicleInfoType.StopCode] )) != null) { - vehicle.mustTeleport = true; - vehicle.journey = network.FindJourney( owner, vehicle.journeyNumber ); - 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; + vehicle.rdx = stop.Point.X; + vehicle.rdy = stop.Point.Y; } + } + else if (float.TryParse( updatedVehicle.infos[(int)VehicleInfoType.Rdx], out var newRdx ) && + float.TryParse( updatedVehicle.infos[(int)VehicleInfoType.Rdy], out var newRdy )) + { + vehicle.rdx = newRdx; + vehicle.rdy = newRdy; + } + } + + private void UpdateVehicleJourney( Vehicle vehicle, VehicleInfo updatedVehicle, long lastTime ) + { + if (vehicle.journeyNumber != updatedVehicle.infos[(int)VehicleInfoType.JourneyNumber]) + { + vehicle.journeyNumber = updatedVehicle.infos[(int)VehicleInfoType.JourneyNumber]; + ResetVehicleJourney( vehicle, lastTime ); + } - // Find the actual position in journey and compare it with interpolated position, if ahead, speedup movement, otherwise slowdown. - if (vehicle.Valid && - NetworkUtils.FindPositionInJourney( + AdjustVehicleSpeed( vehicle ); + } + + private void ResetVehicleJourney( Vehicle vehicle, long lastTime ) + { + vehicle.mustTeleport = true; + vehicle.journey = network.FindJourney( vehicle.owner, vehicle.journeyNumber, vehicle.linePrivateNumber ); + vehicle.prevPointIdxInJourney = -1; + vehicle.prevInterpolatedPointIdxInJourney = -1; + vehicle.lastAdvanceTime = lastTime; + vehicle.speedMtrPerSec = 25 / 3.6f; // initial speed + vehicle.interpolatedX = vehicle.rdx; + vehicle.interpolatedY = vehicle.rdy; + vehicle.reachedLastStop = false; + } + + private void AdjustVehicleSpeed( Vehicle vehicle ) + { + if (vehicle.Valid && + NetworkUtils.FindPositionInJourney( vehicle.journey, ref vehicle.prevPointIdxInJourney, new Vector2( vehicle.rdx, vehicle.rdy ), 0, out _, - out _ - )) + out _ )) + { + if (vehicle.prevPointIdxInJourney != -1 && vehicle.prevInterpolatedPointIdxInJourney != -1) { - if (vehicle.prevPointIdxInJourney != -1 && vehicle.prevInterpolatedPointIdxInJourney != -1) + int segmentDelta = vehicle.prevPointIdxInJourney - vehicle.prevInterpolatedPointIdxInJourney; + if (segmentDelta > 0) { - 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 > 15) vehicle.speedMtrPerSec = 15; + vehicle.speedMtrPerSec *= (1 + 0.2f * segmentDelta); + } + else if (segmentDelta < 0) + { + vehicle.speedMtrPerSec /= (1 + 0.2f * -segmentDelta); } - } - } ); + vehicle.speedMtrPerSec = Math.Clamp( vehicle.speedMtrPerSec, 5, 15 ); + } + } } + void DeleteInactiveVehicles() { long timeNow = stopwatch.ElapsedMilliseconds; diff --git a/ZeroMQTest/Code/Broker.cs b/ZeroMQTest/Code/Broker.cs index 5b5abe04a6537ec709c908a1d725cdc68f6dd06f..9c2416e2c77b7eb21d95b762c062325bf2a2261d 100644 --- a/ZeroMQTest/Code/Broker.cs +++ b/ZeroMQTest/Code/Broker.cs @@ -6,14 +6,24 @@ using System.Text; namespace WanderOV { - class Client + public class Client { internal TcpClient tcp; internal string data; + internal float xmin, xmax, ymin, ymax; // Interest area. + + public void UpdateInterestArea(float _xmin, float _xmax, float _ymin, float _ymax) + { + xmin = _xmin; + xmax = _xmax; + ymin = _ymin; + ymax = _ymax; + } } public class BrokerMsg { + public Client client; public TcpClient from; public string[] fields; } @@ -27,7 +37,7 @@ namespace WanderOV Stopwatch stopwatch; long lastAliveCheck; - public Broker( int port) + public Broker( int port ) { messages = new Queue<BrokerMsg>(); clients = new List<Client>(); @@ -77,17 +87,17 @@ namespace WanderOV clients[i].data = data; break; } - + } } } } - - void ProcessMessage(string msg, TcpClient from) + + void ProcessMessage( string msg, TcpClient from ) { // Messages are a single line ending with ####. The single line may have multiple fields and are seperated by an | . var fields = msg.Split('|'); - if ( fields.Length == 0 ) + if (fields.Length == 0) { Console.WriteLine( $"Invalid msg {msg}" ); return; @@ -95,10 +105,10 @@ namespace WanderOV BrokerMsg brokerMsg = new BrokerMsg(); brokerMsg.fields = fields; brokerMsg.from = from; - messages.Enqueue(brokerMsg ); + messages.Enqueue( brokerMsg ); } - public void SendMessage(string id, string msg, TcpClient to) + public void SendMessage( string id, string msg, TcpClient to ) { string payload = $"{id}|{msg}####"; to.GetStream().WriteAsync( Encoding.UTF8.GetBytes( payload ) ); @@ -115,7 +125,7 @@ namespace WanderOV { Console.WriteLine( DateTime.Now + " " + endpoint.ToString() + " Connected." ); } - clients.Add( new Client() { tcp=newClient, data=string.Empty }); + clients.Add( new Client() { tcp=newClient, data=string.Empty } ); } } @@ -129,7 +139,7 @@ namespace WanderOV lastAliveCheck=stopwatch.ElapsedMilliseconds; IPGlobalProperties ipProperties = IPGlobalProperties.GetIPGlobalProperties(); - + for (int i = 0;i < clients.Count;i++) { diff --git a/ZeroMQTest/Code/Network.cs b/ZeroMQTest/Code/Network.cs index 2e24ccd3bb695d039c1c37882774724c5d73d2f0..1b6d3d35fb03bdf668a68cf5be16bb4c26bab9a7 100644 --- a/ZeroMQTest/Code/Network.cs +++ b/ZeroMQTest/Code/Network.cs @@ -28,7 +28,7 @@ namespace OVAPI.Code { // Note1: dir is normalized. // Note2: the dist in last point is 0, because there is no next point and the dir is a zero vector. - public (Vector2 point, float dist, Vector2 dir) Point(int idx); + public (Vector2 point, float dist, Vector2 dir) Point( int idx ); public int NumPoints { get; } } @@ -37,14 +37,14 @@ namespace OVAPI.Code public ILine Line { get; } public IRoute Route { get; } public IDestination Destination { get; } - public (IStop stop, IDestination dest) Stop(int i); + public (IStop stop, IDestination dest) Stop( int i ); public int NumStops { get; } public string AsMsg() { string msg = $"{Line.Name}|{Line.LinePlanningNumber}|{Line.TransportType}|{Line.LineDisplayNumber}|{NumStops}|"; string prevDest = string.Empty; - for (int i = 0; i < NumStops; i++) + for (int i = 0;i < NumStops;i++) { var st = Stop(i); string dest = string.Empty; @@ -56,7 +56,7 @@ namespace OVAPI.Code msg += $"{st.stop.Name}|{st.stop.Point.X}|{st.stop.Point.Y}|{dest}|"; } msg += $"{Route.NumPoints}|"; - for (int i = 0; i < Route.NumPoints; i++) + for (int i = 0;i < Route.NumPoints;i++) { var pnt = Route.Point(i); msg += $"{pnt.point.X}|{pnt.point.Y}|"; @@ -68,7 +68,7 @@ namespace OVAPI.Code public interface INetwork { // Owner is: QBUZZ, EBS, RET, CXX, ARR, KEOLIS, etc. - public IJourney FindJourney(string owner, string journeyNumber); + public IJourney FindJourney( string owner, string journeyNumber, string priveLineNumber ); public IStop FindStop( string owner, string userStopCode ); } @@ -78,7 +78,7 @@ namespace OVAPI.Code // Given a vehicle's position and journey interpolate the postion along the route in the 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. + // contains a more smoothish output of the request. The angle of the interpolant is also based on the catMull tangent. static public bool FindPositionInJourney( IJourney journey, ref int prevPointIndex, @@ -88,6 +88,8 @@ namespace OVAPI.Code out Vector2 pntCatMull ) { + Debug.Assert( advanceDistance >= 0, "Invalid advance distance." ); + // If already at end, just return last point. var route = journey.Route; if (prevPointIndex >= route.NumPoints-1) @@ -198,50 +200,45 @@ namespace OVAPI.Code } p1 = route.Point( prevPointIndex ).point; p2 = route.Point( nextPointIdx ).point; - (var pos, var _) = CatmullRom2( t, p0, p1, p2, p3, 0.1f ); - (var pos2, var _) = CatmullRom2( t+0.1f, p0, p1, p2, p3, 0.1f ); - //var splinePnt2 = CatmullRom2( t+0.1f, p0, p1, p2, p3 ); - pntCatMull = pos; - var splineTangent = Vector2.Normalize( pos2-pos ); - if (!Sane( splineTangent )) + var pos1 = CatmullRom( t, p0, p1, p2, p3, 0.5f ); + var pos2 = CatmullRom( t+0.1f, p0, p1, p2, p3, 0.5f ); + var tangent = Vector2.Normalize(pos2-pos1); + pntCatMull = pos1; + interpolant = (endPoint, movedDistance+advanceDistance, tangent); + if (!(MathF.Abs( interpolant.dir.Length()-1 ) < 0.001f)) { - splineTangent = route.Point( prevPointIndex ).dir; + interpolant.dir = new Vector2( 1, 0 ); } - interpolant = (endPoint, movedDistance+advanceDistance, splineTangent ); - Debug.Assert( MathF.Abs( interpolant.dir.Length()-1 ) < 0.001f ); return true; - } - - - static 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*.5f ); - return (b + t); } - static (Vector2 pos, Vector2 tangent) CatmullRom2( float t, Vector2 p0, Vector2 p1, Vector2 p2, Vector2 p3, float alpha = .5f /* between 0 and 1 */ ) + public static Vector2 CatmullRom( float t, Vector2 p0, Vector2 p1, Vector2 p2, Vector2 p3, float alpha = 0.5f ) { 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 ); + 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; - // Calculate the tangent using the derivative of the Catmull-Rom equation - Vector2 tangent = ( - (-0.5f * ((1 - alpha) * (1 - alpha)) * (p0 - p1) + - 0.5f * (2 - 3 * alpha) * (p2 - p0) + - 0.5f * alpha * (p3 - p1)) / (t2 - t1)); + 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; - return (C, tangent); + 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; + } + + public static 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); } static int GetPointInSegment( Vector2 p, Vector2 v0, Vector2 v1, out Vector2 interpolated ) diff --git a/ZeroMQTest/Code/OVParsers/Netex.cs b/ZeroMQTest/Code/OVParsers/Netex.cs index f59270bb12327f348a46d3e6959fc584d264b926..622cf55823c6d668f69f6c9e0c75d72d9b6ffab7 100644 --- a/ZeroMQTest/Code/OVParsers/Netex.cs +++ b/ZeroMQTest/Code/OVParsers/Netex.cs @@ -1,9 +1,7 @@ using System.Diagnostics; -using System.IO; using System.IO.Compression; using System.Numerics; using System.Reflection; -using System.Security.Cryptography; using System.Xml; namespace OVAPI.Code.OVParsers @@ -109,7 +107,7 @@ namespace OVAPI.Code.OVParsers Dictionary<string, Destination> destinations = new(); Dictionary<string, Stop> stops = new(); Dictionary<string, JourneyPattern> journeyPatterns = new(); - Dictionary<string, JourneyPattern> journeyNumbersToPattern = new(); + Dictionary<string, Dictionary<string, List<JourneyPattern>>> journeyNumbersToPattern = new(); public void PrintStats() { @@ -359,16 +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 ); - if (!journeyNumbersToPattern.TryGetValue( key, out var existingJourney )) + if (!journeyNumbersToPattern.TryGetValue( key, out var mapOfLinesToPatterns )) { - var journeyPattern = journeyPatterns[journeyRef]; - journeyNumbersToPattern.Add( key, journeyPattern ); + mapOfLinesToPatterns = new Dictionary<string, List<JourneyPattern>>(); + journeyNumbersToPattern.Add( key, mapOfLinesToPatterns ); } - // This happesn due to different availability conditions. Ignore this. We are only intrested in the route. - //else if (journeyRef != existingJourney.id) - //{ - // Console.WriteLine( $"JourneyNumber: {key} already maps to different journeypattern: {existingJourney.id}." ); - //} + 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 ); } catch (Exception e) { @@ -406,7 +407,7 @@ namespace OVAPI.Code.OVParsers Console.WriteLine( "NETEX Loaded " + Path.GetFileName( gzPath ) ); } - // Concacat link points to routes, so one big array of points. + // Concat link points to routes, so one big array of points. List<string> invalidRoutes = new(); foreach (var kvp in routes) { @@ -474,6 +475,13 @@ namespace OVAPI.Code.OVParsers var lastElem = route.points[route.points.Count-1]; lastElem.dir = route.points[route.points.Count-2].dir; route.points[route.points.Count-1] = lastElem; + + // BIG HACK, it appears that there is a final point which is perpendicular to the actual last point, thereby all busses appear 90 deg rotated at their end point. + // Remove last point. + route.points.RemoveAt( route.points.Count-1 ); + + // The same goes for first point. + route.points.RemoveAt( 0 ); } else { @@ -494,22 +502,28 @@ namespace OVAPI.Code.OVParsers } // Network overrides - public IJourney FindJourney( string owner, string journeyNumb ) + public IJourney FindJourney( string owner, string journeyNumb, string privLineNumber ) { string key = string.Join(':', owner, journeyNumb); - if (journeyNumbersToPattern.TryGetValue( key, out var journeyPattern )) + if (journeyNumbersToPattern.TryGetValue( key, out var lineToJourneys )) { - return journeyPattern; + if (lineToJourneys.TryGetValue( privLineNumber, out var listOfJourneys )) + { + return listOfJourneys[0]; + } } return null; } public IStop FindStop( string owner, string userStopCode ) { - string key = string.Join(':', owner, "ScheduledStopPoint", userStopCode); - if (stops.TryGetValue( key, out var stop )) + if (!(string.IsNullOrEmpty( owner ) || string.IsNullOrEmpty( userStopCode ))) { - return stop; + string key = string.Join(':', owner, "ScheduledStopPoint", userStopCode); + if (stops.TryGetValue( key, out var stop )) + { + return stop; + } } return null; } diff --git a/ZeroMQTest/Code/OVRequester.cs b/ZeroMQTest/Code/OVRequester.cs index 0e60dfec4489044893cde557ce530f5ecc3eee2e..bcdaf2b658c489ef8cc6a00452971288a900b00b 100644 --- a/ZeroMQTest/Code/OVRequester.cs +++ b/ZeroMQTest/Code/OVRequester.cs @@ -1,7 +1,5 @@ using NetMQ; using NetMQ.Sockets; -using System.Collections.Generic; -using System.Diagnostics.SymbolStore; using System.IO.Compression; using System.Text; using System.Xml; @@ -73,12 +71,19 @@ namespace WanderOV Rdx, Rdy, StopCode, + Timestamp, Unknown } - public struct VehicelInfo + public struct VehicleInfo { public string[]infos; + + public string LinePlanningNumb => infos[(int)VehicleInfoType.LinePrivateNumber]; + public string JournyNumb => infos[(int)VehicleInfoType.JourneyNumber]; + public string Owner => infos[(int)VehicleInfoType.Owner]; + public string Number => infos[(int)VehicleInfoType.VehicleNumber]; + public string ID => string.Join( ':', Owner, Number ); } @@ -111,11 +116,11 @@ namespace WanderOV } // Blocking call. - public List<VehicelInfo> PeekLiveOVData() + public bool PeekLiveOVData(out Dictionary<string, VehicleInfo> vehicles) { + vehicles = new(); try { - List<VehicelInfo> vehicles = new(); while (socket.HasIn) { var topic = socket.ReceiveFrameString(); @@ -130,14 +135,12 @@ namespace WanderOV UpdateVehicleInfo( vehicles, stream ); } - return vehicles; } catch (Exception e) { Console.WriteLine( e.ToString() ); } - - return null; + return vehicles.Count!=0; } bool IsUsefulXmlNode(XmlReader reader) @@ -146,9 +149,9 @@ namespace WanderOV reader.Name.EndsWith( "ONSTOP" ) || reader.Name.EndsWith( "DEPARTURE" ); } - List<VehicelInfo> UpdateVehicleInfo( List<VehicelInfo> vehicles, Stream stream ) + void UpdateVehicleInfo( Dictionary<string, VehicleInfo> vehicles, Stream stream ) { - var vehicle = new VehicelInfo(); + var vehicle = new VehicleInfo(); var type = VehicleInfoType.Unknown; bool onRoute = false; @@ -163,7 +166,7 @@ namespace WanderOV if (IsUsefulXmlNode(reader)) { onRoute = true; - vehicle.infos = new string[8]; + vehicle.infos = new string[9]; } if (reader.Name.EndsWith( "dataownercode" )) type = VehicleInfoType.Owner; if (reader.Name.EndsWith( "lineplanningnumber" )) type = VehicleInfoType.LinePrivateNumber; @@ -173,6 +176,7 @@ namespace WanderOV if (reader.Name.EndsWith( "rd-x" )) type = VehicleInfoType.Rdx; if (reader.Name.EndsWith( "rd-y" )) type = VehicleInfoType.Rdy; if (reader.Name.EndsWith( "userstopcode" )) type = VehicleInfoType.StopCode; + if (reader.Name.EndsWith( "timestamp" )) type = VehicleInfoType.Timestamp; break; case XmlNodeType.Text: @@ -189,7 +193,21 @@ namespace WanderOV { if (onRoute) { - vehicles.Add( vehicle ); + var id = vehicle.ID; + if (vehicles.TryGetValue( id, out var existingVehicle )) + { + var t1 = DateTime.Parse( existingVehicle.infos[(int)VehicleInfoType.Timestamp] ); + var t2 = DateTime.Parse( vehicle.infos[(int)VehicleInfoType.Timestamp] ); + if (t2 > t1) + { + vehicles.Remove( id ); + vehicles.Add( id, vehicle ); + } + } + else + { + vehicles.Add( id, vehicle ); + } } onRoute = false; } @@ -197,8 +215,6 @@ namespace WanderOV } } } - - return vehicles; } } diff --git a/ZeroMQTest/Code/Program.cs b/ZeroMQTest/Code/Program.cs index d1bdcab6e1ca4794491aeb834080b137084a90f8..f389e5601537b5fdee00b59dc5a378f3f95bb837 100644 --- a/ZeroMQTest/Code/Program.cs +++ b/ZeroMQTest/Code/Program.cs @@ -1,10 +1,13 @@ -using Microsoft.VisualStudio.TestTools.UnitTesting; -using OVApI; +using OVApI; using OVAPI.Code; using OVAPI.Code.OVParsers; using System.Diagnostics; using WanderOV; +//UnitTests unitTests = new UnitTests(); +////unitTests.TestExtrapolation(); +//unitTests.TestCatMullRom(); + while (true) { @@ -22,9 +25,6 @@ while (true) } -//UnitTests unitTests = new UnitTests(); -//unitTests.TestExtrapolation(); - class App { ActiveOV activeOV; @@ -52,45 +52,46 @@ class App { sw.Restart(); - // Request new OV update, returns a list vehicles of that can be updated. - var vehicleUpdates = requester.PeekLiveOVData(); - - // Update the active simulation with new vehicle updates. - //if (vehicleUpdates != null) - //{ - // var varFilteredList = vehicleUpdates.Where( vhi => vhi.infos[0]=="HTM" ).ToList(); /// QQQ - // activeOV.UpdateVehicles( varFilteredList ); - //} - activeOV.UpdateVehicles(vehicleUpdates); + // See if new data is available and process accordingly. + if (requester.PeekLiveOVData(out var updatedVehicles )) + { + activeOV.UpdateVehicles( updatedVehicles ); - // Accept and remove clients and process requests from clients. - broker.Update(); + // Accept and remove clients and process requests from clients. + broker.Update(); - // Requests to the broker results in messages, process them. - for (BrokerMsg msg = broker.GetMessage(); msg !=null; msg = broker.GetMessage()) - { - switch(msg.fields[0] ) + // Requests to the broker results in messages, process them. + for (BrokerMsg msg = broker.GetMessage();msg !=null;msg = broker.GetMessage()) { - case "vehicles": // Advance sim. - // activeOV.AdvanceSimulationToNow(); - var vehicles = activeOV.GetStateAsCSV(); - if (vehicles != null) - { - broker.SendMessage( "vehicles", vehicles, msg.from ); - } - break; - - case "journey": // Expects owner, journeyNumber. - var journey = network.FindJourney( msg.fields[1], msg.fields[2] ); - if (journey!=null) - { - // field[1] is owner, field[2] is journeyNumber, field[3] is vehicleNumber - broker.SendMessage( "journey", string.Join('|', msg.fields[1], msg.fields[3], journey.AsMsg()), msg.from ); - } - break; + switch (msg.fields[0]) + { + case "vehicles": // Advance sim. + if (msg.fields.Length>=5) + { + msg.client.UpdateInterestArea( float.Parse( msg.fields[1] ), float.Parse( msg.fields[2] ), float.Parse( msg.fields[3] ), float.Parse( msg.fields[4] ) ); + } + activeOV.AdvanceSimulationToNow(); + var vehicles = activeOV.GetStateAsCSV(); + if (vehicles != null) + { + broker.SendMessage( "vehicles", vehicles, msg.from ); + } + break; + + case "journey": // Expects owner, journeyNumber. + 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 + broker.SendMessage( "journey", string.Join( '|', msg.fields[1], msg.fields[3], journey.AsMsg() ), msg.from ); + } + break; + } } } + + long timeToWait = 50-sw.ElapsedMilliseconds; if (timeToWait > 0) Thread.Sleep( (int)timeToWait ); } diff --git a/ZeroMQTest/Tests/UnitTests.cs b/ZeroMQTest/Tests/UnitTests.cs index a6bdcb5fce04215a214871915791d5d504d60c63..8949433f3f96aaa01f88f32d553971fcd0110e37 100644 --- a/ZeroMQTest/Tests/UnitTests.cs +++ b/ZeroMQTest/Tests/UnitTests.cs @@ -16,21 +16,17 @@ namespace OVApI netex.LoadAll( "_database" ); netex.PrintStats(); - var stop = netex.FindStop( "ARR", "54441390" ); - Assert.IsNotNull( stop ); - - var journey = netex.FindJourney( "HTM", "36005" ); + var journey = netex.FindJourney( "CXX", "414", "E303" ); int prevPos = -1; var prevPoint = journey.Route.Point( 0 ).point; - prevPoint.X -= 10; - float routeTotDist = 0; + float routeTotalDist = 0; for (int i = 0; i < journey.Route.NumPoints; i++) { - routeTotDist += journey.Route.Point( i ).dist; + routeTotalDist += journey.Route.Point( i ).dist; } - float advDist = 7200; // 1 mtr; + float advDist = 1; int kIters = 0; float totDist = 0; @@ -47,8 +43,32 @@ namespace OVApI } totDist += interpolant.dst; - var delta = MathF.Abs( totDist-routeTotDist ); - Assert.IsTrue( delta < 0.01f ); + var delta = MathF.Abs( totDist-routeTotalDist ); + Assert.IsTrue( delta < 0.01f ); // This only works, if the first point on the route is not halfway somewhere. + } + + [TestMethod] + public void TestCatMullRom() + { + Vector2 p0, p1, p2, p3; + p0 = new Vector2( 0, 0 ); + p1 = new Vector2( 1, 0 ); + p2 = new Vector2( 1, 1 ); + p3 = new Vector2( 1, 2 ); + + int kIters = 1000; + float t = 0.1f; + + while (kIters-- > 0) + { + float t2 = t+0.1f; + var C1 = NetworkUtils.CatmullRom( t, p0, p1, p2, p3, 1 ); + var C2 = NetworkUtils.CatmullRom( t2, p0, p1, p2, p3, 1 ); + var tangent = Vector2.Normalize( (C2 - C1 )); + Assert.IsTrue( NetworkUtils.Sane( tangent ) ); + t += 0.1f; + } + int kt = 0; } } } \ No newline at end of file