private void SplitNode(QuadTreeNode node) { // Quadrants are arranged as +X+Y, -X+Y, -X-Y, +X-Y node.Quadrants = new QuadTreeNode[4]; var centerX = (node.BoundingMin.X + node.BoundingMax.X) / 2; var centerY = (node.BoundingMin.Y + node.BoundingMax.Y) / 2; node.Quadrants[0] = new QuadTreeNode(new Vector3(centerX, centerY, 0), new Vector3(node.BoundingMax.X, node.BoundingMax.Y, 0)); node.Quadrants[1] = new QuadTreeNode(new Vector3(node.BoundingMin.X, centerY, 0), new Vector3(centerX, node.BoundingMax.Y, 0)); node.Quadrants[2] = new QuadTreeNode(new Vector3(node.BoundingMin.X, node.BoundingMin.Y, 0), new Vector3(centerX, centerY, 0)); node.Quadrants[3] = new QuadTreeNode(new Vector3(centerX, node.BoundingMin.Y, 0), new Vector3(node.BoundingMax.X, centerY, 0)); var segmentIds = node.SegmentIds; node.SegmentIds = null; // Simply re-insert all the segments once more after the child nodes have been created foreach (var segId in segmentIds) { InsertSegment(node, _network.GetSegmentById(segId)); } }
private void GoToStation(int stationId, TrainStatus trainStatus) { // find destination station var station = _network.GetStationById(stationId); _log.Feed(_infoPin, $"Planning route to station {station.Name}"); // FIXME: wrong! correct to set this _only_ after arrival confirmation! _lastStationGoneTo = stationId; // FIXME: at the moment, this will only try the first stop of the station foreach (var stop in station.Stops) { int destinationSegmentId = stop.SegmentId; float destinationT = stop.T; var result = _routePlanner.PlanRoute(trainStatus.SegmentId, trainStatus.T, trainStatus.Dir, destinationSegmentId, destinationT); if (result != null) { var currentSegmentLength = _network.GetSegmentById(trainStatus.SegmentId).GetLength(); _currentPlanForStationId = stationId; _currentPlanCommand = new TractionControllerCommand { segmentsToFollow = new (int, SegmentEndpoint, float, float)[1 + result.route.Length]
private static void DrawLinks(INetworkDatabase ndb, Context cr, PointD center, double scale) { foreach (var link in ndb.EnumerateSegmentLinks()) { var pos = ndb.GetSegmentById(link.Segment1).GetEndpoint(link.Ep1); var c = aluminium6; c.A = 0.1; var pt = SimToCanvasSpace(new Vector3(pos.X, pos.Y, 0), center, scale); cr.SetSourceColor(c); cr.Arc(pt.X, pt.Y, 1, 0, 2 * Math.PI); cr.Fill(); } }
public RoutePlan?PlanRoute(int originSegmentId, float originT, SegmentEndpoint originDirection, int destinationSegmentId, float destinationT) { // TODO: handle degenerate case where origin == destination int MAX_ITERATIONS = 1_000; Console.WriteLine($"PlanRoute(({originSegmentId},{originT}->{originDirection}) ==> ({destinationSegmentId}, {destinationT}))"); Segment originSegment = _network.GetSegmentById(originSegmentId); Trace.Assert(originSegment.CanGoTowards(originDirection)); Segment destinationSegment = _network.GetSegmentById(destinationSegmentId); var destinationPoint = destinationSegment.GetPoint(destinationT); // Create priority queue for A* algorithm var queue = new SimplePriorityQueue <RoutePoint>(); var added = new HashSet <(int, SegmentEndpoint)>(); // Find and insert segments connected directly to origin segment float initialCost = originSegment.DistanceToEndpoint(originT, originDirection); float heuristicCost = (originSegment.GetEndpoint(originDirection) - destinationPoint).Length(); var candidates = _network.FindConnectingSegments(originSegmentId, originDirection); foreach (var c in candidates) { if (c.Segment1 != originSegmentId) { queue.Enqueue(new RoutePoint { Previous = null, ChainLength = 1, SegmentId = c.Segment1, EntryEp = c.Ep1, CostToReach = initialCost }, initialCost + heuristicCost); added.Add((c.Segment1, c.Ep1)); } else { queue.Enqueue(new RoutePoint { Previous = null, ChainLength = 1, SegmentId = c.Segment2, EntryEp = c.Ep2, CostToReach = initialCost }, initialCost + heuristicCost); added.Add((c.Segment2, c.Ep2)); } } // Now descend into the priority queue and look for a way towards the goal for (int iteration = 0; queue.Count != 0; iteration++) { var candidate = queue.Dequeue(); var segment = _network.GetSegmentById(candidate.SegmentId); if (!segment.CanEnterFrom(candidate.EntryEp)) { continue; } candidate.segmentLength = segment.GetLength(); //Console.WriteLine($"Try ({candidate.SegmentId}, {candidate.EntryEp})"); if (candidate.SegmentId == destinationSegmentId) { // Found a path float totalCost = candidate.CostToReach + segment.DistanceToEndpoint(destinationT, candidate.EntryEp); if (ExtraVerbosity) { Console.WriteLine($"PlanRoute finished after {iteration} iterations"); Console.WriteLine($"Now displaying {candidate.ChainLength}-element path starting from (Segment={originSegmentId} Pos={originSegment.GetPoint(originT)})"); Console.WriteLine($" - {originSegment.DistanceToEndpoint(originT, originDirection),6:F2}m in origin segment {originSegmentId} from t={originT} to {originDirection}"); DisplayChain(candidate); Console.WriteLine($" - {segment.DistanceToEndpoint(destinationT, candidate.EntryEp),6:F2}m in destination segment {destinationSegmentId} from {candidate.EntryEp} to t={destinationT}"); Console.WriteLine($"Total cost: {totalCost}"); Console.WriteLine(); } var plan = new RoutePlan { route = new (int, SegmentEndpoint, float, float)[candidate.ChainLength], totalCost = totalCost
private static void RenderToContext(SimulationCoordinateSpace coordinateSpace, INetworkDatabase ndb, IUnitDatabase units, IDictionary <int, TrainControlStateSummary>?controllerMap, Context cr, PointD center, double scale, int fontSize) { //Console.WriteLine("RenderFullView start"); cr.SetSourceColor(aluminium1); cr.Paint(); // cr.MoveTo(0, h); // cr.ShowText($"Scale: full width = {(w / scale)} meters"); // Draw quadtree var maybeQuadTree = ndb.GetQuadTreeIfYouHaveOne(); if (maybeQuadTree is {} quadTree) { DrawQuadTreeNode(quadTree.Root, cr, center, scale); } // Draw rail links DrawLinks(ndb, cr, center, scale); // Draw railway foreach (var seg in ndb.EnumerateSegments()) { Trace.Assert(seg.ControlPoints.Length == 2); var start = SimToCanvasSpace(seg.ControlPoints[0], center, scale); var end = SimToCanvasSpace(seg.ControlPoints[1], center, scale); cr.LineWidth = railLineWidth; cr.SetSourceColor(seg.Oneway == false ? aluminium6 : aluminium5); cr.MoveTo(start); cr.LineTo(end); cr.Stroke(); // DrawTextRegular(cr, $"{seg.Id}", new PointD((start.X + end.X) / 2, (start.Y + end.Y) / 2), 9); } // Draw stations foreach (var station in ndb.EnumerateStations()) { PointD pos0 = new PointD(); cr.SetSourceColor(skyBlue2); foreach (var stop in station.Stops) { var pos = ndb.GetSegmentById(stop.SegmentId).GetPoint(stop.T); var posCS = SimToCanvasSpace(pos, center, scale); pos0 = posCS; cr.LineWidth = 0.5; DrawCrosshair(cr, posCS, 5); } cr.SetSourceColor(aluminium2); DrawTextRegular(cr, station.Name, pos0, fontSize); } // Draw trains int unitIndex = 0; foreach (var unit in units.EnumerateUnits()) { var pos = unit.Pos; var head = unit.Pos + Vector3.Transform(new Vector3((float)(5 / scale), 0, 0), unit.Orientation); var posCS = SimToCanvasSpace(pos, center, scale); var headCS = SimToCanvasSpace(head, center, scale); var info = $"{unit.Class.Name}\n{unit.Velocity.Length() * 3.6:F1} km/h"; if (controllerMap != null && controllerMap.ContainsKey(unitIndex)) { info += "\n" + controllerMap[unitIndex].SchedulerState; var route = controllerMap[unitIndex].SegmentsToFollow; if (route != null) { foreach (var entry in route) { // try { // Console.WriteLine($"Segment => {entry.SegmentId}"); var seg = ndb.GetSegmentById(entry.SegmentId); Trace.Assert(seg.ControlPoints.Length == 2); var startXyz = seg.GetEndpoint(entry.EntryEp); var endXyz = entry.GoalT >= 0 ? seg.GetPoint(entry.GoalT) : seg.GetEndpoint(entry.EntryEp.Other()); var start = SimToCanvasSpace(startXyz, center, scale); var end = SimToCanvasSpace(endXyz, center, scale); cr.LineWidth = railLineWidth * 2; cr.SetSourceColor(plum1); cr.MoveTo(start); cr.LineTo(end); cr.Stroke(); // } // catch (System.InvalidOperationException ex) { // } } } } cr.LineWidth = railLineWidth * 2; cr.SetSourceColor(chameleon1); cr.MoveTo(posCS.X * 2 - headCS.X, posCS.Y * 2 - headCS.Y); cr.LineTo(headCS); cr.Stroke(); cr.SetSourceColor(chameleon3); DrawTextBold(cr, info, posCS, fontSize); unitIndex++; } // Draw info about agents // if (agents != null) // { // int i = 0; // foreach (var agent in agents) // { // DrawTextRegular(cr, agent.ToString(), new PointD(0, i * 10), 9); // i++; // } // } cr.LineWidth = 1; cr.SetSourceColor(scarledRed1); DrawCrosshair(cr, center, 10); //Console.WriteLine("RenderFullView done"); }