Merge pull request #835 from compas-dev/astar_network

Add astar_lightest_path and refactor astart_shortest_path

Author: beverlylytle

CHANGELOG.md

@@ -10,8 +10,12 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+* Added `compas.topology.astar_lightest_path`.
+
 ### Changed
 
+* Extended `compas.topology.astar_shortest_path` to work on `compas.datastructures.Mesh` and `compas.datastructures.Network`.
+
 ### Removed
 
 

src/compas/topology/__init__.py

@@ -49,6 +49,7 @@
     :toctree: generated/
     :nosignatures:
 
+    astar_lightest_path
     astar_shortest_path
     breadth_first_ordering
     breadth_first_traverse
@@ -69,6 +70,7 @@
     breadth_first_traverse,
     breadth_first_paths,
     shortest_path,
+    astar_lightest_path,
     astar_shortest_path,
     dijkstra_distances,
     dijkstra_path
@@ -101,6 +103,7 @@
     'breadth_first_traverse',
     'breadth_first_paths',
     'shortest_path',
+    'astar_lightest_path',
     'astar_shortest_path',
     'dijkstra_distances',
     'dijkstra_path',

src/compas/topology/traversal.py

@@ -18,6 +18,7 @@
     'breadth_first_traverse',
     'breadth_first_paths',
     'shortest_path',
+    'astar_lightest_path',
     'astar_shortest_path',
     'dijkstra_distances',
     'dijkstra_path'
@@ -323,6 +324,8 @@ def shortest_path(adjacency, root, goal):
 
 
 def reconstruct_path(came_from, current):
+    if current not in came_from:
+        return None
     total_path = [current]
     while current in came_from:
         current = came_from[current]
@@ -331,97 +334,124 @@ def reconstruct_path(came_from, current):
     return total_path
 
 
-def astar_shortest_path(network, root, goal):
-    """Find the shortest path between two vertices of a network using the A* search algorithm.
+def astar_lightest_path(adjacency, weights, heuristic, root, goal):
+    """Find the path of least weight between two vertices of a graph using the A* search algorithm.
 
     Parameters
     ----------
-    network : instance of the Network class
+    adjacency : dict
+        An adjacency dictionary. Each key represents a vertex
+        and maps to a list of neighboring vertex keys.
+    weights : dict
+        A dictionary of edge weights.
+    heuristic : dict
+        A dictionary of guesses of weights of paths from a node to the goal.
     root : hashable
-        The identifier of the starting node.
+        The start vertex.
     goal : hashable
-        The identifier of the ending node.
+        The end vertex.
 
     Returns
     -------
     list, None
         The path from root to goal, or None, if no path exists between the vertices.
 
