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");
}
