├── .coveragerc ├── .github └── workflows │ ├── python-publish-linux.yml │ ├── python-publish-macos.yml │ └── python-publish-windows.yml ├── .gitignore ├── .travis.yml ├── LICENSE ├── MANIFEST.in ├── README.md ├── examples ├── example.py └── maze_solver.py ├── mazes ├── maze_large.png └── maze_small.png ├── requirements-dev.txt ├── requirements.txt ├── setup.py ├── solns ├── maze_large_soln.png └── maze_small_soln.png ├── src ├── cpp │ ├── astar.cpp │ ├── experimental_heuristics.cpp │ └── experimental_heuristics.h └── pyastar2d │ ├── __init__.py │ └── astar_wrapper.py └── tests └── test_astar.py /.coveragerc: -------------------------------------------------------------------------------- 1 | [run] 2 | omit = tests/* 3 | 4 | [paths] 5 | source = 6 | pyastar2d 7 | **/site-packages/pyastar2d 8 | -------------------------------------------------------------------------------- /.github/workflows/python-publish-linux.yml: -------------------------------------------------------------------------------- 1 | # This workflow will upload a Python Package using Twine when a release is created 2 | # For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries 3 | 4 | # This workflow uses actions that are not certified by GitHub. 5 | # They are provided by a third-party and are governed by 6 | # separate terms of service, privacy policy, and support 7 | # documentation. 8 | 9 | name: Upload Python Package Linux 10 | 11 | on: 12 | release: 13 | types: [published] 14 | 15 | permissions: 16 | contents: read 17 | 18 | jobs: 19 | deploy: 20 | 21 | runs-on: ubuntu-latest 22 | 23 | steps: 24 | - uses: actions/checkout@v3 25 | - name: Set up Python 26 | uses: actions/setup-python@v3 27 | with: 28 | python-version: '3.7' 29 | - name: Run tests 30 | run: | 31 | pip install . 32 | pip install -r requirements-dev.txt 33 | py.test 34 | - name: Install dependencies 35 | run: | 36 | python -m pip install --upgrade pip 37 | python -m pip install twine build 38 | - name: Build source distribution 39 | run: | 40 | python -m build --sdist 41 | - name: Build manylinux Python wheels 42 | uses: RalfG/python-wheels-manylinux-build@v0.4.2-manylinux2014_x86_64 43 | with: 44 | python-versions: 'cp37-cp37m cp38-cp38 cp39-cp39 cp310-cp310' 45 | build-requirements: 'numpy' 46 | - name: Publish distribution 📦 to Test PyPI 47 | env: 48 | TWINE_USERNAME: __token__ 49 | #TWINE_PASSWORD: ${{ secrets.TEST_PYPI_API_TOKEN }} 50 | TWINE_PASSWORD: ${{ secrets.PYPI_API_TOKEN }} 51 | run: | 52 | #python -m twine upload --repository testpypi dist/*-manylinux*.whl dist/*.tar.gz --verbose --skip-existing 53 | python -m twine upload dist/*-manylinux*.whl dist/*.tar.gz --verbose --skip-existing 54 | -------------------------------------------------------------------------------- /.github/workflows/python-publish-macos.yml: -------------------------------------------------------------------------------- 1 | # This workflow will upload a Python Package using Twine when a release is created 2 | # For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries 3 | 4 | # This workflow uses actions that are not certified by GitHub. 5 | # They are provided by a third-party and are governed by 6 | # separate terms of service, privacy policy, and support 7 | # documentation. 8 | 9 | name: Upload Python Package MacOs 10 | 11 | on: 12 | release: 13 | types: [published] 14 | 15 | permissions: 16 | contents: read 17 | 18 | jobs: 19 | deploy: 20 | 21 | runs-on: macos-11 22 | 23 | strategy: 24 | matrix: 25 | python-version: ["3.7", "3.8", "3.9", "3.10"] 26 | 27 | steps: 28 | - uses: actions/checkout@v3 29 | - name: Set up Python 30 | uses: actions/setup-python@v3 31 | with: 32 | python-version: "${{ matrix.python-version }}" 33 | - name: Run tests 34 | run: | 35 | pip install . 36 | pip install -r requirements-dev.txt 37 | py.test 38 | - name: Install dependencies 39 | run: | 40 | python -m pip install --upgrade pip 41 | python -m pip install wheel twine 42 | - name: "Build package for python ${{ matrix.python-version }}" 43 | run: | 44 | python setup.py bdist_wheel 45 | - name: Publish distribution 📦 to Test PyPI 46 | env: 47 | TWINE_USERNAME: __token__ 48 | #TWINE_PASSWORD: ${{ secrets.TEST_PYPI_API_TOKEN }} 49 | TWINE_PASSWORD: ${{ secrets.PYPI_API_TOKEN }} 50 | run: | 51 | #python -m twine upload --repository testpypi dist/* --verbose --skip-existing 52 | python -m twine upload dist/* --verbose --skip-existing 53 | 54 | -------------------------------------------------------------------------------- /.github/workflows/python-publish-windows.yml: -------------------------------------------------------------------------------- 1 | # This workflow will upload a Python Package using Twine when a release is created 2 | # For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries 3 | 4 | # This workflow uses actions that are not certified by GitHub. 5 | # They are provided by a third-party and are governed by 6 | # separate terms of service, privacy policy, and support 7 | # documentation. 8 | 9 | name: Upload Python Package Windows 10 | 11 | on: 12 | release: 13 | types: [published] 14 | 15 | permissions: 16 | contents: read 17 | 18 | jobs: 19 | deploy: 20 | 21 | runs-on: windows-2022 22 | 23 | strategy: 24 | matrix: 25 | python-version: ["3.7", "3.8", "3.9", "3.10"] 26 | 27 | steps: 28 | - uses: actions/checkout@v3 29 | - name: Set up Python 30 | uses: actions/setup-python@v3 31 | with: 32 | python-version: "${{ matrix.python-version }}" 33 | - name: Run tests 34 | run: | 35 | pip install . 36 | pip install -r requirements-dev.txt 37 | py.test 38 | - name: Install dependencies 39 | run: | 40 | python -m pip install --upgrade pip 41 | python -m pip install wheel twine 42 | - name: "Build package for python ${{ matrix.python-version }}" 43 | run: | 44 | python setup.py bdist_wheel 45 | - name: Publish distribution 📦 to Test PyPI 46 | env: 47 | TWINE_USERNAME: __token__ 48 | #TWINE_PASSWORD: ${{ secrets.TEST_PYPI_API_TOKEN }} 49 | TWINE_PASSWORD: ${{ secrets.PYPI_API_TOKEN }} 50 | run: | 51 | #python -m twine upload --repository testpypi dist/* --verbose --skip-existing 52 | python -m twine upload dist/* --verbose --skip-existing 53 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *.swo 3 | *.so 4 | *.pyc 5 | *.png 6 | *.cache 7 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | python: 3 | - 3.8 4 | before_install: 5 | - pip install -U pip 6 | # command to install dependencies 7 | install: 8 | - pip install -r requirements-dev.txt 9 | - pip install coveralls # python-coveralls leads to this issue: https://github.com/z4r/python-coveralls/issues/73 10 | - pip install . 11 | # command to run tests 12 | script: 13 | - pytest --cov pyastar2d --cov-report term-missing 14 | after_success: 15 | - coveralls 16 | notifications: 17 | email: false 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Hendrik Weideman 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include requirements.txt 2 | include src/cpp/experimental_heuristics.h 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Build Status](https://travis-ci.com/hjweide/pyastar2d.svg?branch=master)](https://travis-ci.com/hjweide/pyastar2d) 2 | [![Coverage Status](https://coveralls.io/repos/github/hjweide/pyastar2d/badge.svg?branch=master)](https://coveralls.io/github/hjweide/pyastar2d?branch=master) 3 | [![PyPI version](https://badge.fury.io/py/pyastar2d.svg)](https://badge.fury.io/py/pyastar2d) 4 | # PyAstar2D 5 | This is a very simple C++ implementation of the A\* algorithm for pathfinding 6 | on a two-dimensional grid. The solver itself is implemented in C++, but is 7 | callable from Python. This combines the speed of C++ with the convenience of 8 | Python. 9 | 10 | I have not done any formal benchmarking, but the solver finds the solution to a 11 | 1802 by 1802 maze in 0.29s and a 4008 by 4008 maze in 0.83s when running on my 12 | nine-year-old Intel(R) Core(TM) i7-2630QM CPU @ 2.00GHz. See [Example 13 | Results](#example-results) for more details. 14 | 15 | See `src/cpp/astar.cpp` for the core C++ implementation of the A\* shortest 16 | path search algorithm, `src/pyastar2d/astar_wrapper.py` for the Python wrapper 17 | and `examples/example.py` for example usage. 18 | 19 | When determining legal moves, 4-connectivity is the default, but it is possible 20 | to set `allow_diagonal=True` for 8-connectivity. 21 | 22 | ## Installation 23 | Instructions for installing `pyastar2d` are given below. 24 | 25 | ### From PyPI 26 | The easiest way to install `pyastar2d` is directly from the Python package index: 27 | ``` 28 | pip install pyastar2d 29 | ``` 30 | 31 | ### From source 32 | You can also install `pyastar2d` by cloning this repository and building it 33 | yourself. If running on Linux or MacOS, simply run 34 | ```bash 35 | pip install . 36 | ```` 37 | from the root directory. If you are using Windows you may have to install Cython manually first: 38 | ```bash 39 | pip install Cython 40 | pip install . 41 | ``` 42 | To check that everything worked, run the example: 43 | ```bash 44 | python examples/example.py 45 | ``` 46 | 47 | ### As a dependency 48 | Include `pyastar2d` in your `requirements.txt` to install from `pypi`, or add 49 | this line to `requirements.txt`: 50 | ``` 51 | pyastar2d @ git+git://github.com/hjweide/pyastar2d.git@master#egg=pyastar2d 52 | ``` 53 | 54 | ## Usage 55 | A simple example is given below: 56 | ```python 57 | import numpy as np 58 | import pyastar2d 59 | # The minimum cost must be 1 for the heuristic to be valid. 60 | # The weights array must have np.float32 dtype to be compatible with the C++ code. 61 | weights = np.array([[1, 3, 3, 3, 3], 62 | [2, 1, 3, 3, 3], 63 | [2, 2, 1, 3, 3], 64 | [2, 2, 2, 1, 3], 65 | [2, 2, 2, 2, 1]], dtype=np.float32) 66 | # The start and goal coordinates are in matrix coordinates (i, j). 67 | path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=True) 68 | print(path) 69 | # The path is returned as a numpy array of (i, j) coordinates. 70 | array([[0, 0], 71 | [1, 1], 72 | [2, 2], 73 | [3, 3], 74 | [4, 4]]) 75 | ``` 76 | Note that all grid points are represented as `(i, j)` coordinates. An example 77 | of using `pyastar2d` to solve a maze is given in `examples/maze_solver.py`. 78 | 79 | ## Example Results 80 | 81 | To test the implementation, I grabbed two nasty mazes from Wikipedia. They are 82 | included in the ```mazes``` directory, but are originally from here: 83 | [Small](https://upload.wikimedia.org/wikipedia/commons/c/cf/MAZE.png) and 84 | [Large](https://upload.wikimedia.org/wikipedia/commons/3/32/MAZE_2000x2000_DFS.png). 85 | I load the ```.png``` files as grayscale images, and set the white pixels to 1 86 | (open space) and the black pixels to `INF` (walls). 87 | 88 | To run the examples specify the input and output files using the `--input` and 89 | `--output` flags. For example, the following commands will solve the small and 90 | large mazes: 91 | ``` 92 | python examples/maze_solver.py --input mazes/maze_small.png --output solns/maze_small.png 93 | python examples/maze_solver.py --input mazes/maze_large.png --output solns/maze_large.png 94 | ``` 95 | 96 | ### Small Maze (1802 x 1802): 97 | ```bash 98 | time python examples/maze_solver.py --input mazes/maze_small.png --output solns/maze_small.png 99 | Loaded maze of shape (1802, 1802) from mazes/maze_small.png 100 | Found path of length 10032 in 0.292794s 101 | Plotting path to solns/maze_small.png 102 | Done 103 | 104 | real 0m1.214s 105 | user 0m1.526s 106 | sys 0m0.606s 107 | ``` 108 | The solution found for the small maze is shown below: 109 | Maze Small Solution 110 | 111 | ### Large Maze (4002 x 4002): 112 | ```bash 113 | time python examples/maze_solver.py --input mazes/maze_large.png --output solns/maze_large.png 114 | Loaded maze of shape (4002, 4002) from mazes/maze_large.png 115 | Found path of length 783737 in 0.829181s 116 | Plotting path to solns/maze_large.png 117 | Done 118 | 119 | real 0m29.385s 120 | user 0m29.563s 121 | sys 0m0.728s 122 | ``` 123 | The solution found for the large maze is shown below: 124 | Maze Large Solution 125 | 126 | ## Motivation 127 | I recently needed an implementation of the A* algorithm in Python to find the 128 | shortest path between two points in a cost matrix representing an image. 129 | Normally I would simply use [networkx](https://networkx.github.io/), but for 130 | graphs with millions of nodes the overhead incurred to construct the graph can 131 | be expensive. Considering that I was only interested in graphs that may be 132 | represented as two-dimensional grids, I decided to implement it myself using 133 | this special structure of the graph to make various optimizations. 134 | Specifically, the graph is represented as a one-dimensional array because there 135 | is no need to store the neighbors. Additionally, the lookup tables for 136 | previously-explored nodes (their costs and paths) are also stored as 137 | one-dimensional arrays. The implication of this is that checking the lookup 138 | table can be done in O(1), at the cost of using O(n) memory. Alternatively, we 139 | could store only the nodes we traverse in a hash table to reduce the memory 140 | usage. Empirically I found that replacing the one-dimensional array with a 141 | hash table (`std::unordered_map`) was about five times slower. 142 | 143 | ## Tests 144 | The default installation does not include the dependencies necessary to run the 145 | tests. To install these, first run 146 | ```bash 147 | pip install -r requirements-dev.txt 148 | ``` 149 | before running 150 | ```bash 151 | py.test 152 | ``` 153 | The tests are fairly basic but cover some of the 154 | more common pitfalls. Pull requests for more extensive tests are welcome. 155 | 156 | ## References 157 | 1. [A\* search algorithm on Wikipedia](https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode) 158 | 2. [Pathfinding with A* on Red Blob Games](http://www.redblobgames.com/pathfinding/a-star/introduction.html) 159 | 160 | -------------------------------------------------------------------------------- /examples/example.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pyastar2d 3 | 4 | 5 | # The start and goal coordinates are in matrix coordinates (i, j). 6 | start = (0, 0) 7 | goal = (4, 4) 8 | 9 | # The minimum cost must be 1 for the heuristic to be valid. 10 | weights = np.array([[1, 3, 3, 3, 3], 11 | [2, 1, 3, 3, 3], 12 | [2, 2, 1, 3, 3], 13 | [2, 2, 2, 1, 3], 14 | [2, 2, 2, 2, 1]], dtype=np.float32) 15 | print("Cost matrix:") 16 | print(weights) 17 | path = pyastar2d.astar_path(weights, start, goal, allow_diagonal=True) 18 | 19 | # The path is returned as a numpy array of (i, j) coordinates. 20 | print(f"Shortest path from {start} to {goal} found:") 21 | print(path) 22 | -------------------------------------------------------------------------------- /examples/maze_solver.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import numpy as np 3 | import imageio 4 | import time 5 | 6 | import pyastar2d 7 | 8 | from os.path import basename, join 9 | 10 | 11 | def parse_args(): 12 | parser = argparse.ArgumentParser( 13 | "An example of using pyastar2d to find the solution to a maze" 14 | ) 15 | parser.add_argument( 16 | "--input", type=str, default="mazes/maze_small.png", 17 | help="Path to the black-and-white image to be used as input.", 18 | ) 19 | parser.add_argument( 20 | "--output", type=str, help="Path to where the output will be written", 21 | ) 22 | 23 | args = parser.parse_args() 24 | 25 | if args.output is None: 26 | args.output = join("solns", basename(args.input)) 27 | 28 | return args 29 | 30 | 31 | def main(): 32 | args = parse_args() 33 | maze = imageio.imread(args.input) 34 | 35 | if maze is None: 36 | print(f"No file found: {args.input}") 37 | return 38 | else: 39 | print(f"Loaded maze of shape {maze.shape} from {args.input}") 40 | 41 | grid = maze.astype(np.float32) 42 | grid[grid == 0] = np.inf 43 | grid[grid == 255] = 1 44 | 45 | assert grid.min() == 1, "cost of moving must be at least 1" 46 | 47 | # start is the first white block in the top row 48 | start_j, = np.where(grid[0, :] == 1) 49 | start = np.array([0, start_j[0]]) 50 | 51 | # end is the first white block in the final column 52 | end_i, = np.where(grid[:, -1] == 1) 53 | end = np.array([end_i[0], grid.shape[0] - 1]) 54 | 55 | t0 = time.time() 56 | # set allow_diagonal=True to enable 8-connectivity 57 | path = pyastar2d.astar_path(grid, start, end, allow_diagonal=False) 58 | dur = time.time() - t0 59 | 60 | if path.shape[0] > 0: 61 | print(f"Found path of length {path.shape[0]} in {dur:.6f}s") 62 | maze = np.stack((maze, maze, maze), axis=2) 63 | maze[path[:, 0], path[:, 1]] = (255, 0, 0) 64 | 65 | print(f"Plotting path to {args.output}") 66 | imageio.imwrite(args.output, maze) 67 | else: 68 | print("No path found") 69 | 70 | print("Done") 71 | 72 | 73 | if __name__ == "__main__": 74 | main() 75 | -------------------------------------------------------------------------------- /mazes/maze_large.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hjweide/pyastar2d/09fbb7325e3f8a0023ae474f044b6a0724b83412/mazes/maze_large.png -------------------------------------------------------------------------------- /mazes/maze_small.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hjweide/pyastar2d/09fbb7325e3f8a0023ae474f044b6a0724b83412/mazes/maze_small.png -------------------------------------------------------------------------------- /requirements-dev.txt: -------------------------------------------------------------------------------- 1 | pytest 2 | pytest-cov 3 | pytest-pep8 4 | pyyaml 5 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | imageio 2 | numpy 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | from distutils.core import Extension 3 | from setuptools import dist 4 | dist.Distribution().fetch_build_eggs(["numpy"]) 5 | import numpy 6 | 7 | astar_module = Extension( 8 | 'pyastar2d.astar', sources=['src/cpp/astar.cpp', 'src/cpp/experimental_heuristics.cpp'], 9 | include_dirs=[ 10 | numpy.get_include(), # for numpy/arrayobject.h 11 | 'src/cpp' # for experimental_heuristics.h 12 | ], 13 | extra_compile_args=["-O3", "-Wall", "-shared", "-fpic"], 14 | ) 15 | 16 | 17 | with open("requirements.txt", "r") as fh: 18 | install_requires = fh.readlines() 19 | 20 | with open("README.md", "r") as fh: 21 | long_description = fh.read() 22 | 23 | setuptools.setup( 24 | name="pyastar2d", 25 | version="1.0.6", 26 | author="Hendrik Weideman", 27 | author_email="hjweide@gmail.com", 28 | description=( 29 | "A simple implementation of the A* algorithm for " 30 | "path-finding on a two-dimensional grid."), 31 | long_description=long_description, 32 | long_description_content_type="text/markdown", 33 | url="https://github.com/hjweide/pyastar2d", 34 | install_requires=install_requires, 35 | packages=setuptools.find_packages(where="src", exclude=("tests",)), 36 | package_dir={"": "src"}, 37 | ext_modules=[astar_module], 38 | classifiers=[ 39 | "Programming Language :: Python :: 3", 40 | "License :: OSI Approved :: MIT License", 41 | "Operating System :: OS Independent", 42 | ], 43 | python_requires='>=3.7', 44 | ) 45 | -------------------------------------------------------------------------------- /solns/maze_large_soln.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hjweide/pyastar2d/09fbb7325e3f8a0023ae474f044b6a0724b83412/solns/maze_large_soln.png -------------------------------------------------------------------------------- /solns/maze_small_soln.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hjweide/pyastar2d/09fbb7325e3f8a0023ae474f044b6a0724b83412/solns/maze_small_soln.png -------------------------------------------------------------------------------- /src/cpp/astar.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | const float INF = std::numeric_limits::infinity(); 11 | 12 | // represents a single pixel 13 | class Node { 14 | public: 15 | int idx; // index in the flattened grid 16 | float cost; // cost of traversing this pixel 17 | int path_length; // the length of the path to reach this node 18 | 19 | Node(int i, float c, int path_length) : idx(i), cost(c), path_length(path_length) {} 20 | }; 21 | 22 | // the top of the priority queue is the greatest element by default, 23 | // but we want the smallest, so flip the sign 24 | bool operator<(const Node &n1, const Node &n2) { 25 | return n1.cost > n2.cost; 26 | } 27 | 28 | // See for various grid heuristics: 29 | // http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#S7 30 | // L_\inf norm (diagonal distance) 31 | inline float linf_norm(int i0, int j0, int i1, int j1) { 32 | return std::max(std::abs(i0 - i1), std::abs(j0 - j1)); 33 | } 34 | 35 | // L_1 norm (manhattan distance) 36 | inline float l1_norm(int i0, int j0, int i1, int j1) { 37 | return std::abs(i0 - i1) + std::abs(j0 - j1); 38 | } 39 | 40 | 41 | // weights: flattened h x w grid of costs 42 | // h, w: height and width of grid 43 | // start, goal: index of start/goal in flattened grid 44 | // diag_ok: if true, allows diagonal moves (8-conn.) 45 | // paths (output): for each node, stores previous node in path 46 | static PyObject *astar(PyObject *self, PyObject *args) { 47 | const PyArrayObject* weights_object; 48 | int h; 49 | int w; 50 | int start; 51 | int goal; 52 | int diag_ok; 53 | int heuristic_override; 54 | 55 | if (!PyArg_ParseTuple( 56 | args, "Oiiiiii", // i = int, O = object 57 | &weights_object, 58 | &h, &w, 59 | &start, &goal, 60 | &diag_ok, &heuristic_override 61 | )) 62 | return NULL; 63 | 64 | float* weights = (float*) weights_object->data; 65 | int* paths = new int[h * w]; 66 | int path_length = -1; 67 | 68 | Node start_node(start, 0., 1); 69 | 70 | float* costs = new float[h * w]; 71 | for (int i = 0; i < h * w; ++i) 72 | costs[i] = INF; 73 | costs[start] = 0.; 74 | 75 | std::priority_queue nodes_to_visit; 76 | nodes_to_visit.push(start_node); 77 | 78 | int* nbrs = new int[8]; 79 | 80 | int goal_i = goal / w; 81 | int goal_j = goal % w; 82 | int start_i = start / w; 83 | int start_j = start % w; 84 | 85 | heuristic_ptr heuristic_func = select_heuristic(heuristic_override); 86 | 87 | while (!nodes_to_visit.empty()) { 88 | // .top() doesn't actually remove the node 89 | Node cur = nodes_to_visit.top(); 90 | 91 | if (cur.idx == goal) { 92 | path_length = cur.path_length; 93 | break; 94 | } 95 | 96 | nodes_to_visit.pop(); 97 | 98 | int row = cur.idx / w; 99 | int col = cur.idx % w; 100 | // check bounds and find up to eight neighbors: top to bottom, left to right 101 | nbrs[0] = (diag_ok && row > 0 && col > 0) ? cur.idx - w - 1 : -1; 102 | nbrs[1] = (row > 0) ? cur.idx - w : -1; 103 | nbrs[2] = (diag_ok && row > 0 && col + 1 < w) ? cur.idx - w + 1 : -1; 104 | nbrs[3] = (col > 0) ? cur.idx - 1 : -1; 105 | nbrs[4] = (col + 1 < w) ? cur.idx + 1 : -1; 106 | nbrs[5] = (diag_ok && row + 1 < h && col > 0) ? cur.idx + w - 1 : -1; 107 | nbrs[6] = (row + 1 < h) ? cur.idx + w : -1; 108 | nbrs[7] = (diag_ok && row + 1 < h && col + 1 < w ) ? cur.idx + w + 1 : -1; 109 | 110 | float heuristic_cost; 111 | for (int i = 0; i < 8; ++i) { 112 | if (nbrs[i] >= 0) { 113 | // the sum of the cost so far and the cost of this move 114 | float new_cost = costs[cur.idx] + weights[nbrs[i]]; 115 | if (new_cost < costs[nbrs[i]]) { 116 | // estimate the cost to the goal based on legal moves 117 | // Get the heuristic method to use 118 | if (heuristic_override == DEFAULT) { 119 | if (diag_ok) { 120 | heuristic_cost = linf_norm(nbrs[i] / w, nbrs[i] % w, goal_i, goal_j); 121 | } else { 122 | heuristic_cost = l1_norm(nbrs[i] / w, nbrs[i] % w, goal_i, goal_j); 123 | } 124 | } else { 125 | heuristic_cost = heuristic_func( 126 | nbrs[i] / w, nbrs[i] % w, goal_i, goal_j, start_i, start_j); 127 | } 128 | 129 | // paths with lower expected cost are explored first 130 | float priority = new_cost + heuristic_cost; 131 | nodes_to_visit.push(Node(nbrs[i], priority, cur.path_length + 1)); 132 | 133 | costs[nbrs[i]] = new_cost; 134 | paths[nbrs[i]] = cur.idx; 135 | } 136 | } 137 | } 138 | } 139 | 140 | PyObject *return_val; 141 | if (path_length >= 0) { 142 | npy_intp dims[2] = {path_length, 2}; 143 | PyArrayObject* path = (PyArrayObject*) PyArray_SimpleNew(2, dims, NPY_INT32); 144 | npy_int32 *iptr, *jptr; 145 | int idx = goal; 146 | for (npy_intp i = dims[0] - 1; i >= 0; --i) { 147 | iptr = (npy_int32*) (path->data + i * path->strides[0]); 148 | jptr = (npy_int32*) (path->data + i * path->strides[0] + path->strides[1]); 149 | 150 | *iptr = idx / w; 151 | *jptr = idx % w; 152 | 153 | idx = paths[idx]; 154 | } 155 | 156 | return_val = PyArray_Return(path); 157 | } 158 | else { 159 | return_val = Py_BuildValue(""); // no soln --> return None 160 | } 161 | 162 | delete[] costs; 163 | delete[] nbrs; 164 | delete[] paths; 165 | 166 | return return_val; 167 | } 168 | 169 | static PyMethodDef astar_methods[] = { 170 | {"astar", (PyCFunction)astar, METH_VARARGS, "astar"}, 171 | {NULL, NULL, 0, NULL} 172 | }; 173 | 174 | static struct PyModuleDef astar_module = { 175 | PyModuleDef_HEAD_INIT,"astar", NULL, -1, astar_methods 176 | }; 177 | 178 | PyMODINIT_FUNC PyInit_astar(void) { 179 | import_array(); 180 | return PyModule_Create(&astar_module); 181 | } 182 | -------------------------------------------------------------------------------- /src/cpp/experimental_heuristics.cpp: -------------------------------------------------------------------------------- 1 | // Please note below heuristics are experimental and only for pretty lines. 2 | // They may not take the shortest path and require additional cpu cycles. 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | heuristic_ptr select_heuristic(int h) { 10 | switch (h) { 11 | case ORTHOGONAL_X: 12 | return orthogonal_x; 13 | case ORTHOGONAL_Y: 14 | return orthogonal_y; 15 | default: 16 | return NULL; 17 | } 18 | } 19 | 20 | // Orthogonal x (moves by x first, then half way by y) 21 | float orthogonal_x(int i0, int j0, int i1, int j1, int i2, int j2) { 22 | int di = std::abs(i0 - i1); 23 | int dim = std::abs(i1 - i2); 24 | int djm = std::abs(j1 - j2); 25 | if (di > (dim * 0.5)) { 26 | return di + djm; 27 | } else { 28 | return std::abs(j0 - j1); 29 | } 30 | } 31 | 32 | // Orthogonal y (moves by y first, then half way by x) 33 | float orthogonal_y(int i0, int j0, int i1, int j1, int i2, int j2) { 34 | int dj = std::abs(j0 - j1); 35 | int djm = std::abs(j1 - j2); 36 | int dim = std::abs(i1 - i2); 37 | if (dj > (djm * 0.5)) { 38 | return dj + dim; 39 | } else { 40 | return std::abs(i0 - i1); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /src/cpp/experimental_heuristics.h: -------------------------------------------------------------------------------- 1 | // Please note below heuristics are experimental and only for pretty lines. 2 | // They may not take the shortest path and require additional cpu cycles. 3 | 4 | #ifndef EXPERIMENTAL_HEURISTICS_H_ 5 | #define EXPERIMENTAL_HEURISTICS_H_ 6 | 7 | 8 | enum Heuristic { DEFAULT, ORTHOGONAL_X, ORTHOGONAL_Y }; 9 | 10 | typedef float (*heuristic_ptr)(int, int, int, int, int, int); 11 | 12 | heuristic_ptr select_heuristic(int); 13 | 14 | // Orthogonal x (moves by x first, then half way by y) 15 | float orthogonal_x(int, int, int, int, int, int); 16 | 17 | // Orthogonal y (moves by y first, then half way by x) 18 | float orthogonal_y(int, int, int, int, int, int); 19 | 20 | #endif 21 | -------------------------------------------------------------------------------- /src/pyastar2d/__init__.py: -------------------------------------------------------------------------------- 1 | from pyastar2d.astar_wrapper import astar_path, Heuristic 2 | __all__ = ["astar_path", "Heuristic"] 3 | -------------------------------------------------------------------------------- /src/pyastar2d/astar_wrapper.py: -------------------------------------------------------------------------------- 1 | import ctypes 2 | import numpy as np 3 | import pyastar2d.astar 4 | from enum import IntEnum 5 | from typing import Optional, Tuple 6 | 7 | 8 | # Define array types 9 | ndmat_f_type = np.ctypeslib.ndpointer( 10 | dtype=np.float32, ndim=1, flags="C_CONTIGUOUS") 11 | ndmat_i2_type = np.ctypeslib.ndpointer( 12 | dtype=np.int32, ndim=2, flags="C_CONTIGUOUS") 13 | 14 | # Define input/output types 15 | pyastar2d.astar.restype = ndmat_i2_type # Nx2 (i, j) coordinates or None 16 | pyastar2d.astar.argtypes = [ 17 | ndmat_f_type, # weights 18 | ctypes.c_int, # height 19 | ctypes.c_int, # width 20 | ctypes.c_int, # start index in flattened grid 21 | ctypes.c_int, # goal index in flattened grid 22 | ctypes.c_bool, # allow diagonal 23 | ctypes.c_int, # heuristic_override 24 | ] 25 | 26 | class Heuristic(IntEnum): 27 | """The supported heuristics.""" 28 | 29 | DEFAULT = 0 30 | ORTHOGONAL_X = 1 31 | ORTHOGONAL_Y = 2 32 | 33 | def astar_path( 34 | weights: np.ndarray, 35 | start: Tuple[int, int], 36 | goal: Tuple[int, int], 37 | allow_diagonal: bool = False, 38 | heuristic_override: Heuristic = Heuristic.DEFAULT) -> Optional[np.ndarray]: 39 | """ 40 | Run astar algorithm on 2d weights. 41 | 42 | param np.ndarray weights: A grid of weights e.g. np.ones((10, 10), dtype=np.float32) 43 | param Tuple[int, int] start: (i, j) 44 | param Tuple[int, int] goal: (i, j) 45 | param bool allow_diagonal: Whether to allow diagonal moves 46 | param Heuristic heuristic_override: Override heuristic, see Heuristic(IntEnum) 47 | 48 | """ 49 | assert weights.dtype == np.float32, ( 50 | f"weights must have np.float32 data type, but has {weights.dtype}" 51 | ) 52 | # For the heuristic to be valid, each move must cost at least 1. 53 | if weights.min(axis=None) < 1.: 54 | raise ValueError("Minimum cost to move must be 1, but got %f" % ( 55 | weights.min(axis=None))) 56 | # Ensure start is within bounds. 57 | if (start[0] < 0 or start[0] >= weights.shape[0] or 58 | start[1] < 0 or start[1] >= weights.shape[1]): 59 | raise ValueError(f"Start of {start} lies outside grid.") 60 | # Ensure goal is within bounds. 61 | if (goal[0] < 0 or goal[0] >= weights.shape[0] or 62 | goal[1] < 0 or goal[1] >= weights.shape[1]): 63 | raise ValueError(f"Goal of {goal} lies outside grid.") 64 | 65 | height, width = weights.shape 66 | start_idx = np.ravel_multi_index(start, (height, width)) 67 | goal_idx = np.ravel_multi_index(goal, (height, width)) 68 | 69 | path = pyastar2d.astar.astar( 70 | weights.flatten(), height, width, start_idx, goal_idx, allow_diagonal, 71 | int(heuristic_override) 72 | ) 73 | return path 74 | -------------------------------------------------------------------------------- /tests/test_astar.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pytest 3 | 4 | import os 5 | import sys 6 | import pyastar2d 7 | 8 | from pyastar2d import Heuristic 9 | 10 | 11 | def test_small(): 12 | weights = np.array([[1, 3, 3, 3, 3], 13 | [2, 1, 3, 3, 3], 14 | [2, 2, 1, 3, 3], 15 | [2, 2, 2, 1, 3], 16 | [2, 2, 2, 2, 1]], dtype=np.float32) 17 | # Run down the diagonal. 18 | path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=True) 19 | expected = np.array([[0, 0], [1, 1], [2, 2], [3, 3], [4, 4]]) 20 | 21 | assert np.all(path == expected) 22 | 23 | # Down, right, down, right, etc. 24 | path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False) 25 | expected = np.array([[0, 0], [1, 0], [1, 1], [2, 1], 26 | [2, 2], [3, 2], [3, 3], [4, 3], [4, 4]]) 27 | 28 | assert np.all(path == expected) 29 | 30 | 31 | def test_no_solution(): 32 | # Vertical wall. 33 | weights = np.ones((5, 5), dtype=np.float32) 34 | weights[:, 2] = np.inf 35 | 36 | path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=True) 37 | assert not path 38 | 39 | # Horizontal wall. 40 | weights = np.ones((5, 5), dtype=np.float32) 41 | weights[2, :] = np.inf 42 | 43 | path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=True) 44 | assert not path 45 | 46 | 47 | def test_match_reverse(): 48 | # Might fail if there are multiple paths, but this should be rare. 49 | h, w = 25, 25 50 | weights = (1. + 5. * np.random.random((h, w))).astype(np.float32) 51 | 52 | fwd = pyastar2d.astar_path(weights, (0, 0), (h - 1, w - 1)) 53 | rev = pyastar2d.astar_path(weights, (h - 1, w - 1), (0, 0)) 54 | 55 | assert np.all(fwd[::-1] == rev) 56 | 57 | fwd = pyastar2d.astar_path(weights, (0, 0), (h - 1, w - 1), 58 | allow_diagonal=True) 59 | rev = pyastar2d.astar_path(weights, (h - 1, w - 1), (0, 0), 60 | allow_diagonal=True) 61 | 62 | assert np.all(fwd[::-1] == rev) 63 | 64 | 65 | def test_narrow(): 66 | # Column weights. 67 | weights = np.ones((5, 1), dtype=np.float32) 68 | path = pyastar2d.astar_path(weights, (0, 0), (4, 0)) 69 | 70 | expected = np.array([[0, 0], [1, 0], [2, 0], [3, 0], [4, 0]]) 71 | 72 | assert np.all(path == expected) 73 | 74 | # Row weights. 75 | weights = np.ones((1, 5), dtype=np.float32) 76 | path = pyastar2d.astar_path(weights, (0, 0), (0, 4)) 77 | 78 | expected = np.array([[0, 0], [0, 1], [0, 2], [0, 3], [0, 4]]) 79 | 80 | assert np.all(path == expected) 81 | 82 | 83 | def test_bad_heuristic(): 84 | # For valid heuristics, the cost to move must be at least 1. 85 | weights = (1. + 5. * np.random.random((10, 10))).astype(np.float32) 86 | # An element smaller than 1 should raise a ValueError. 87 | bad_cost = np.random.random() / 2. 88 | weights[4, 4] = bad_cost 89 | 90 | with pytest.raises(ValueError) as exc: 91 | pyastar2d.astar_path(weights, (0, 0), (9, 9)) 92 | assert '.f' % bad_cost in exc.value.args[0] 93 | 94 | 95 | def test_invalid_start_and_goal(): 96 | weights = (1. + 5. * np.random.random((10, 10))).astype(np.float32) 97 | # Test bad start indices. 98 | with pytest.raises(ValueError) as exc: 99 | pyastar2d.astar_path(weights, (-1, 0), (9, 9)) 100 | assert '-1' in exc.value.args[0] 101 | with pytest.raises(ValueError) as exc: 102 | pyastar2d.astar_path(weights, (10, 0), (9, 9)) 103 | assert '10' in exc.value.args[0] 104 | with pytest.raises(ValueError) as exc: 105 | pyastar2d.astar_path(weights, (0, -1), (9, 9)) 106 | assert '-1' in exc.value.args[0] 107 | with pytest.raises(ValueError) as exc: 108 | pyastar2d.astar_path(weights, (0, 10), (9, 9)) 109 | assert '10' in exc.value.args[0] 110 | # Test bad goal indices. 111 | with pytest.raises(ValueError) as exc: 112 | pyastar2d.astar_path(weights, (0, 0), (-1, 9)) 113 | assert '-1' in exc.value.args[0] 114 | with pytest.raises(ValueError) as exc: 115 | pyastar2d.astar_path(weights, (0, 0), (10, 9)) 116 | assert '10' in exc.value.args[0] 117 | with pytest.raises(ValueError) as exc: 118 | pyastar2d.astar_path(weights, (0, 0), (0, -1)) 119 | assert '-1' in exc.value.args[0] 120 | with pytest.raises(ValueError) as exc: 121 | pyastar2d.astar_path(weights, (0, 0), (0, 10)) 122 | assert '10' in exc.value.args[0] 123 | 124 | 125 | def test_bad_weights_dtype(): 126 | weights = np.array([[1, 2, 3], [1, 2, 3], [1, 2, 3]], dtype=np.float64) 127 | with pytest.raises(AssertionError) as exc: 128 | pyastar2d.astar_path(weights, (0, 0), (2, 2)) 129 | assert "float64" in exc.value.args[0] 130 | 131 | 132 | def test_orthogonal_x(): 133 | weights = np.ones((5, 5), dtype=np.float32) 134 | path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=Heuristic.ORTHOGONAL_X) 135 | expected = np.array([[0, 0], [1, 0], [2, 0], [2, 1], [2, 2], [2, 3], [2, 4], [3, 4], [4, 4]]) 136 | 137 | assert np.all(path == expected) 138 | 139 | 140 | def test_orthogonal_y(): 141 | weights = np.ones((5, 5), dtype=np.float32) 142 | path = pyastar2d.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False, heuristic_override=Heuristic.ORTHOGONAL_Y) 143 | expected = np.array([[0, 0], [0, 1], [0, 2], [1, 2], [2, 2], [3, 2], [4, 2], [4, 3], [4, 4]]) 144 | 145 | assert np.all(path == expected) 146 | --------------------------------------------------------------------------------