public double PathSize(RouteMeasures route)
{
double size = 0;
if (route.Status != EPathStatus.Found)
{
return(-1);
}
var nodes = route.NodeIDs;
Node current = this._graph.GetNodeById(long.Parse(nodes[0]));
for (var i = 1; i < nodes.Length; i++)
{
long nextNodeId = long.Parse(nodes[i]);
var nextNode = this._graph.GetNodeById(nextNodeId);
PathRoute pathRoute = Node.ShortestPathBetweenNeihbors(current, nextNode);
if (pathRoute.Status == EPathStatus.Found)
{
current = nextNode;
size += pathRoute.Distance;
}
else
{
return(-1);
}
}
return(size);
}
public void CorrectlyBuildPathRouteFromEdges_AradToSibiu()
{
// to bucharest from arad running 418km (arad > sibiu > rimnicu vilcea > pitesti > bucharest)
Graph graph = Import.LoadCityFromText(InfraTests.file_path + "test_graph_3.norvig.txt");
var arad = graph.GetNodeByLabel("Arad");
var sibiu = graph.GetNodeByLabel("Sibiu");
var rimnicuVilcea = graph.GetNodeByLabel("Rimnicu Vilcea");
var pitesti = graph.GetNodeByLabel("Pitesti");
var bucharest = graph.GetNodeByLabel("Bucharest");
var edge_arad_sibiu = graph.GetEdgeByLabel("arad_sibiu");
var edge_sibiu_rimnicu_vilcea = graph.GetEdgeByLabel("sibiu_rimnicu_vilcea");
var edge_rimnicu_vilcea_pitesti = graph.GetEdgeByLabel("rimnicu_vilcea_pitesti");
var edge_pitesti_bucharest = graph.GetEdgeByLabel("pitesti_bucharest");
var edges = new Edge[] { edge_arad_sibiu, edge_sibiu_rimnicu_vilcea, edge_rimnicu_vilcea_pitesti, edge_pitesti_bucharest };
var pathRoute = new PathRoute(EPathStatus.Found, arad, sibiu, edges, 1);
// pathRoute.Nodes.ToList().ForEach(Console.WriteLine);
Assert.Equal(EPathStatus.Found, pathRoute.Status);
Assert.Equal(new Node[] { arad, sibiu, rimnicuVilcea, pitesti, bucharest }, pathRoute.Nodes);
Assert.Equal(4, pathRoute.Jumps);
}
public static PathRoute FindRoute(PathPosition startPos, PathPosition endPos)
{
if (startPos == null || endPos == null || startPos._path == null || endPos._path == null)
{
return(null);
}
PathRoute bestRoute = null;
List <PathNode> linkedNodes = new List <PathNode>();
PathNode nodeA = GetNextNode(startPos, Direction1D.Forwards);
if (nodeA != null && nodeA.isActiveAndEnabled)
{
linkedNodes.Add(nodeA);
}
PathNode nodeB = GetNextNode(startPos, Direction1D.Backwards);
if (nodeB != null && nodeA != nodeB && nodeB.isActiveAndEnabled)
{
linkedNodes.Add(nodeB);
}
List <PathNode> traversedNodes = new List <PathNode>();
FindRoute(startPos, endPos, startPos._path, startPos._pathT, linkedNodes, 0.0f, new List <PathRouteWaypoint>(), ref bestRoute, ref traversedNodes);
return(bestRoute);
}
private void WipeData()
{
Log = new List <Tuple <string, Color> >();
MapMarkers = new Dictionary <string, GMapMarker>();
MarkersQueue = new Queue <NewMapObject>();
LogQueue = new Queue <Tuple <string, Color> >();
PathRoute.Points.Clear();
PathRoute.RegenerateShape(null);
}
public override PathRoute Search(Node source, Node target, double radius)
{
PathRoute pathRoute = this.SearchParametersEvaluation(source, target, radius);
if (pathRoute != null)
{
return(pathRoute);
}
var superSource = Collapse.collapse(this.Graph, source, radius, -1, 0);
var superTarget = Collapse.collapse(this.Graph, target, radius, -2, 0);
try
{
pathRoute = Graph.ShortestPathHeuristic(superSource, superTarget);
if (pathRoute.Status == EPathStatus.Found)
{
var fakeSource = Node.ShortestPathBetweenNeihbors(pathRoute.Nodes[0], pathRoute.Nodes[1]).Edges[0];
var fakeTarget = Node.ShortestPathBetweenNeihbors(pathRoute.Nodes[pathRoute.Nodes.Count() - 2], pathRoute.Nodes[pathRoute.Nodes.Count() - 1]).Edges[0];
var originalSource = (Edge)fakeSource.GetAttribute("original_edge");
var originalTarget = (Edge)fakeTarget.GetAttribute("original_edge");
//TODO: update the pathroute distance too?
pathRoute.Nodes[0] = originalSource.Source;
pathRoute.Nodes[pathRoute.Nodes.Count() - 1] = originalTarget.Target;
pathRoute.Source = source;
pathRoute.Edges[0] = originalSource;
pathRoute.Edges[pathRoute.Edges.Count() - 1] = originalTarget;
pathRoute.Target = target;
}
else
{
pathRoute.Source = source;
pathRoute.Target = target;
}
}
catch (Exception e)
{
pathRoute = new PathRoute(EPathStatus.UnexpectedException, source, target, e);
}
finally
{
Collapse.Expand(this.Graph, -1);
Collapse.Expand(this.Graph, -2);
}
return(pathRoute);
}
public void ForTheGivenPathReturnTheCorrectNodesForGraph0()
{
var g = G0();
Edge[] edges = new Edge[3];
edges[0] = g.GetNodeById(1).EdgesWhenSourceOf(g.GetNodeById(2))[0];
edges[1] = g.GetNodeById(2).EdgesWhenSourceOf(g.GetNodeById(3))[0];
edges[2] = g.GetNodeById(3).EdgesWhenSourceOf(g.GetNodeById(4))[0];
Node[] nodes = new Node[] { g.GetNodeById(1), g.GetNodeById(2), g.GetNodeById(3), g.GetNodeById(4) };
PathRoute pr = new PathRoute(EPathStatus.Found, g.GetNodeById(1), g.GetNodeById(3), edges, 4);
Assert.Equal(nodes, pr.Nodes);
}
public void CorrectlyBuildPathRouteFromEdges_1To3()
{
Graph g = getGraph(3);
var p_1 = g.GetNodeById(1);
var p_3 = g.GetNodeById(3);
var e_1_3 = g.GetEdgeByLabel("1_3");
var edges = new Edge[] { e_1_3 };
var pathRoute = new PathRoute(EPathStatus.Found, p_1, p_3, edges, 1);
Assert.Equal(new Node[] { p_1, p_3 }, pathRoute.Nodes);
Assert.Equal(1, pathRoute.Jumps);
Assert.Equal(EPathStatus.Found, pathRoute.Status);
}
public override PathRoute Search(Node source, Node target, double radius)
{
PathRoute betterPathRoute = this.SearchParametersEvaluation(source, target, radius);
if (betterPathRoute != null)
{
betterPathRoute.Source = source;
betterPathRoute.Target = target;
return(betterPathRoute);
}
var sources = this.Graph.GetNodesByRadius(source, radius);
var targets = this.Graph.GetNodesByRadius(target, radius);
foreach (var origin in sources)
{
foreach (var destination in targets)
{
if (origin != destination)
{
PathRoute pathRoute = Graph.ShortestPathHeuristic(origin, destination);
if (betterPathRoute == null || betterPathRoute.Status != EPathStatus.Found ||
(betterPathRoute.Status == EPathStatus.Found &&
pathRoute.Status == EPathStatus.Found &&
betterPathRoute.Distance > pathRoute.Distance))
{
betterPathRoute = pathRoute;
}
}
}
}
if (betterPathRoute != null)// && betterPathRoute.Status != EPathStatus.Found)
{
betterPathRoute.Source = source;
betterPathRoute.Target = target;
}
return(betterPathRoute);
}
private void WipeData()
{
try
{
Log = new List <Tuple <string, Color> >();
MapMarkers = new Dictionary <string, GMapMarker>();
MarkersQueue = new Queue <NewMapObject>();
LogQueue = new Queue <Tuple <string, Color> >();
PathRoute.Points.Clear();
PathRoute.RegenerateShape(null);
}
catch (Exception ex)
{
Session.EventDispatcher.Send(new WarnEvent
{
Message = "Error during wiping bot data!"
});
Logger.Write($"[WIPE FAIL] Error: {ex.Message}", LogLevel.Error);
}
}
public void CorrectlyBuildPathRouteFromEdges_1To3_3To2_2To4()
{
Graph g = getGraph(3);
var p_1 = g.GetNodeById(1);
var p_3 = g.GetNodeById(3);
var p_2 = g.GetNodeById(2);
var p_4 = g.GetNodeById(4);
var e_1_3 = g.GetEdgeByLabel("1_3");
var e_3_2 = g.GetEdgeByLabel("3_2");
var e_2_4 = g.GetEdgeByLabel("2_4");
var edges = new Edge[] { e_1_3, e_3_2, e_2_4 };
var pathRoute = new PathRoute(EPathStatus.Found, p_1, p_4, edges, 3);
Assert.Equal(new Node[] { p_1, p_3, p_2, p_4 }, pathRoute.Nodes);
Assert.Equal(3, pathRoute.Jumps);
Assert.Equal(EPathStatus.Found, pathRoute.Status);
}
public static PathRoute Search(Map.MapNode[,] MapNode, Vector3 Position, Vector3 objPosition, PathRoute Path, List <Map.MapNode> openList, List <Map.MapNode> closedList)
{
Map.MapNode start = Map.ConvertPositionToGrid(MapNode, Position);
Map.MapNode obj = Map.ConvertPositionToGrid(MapNode, objPosition);
return(aStarSearch(start, obj, openList, closedList, MapNode, Path));
}
private static PathRoute aStarSearch(Map.MapNode start, Map.MapNode obj, List <Map.MapNode> openList, List <Map.MapNode> closedList, Map.MapNode[,] grid, PathRoute path)
{
Reset(grid, openList, closedList);
Map.MapNode currentNode = start; // will be changed to whichever has the lowerest cost
int x = currentNode.posX;
int y = currentNode.posY;
currentNode.fcost = Map.HCost(obj, currentNode); // calc f cost
currentNode.cameFrom[0] = currentNode;
openList.Add(currentNode);
while (openList.Count != 0)
{
if (currentNode.id == obj.id)
{
closedList.Add(currentNode);
path = generatePath(closedList, grid, start, path);
return(path);
}
else
{
for (int i = 0; i < currentNode.neighbours.Count; i++) //loop all neighbours
{
x = currentNode.neighbours[i].posX;
y = currentNode.neighbours[i].posY;
var temp = currentNode.neighbours[i];
temp.gcost = Map.GCost(currentNode, currentNode.neighbours[i]);
temp.hcost = Map.HCost(obj, currentNode.neighbours[i]);
temp.cameFrom[0] = currentNode;
temp.fcost = temp.gcost + temp.hcost;
currentNode.neighbours[i] = temp;
for (int j = 0; j < openList.Count; j++)
{
if (openList[j].id == currentNode.neighbours[i].id) //if it's already on the open list then
{
grid[x, y].state = Map.status.onOpenList;
if (temp.fcost >= openList[j].fcost) //we already have a cheaper node
{
break;
}
if (temp.fcost < openList[j].fcost) //if this happens our heuristic is broken
{
openList[j].parent[0] = currentNode;
}
}
}
if (grid[x, y].state != Map.status.onOpenList) //if not on the openList we check if it's already on the closed list if it isn't add it to the openlist
{
for (int k = 0; k < closedList.Count; k++)
{
if (closedList[k].id == currentNode.neighbours[i].id) //if already on the closed list, check if neighbour is cheaper if it is set parent
{
grid[x, y].state = Map.status.searched;
if (temp.fcost >= closedList[k].fcost)
{
break;
}
if (temp.fcost < closedList[k].fcost)
{
closedList[k].parent[0] = currentNode;
}
}
}
if (grid[x, y].state != Map.status.searched)
{
currentNode.neighbours[i].parent[0] = currentNode;
grid[x, y].state = Map.status.onOpenList;
openList.Add(currentNode.neighbours[i]);
}
}
}
x = currentNode.posX;
y = currentNode.posY;
grid[x, y].state = Map.status.searched;
openList.Remove(currentNode);
closedList.Add(currentNode);
openList.Sort(delegate(Map.MapNode a, Map.MapNode b) { return(a.fcost.CompareTo(b.fcost)); }); //sorting function so first one on openList is cheapest
currentNode = openList[0];
}
}
return(path);
}
public static PathRoute generatePath(List <Map.MapNode> closedList, Map.MapNode[,] grid, Map.MapNode Start, PathRoute Path)
{
int index = closedList.Count - 1; //need to work backwards to try and get to the start node
int PathIndex = 0;
Map.MapNode a = closedList[index];
Map.MapNode temp = a;
Path.PathList[0] = Map.ConvertGridNodeToVector(a);
++PathIndex;
while (temp.id != Start.id)
{
Path.PathList[PathIndex] = (Map.ConvertGridNodeToVector(a.parent[0]));
temp = a.parent[0];
a = temp;
++PathIndex;
}
Path.PathSize = PathIndex;
Path.Index = 0;
Array.Reverse(Path.PathList, 0, PathIndex);
return(Path);
}
private static void FindRoute(PathPosition startPos, PathPosition endPos, Path currentPath, float currentPosT, List <PathNode> nextNodes, float currentDistance, List <PathRouteWaypoint> currentRoute, ref PathRoute bestRoute, ref List <PathNode> traversedNodes)
{
//Check if the end position is on the current path.
bool endPosOnCurrentPath = false;
//If end position is a path node
if (endPos._pathNode != null)
{
foreach (Path path in endPos._pathNode.GetPaths())
{
if (path == currentPath)
{
endPos._path = currentPath;
endPos._pathT = currentPath.GetPathT(endPos._pathNode);
endPosOnCurrentPath = true;
break;
}
}
}
else if (currentPath == endPos._path)
{
endPosOnCurrentPath = true;
}
//If current path is the same as the end pos path, check direct route to end pos
if (endPosOnCurrentPath)
{
float routeDistance = currentDistance + currentPath.GetDistanceBetween(currentPosT, endPos._pathT);
if (bestRoute == null || routeDistance < bestRoute._distance)
{
bestRoute = new PathRoute();
bestRoute._startPosition = startPos;
bestRoute._endPosition = endPos;
bestRoute._waypoints = currentRoute.ToArray();
bestRoute._distance = routeDistance;
}
}
//loop through all possible routes from this node
foreach (PathNode node in nextNodes)
{
//Need to check if already searched this node (to stop loops)
if (node != null && !traversedNodes.Contains(node))
{
//First work out the rating to this path node. only consider a route to this node if the rating is still better than the best route rating
float nodeT = currentPath.GetPathT(node);
float routeDistance = currentDistance + currentPath.GetDistanceBetween(currentPosT, nodeT);
if (bestRoute == null || routeDistance < bestRoute._distance)
{
traversedNodes.Add(node);
foreach (Path path in node.GetPaths())
{
if (path.IsActive())
{
List <PathRouteWaypoint> route = new List <PathRouteWaypoint>(currentRoute);
PathRouteWaypoint waypoint = new PathRouteWaypoint();
waypoint._path = path;
waypoint._pathNode = node;
route.Add(waypoint);
FindRoute(startPos, endPos, path, path.GetPathT(node), GetLinkedPathNodes(node, path), routeDistance, route, ref bestRoute, ref traversedNodes);
}
}
}
}
}
}
