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