/// <summary>
/// Parse the given JOSM GeoJson output to extract nodes.
/// </summary>
/// <param name="collection">GeoJson from JOSM</param>
/// <param name="nodes">Extracted nodes</param>
/// <param name="edges">Extracted edges</param>
/// <returns>True if successful</returns>
public static bool ParseNodeSource(FeatureCollection collection, out List <Node> nodes, out List <NodeEdge> edges)
{
nodes = new List <Node>(100);
edges = new List <NodeEdge>(200);
try
{
List <(IPosition, IPosition)> CorridorPointPairs = new List <(IPosition, IPosition)>(200);
List <Feature> features = collection.Features;
if (features == null)
{
return(false);
}
// Iterate through features.
foreach (Feature f in features)
{
Node node = new Node();
if (f.Properties != null)
{
// Check for flag property. This is a room/stairs/lift/block node.
if (f.Properties.ContainsKey("NAV_FEATURE") && f.Properties.ContainsKey("node_type") && !string.Equals(f.Properties["node_type"].ToString(), "block", StringComparison.OrdinalIgnoreCase))
{
// We don't need to add block nodes as they are not used for navigation.
node.NodeId = f.Properties["id"].ToString().ToLower().Trim();
node.BuildingCode = f.Properties["building"].ToString().ToLower().Trim();
node.Floor = Convert.ToByte(f.Properties["level"].ToString().ToLower().Trim());
node.Latitude = null;
node.Longitude = null;
node.Type = SharedFunctions.GetNodeType(f.Properties["node_type"].ToString());
node.LeafletNodeType = f.Properties["node_type"].ToString().ToLower().Trim();
if (node.NodeId[0] == 'm')
{
node.Type = NodeType.Unrouteable;
}
// Check for a name.
if (f.Properties.ContainsKey("name") && f.Properties["name"] != null)
{
node.RoomName = f.Properties["name"].ToString();
}
// Check for connected nodes property.
if (f.Properties.ContainsKey("connected_nodes") && f.Properties["connected_nodes"] != null)
{
string[] cons = f.Properties["connected_nodes"].ToString().ToLower().Split(',');
foreach (string con in cons)
{
edges.Add(new NodeEdge(node.NodeId, con.ToLower().Trim(), 999999));
}
}
nodes.Add(node);
}
// Is this feature a line string?
if (f.Geometry?.Type == GeoJSON.Net.GeoJSONObjectType.LineString)
{
LineString linestring = f.Geometry as LineString;
if (linestring.Coordinates.Count > 0)
{
// Iterate through the coordinates.
for (int i = 1; i < linestring.Coordinates.Count; i++)
{
// Extract all coordinate pairs for later use matching against extracted corridor nodes.
IPosition point1 = linestring.Coordinates[i - 1];
IPosition point2 = linestring.Coordinates[i];
CorridorPointPairs.Add((point1, point2));
}
}
}
// Check for flag property.
if (f.Properties.ContainsKey("NAV_CORRIDOR"))
{
// This is a corridor.
node.NodeId = f.Properties["id"].ToString().ToLower().Trim();
node.BuildingCode = f.Properties["building"].ToString().ToLower().Trim();
node.Floor = Convert.ToByte(f.Properties["level"].ToString().ToLower().Trim());
node.Latitude = ((Point)f.Geometry).Coordinates.Latitude;
node.Longitude = ((Point)f.Geometry).Coordinates.Longitude;
node.Type = SharedFunctions.GetNodeType(f.Properties["node_type"].ToString());
node.CorridorWidth = Convert.ToDouble(f.Properties["corridor_width"].ToString());
node.LeafletNodeType = f.Properties["node_type"].ToString().ToLower().Trim();
if (f.Properties.ContainsKey("name"))
{
node.RoomName = f.Properties["name"].ToString().Trim();
}
// Check for connected nodes property.
if (f.Properties.ContainsKey("connected_nodes") && f.Properties["connected_nodes"] != null)
{
string[] cons = f.Properties["connected_nodes"].ToString().ToLower().Split(',');
foreach (string con in cons)
{
edges.Add(new NodeEdge(node.NodeId, con.ToLower().Trim(), 999999));
}
}
nodes.Add(node);
}
}
}// End foreach loop.
// Sort out line-string connections - Corridors can be connected to each other using
// a simple line string.
edges.AddRange(LineStringsToEdges(CorridorPointPairs, nodes, features));
return(true);
}
catch
{
return(false);
}
}
/// <summary>
/// Verify Node edges on this single floor.
/// </summary>
/// <param name="nodes">Nodes on this floor</param>
/// <param name="edges">Corresponding edges on this floor</param>
/// <returns>A list of broken connection errors, if any</returns>
public static List <string> VerifyNodeEdgesSingleFloor(List <Node> nodes, List <NodeEdge> edges)
{
List <string> brokenConnections = new List <string>();
// Iterate through all nodes.
foreach (Node node in nodes)
{
// Ensure this node has connections.
NodeEdge[] connections = edges.Where(e => e.Node1Id == node.NodeId || e.Node2Id == node.NodeId).ToArray();
// Iterate through its connections.
foreach (NodeEdge connection in connections)
{
// Skip connections between stairs and lifts because they require more than one floor to verify.
if ((connection.Node1Id.StartsWith("s", StringComparison.OrdinalIgnoreCase) && connection.Node2Id.StartsWith("s", StringComparison.OrdinalIgnoreCase)) ||
(connection.Node1Id.StartsWith("l", StringComparison.OrdinalIgnoreCase) && connection.Node2Id.StartsWith("l", StringComparison.OrdinalIgnoreCase)))
{
continue;
}
string node1BC = SharedFunctions.GetBuildingCodeFromId(connection.Node1Id);
string node2BC = SharedFunctions.GetBuildingCodeFromId(connection.Node2Id);
sbyte node1Floor = -1;
try
{
node1Floor = SharedFunctions.GetLevelFromId(connection.Node1Id);
if (node1Floor < 0)
{
throw new InvalidOperationException("No Level!");
}
}
catch
{
brokenConnections.Add($"{connection.Node1Id} is missing a level number in the connection to {connection.Node2Id}");
continue;
}
sbyte node2Floor = -1;
try
{
node2Floor = SharedFunctions.GetLevelFromId(connection.Node2Id);
if (node2Floor < 0)
{
throw new InvalidOperationException("No Level!");
}
}
catch
{
brokenConnections.Add($"{connection.Node2Id} is missing a level number in the connection to {connection.Node1Id}");
continue;
}
if (string.IsNullOrWhiteSpace(node1BC))
{
brokenConnections.Add($"{connection.Node1Id} is missing a building code in its connection to {connection.Node2Id}");
continue;
}
if (string.IsNullOrWhiteSpace(node2BC))
{
brokenConnections.Add($"{connection.Node2Id} is missing a building code in its connection to {connection.Node1Id}");
continue;
}
// Skip connections between outdoors and indoors - different building, or different floors - codes as they require more than on json file to verify.
if (node1BC != node2BC || node1Floor != node2Floor)
{
continue;
}
Node node1 = nodes.Find(n => n.NodeId == connection.Node1Id);
Node node2 = nodes.Find(n => n.NodeId == connection.Node2Id);
// Check both nodes exist.
if (node1 == default)
{
brokenConnections.Add($"Cannot find node: {connection.Node1Id}");
continue;
}
if (node2 == default)
{
brokenConnections.Add($"Cannot find node: {connection.Node2Id}");
continue;
}
NodeEdge result = Array.Find(connections, c => c.Node1Id == node2.NodeId && c.Node2Id == node1.NodeId);
// Check a connection exists both ways.
if (result == null)
{
// Connection does not exist.
brokenConnections.Add($"Broken Connection: {node1.NodeId} is only connected to {node2.NodeId} one way");
}
}
}
return(brokenConnections);
}
/// <summary>
/// Check if a feature has a valid id property.
/// </summary>
/// <param name="f">Feature to check</param>
/// <param name="errors">reference to errors list</param>
/// <param name="warnings">reference to warnings list</param>
/// <returns>True if valid</returns>
private static bool IsIdValid(Feature f, List <string> errors, List <string> warnings)
{
// Get property values.
string id = f.Properties["id"].ToString().ToLower().Trim();
string level = f.Properties["level"].ToString().ToLower().Trim();
string building = f.Properties["building"].ToString().ToLower().Trim();
string nodeType = f.Properties["node_type"].ToString().ToLower().Trim();
// Split Id string.
string[] idParts = id.Split('_');
StringBuilder buildingCode = new StringBuilder();
StringBuilder floor = new StringBuilder();
string idNodeType;
if (idParts.Length > 0)
{
// Get the node type character.
idNodeType = idParts[0];
if (idParts.Length > 1)
{
// Get the building code and floor number string.
string buildingFloorCode = idParts[1];
// Get building code and floor number separately.
for (int i = 0; i < buildingFloorCode.Length; i++)
{
if (char.IsLetter(buildingFloorCode[i]))
{
buildingCode.Append(buildingFloorCode[i]);
}
else if (char.IsDigit(buildingFloorCode[i]))
{
floor.Append(buildingFloorCode[i]);
}
}
}
else
{
errors.Add($"Could not find a valid id for feature.");
return(false);
}
}
else
{
errors.Add($"Could not find a valid id for feature.");
return(false);
}
// Check node ID matches name if it is a routable room.
if (nodeType == "room" || nodeType == "other")
{
string roomCodeFromId = SharedFunctions.GetRoomCodeFromId(id);
string name = f.Properties["name"].ToString();
if (!name.StartsWith(roomCodeFromId, StringComparison.OrdinalIgnoreCase))
{
warnings.Add($"The room {id} has a potentially invalid name: '{name}' for the given id. Human Check Required. (Consider if this node should be named with the room code or not)");
}
}
// Validation checks.
if (idNodeType == "r" && nodeType != "room" && nodeType != "other")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "m" && nodeType != "room" && nodeType != "other")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "s" && nodeType != "stairs")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "l" && nodeType != "lift")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "c" && nodeType != "corridor")
{
errors.Add($"The corridor node {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "p" && nodeType != "parking")
{
errors.Add($"The parking node {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "wcm" && nodeType != "wcm")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "wcf" && nodeType != "wcf")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "wcb" && nodeType != "wcb")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "wcd" && nodeType != "wcd")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "wcmf" && nodeType != "wcmf")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "wcn" && nodeType != "wcn")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (idNodeType == "wcs" && nodeType != "wcs")
{
errors.Add($"The room {id} does not have a valid node_type tag");
return(false);
}
if (building != buildingCode.ToString())
{
errors.Add($"The feature {id} does not have a valid building tag");
return(false);
}
if (floor.ToString() != level)
{
errors.Add($"The feature {id} does not have a valid level tag");
return(false);
}
return(true);
}
/// <summary>
/// Calculate the weights of each edge.
/// </summary>
/// <param name="nodes">A list of all nodes involved</param>
/// <param name="edges">A list of all edges to weigh</param>
/// <returns>A list of edges with correct weights</returns>
public static List <NodeEdge> CalculateWeights(List <Node> nodes, List <NodeEdge> edges, EdgeCaseWeightsConfiguration edgeCases)
{
// Iterate through edges
foreach (NodeEdge edge in edges)
{
// Default weight is high to avoid routing weird ways - any arbitrary high number will do.
double weight = 999999;
double?area = null;
// Lower weight for stairs to prioritise them when changing floors.
if (edge.Node1Id.StartsWith("s_") || edge.Node2Id.StartsWith("s_"))
{
weight = 888888;
}
// Get nodes for each side of the edge.
Node node1 = nodes.Find(n => n.NodeId == edge.Node1Id);
Node node2 = nodes.Find(n => n.NodeId == edge.Node2Id);
double distance;
if (node1 != null && node2 != null)
{
if (node1.Latitude.HasValue && node2.Latitude.HasValue && node1.Longitude.HasValue && node2.Longitude.HasValue)
{
// Calculate weight from physical distance.
distance = SharedFunctions.GetDistanceFromLatLonInMeters(node1.Latitude.GetValueOrDefault(), node1.Longitude.GetValueOrDefault(), node2.Latitude.GetValueOrDefault(), node2.Longitude.GetValueOrDefault());
weight = distance;
}
else
{
distance = SharedFunctions.NonCorridorDistance;
}
if (node1.CorridorWidth != null && node2.CorridorWidth != null)
{
// Calculate average area between points.
// Get average width.
double width = (node1.CorridorWidth.GetValueOrDefault() + node2.CorridorWidth.GetValueOrDefault()) / 2;
// Calculate area.
area = width * distance;
}
}
// Handle special cases.
// Iterate through either node list.
foreach (EitherNodeEntry item in edgeCases.EitherNode)
{
if (NodeIdStartsWith(edge.Node1Id, item.StringStart) || NodeIdStartsWith(edge.Node2Id, item.StringStart))
{
weight = item.Weight;
}
}
// Iterate through both nodes list.
foreach (BothNodesEntry item in edgeCases.BothNodes)
{
if (NodeIdStartsWith(edge.Node1Id, item.Node1String) && NodeIdStartsWith(edge.Node2Id, item.Node2String))
{
weight = item.Weight;
}
}
edge.Weight = weight;
edge.CorridorArea = area;
}
return(edges);
}
/// <summary>
/// Use the A* algorithm to calculate a route, adjusted for congestion.
/// </summary>
/// <param name="start">Start node id</param>
/// <param name="end">End node id</param>
/// <param name="allStudentRoutes">A collection of all student routes for today.</param>
/// <param name="startingTime">The requested starting time of this route.</param>
/// <returns>A route object with the calculated route</returns>
/// <exception cref="Exception"></exception>
public Route BuildAStar(string start, string end, StudentRoute[] allStudentRoutes, TimeSpan startingTime)
{
// Check if the start is the same as the end, or either the start or end are invalid nodes.
if (start == end || !AllNodes.ContainsKey(start) || !AllNodes.ContainsKey(end))
{
// return an empty route
return(new Route());
}
// Get the start node object from its id.
Node startNode = AllNodes[start];
// Get the end node object from its id.
Node endNode = AllNodes[end];
// Find the closest corridor node from the end node - this is used by A* as the lat/long end point for heuristic function.
// This is often one of the direct connections most cases, however we may need to look further into the graph for outlier cases.
Node endCorridor = FindNextCorridorNode(endNode, AllNodes);
// Initialise the frontier priority queue.
FastPriorityQueue <AStarNode> frontier = new FastPriorityQueue <AStarNode>(AllNodes.Count);
// Initialise a lookup table for the queue.
Dictionary <string, AStarNode> queueTable = new Dictionary <string, AStarNode>();
// Heuristic for A* algorithm - Calculate the distance between the given node and the end corridor, or default to 1 if the given node is not a corridor.
float heuristicFunction(Node node) => node.Type == NodeType.Corridor || node.Type == NodeType.Parking ? (float)node.DistanceInMetersTo(endCorridor) : SharedFunctions.NonCorridorDistance;
// Instantiate an AStarNode from the start node.
AStarNode starter = new AStarNode(startNode, null, 0f);
// Add to the queue table
queueTable.Add(startNode.NodeId, starter);
// Add to the priority queue.
frontier.Enqueue(starter, heuristicFunction(startNode));
// Loop until the priority queue is empty.
while (frontier.Count > 0)
{
// Dequeue the lowest cost node.
AStarNode current = frontier.Dequeue();
// Get its true cost.
float currentCost = current.TrueCost;
// Check if the current node is the end corridor.
if (current.Node.NodeId == endCorridor.NodeId)
{
// Route found.
// Backtrack to build the route.
List <Node> route = BackTrack(current);
// Add end node as we only routed to the nearest corridor.
route.Add(endNode);
// Return the final route.
return(new Route(route, current.TrueCost));
}
// Mark the current node as visited.
current.MarkVisited();
AStarNode currentNode = current;
double distanceToThisPoint = 0;
// Calculate the time spent getting to this point.
while (currentNode.LeadingNode != null)
{
// Check this is a corridor node.
if (currentNode.Node.Type == NodeType.Corridor && currentNode.LeadingNode.Node.Type == NodeType.Corridor)
{
// Get the distance to the leading node.
distanceToThisPoint += currentNode.Node.DistanceInMetersTo(currentNode.LeadingNode.Node);
}
else
{
distanceToThisPoint += SharedFunctions.NonCorridorDistance;
}
currentNode = currentNode.LeadingNode;
}
// Calculate the time taken to get to this point.
double timeElapsed = SharedFunctions.CalculateWalkingTimeNoRounding(distanceToThisPoint);
// Calculate the time of day from the elapsed time.
TimeSpan currentTime = startingTime.Add(TimeSpan.FromSeconds(timeElapsed));
// Calculate the occupancies at this point.
Dictionary <(string entryId, string exitId), int> edgeOccupancies = CongestionHelper.CalculateEdgeOccupanciesAtTime(allStudentRoutes, currentTime);
// Iterate through all edges of the current node.
foreach (NodeEdge edge in current.Node.OutgoingEdges)
{
// End node of edge.
Node edgeEnd = edge.Node2;
// Check if the node already exists in the queue table.
bool edgeEndAlreadyPresent = queueTable.TryGetValue(edgeEnd.NodeId, out AStarNode target);
// Check if we have already visited this node.
bool alreadyVisitedEdgeEnd = edgeEndAlreadyPresent && target.Visited;
// If we have already visited this node, skip it.
if (alreadyVisitedEdgeEnd)
{
continue;
}
float congestionMultiplier = CalculateCongestionMultiplier(edge, edgeOccupancies);
// Calculate the true cost of this edge.
float trueCost = currentCost + ((float)edge.Weight * congestionMultiplier);
// Calculate the heuristic cost of this node.
float heuristicCost = heuristicFunction(edgeEnd);
// Total the costs and use the congestion multiplier.
float combinedCost = trueCost + heuristicCost;
// Is the edge already in the queue?
if (edgeEndAlreadyPresent)
{
// Only use new route if it is better.
if (combinedCost < target.TrueCost)
{
// It is better, update the nodes in the queue.
target.Update(current, trueCost);
frontier.UpdatePriority(target, combinedCost);
}
}
else
{
// It is not in the queue, instantiate an AStarNode object.
target = new AStarNode(AllNodes[edgeEnd.NodeId], current, trueCost);
// Add to the queue table.
queueTable.Add(edgeEnd.NodeId, target);
// Add to the queue.
frontier.Enqueue(target, combinedCost);
}
}
}
// No route could be found.
return(new Route());
}
