public SegmentLink(int id, int segment1, SegmentEndpoint ep1, int segment2, SegmentEndpoint ep2)
{
Trace.Assert(segment1 <= segment2);
Trace.Assert(segment1 != segment2 || ep1 != ep2);
Id = id;
Segment1 = segment1;
Ep1 = ep1;
Segment2 = segment2;
Ep2 = ep2;
}
private static void CreateLinksForSegmentEndpoint(List <SegmentLink> segmentLinks, QuadTree quadTree, Segment seg, SegmentEndpoint ep, float range, float maxAngle, float warningAngle)
{
var point = seg.GetEndpoint(ep);
// Get a list of (segment, endpoint) tuples around point of interest
// TODO: also implement a warningRange akin to warningAngle
var candidates = quadTree.FindSegmentEndpointsNear(point, range);
var maxCosine = Math.Cos(maxAngle);
var warningCosine = Math.Cos(warningAngle);
foreach (var(candiSeg, candiEp) in candidates)
{
var linkId = 1 + segmentLinks.Count;
// Only add if candidate ID precedes segment ID -- this prevents adding everything twice
// We're also not interested in cases where (seg1, ep1) == (seg2, ep2)
if (seg.Id < candiSeg.Id)
{
// TODO: we could also check 1st-order continuity (tangent)
var tangent1 = seg.GetEndpointTangent(ep, true);
var tangent2 = candiSeg.GetEndpointTangent(candiEp, false);
// Angle too great?
var dot = Vector3.Dot(tangent1, tangent2);
if (dot < maxCosine)
{
// ...but within warning area?
if (dot > warningCosine)
{
Console.WriteLine($"Warning: angle between segments {seg.Id}, {candiSeg.Id} too large: {Math.Acos(dot) * 180.0f / Math.PI:F1} deg > {maxAngle * 180.0f / Math.PI:F1} deg");
}
// Reject pair of segments on basis of too great angle
continue;
}
segmentLinks.Add(new SegmentLink(linkId, seg.Id, ep, candiSeg.Id, candiEp));
}
}
}
public (Station station, StationStop stop, float distance, TrajectorySegment[] plan)? FindNearestStationAlongTrack(int segmentId, float t,
SegmentEndpoint dir, int?excludedStationId, bool verbose)
{
throw new NotImplementedException();
}
public SegmentLink[] FindConnectingSegments(int segmentId, SegmentEndpoint ep)
{
throw new NotImplementedException();
}
// constructors
/// <summary>Creates a new instance of this structure.</summary>
/// <param name="segment">The segment this connection points to, or a null reference.</param>
/// <param name="endpoint">The endpoint of the segment this connection points to, or SegmentEndpoint.Invalid.</param>
public SegmentConnection(Segment segment, SegmentEndpoint endpoint)
{
this.Segment = segment;
this.Endpoint = endpoint;
}
public Vector3 GetEndpoint(SegmentEndpoint ep)
{
return(ep switch {
SegmentEndpoint.Start => ControlPoints[0],
SegmentEndpoint.End => ControlPoints[^ 1]
public static float DistanceToEndpoint(this Segment segment, float t, SegmentEndpoint toEp)
{
return(toEp switch {
SegmentEndpoint.Start => segment.GetLength() * t,
SegmentEndpoint.End => segment.GetLength() * (1 - t),
});
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
public (int, SegmentEndpoint)? GetPreferredContinuationSegment(int fromSegmentId, SegmentEndpoint fromEp)
{
return(_tractionController.GetPreferredContinuationSegment(fromSegmentId, fromEp));
}
