scgraph.grid

  1from typing import Literal
  2from scgraph import Graph
  3from math import ceil
  4
  5
  6class GridGraph:
  7    def __init__(
  8        self,
  9        x_size: int,
 10        y_size: int,
 11        blocks: list[tuple[int, int]],
 12        shape: list[tuple[int | float, int | float]] | None = None,
 13        add_exterior_walls: bool = True,
 14        conn_data: list[tuple[int, int, float]] | None = None,
 15    ) -> None:
 16        """
 17        Initializes a GridGraph object.
 18
 19        Required Arguments:
 20
 21        - `x_size`
 22            - Type: int
 23            - What: The size of the grid in the x direction
 24        - `y_size`
 25            - Type: int
 26            - What: The size of the grid in the y direction
 27        - `blocks`
 28            - Type: list of tuples
 29            - What: A list of tuples representing the coordinates of the blocked cells
 30
 31        Optional Arguments:
 32
 33        - `shape`
 34            - Type: list of tuples
 35            - What: A list of tuples representing the shape of the moving object relative to the object location in the grid
 36            - Default: [(0, 0), (0, 1), (1, 0), (1, 1)]
 37            - Example: [(0, 0), (0, 1), (1, 0), (1, 1)]
 38                - Note: This would form a square of size 1x1
 39                - Assuming the grid location is at (0,0) the square takes up the grid space between (0, 0) and (1, 1)
 40                - Assumint the grid location is at (1,1) the square takes up the grid space between (1, 1) and (2, 2) with the same shape
 41            - Note: This shape is used to determine which connections are allowed when passing through the grid.
 42                - For example
 43                    - The shape is a square of size 1x1
 44                    - The location of the square is (0, 0)
 45                    - There is a blocked cell at (0, 1)
 46                    - There is a potential connection to (1, 1), (0, 1), and (1, 0)
 47                    - The square can not move to (1, 1) as it would collide with the blocked cell at (0, 1)
 48                      - This is because the square would pass partly through the blocked cell on its way to (1, 1)
 49                      - To achieve the same result, it can move to (1, 0) and then up to (1, 1) taking extra time / distance
 50        - `add_exterior_walls`
 51            - Type: bool
 52            - What: Whether to add exterior walls to the grid
 53            - Default: True
 54        - `conn_data`
 55            - Type: list of tuples
 56            - What: A list of tuples representing movements allowed in the grid
 57                - Each tuple should contain three values: (x_offset, y_offset, distance)
 58                - x_offset: The x offset from the current cell
 59                - y_offset: The y offset from the current cell
 60                - distance: The distance to the connected cell
 61            - Default: [(0, 1, 1), (1, 0, 1), (0, -1, 1), (-1, 0, 1), (1, 1, sqrt(2)), (-1, -1, sqrt(2)), (1, -1, sqrt(2)), (-1, 1, sqrt(2))]
 62                - Note: If None, the default connection data will be used
 63                - Note: This includes 8 directions, 4 cardinal and 4 diagonal
 64                - Cardinal directions are 1 unit away, diagonal directions are sqrt(2) units away
 65        """
 66        # Validate the inputs
 67        assert x_size > 0, "x_size must be greater than 0"
 68        assert y_size > 0, "y_size must be greater than 0"
 69
 70        self.x_size = x_size
 71        self.y_size = y_size
 72        self.blocks = blocks
 73        self.add_exterior_walls = add_exterior_walls
 74
 75        # Update the blocks if add_exterior_walls is True
 76        # This is done by adding all the cells in the first and last row and column
 77        if add_exterior_walls:
 78            self.blocks += [
 79                (x, y) for x in range(x_size) for y in [0, y_size - 1]
 80            ] + [(x, y) for x in [0, x_size - 1] for y in range(y_size)]
 81
 82        # If shape is None, set the default shape
 83        if shape is None:
 84            self.shape = [(0, 0), (0, 1), (1, 0), (1, 1)]
 85        else:
 86            self.shape = shape
 87
 88        # If conn_data is None, set the default connection data
 89        if conn_data is None:
 90            sqrt_2 = 1.4142
 91            self.conn_data = [
 92                (-1, -1, sqrt_2),
 93                (-1, 0, 1),
 94                (-1, 1, sqrt_2),
 95                (0, -1, 1),
 96                (0, 1, 1),
 97                (1, -1, sqrt_2),
 98                (1, 0, 1),
 99                (1, 1, sqrt_2),
100            ]
101        else:
102            self.conn_data = conn_data
103
104        self.graph = self.__create_graph__()
105        self.nodes = [self.__get_x_y__(idx) for idx in range(len(self.graph))]
106        self.graph_object = Graph(self.graph)
107
108    def __get_idx__(self, x: int, y: int):
109        """
110        Function:
111        - Get the index of a cell in a 2D grid given its x and y coordinates
112        - This does not have any error checking, so be careful with the inputs
113
114        Required Arguments:
115        - `x`
116            - Type: int
117            - What: The x coordinate of the cell
118        - `y`
119            - Type: int
120            - What: The y coordinate of the cell
121
122        Returns:
123
124        - `idx`
125            - Type: int
126            - What: The index of the cell in the grid
127        """
128        return x + y * self.x_size
129
130    def __get_x_y__(self, idx: int):
131        """
132        Function:
133        - Get the x and y coordinates of a cell in a 2D grid given its index
134        - This does not have any error checking so be careful with the inputs
135
136        Required Arguments:
137
138        - `idx`
139            - Type: int
140            - What: The index of the cell
141
142        Returns:
143        - `x`
144            - Type: int
145            - What: The x coordinate of the cell
146        - `y`
147            - Type: int
148            - What: The y coordinate of the cell
149        """
150        return idx % self.x_size, idx // self.x_size
151
152    def get_idx(self, x: int, y: int):
153        """
154        Function:
155        - Get the index of a cell in a 2D grid given its x and y coordinates
156
157        Required Arguments:
158        - `x`
159            - Type: int
160            - What: The x coordinate of the cell
161        - `y`
162            - Type: int
163            - What: The y coordinate of the cell
164
165        Returns:
166
167        - `idx`
168            - Type: int
169            - What: The index of the cell in the grid
170        """
171        assert x >= 0 and x < self.x_size, "x coordinate is out of bounds"
172        assert y >= 0 and y < self.y_size, "y coordinate is out of bounds"
173        return x + y * self.x_size
174
175    def get_x_y(self, idx: int):
176        """
177        Function:
178        - Get the x and y coordinates of a cell in a 2D grid given its index
179
180        Required Arguments:
181
182        - `idx`
183            - Type: int
184            - What: The index of the cell
185
186        Returns:
187        - `x`
188            - Type: int
189            - What: The x coordinate of the cell
190        - `y`
191            - Type: int
192            - What: The y coordinate of the cell
193        """
194        assert idx >= 0 and idx < len(self.graph), "idx is out of bounds"
195        return idx % self.x_size, idx // self.x_size
196
197    def __get_relative_shape_offsets__(
198        self,
199        shape: list[tuple[int | float, int | float]],
200        x_off: int,
201        y_off: int,
202    ):
203        """
204        Returns the list of (x, y) integer grid cells that the shape overlaps
205        as it moves from (0, 0) to (x_off, y_off).
206        """
207        # Get bounding box of the shape at start and end positions
208        xs = [coord[0] for coord in shape]
209        ys = [coord[1] for coord in shape]
210
211        # Calculate the 1d overlaps in x and y directions
212        x_cells = list(
213            range(
214                int(min(xs) + min(0, x_off)), int(ceil(max(xs) + max(0, x_off)))
215            )
216        )
217        y_cells = list(
218            range(
219                int(min(ys) + min(0, y_off)), int(ceil(max(ys) + max(0, y_off)))
220            )
221        )
222
223        # Combine to get 2D cell overlaps
224        result = set()
225        for x_key in x_cells:
226            for y_key in y_cells:
227                result.add((x_key, y_key))
228
229        # Remove untouched cells if moving diagonally
230        if x_off != 0 and y_off != 0:
231            slope = y_off / x_off
232
233            # Find min and max vertex given the slope
234            orthogonal = -1 / slope  # Compute orthogonal slope
235            # Define direction projections (a normalized direction vector, but without the linear algebra)
236            # Note: Technically normalized length is (1**2 + orthogonal**2)**.5, but we avoid the extra square for performance
237            length = (1 + orthogonal**2) ** 0.5
238            projections = [
239                x * 1.0 / length + y * orthogonal / length for x, y in shape
240            ]
241            # Return the min and max verticies
242            min_vertex = shape[
243                min(enumerate(projections), key=lambda x: x[1])[0]
244            ]
245            max_vertex = shape[
246                max(enumerate(projections), key=lambda x: x[1])[0]
247            ]
248            if slope > 0:
249                min_vertex, max_vertex = max_vertex, min_vertex
250
251            shape_min_intercept = min_vertex[1] - slope * min_vertex[0]
252            shape_max_intercept = max_vertex[1] - slope * max_vertex[0]
253
254            ltx_increment, gtx_increment = (1, 0) if slope < 0 else (0, 1)
255
256            remove_cells = []
257            for x_cell, y_cell in result:
258                cell_min_intercept = y_cell - (slope * (x_cell + gtx_increment))
259                cell_max_intercept = (y_cell + 1) - (
260                    slope * (x_cell + ltx_increment)
261                )
262
263                if not (
264                    cell_min_intercept < shape_max_intercept
265                    and shape_min_intercept < cell_max_intercept
266                ):
267                    remove_cells.append((x_cell, y_cell))
268            for key in remove_cells:
269                result.remove(key)
270        # Return the list of offsets
271        return list(result)
272
273    def __create_graph__(
274        self,
275    ):
276        """
277        Function:
278
279        - Create a graph from the grid specifications
280
281        Returns:
282
283        - `graph`
284            - Type: list
285            - What: The adjacency list representation of the graph
286        """
287        ####################
288        # Create a list of lists to hold all the possible connections in an easy to access/understand format
289        ####################
290
291        # Each cell in the list will be a dictionary of connections
292        # The keys will be the coordinates of the cell that can be moved to
293        # The values will be the distance to that cell
294        # This essentially creates a sparse matrix
295        graph_matrix = [
296            [{} for _ in range(self.x_size)] for _ in range(self.y_size)
297        ]
298        for y in range(self.y_size):
299            for x in range(self.x_size):
300                for x_off, y_off, dist in self.conn_data:
301                    if (
302                        0 <= x + x_off < self.x_size
303                        and 0 <= y + y_off < self.y_size
304                    ):
305                        graph_matrix[y][x][(x + x_off, y + y_off)] = dist
306
307        ####################
308        # Remove all connections that would not be possible based on the blocks and the shape
309        ####################
310
311        # Create a set of offsets that will be used to help remove connections relative to each blocked cell
312        delta_offsets = {
313            (x_delta, y_delta): self.__get_relative_shape_offsets__(
314                self.shape, x_delta, y_delta
315            )
316            for x_delta, y_delta, _ in self.conn_data
317        }
318
319        # Loop over all the blocks and remove all connections that would be not possible based on the blocks
320        for block in self.blocks:
321            # Clear out all connections leaving from the blocked cell
322            graph_matrix[block[1]][block[0]] = {}
323            # Clear out all connections that would be blocked by each movement given the blocked cell and the delta offsets
324            # Our delta offsets dictionary essentially tells us which relative cells would be blocked by the moving shape
325            for delta, offsets in delta_offsets.items():
326                for offset in offsets:
327                    x_cell = block[0] - offset[0]
328                    y_cell = block[1] - offset[1]
329                    if 0 <= x_cell < self.x_size and 0 <= y_cell < self.y_size:
330                        x_move_to = x_cell + delta[0]
331                        y_move_to = y_cell + delta[1]
332                        graph_matrix[y_cell][x_cell].pop(
333                            (x_move_to, y_move_to), None
334                        )
335
336        ####################
337        # Convert the graph matrix into the very efficient graph format as specified by the scgraph library
338        ####################
339        graph = []
340        for x_data in graph_matrix:
341            for connection_dict in x_data:
342                graph.append(
343                    {
344                        self.__get_idx__(x, y): dist
345                        for (x, y), dist in connection_dict.items()
346                    }
347                )
348
349        return graph
350
351    def euclidean_heuristic(self, origin_id: int, destination_id: int) -> float:
352        """
353        Function:
354
355        - Calculate the Euclidean distance between two nodes in the grid graph
356
357        Required Arguments:
358
359        - `origin_id`
360            - Type: int
361            - What: The id of the origin node
362        - `destination_id`
363            - Type: int
364            - What: The id of the destination node
365
366        Returns:
367
368        - `distance`
369            - Type: float
370            - What: The Euclidean distance between the two nodes
371        """
372        origin_location = self.nodes[origin_id]
373        destination_location = self.nodes[destination_id]
374        return (
375            (origin_location[0] - destination_location[0]) ** 2
376            + (origin_location[1] - destination_location[1]) ** 2
377        ) ** 0.5
378
379    def __get_closest_node_with_connections__(
380        self,
381        x: int | float,
382        y: int | float,
383    ):
384        """
385        Function:
386
387        - Get the closest node in the graph that has connections
388
389        Required Arguments:
390
391        - `x`
392            - Type: int | float
393            - What: The x coordinate of the node
394        - `y`
395            - Type: int | float
396            - What: The y coordinate of the node
397
398        Returns:
399
400        - `closest_node_id`
401            - Type: int
402            - What: The id of the closest node with connections
403        - `closest_distance`
404            - Type: float
405            - What: The distance to the closest node with connections
406        """
407        x = int(x) if int(x) == x else x
408        y = int(y) if int(y) == y else y
409        if isinstance(x, int) and isinstance(y, int):
410            return self.get_idx(x, y), 0.0
411        closest_node_id = None
412        closest_distance = float("inf")
413        for x_off, y_off in [(0, 0), (1, 0), (0, 1), (1, 1)]:
414            node_x = int(x) + x_off
415            node_y = int(y) + y_off
416            try:
417                node_id = self.get_idx(node_x, node_y)
418            except AssertionError:
419                continue
420            if self.graph[node_id] == {}:
421                continue
422            distance = ((node_x - x) ** 2 + (node_y - y) ** 2) ** 0.5
423            if distance < closest_distance:
424                closest_distance = distance
425                closest_node_id = node_id
426        if closest_node_id is None:
427            raise ValueError(
428                "No valid adjacent node with connections found for the given coordinates"
429            )
430        return closest_node_id, closest_distance
431
432    def format_coordinate(self, cooinate_dict, output_coorinate_path):
433        if output_coorinate_path == "list_of_tuples":
434            return (cooinate_dict["x"], cooinate_dict["y"])
435        elif output_coorinate_path == "list_of_lists":
436            return [cooinate_dict["x"], cooinate_dict["y"]]
437        elif output_coordinate_path == "list_of_dicts":
438            return {"x": cooinate_dict["x"], "y": cooinate_dict["y"]}
439        else:
440            raise ValueError("Invalid output_coordinate_path format")
441
442    def get_shortest_path(
443        self,
444        origin_node: (
445            dict[Literal["x", "y"], int | float]
446            | tuple[int | float, int | float]
447            | list[int | float]
448        ),
449        destination_node: (
450            dict[Literal["x", "y"], int | float]
451            | tuple[int | float, int | float]
452            | list[int | float]
453        ),
454        output_coordinate_path: str = "list_of_dicts",
455        output_path: bool = False,
456        algorithm_fn: str = "dijkstra",
457        algorithm_kwargs: dict | None = None,
458        **kwargs,
459    ) -> dict:
460        """
461        Function:
462
463        - Identify the shortest path between two nodes in a sparse network graph
464        - If an off graph origin and/or destination node is provided, it will find the closest adjacent node with connections
465          as the origin and/or destination node
466            - This is done to increase the likelihood that the pathfinding algorithm can find a valid path
467            - If no valid adjacent node is found, an error will be raised
468
469        - Return a dictionary of various path information including:
470            - `path`: A list of graph ids in the order they are visited
471            - `coordinate_path`: A list of dicts (x, y) in the order they are visited
472            - `length`: The length of the path
473
474         Required Arguments:
475
476        - `origin_node`
477            - Type: dict of int | float
478            - What: A dictionary with the keys 'x' and 'y'
479            - Alternatively, a tuple or list of two values (x, y) can be used
480        - `destination_node`
481            - Type: dict of int | float
482            - What: A dictionary with the keys 'x' and 'y'
483            - Alternatively, a tuple or list of two values (x, y) can be used
484
485        Optional Arguments:
486
487        - `output_coordinate_path`
488            - Type: str
489            - What: The format of the output coordinate path
490            - Default: 'list_of_lists'
491            - Options:
492                - `list_of_tuples`: A list of tuples with the first value being x and the second being y
493                - 'list_of_dicts': A list of dictionaries with keys 'x' and 'y'
494                - 'list_of_lists': A list of lists with the first value being x and the second being y
495        - `output_path`
496            - Type: bool
497            - What: Whether to output the path as a list of graph idxs (mostly for debugging purposes)
498            - Default: False
499        - `algorithm_fn`
500            - Type: str | callable
501            - What: The algorithm to use for pathfinding
502            - Default: 'dijkstra'
503            - Options:
504                - 'dijkstra': Standard Dijkstra's algorithm (default)
505                - Any method name from the Graph class (e.g. 'bellman_ford', 'a_star', 'bmssp', 'cached_shortest_path', etc.)
506                - Any user defined function that takes origin_id, destination_id, and **algorithm_kwargs
507        - `algorithm_kwargs`
508            - Type: dict
509            - What: Additional keyword arguments to pass to the algorithm function
510            - Default: {}
511        - `**kwargs`
512            - Additional keyword arguments. These are included for forwards and backwards compatibility reasons, but are not currently used.
513        """
514        if isinstance(origin_node, (tuple, list)):
515            origin_node = {"x": origin_node[0], "y": origin_node[1]}
516        if isinstance(destination_node, (tuple, list)):
517            destination_node = {
518                "x": destination_node[0],
519                "y": destination_node[1],
520            }
521        if algorithm_kwargs is None:
522            algorithm_kwargs = {}
523        if callable(algorithm_fn):
524            algorithm_kwargs["graph"] = self.graph
525        elif isinstance(algorithm_fn, str) and hasattr(
526            self.graph_object, algorithm_fn
527        ):
528            algorithm_fn = getattr(self.graph_object, algorithm_fn)
529        else:
530            raise ValueError("algorithm_fn must be a string or callable")
531
532        origin_id, origin_distance = self.__get_closest_node_with_connections__(
533            **origin_node
534        )
535        destination_id, destination_distance = (
536            self.__get_closest_node_with_connections__(**destination_node)
537        )
538
539        if self.graph[origin_id] == {}:
540            raise ValueError(
541                "Origin node is not connected to any other nodes. This is likely caused by the origin node not being possible given a blocked cell or nearby blocked cell"
542            )
543        if self.graph[destination_id] == {}:
544            raise ValueError(
545                "Destination node is not connected to any other nodes. This is likely caused by the destination node not being possible given a blocked cell or nearby blocked cell"
546            )
547        output = algorithm_fn(
548            origin_id=origin_id,
549            destination_id=destination_id,
550            **algorithm_kwargs,
551        )
552        output["coordinate_path"] = self.__get_coordinate_path__(
553            output["path"], output_coordinate_path
554        )
555        if origin_distance > 0:
556            output["coordinate_path"] = [
557                self.format_coordinate(origin_node, output_coordinate_path)
558            ] + output["coordinate_path"]
559            output["path"] = [-1] + output["path"]
560            output["length"] += origin_distance
561        if destination_distance > 0:
562            output["coordinate_path"].append(
563                self.format_coordinate(destination_node, output_coordinate_path)
564            )
565            output["path"].append(-1)
566            output["length"] += destination_distance
567        if not output_path:
568            del output["path"]
569        return output
570
571    def __get_coordinate_path__(
572        self, path: list[int], output_coordinate_path: str = "list_of_dicts"
573    ) -> list[dict[str, int]]:
574        """
575        Function:
576
577        - Return a list of node dictionaries (lat + long) in the order they are visited
578
579        Required Arguments:
580
581        - `path`
582            - Type: list
583            - What: A list of node ids in the order they are visited
584
585        Optional Arguments:
586
587        - `output_coordinate_path`
588            - Type: str
589            - What: The format of the output coordinate path
590            - Default: 'list_of_dicts'
591            - Options:
592                - 'list_of_dicts': A list of dictionaries with keys 'x' and 'y'
593                - 'list_of_lists': A list of lists with the first value being x and the second being y
594                - `list_of_tuples`: A list of tuples with the first value being x and the second being y
595
596        Returns:
597
598        - `coordinate_path`
599            - Type: list
600            - What: A list of dictionaries with keys 'x' and 'y' in the order they are visited
601        """
602        output = [self.__get_x_y__(idx) for idx in path]
603        if output_coordinate_path == "list_of_tuples":
604            return output
605        elif output_coordinate_path == "list_of_lists":
606            return [[x, y] for x, y in output]
607        elif output_coordinate_path == "list_of_dicts":
608            return [{"x": x, "y": y} for x, y in output]
609        else:
610            raise ValueError(
611                "output_coordinate_path must be 'list_of_dicts' or 'list_of_lists' or 'list_of_tuples'"
612            )
613
614    def export_object(
615        self, filename: str = "", include_blocks: bool = False
616    ) -> dict:
617        """
618        Function:
619
620        - Export the graph as a list of dictionaries
621
622        Arguments:
623
624        - `filename`
625            - Type: str
626            - What: The name of the file to export the graph to.
627            - An extension of .gridgraph will be added to the file name if not already present
628
629        Optional Arguments:
630
631        - `include_blocks`
632            - Type: bool
633            - What: Whether to include the blocks in the export
634            - Default: False
635            - Note: This is not needed as the graph is already created
636            - Note: You can include blocks in the export if you need them for some reason
637                - This will be set as the blocks attribute in the imported object
638                - This will increase the size of the export file
639        """
640        if filename == "":
641            raise ValueError("filename must be specified to export the graph")
642        filename = (
643            filename
644            if filename.endswith(".gridgraph")
645            else filename + ".gridgraph"
646        )
647        try:
648            import pickle, zlib, os
649        except ImportError:
650            raise ImportError(
651                "os, pickle and zlib are required to export the graph"
652            )
653        export_data = {
654            "graph_attributes": {
655                "graph": self.graph,
656                "nodes": self.nodes,
657                "x_size": self.x_size,
658                "y_size": self.y_size,
659                "shape": self.shape,
660                "add_exterior_walls": self.add_exterior_walls,
661                "conn_data": self.conn_data,
662            },
663            "graph_cache": self.graph_object.get_cache(),
664            "export_version": 1,
665        }
666        if include_blocks:
667            export_data["graph_attributes"]["blocks"] = self.blocks
668
669        if not os.path.exists(os.path.dirname(filename)):
670            os.makedirs(os.path.dirname(filename))
671        with open(filename, "wb") as f:
672            f.write(zlib.compress(pickle.dumps(export_data)))
673
674    @staticmethod
675    def import_object(filename: str = "") -> None:
676        """
677        Function:
678
679        - A staticmethod to import the graph from a file
680
681        Arguments:
682
683        - `filename`
684            - Type: str
685            - What: The name of the file to import the graph from.
686            - An extension of .gridgraph will be added to the file name if not already present
687
688        Returns:
689
690        - `GridGraph`
691            - Type: GridGraph
692            - What: The imported graph object
693        """
694        if filename == "":
695            raise ValueError("filename must be specified to import the graph")
696        filename = (
697            filename
698            if filename.endswith(".gridgraph")
699            else filename + ".gridgraph"
700        )
701        try:
702            import pickle, zlib
703        except ImportError:
704            raise ImportError(
705                "pickle and zlib are required to import the graph"
706            )
707        with open(filename, "rb") as f:
708            import_data = pickle.loads(zlib.decompress(f.read()))
709
710        # Check the version of the export
711        if import_data["export_version"] != 1:
712            raise ValueError(
713                f"Incompatible export version {import_data['export_version']}. The current version is 1"
714            )
715        # Create a new basic GridGraph object and overwrite the attributes with the imported data
716        # This is done as the init method calls the __create_graph__ method which is not needed here
717        GridGraph_object = GridGraph(
718            x_size=1, y_size=1, blocks=[], add_exterior_walls=False
719        )
720        for key, value in import_data["graph_attributes"].items():
721            GridGraph_object.__setattr__(key, value)
722
723        GridGraph_object.graph_object = Graph(GridGraph_object.graph)
724        GridGraph_object.graph_object.set_cache(import_data["graph_cache"])
725        return GridGraph_object