-    Examples
-    --------
-    >>>
-
     References
     ----------
     https://en.wikipedia.org/wiki/A*_search_algorithm
     """
-    root_coords = network.vertex_coordinates(root)
-    goal_coords = network.vertex_coordinates(goal)
-
-    # The set of nodes already evaluated
     visited_set = set()
 
-    # The set of currently discovered nodes that are not evaluated yet.
-    # Initially, only the start node is known.
     candidates_set = {root}
     best_candidate_heap = PriorityQueue()
-    best_candidate_heap.put((0, root))
+    best_candidate_heap.put((heuristic[root], root))
 
-    # For each node, which node it can most efficiently be reached from.
-    # If a node can be reached from many nodes, came_from will eventually contain the
-    # most efficient previous step.
     came_from = dict()
 
-    # g_score is a dict mapping node index to the cost of getting from the root node to that node.
-    # The default value is Infinity.
-    # The cost of going from start to start is zero.
     g_score = dict()
-
-    for v in network.vertices():
-        g_score[v] = float("inf")
-
+    for v in adjacency:
+        g_score[v] = float('inf')
     g_score[root] = 0
 
-    # For each node, the total cost of getting from the start node to the goal
-    # by passing by that node. That value is partly known, partly heuristic.
-    # The default value of f_score is Infinity
-    f_score = dict()
-
-    for v in network.vertices():
-        f_score[v] = float("inf")
-
-    # For the first node, that value is completely heuristic.
-    f_score[root] = distance_point_point(root_coords, goal_coords)
-
     while not best_candidate_heap.empty():
         _, current = best_candidate_heap.get()
         if current == goal:
             break
 
         visited_set.add(current)
-        current_coords = network.vertex_coordinates(current)
-        for neighbor in network.vertex_neighbors(current):
+        for neighbor in adjacency[current]:
             if neighbor in visited_set:
-                continue  # Ignore the neighbor which is already evaluated.
+                continue
 
-            # The distance from start to a neighbor
-            neighbor_coords = network.vertex_coordinates(neighbor)
-            tentative_gScore = g_score[current] + distance_point_point(current_coords, neighbor_coords)
-            if neighbor not in candidates_set:  # Discover a new node
+            tentative_g_score = g_score[current] + weights[(current, neighbor)]
+            if neighbor not in candidates_set:
                 candidates_set.add(neighbor)
-            elif tentative_gScore >= g_score[neighbor]:
+            elif tentative_g_score >= g_score[neighbor]:
                 continue
 
-            # This path is the best until now. Record it!
             came_from[neighbor] = current
-            g_score[neighbor] = tentative_gScore
-            new_fscore = g_score[neighbor] + distance_point_point(neighbor_coords, goal_coords)
-            f_score[neighbor] = new_fscore
-            best_candidate_heap.put((new_fscore, neighbor))
+            g_score[neighbor] = tentative_g_score
+            new_f_score = g_score[neighbor] + heuristic[neighbor]
+            best_candidate_heap.put((new_f_score, neighbor))
 
     return reconstruct_path(came_from, goal)
 
 
+def _get_coordinates(key, structure):
+    if hasattr(structure, 'node_attributes'):
+        return structure.node_attributes(key, 'xyz')
+    if hasattr(structure, 'vertex_coordinates'):
+        return structure.vertex_coordinates(key)
+    raise Exception("Coordinates cannot be found for object of type {}".format(type(structure)))
+
+
+def _get_points(structure):
+    if hasattr(structure, 'nodes'):
+        return structure.nodes()
+    if hasattr(structure, 'vertices'):
+        return structure.vertices()
+    raise Exception("Points cannot be found for object of type {}".format(type(structure)))
+
+
+def astar_shortest_path(network, root, goal):
+    """Find the shortest path between two vertices of a network or mesh using the A* search algorithm.
+
+    Parameters
+    ----------
+    network : instance of the Network or Mesh class
+    root : hashable
+        The identifier of the starting node.
+    goal : hashable
+        The identifier of the ending node.
+
+    Returns
+    -------
+    list, None
+        The path from root to goal, or None, if no path exists between the vertices.
+
+    References
+    ----------
+    https://en.wikipedia.org/wiki/A*_search_algorithm
+    """
+    adjacency = network.adjacency
+    weights = {}
+    for u, v in network.edges():
+        u_coords = _get_coordinates(u, network)
+        v_coords = _get_coordinates(v, network)
+        distance = distance_point_point(u_coords, v_coords)
+        weights[(u, v)] = distance
+        weights[(v, u)] = distance
+
+    heuristic = {}
+    goal_coords = _get_coordinates(goal, network)
+    points = _get_points(network)
+    for u in points:
+        u_coords = _get_coordinates(u, network)
+        heuristic[u] = distance_point_point(u_coords, goal_coords)
+
+    return astar_lightest_path(adjacency, weights, heuristic, root, goal)
+
+
 def dijkstra_distances(adjacency, weight, target):
     """Compute Dijkstra distances from all vertices in a connected set to one target vertex.
 

tests/compas/topology/test_traversal.py

@@ -0,0 +1,67 @@
+from compas.datastructures import Graph
+from compas.datastructures import Mesh
+from compas.datastructures import Network
+from compas.geometry import Box, Frame
+from compas.topology import astar_shortest_path
+from compas.topology.traversal import astar_lightest_path
+
+
+def test_astar_shortest_path():
+    n = Network()
+    a = n.add_node(x=1, y=2, z=0)
+    b = n.add_node(x=3, y=1, z=0)
+    n.add_edge(a, b)
+    path = astar_shortest_path(n, a, b)
+    assert path == [a, b]
+
+
+def test_astar_shortest_path_cycle():
+    n = Network()
+    a = n.add_node(x=1, y=0, z=0)
+    b = n.add_node(x=2, y=0, z=0)
+    c = n.add_node(x=3, y=0, z=0)
+    d = n.add_node(x=4, y=0, z=0)
+    e = n.add_node(x=3.5, y=5, z=0)
+    n.add_edge(a, b)
+    n.add_edge(a, e)
+    n.add_edge(b, c)
+    n.add_edge(c, d)
+    n.add_edge(e, d)
+    path = astar_shortest_path(n, a, d)
+    assert path == [a, b, c, d]
+
+
+def test_astar_shortest_path_disconnected():
+    n = Network()
+    a = n.add_node(x=1, y=0, z=0)
+    b = n.add_node(x=2, y=0, z=0)
+    c = n.add_node(x=3, y=0, z=0)
+    n.add_edge(a, b)
+    path = astar_shortest_path(n, a, c)
+    assert path is None
+
+
+def test_astar_shortest_path_mesh():
+    mesh = Mesh.from_shape(Box(Frame.worldXY(), 1, 1, 1))
+    a, b = mesh.get_any_vertices(2)
+    path = astar_shortest_path(mesh, a, b)
+    assert path is not None
+
+
+def test_astar_lightest_path():
+    g = Graph()
+    for i in range(4):
+        g.add_node(i)
+    g.add_edge(0, 1)
+    g.add_edge(0, 2)
+    g.add_edge(1, 3)
+    g.add_edge(2, 3)
+    weights = {
+        (0, 1): 1,
+        (0, 2): 1,
+        (1, 3): 2,
+        (2, 3): 1,
+    }
+    heuristic = {i: 1 for i in range(4)}
+    path = astar_lightest_path(g.adjacency, weights, heuristic, 0, 3)
+    assert path == [0, 2, 3]