├── tests ├── __init__.py ├── cli_test.py ├── helpers.py ├── main_test.py ├── helper_fcts_test.py ├── test_cases.py └── test_find_visible.py ├── docs ├── 6_changelog.rst ├── 5_contributing.rst ├── requirements.txt ├── _static │ ├── graph_plot.png │ ├── map_plot.png │ ├── path_plot.png │ ├── graph_path_plot.png │ ├── grid_map_plot.png │ ├── title_demo_plot.png │ ├── prepared_map_plot.png │ ├── grid_graph_path_plot.png │ ├── path_overlapping_edges.png │ └── path_overlapping_vertices.png ├── 4_api.rst ├── 7_performance.rst ├── Makefile ├── index.rst ├── badges.rst ├── 0_getting_started.rst ├── conf.py ├── 1_usage.rst └── 3_about.rst ├── .github ├── FUNDING.yml ├── dependabot.yml └── workflows │ └── build.yml ├── extremitypathfinder ├── __init__.py ├── configs.py ├── types.py ├── numba_replacements.py ├── command_line.py ├── plotting.py ├── utils_numba.py ├── extremitypathfinder.py └── utils.py ├── .editorconfig ├── readthedocs.yml ├── example.json ├── .travis.yml ├── LICENSE ├── Makefile ├── scripts └── speed_benchmarks.py ├── .gitignore ├── .pre-commit-config.yaml ├── README.rst ├── CONTRIBUTING.rst ├── pyproject.toml └── CHANGELOG.rst /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/6_changelog.rst: -------------------------------------------------------------------------------- 1 | .. include:: ../CHANGELOG.rst 2 | -------------------------------------------------------------------------------- /docs/5_contributing.rst: -------------------------------------------------------------------------------- 1 | .. _contributing: 2 | 3 | .. include:: ../CONTRIBUTING.rst 4 | -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | networkx 2 | numpy 3 | Sphinx 4 | sphinx-rtd-theme 5 | urllib3<3 6 | -------------------------------------------------------------------------------- /docs/_static/graph_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/graph_plot.png -------------------------------------------------------------------------------- /docs/_static/map_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/map_plot.png -------------------------------------------------------------------------------- /docs/_static/path_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/path_plot.png -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | github: [MrMinimal64] 3 | issuehunt: mrminimal64 4 | -------------------------------------------------------------------------------- /docs/_static/graph_path_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/graph_path_plot.png -------------------------------------------------------------------------------- /docs/_static/grid_map_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/grid_map_plot.png -------------------------------------------------------------------------------- /docs/_static/title_demo_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/title_demo_plot.png -------------------------------------------------------------------------------- /docs/_static/prepared_map_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/prepared_map_plot.png -------------------------------------------------------------------------------- /docs/_static/grid_graph_path_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/grid_graph_path_plot.png -------------------------------------------------------------------------------- /docs/_static/path_overlapping_edges.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/path_overlapping_edges.png -------------------------------------------------------------------------------- /docs/_static/path_overlapping_vertices.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jannikmi/extremitypathfinder/HEAD/docs/_static/path_overlapping_vertices.png -------------------------------------------------------------------------------- /extremitypathfinder/__init__.py: -------------------------------------------------------------------------------- 1 | from .extremitypathfinder import PolygonEnvironment 2 | from .utils import load_pickle 3 | 4 | __all__ = ("PolygonEnvironment", "load_pickle") 5 | -------------------------------------------------------------------------------- /.editorconfig: -------------------------------------------------------------------------------- 1 | # http://editorconfig.org 2 | 3 | root = true 4 | 5 | [*] 6 | indent_style = space 7 | indent_size = 4 8 | trim_trailing_whitespace = true 9 | insert_final_newline = true 10 | charset = utf-8 11 | end_of_line = lf 12 | 13 | [*.py] 14 | line_length = 120 15 | -------------------------------------------------------------------------------- /extremitypathfinder/configs.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | DEFAULT_PICKLE_NAME = "environment.pickle" 4 | 5 | # command line interface 6 | # json data input format: 7 | BOUNDARY_JSON_KEY = "boundary" 8 | HOLES_JSON_KEY = "holes" 9 | 10 | 11 | DTYPE_FLOAT = np.float64 12 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: "pip" 4 | directory: "pyproject.toml" 5 | schedule: 6 | interval: "weekly" 7 | - package-ecosystem: "github-actions" 8 | directory: "/" 9 | schedule: 10 | interval: "weekly" 11 | -------------------------------------------------------------------------------- /extremitypathfinder/types.py: -------------------------------------------------------------------------------- 1 | from typing import Iterable, List, Optional, Tuple, Union 2 | 3 | import networkx as nx 4 | import numpy as np 5 | 6 | Coordinate = Tuple[float, float] 7 | Path = List[Coordinate] 8 | Length = Optional[float] 9 | InputNumerical = Union[float, int] 10 | InputCoord = Tuple[InputNumerical, InputNumerical] 11 | InputCoordList = Union[np.ndarray, List[InputCoord]] 12 | ObstacleIterator = Iterable[InputCoord] 13 | Graph = nx.Graph 14 | -------------------------------------------------------------------------------- /docs/4_api.rst: -------------------------------------------------------------------------------- 1 | .. _api: 2 | 3 | ================= 4 | API documentation 5 | ================= 6 | 7 | 8 | .. py:module:: extremitypathfinder 9 | 10 | .. _env: 11 | 12 | 13 | PolygonEnvironment 14 | ------------------ 15 | .. autoclass:: PolygonEnvironment 16 | 17 | 18 | .. py:module:: extremitypathfinder.plotting 19 | 20 | PlottingEnvironment 21 | ------------------- 22 | .. autoclass:: PlottingEnvironment 23 | :members: store, prepare, find_shortest_path 24 | :no-inherited-members: 25 | -------------------------------------------------------------------------------- /readthedocs.yml: -------------------------------------------------------------------------------- 1 | # .readthedocs.yml 2 | # Read the Docs configuration file 3 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 4 | 5 | # Required 6 | version: 2 7 | build: 8 | os: "ubuntu-22.04" 9 | tools: 10 | python: "3.12" 11 | 12 | # Build documentation in the docs/ directory with Sphinx 13 | sphinx: 14 | configuration: docs/conf.py 15 | 16 | # Optionally build your docs in additional formats such as PDF and ePub 17 | formats: all 18 | 19 | # Optionally set the version of Python and requirements required to build your docs 20 | python: 21 | install: 22 | - requirements: docs/requirements.txt 23 | -------------------------------------------------------------------------------- /tests/cli_test.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | from pathlib import Path 3 | from typing import List 4 | 5 | import pytest 6 | 7 | PROJECT_DIR = Path(__file__).parent.parent 8 | EXAMPLE_FILE = PROJECT_DIR / "example.json" 9 | 10 | 11 | @pytest.mark.parametrize( 12 | "cmd", 13 | [ 14 | f"extremitypathfinder {EXAMPLE_FILE} -s 2.5 3.2 -g 7.9 6.8", 15 | ], 16 | ) 17 | def test_main(cmd: List[str]): 18 | res = subprocess.getoutput(cmd) 19 | assert not res.endswith("command not found"), "package not installed" 20 | splits = res.split(" ") 21 | length = float(splits[-1]) 22 | print("length:", length) 23 | path = " ".join(splits[:-1]) 24 | print("path:", path) 25 | -------------------------------------------------------------------------------- /docs/7_performance.rst: -------------------------------------------------------------------------------- 1 | 2 | Performance 3 | =========== 4 | 5 | 6 | .. _speed-tests: 7 | 8 | Speed Benchmark Results 9 | ----------------------- 10 | 11 | obtained on MacBook Pro (15-inch, 2017), 2,8 GHz Intel Core i7 and `extremitypathfinder` version ``2.4.1`` using the script 12 | ``scripts/speed_benchmarks.py`` 13 | 14 | 15 | :: 16 | 17 | speed_benchmarks.py::test_env_preparation_speed PASSED [ 50%] 18 | avg. environment preparation time 7.4e-03 s/run, 1.4e+02 runs/s 19 | averaged over 1,000 runs 20 | 21 | speed_benchmarks.py::test_query_speed PASSED [100%] 22 | avg. query time 7.1e-04 s/run, 1.4e+03 runs/s 23 | averaged over 1,000 runs 24 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | 2 | ============================ 3 | extremitypathfinder 4 | ============================ 5 | 6 | 7 | .. include:: ./badges.rst 8 | 9 | 10 | python package for geometric shortest path computation in 2D multi-polygon or grid environments based on visibility graphs. 11 | 12 | .. image:: _static/title_demo_plot.png 13 | 14 | 15 | .. toctree:: 16 | :maxdepth: 2 17 | :caption: Contents: 18 | 19 | Getting Started <0_getting_started> 20 | Usage <1_usage> 21 | About <3_about> 22 | API <4_api> 23 | Performance <7_performance> 24 | Contributing <5_contributing> 25 | Changelog <6_changelog> 26 | 27 | 28 | 29 | Indices and tables 30 | ================== 31 | 32 | * :ref:`genindex` 33 | * :ref:`modindex` 34 | * :ref:`search` 35 | -------------------------------------------------------------------------------- /example.json: -------------------------------------------------------------------------------- 1 | { 2 | "boundary": [ 3 | [ 4 | 0.0, 5 | 0.0 6 | ], 7 | [ 8 | 10.0, 9 | 0.0 10 | ], 11 | [ 12 | 9.0, 13 | 5.0 14 | ], 15 | [ 16 | 10.0, 17 | 10.0 18 | ], 19 | [ 20 | 0.0, 21 | 10.0 22 | ] 23 | ], 24 | "holes": [ 25 | [ 26 | [ 27 | 3.0, 28 | 7.0 29 | ], 30 | [ 31 | 5.0, 32 | 9.0 33 | ], 34 | [ 35 | 4.5, 36 | 7.0 37 | ], 38 | [ 39 | 5.0, 40 | 4.0 41 | ] 42 | ], 43 | [ 44 | [ 45 | 1.0, 46 | 2.0 47 | ], 48 | [ 49 | 2.0, 50 | 2.0 51 | ], 52 | [ 53 | 2.0, 54 | 1.0 55 | ], 56 | [ 57 | 1.0, 58 | 1.0 59 | ] 60 | ] 61 | ] 62 | } 63 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: false 2 | language: python 3 | 4 | 5 | # Test matrix spells out each supported version of Python explicitly: 6 | matrix: 7 | include: 8 | # has to be the same py version specified in tox.ini 9 | 10 | # - python: 3.6 11 | # env: TOXENV=py36 12 | - python: 3.7 13 | env: TOXENV=py37 14 | # - python: 3.8 15 | # env: TOXENV=py38 16 | 17 | 18 | # allow_failures: 19 | # # Tests with ... are allowed to fail 20 | # - env: ... 21 | 22 | install: 23 | - ./.travis/install.sh 24 | 25 | script: 26 | - ./.travis/run.sh tox 27 | 28 | #after_success: 29 | # - coverage combine 30 | # - coveralls 31 | 32 | branches: 33 | only: 34 | - master 35 | - dev 36 | 37 | notifications: 38 | email: 39 | recipients: 40 | - python@michelfe.it 41 | on_success: always 42 | on_failure: always 43 | on_start: never # options: [always|never|change] default: always 44 | -------------------------------------------------------------------------------- /tests/helpers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from extremitypathfinder import utils 4 | 5 | 6 | def proto_test_case(data, fct): 7 | for input, expected_output in data: 8 | # print(input, expected_output, fct(input)) 9 | actual_output = fct(input) 10 | if actual_output != expected_output: 11 | print( 12 | "input: {} expected: {} got: {}".format( 13 | input, expected_output, actual_output 14 | ) 15 | ) 16 | assert actual_output == expected_output 17 | 18 | 19 | def other_edge_intersects( 20 | n1: int, n2: int, edge_vertex_idxs: np.ndarray, coords: np.ndarray 21 | ) -> bool: 22 | p1 = coords[n1] 23 | p2 = coords[n2] 24 | for i1, i2 in edge_vertex_idxs: 25 | if i1 == n1 or i2 == n2: 26 | # no not check the same edge 27 | continue 28 | q1 = coords[i1] 29 | q2 = coords[i2] 30 | if utils._get_intersection_status(p1, p2, q1, q2) == 1: 31 | return True 32 | return False 33 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 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 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # NOTE: install the package itselt to make the CLI commands available (required for the tests) 2 | install: 3 | @echo "installing the development dependencies..." 4 | @poetry install --all-extras --sync --with dev,docs,plot 5 | @#poetry install --no-dev 6 | 7 | 8 | update: 9 | @echo "updating the dependencies pinned in 'pyproject.toml':" 10 | @poetry update -vvv 11 | #poetry export -f requirements.txt --output docs/requirements.txt --without-hashes 12 | 13 | lock: 14 | @echo "pinning the dependencies in 'pyproject.toml':" 15 | @poetry lock -vvv 16 | 17 | 18 | test: 19 | poetry run pytest 20 | 21 | tox: 22 | @poetry run tox 23 | 24 | hook: 25 | @pre-commit install 26 | @pre-commit run --all-files 27 | 28 | hookup: 29 | @pre-commit autoupdate 30 | @pre-commit install 31 | 32 | clean: 33 | rm -rf .pytest_cache .coverage coverage.xml tests/__pycache__ .mypyp_cache/ .tox 34 | 35 | 36 | build: 37 | poetry build 38 | 39 | # documentation generation: 40 | # https://docs.readthedocs.io/en/stable/intro/getting-started-with-sphinx.html 41 | docs: 42 | (cd docs && make html) 43 | 44 | 45 | .PHONY: clean test build docs 46 | -------------------------------------------------------------------------------- /extremitypathfinder/numba_replacements.py: -------------------------------------------------------------------------------- 1 | """'transparent' numba functionality replacements 2 | 3 | njit decorator 4 | data types 5 | 6 | dtype_2int_tuple = typeof((1, 1)) 7 | @njit(b1(i4, i4, i4[:, :]), cache=True) 8 | @njit(dtype_2int_tuple(f8, f8), cache=True) 9 | """ 10 | 11 | 12 | def njit(*args, **kwargs): 13 | def wrapper(f): 14 | return f 15 | 16 | return wrapper 17 | 18 | 19 | class SubscriptAndCallable: 20 | def __init__(self, *args, **kwargs): 21 | pass 22 | 23 | def __class_getitem__(cls, item): 24 | return None 25 | 26 | 27 | # DTYPES 28 | # @njit(b1(i4, i4, i4[:, :]), cache=True) 29 | 30 | 31 | class b1(SubscriptAndCallable): 32 | pass 33 | 34 | 35 | class f8(SubscriptAndCallable): 36 | pass 37 | 38 | 39 | class i2(SubscriptAndCallable): 40 | pass 41 | 42 | 43 | class i4(SubscriptAndCallable): 44 | pass 45 | 46 | 47 | class i8(SubscriptAndCallable): 48 | pass 49 | 50 | 51 | class u2(SubscriptAndCallable): 52 | pass 53 | 54 | 55 | class void(SubscriptAndCallable): 56 | pass 57 | 58 | 59 | def typeof(*args, **kwargs): 60 | return void 61 | -------------------------------------------------------------------------------- /docs/badges.rst: -------------------------------------------------------------------------------- 1 | 2 | .. image:: https://github.com/jannikmi/extremitypathfinder/actions/workflows/build.yml/badge.svg?branch=master 3 | :target: https://github.com/jannikmi/extremitypathfinder/actions?query=branch%3Amaster 4 | 5 | .. image:: https://readthedocs.org/projects/extremitypathfinder/badge/?version=latest 6 | :alt: documentation status 7 | :target: https://extremitypathfinder.readthedocs.io/en/latest/?badge=latest 8 | 9 | .. image:: https://img.shields.io/pypi/wheel/extremitypathfinder.svg 10 | :target: https://pypi.python.org/pypi/extremitypathfinder 11 | 12 | .. image:: https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit&logoColor=white 13 | :target: https://github.com/pre-commit/pre-commit 14 | :alt: pre-commit 15 | 16 | .. image:: https://pepy.tech/badge/extremitypathfinder 17 | :alt: Total PyPI downloads 18 | :target: https://pepy.tech/project/extremitypathfinder 19 | 20 | .. image:: https://img.shields.io/pypi/v/extremitypathfinder.svg 21 | :alt: latest version on PyPI 22 | :target: https://pypi.python.org/pypi/extremitypathfinder 23 | 24 | .. image:: https://img.shields.io/badge/code%20style-black-000000.svg 25 | :target: https://github.com/psf/black 26 | -------------------------------------------------------------------------------- /docs/0_getting_started.rst: -------------------------------------------------------------------------------- 1 | 2 | 3 | =============== 4 | Getting started 5 | =============== 6 | 7 | 8 | Installation 9 | ------------ 10 | 11 | Installation with pip: 12 | 13 | 14 | .. code-block:: console 15 | 16 | pip install extremitypathfinder 17 | 18 | 19 | Installation with Numba for a significant speedup, with the tradeoff of a larger installation footprint and slight initial compilation time (until caching kicks in): 20 | 21 | .. code-block:: console 22 | 23 | pip install extremitypathfinder[numba] 24 | 25 | 26 | 27 | Dependencies 28 | ------------ 29 | 30 | please refer to the ``pyproject.toml`` file for current dependency specification. 31 | 32 | 33 | 34 | Basics 35 | ------ 36 | 37 | 38 | 39 | .. code-block:: python 40 | 41 | from extremitypathfinder import PolygonEnvironment 42 | 43 | environment = PolygonEnvironment() 44 | # counter clockwise vertex numbering! 45 | boundary_coordinates = [(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)] 46 | # clockwise numbering! 47 | list_of_holes = [ 48 | [ 49 | (3.0, 7.0), 50 | (5.0, 9.0), 51 | (4.5, 7.0), 52 | (5.0, 4.0), 53 | ], 54 | ] 55 | environment.store(boundary_coordinates, list_of_holes, validate=False) 56 | start_coordinates = (4.5, 1.0) 57 | goal_coordinates = (4.0, 8.5) 58 | path, length = environment.find_shortest_path(start_coordinates, goal_coordinates) 59 | 60 | 61 | 62 | All available features of this package are explained :ref:`HERE `. 63 | -------------------------------------------------------------------------------- /scripts/speed_benchmarks.py: -------------------------------------------------------------------------------- 1 | import random 2 | import timeit 3 | from typing import Callable 4 | 5 | from extremitypathfinder import PolygonEnvironment 6 | from tests.test_cases import POLYGON_ENVS 7 | 8 | RUNS_ENV_PREP = int(1e3) 9 | RUNS_QUERY = int(1e3) 10 | 11 | 12 | def timefunc(function: Callable, nr_runs: int, *args): 13 | def wrap(): 14 | function(*args) 15 | 16 | timer = timeit.Timer(wrap) 17 | t_in_sec = timer.timeit(nr_runs) 18 | return t_in_sec 19 | 20 | 21 | def get_random_env(): 22 | return random.choice(POLYGON_ENVS) 23 | 24 | 25 | def get_prepared_env(env_data): 26 | env = PolygonEnvironment() 27 | env.store(*env_data) 28 | return env 29 | 30 | 31 | def get_rnd_query_pts(env): 32 | # return any of the environments points 33 | start_idx = random.randint(0, env.nr_vertices - 1) 34 | goal_idx = random.randint(0, env.nr_vertices - 1) 35 | start = env.coords[start_idx] 36 | goal = env.coords[goal_idx] 37 | return goal, start 38 | 39 | 40 | def eval_time_fct(): 41 | global tf, point_list 42 | for point in point_list: 43 | tf.timezone_at(lng=point[0], lat=point[1]) 44 | 45 | 46 | def test_env_preparation_speed(): 47 | def run_func(): 48 | env_data = get_random_env() 49 | _ = get_prepared_env(env_data) 50 | 51 | print() 52 | t = timefunc(run_func, RUNS_ENV_PREP) 53 | t_avg = t / RUNS_ENV_PREP 54 | pts_p_sec = t_avg**-1 55 | print( 56 | f"avg. environment preparation time {t_avg:.1e} s/run, {pts_p_sec:.1e} runs/s" 57 | ) 58 | print(f"averaged over {RUNS_ENV_PREP:,} runs") 59 | 60 | 61 | def test_query_speed(): 62 | prepared_envs = [get_prepared_env(d) for d in POLYGON_ENVS] 63 | 64 | def run_func(): 65 | env = random.choice(prepared_envs) 66 | goal, start = get_rnd_query_pts(env) 67 | p, d = env.find_shortest_path(start, goal, verify=False) 68 | 69 | print() 70 | t = timefunc(run_func, RUNS_QUERY) 71 | t_avg = t / RUNS_QUERY 72 | pts_p_sec = t_avg**-1 73 | print(f"avg. query time {t_avg:.1e} s/run, {pts_p_sec:.1e} runs/s") 74 | print(f"averaged over {RUNS_QUERY:,} runs") 75 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | scrapyard.py 2 | playground.py 3 | tmp.py 4 | .pytest_cache 5 | .DS_Store 6 | plots/ 7 | venv/ 8 | map.pickle 9 | TODOs.txt 10 | *.coverage 11 | 12 | *.bat 13 | 14 | 15 | # GITIGNORE DEFAULTS 16 | 17 | 18 | # PYTHON 19 | 20 | 21 | # Byte-compiled / optimized / DLL files 22 | __pycache__/ 23 | *.py[cod] 24 | *$py.class 25 | 26 | # C extensions 27 | *.so 28 | 29 | 30 | # Distribution / packaging deleted rule for build/ 31 | 32 | .Python 33 | env/ 34 | 35 | develop-eggs/ 36 | dist/ 37 | downloads/ 38 | eggs/ 39 | .eggs/ 40 | lib/ 41 | lib64/ 42 | parts/ 43 | sdist/ 44 | var/ 45 | *.egg-info/ 46 | .installed.cfg 47 | *.egg 48 | 49 | # PyInstaller 50 | # Usually these files are written by a python script from a template 51 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 52 | *.manifest 53 | *.spec 54 | 55 | # Installer logs 56 | pip-log.txt 57 | pip-delete-this-directory.txt 58 | 59 | # Unit test / coverage reports 60 | htmlcov/ 61 | .tox/ 62 | .coverage 63 | .coverage.* 64 | .cache 65 | nosetests.xml 66 | coverage.xml 67 | *,cover 68 | .hypothesis/ 69 | 70 | # Translations 71 | *.mo 72 | *.pot 73 | 74 | # Django stuff: 75 | *.log 76 | 77 | # Sphinx documentation 78 | docs/_build/ 79 | 80 | # PyBuilder 81 | target/ 82 | 83 | 84 | 85 | 86 | # JETBRAINS 87 | 88 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm 89 | 90 | *.iml 91 | 92 | ## Directory-based project format: 93 | .idea/ 94 | # if you remove the above rule, at least ignore the following: 95 | 96 | # User-specific stuff: 97 | # .idea/workspace.xml 98 | # .idea/tasks.xml 99 | # .idea/dictionaries 100 | # .idea/shelf 101 | 102 | # Sensitive or high-churn files: 103 | # .idea/dataSources.ids 104 | # .idea/dataSources.xml 105 | # .idea/sqlDataSources.xml 106 | # .idea/dynamic.xml 107 | # .idea/uiDesigner.xml 108 | 109 | # Gradle: 110 | # .idea/gradle.xml 111 | # .idea/libraries 112 | 113 | # Mongo Explorer plugin: 114 | # .idea/mongoSettings.xml 115 | 116 | ## File-based project format: 117 | *.ipr 118 | *.iws 119 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v4.6.0 4 | hooks: 5 | - id: check-ast # Is it valid Python? 6 | - id: debug-statements # no debbuging statements used 7 | - id: trailing-whitespace 8 | - id: end-of-file-fixer 9 | - id: check-yaml 10 | - id: check-added-large-files 11 | - id: check-case-conflict 12 | # - id: check-executables-have-shebangs 13 | - id: check-json 14 | - id: pretty-format-json 15 | args: [ "--autofix" ] 16 | - id: check-merge-conflict 17 | - id: check-docstring-first 18 | - id: requirements-txt-fixer 19 | # - id: detect-aws-credentials 20 | - id: detect-private-key 21 | 22 | - repo: https://github.com/astral-sh/ruff-pre-commit 23 | rev: v0.3.5 24 | hooks: 25 | # linter. 26 | - id: ruff 27 | args: [ --fix ] 28 | - id: ruff-format 29 | 30 | - repo: https://github.com/asottile/blacken-docs 31 | rev: 1.16.0 32 | hooks: 33 | - id: blacken-docs 34 | additional_dependencies: [ black ] 35 | 36 | - repo: https://github.com/pycqa/flake8 37 | rev: 7.0.0 38 | hooks: 39 | - id: flake8 40 | exclude: ^(docs|scripts|tests)/ 41 | # E203 whitespace before ':' 42 | args: 43 | - --max-line-length=120 44 | - --ignore=E203 45 | additional_dependencies: 46 | - flake8-bugbear 47 | - flake8-comprehensions 48 | - flake8-tidy-imports 49 | 50 | - repo: https://github.com/mgedmin/check-manifest 51 | rev: "0.49" 52 | hooks: 53 | - id: check-manifest 54 | args: [ "--no-build-isolation", "--ignore", "*.png,docs/*,publish.py,readthedocs.yml,poetry.lock,setup.py,scripts/*" ] 55 | additional_dependencies: [ numpy, poetry>=1.4 ] 56 | 57 | - repo: https://github.com/asottile/pyupgrade 58 | rev: v3.15.2 59 | hooks: 60 | - id: pyupgrade 61 | 62 | # TODO enable for very detailed linting: 63 | # - repo: https://github.com/pycqa/pylint 64 | # rev: pylint-2.6.0 65 | # hooks: 66 | # - id: pylint 67 | -------------------------------------------------------------------------------- /extremitypathfinder/command_line.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | from extremitypathfinder import PolygonEnvironment 4 | from extremitypathfinder.configs import BOUNDARY_JSON_KEY, HOLES_JSON_KEY 5 | from extremitypathfinder.utils import read_json 6 | 7 | JSON_HELP_MSG = ( 8 | "path to the JSON file to be read. " 9 | f'The JSON file must have 2 keys: "{BOUNDARY_JSON_KEY}" and "{HOLES_JSON_KEY}".' 10 | 'Example: (also see "example.json")' 11 | '{ "boundary":[[0.0, 0.0], [10.0, 0.0],[9.0, 5.0], [10.0, 10.0], [0.0, 10.0]],' 12 | '"holes": [[[3.0, 7.0], [5.0, 9.0], [4.5, 7.0], [5.0, 4.0]],' 13 | "[[1.0, 2.0], [2.0, 2.0], [2.0, 1.0], [1.0, 1.0]]] }" 14 | f' The value assigned to the "{BOUNDARY_JSON_KEY}" key must be list containing ' 15 | "a list of length 2 for each coordinate pair." 16 | f' The value assigned to the "{HOLES_JSON_KEY}" key must be ' 17 | "a list of possibly multiple hole polygon lists." 18 | ) 19 | 20 | # environment = PlottingEnvironment() 21 | environment = PolygonEnvironment() 22 | 23 | 24 | def main(): 25 | parser = argparse.ArgumentParser(description="parse extremitypathfinder parameters") 26 | parser.add_argument("path2json_file", type=str, help=JSON_HELP_MSG) 27 | parser.add_argument( 28 | "-s", 29 | "--start", 30 | nargs=2, 31 | type=float, 32 | required=True, 33 | help="the start coordinates given as two float values, e.g. <2.5 3.2>", 34 | ) 35 | parser.add_argument( 36 | "-g", 37 | "--goal", 38 | nargs=2, 39 | type=float, 40 | required=True, 41 | help="the goal coordinates given as two float values, e.g. <7.9 6.8>", 42 | ) 43 | # TODO grid input requires different json data format 44 | # parser.add_argument('-g', '--grid', type=bool, help='weather the input data is a specifying a grid') 45 | parsed_args = parser.parse_args() # Takes input from sys.argv 46 | 47 | # Parse JSON file 48 | boundary_coordinates, list_of_holes = read_json(parsed_args.path2json_file) 49 | 50 | # Parse tuples from the input arguments (interpreted as lists): 51 | start_coordinates = tuple(parsed_args.start) 52 | goal_coordinates = tuple(parsed_args.goal) 53 | 54 | # Execute search for the given parameters 55 | environment.store(boundary_coordinates, list_of_holes, validate=False) 56 | path, distance = environment.find_shortest_path(start_coordinates, goal_coordinates) 57 | print(path, distance) 58 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: build 2 | 3 | on: 4 | # allows running this workflow manually from the Actions tab 5 | workflow_dispatch: 6 | pull_request: 7 | push: 8 | branches: 9 | - master 10 | 11 | jobs: 12 | test: 13 | runs-on: ubuntu-latest 14 | strategy: 15 | # By default, GitHub will maximize the number of jobs run in parallel 16 | # depending on the available runners on GitHub-hosted virtual machines. 17 | # max-parallel: 8 18 | fail-fast: false 19 | matrix: 20 | python-version: 21 | - "3.8" 22 | - "3.9" 23 | - "3.10" 24 | - "3.11" 25 | - "3.12" 26 | env: 27 | TOXENV: ${{ matrix.tox-env }} 28 | TOX_SKIP_MISSING_INTERPRETERS: False 29 | 30 | steps: 31 | - uses: actions/checkout@v6 32 | 33 | - name: Run pre-commit hook 34 | uses: pre-commit/action@v3.0.1 35 | 36 | - name: Set up Python ${{ matrix.python-version }} 37 | uses: actions/setup-python@v6 38 | with: 39 | python-version: ${{ matrix.python-version }} 40 | 41 | - name: Upgrade pip version 42 | run: pip install -U pip 43 | 44 | - name: Install test dependencies 45 | run: pip install tox tox-gh-actions poetry 46 | 47 | - name: Run tox 48 | run: tox 49 | 50 | deploy: 51 | runs-on: ubuntu-latest 52 | needs: test 53 | if: endsWith(github.ref, '/master') 54 | permissions: 55 | id-token: write 56 | contents: write 57 | steps: 58 | - uses: actions/checkout@v6 59 | 60 | - name: Set up Python 61 | uses: actions/setup-python@v6 62 | with: 63 | python-version: 3.8 64 | 65 | - name: Install build dependencies 66 | run: | 67 | pip install poetry 68 | 69 | - name: Fetch version 70 | id: fetch_version 71 | run: echo "version_nr=$(poetry version -s)" >> $GITHUB_OUTPUT 72 | 73 | - name: Build a binary wheel and a source tarball 74 | run: | 75 | poetry build --no-interaction 76 | 77 | - name: Create GitHub Release 78 | id: create_gh_release 79 | uses: ncipollo/release-action@v1 80 | env: 81 | VERSION: ${{ steps.fetch_version.outputs.version_nr }} 82 | with: 83 | tag: ${{env.VERSION}} 84 | name: Release ${{env.VERSION}} 85 | draft: false 86 | prerelease: false 87 | skipIfReleaseExists: true 88 | 89 | - name: PyPI Publishing 90 | uses: pypa/gh-action-pypi-publish@release/v1 91 | with: 92 | password: ${{ secrets.PYPI_DEPLOYMENT_API_KEY }} 93 | skip-existing: true 94 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | =================== 2 | extremitypathfinder 3 | =================== 4 | 5 | .. 6 | Note: can't include the badges file from the docs here, as it won't render on PyPI -> sync manually 7 | 8 | 9 | .. image:: https://github.com/jannikmi/extremitypathfinder/actions/workflows/build.yml/badge.svg?branch=master 10 | :target: https://github.com/jannikmi/extremitypathfinder/actions?query=branch%3Amaster 11 | 12 | .. image:: https://readthedocs.org/projects/extremitypathfinder/badge/?version=latest 13 | :alt: documentation status 14 | :target: https://extremitypathfinder.readthedocs.io/en/latest/?badge=latest 15 | 16 | .. image:: https://img.shields.io/pypi/wheel/extremitypathfinder.svg 17 | :target: https://pypi.python.org/pypi/extremitypathfinder 18 | 19 | .. image:: https://img.shields.io/badge/pre--commit-enabled-brightgreen?logo=pre-commit&logoColor=white 20 | :target: https://github.com/pre-commit/pre-commit 21 | :alt: pre-commit 22 | 23 | .. image:: https://pepy.tech/badge/extremitypathfinder 24 | :alt: Total PyPI downloads 25 | :target: https://pepy.tech/project/extremitypathfinder 26 | 27 | .. image:: https://img.shields.io/pypi/v/extremitypathfinder.svg 28 | :alt: latest version on PyPI 29 | :target: https://pypi.python.org/pypi/extremitypathfinder 30 | 31 | .. image:: https://img.shields.io/badge/code%20style-black-000000.svg 32 | :target: https://github.com/psf/black 33 | 34 | python package for fast geometric shortest path computation in 2D multi-polygon or grid environments based on visibility graphs. 35 | 36 | 37 | .. image:: ./docs/_static/title_demo_plot.png 38 | 39 | 40 | Quick Guide: 41 | 42 | Install the package with the optional Numba extra for a significant speedup: 43 | 44 | .. code-block:: console 45 | 46 | pip install extremitypathfinder[numba] 47 | 48 | 49 | .. code-block:: python 50 | 51 | from extremitypathfinder import PolygonEnvironment 52 | 53 | environment = PolygonEnvironment() 54 | # counter clockwise vertex numbering! 55 | boundary_coordinates = [(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)] 56 | # clockwise numbering! 57 | list_of_holes = [ 58 | [ 59 | (3.0, 7.0), 60 | (5.0, 9.0), 61 | (4.5, 7.0), 62 | (5.0, 4.0), 63 | ], 64 | ] 65 | environment.store(boundary_coordinates, list_of_holes, validate=False) 66 | start_coordinates = (4.5, 1.0) 67 | goal_coordinates = (4.0, 8.5) 68 | path, length = environment.find_shortest_path(start_coordinates, goal_coordinates) 69 | 70 | 71 | For more refer to the `documentation `__. 72 | 73 | 74 | Also see: 75 | `GitHub `__, 76 | `PyPI `__ 77 | -------------------------------------------------------------------------------- /CONTRIBUTING.rst: -------------------------------------------------------------------------------- 1 | ======================= 2 | Contribution Guidelines 3 | ======================= 4 | 5 | Contributions are welcome, and they are greatly appreciated! Every little bit 6 | helps, and credit will always be given. 7 | 8 | You can contribute in many ways: 9 | 10 | Types of Contributions 11 | ---------------------- 12 | 13 | Report Bugs 14 | ~~~~~~~~~~~ 15 | 16 | Report bugs via `Github Issues`_. 17 | 18 | If you are reporting a bug, please include: 19 | 20 | * Your version of this package, python and Numba (if you use it) 21 | * Any other details about your local setup that might be helpful in troubleshooting, e.g. operating system. 22 | * Detailed steps to reproduce the bug. 23 | * Detailed description of the bug (error log etc.). 24 | 25 | 26 | Fix Bugs 27 | ~~~~~~~~ 28 | 29 | Look through the GitHub issues for bugs. Anything tagged with "bug" is open to whoever wants to implement it. 30 | 31 | Implement Features 32 | ~~~~~~~~~~~~~~~~~~ 33 | 34 | Look through the GitHub issues for features. Anything tagged with "help wanted" 35 | and not assigned to anyone is open to whoever wants to implement it - please 36 | leave a comment to say you have started working on it, and open a pull request 37 | as soon as you have something working, so that Travis starts building it. 38 | 39 | Issues without "help wanted" generally already have some code ready in the 40 | background (maybe it's not yet open source), but you can still contribute to 41 | them by saying how you'd find the fix useful, linking to known prior art, or 42 | other such help. 43 | 44 | Write Documentation 45 | ~~~~~~~~~~~~~~~~~~~ 46 | 47 | Probably for some features the documentation is missing or unclear. You can help with that! 48 | 49 | 50 | Submit Feedback 51 | ~~~~~~~~~~~~~~~ 52 | 53 | The best way to send feedback is to file an issue via `Github Issues`_. 54 | 55 | If you are proposing a feature: 56 | 57 | * Explain in detail how it would work. 58 | * Keep the scope as narrow as possible, to make it easier to implement. Create multiple issues if necessary. 59 | * Remember that this is a volunteer-driven project, and that contributions are welcome :) 60 | 61 | 62 | Get Started! 63 | ------------ 64 | 65 | Ready to contribute? Here's how to set up this package for local development. 66 | 67 | * Fork this repo on GitHub. 68 | * Clone your fork locally 69 | 70 | * To make changes, create a branch for local development: 71 | 72 | .. code-block:: sh 73 | 74 | $ git checkout -b name-of-your-bugfix-or-feature 75 | 76 | 77 | 78 | * Check out the instructions and notes in ``publish.py`` 79 | * Install ``tox`` and run the tests: 80 | 81 | .. code-block:: sh 82 | 83 | $ pip install tox 84 | $ tox 85 | 86 | The ``tox.ini`` file defines a large number of test environments, for 87 | different Python etc., plus for checking codestyle. During 88 | development of a feature/fix, you'll probably want to run just one plus the 89 | relevant codestyle: 90 | 91 | .. code-block:: sh 92 | 93 | $ tox -e codestyle 94 | 95 | 96 | * Commit your changes and push your branch to GitHub: 97 | 98 | .. code-block:: sh 99 | 100 | $ git add . 101 | $ git commit -m "Your detailed description of your changes." 102 | $ git push origin name-of-your-bugfix-or-feature 103 | 104 | * Submit a pull request through the GitHub website. This will trigger the Travis CI build which runs the tests against all supported versions of Python. 105 | 106 | 107 | 108 | .. _Github Issues: https://github.com/MrMinimal64/extremitypathfinder/issues 109 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | name = "extremitypathfinder" 3 | version = "2.7.2" 4 | license = "MIT" 5 | readme = "README.rst" 6 | repository = "https://github.com/jannikmi/extremitypathfinder" 7 | homepage = "https://extremitypathfinder.readthedocs.io/en/latest/" 8 | documentation = "https://extremitypathfinder.readthedocs.io/en/latest/" 9 | keywords = ["path-planning", "path-finding", "shortest-path", "visibility", "graph", "polygon", "grid", "map", "robotics", "navigation", "offline"] 10 | classifiers = [ 11 | "Development Status :: 5 - Production/Stable", 12 | "Intended Audience :: Developers", 13 | "Intended Audience :: Information Technology", 14 | "Intended Audience :: Science/Research", 15 | "Natural Language :: English", 16 | "Operating System :: OS Independent", 17 | "Topic :: Scientific/Engineering", 18 | "Topic :: Scientific/Engineering :: Artificial Intelligence", 19 | "Topic :: Education", 20 | "Topic :: Games/Entertainment" 21 | ] 22 | description = "python package for fast shortest path computation on 2D polygon or grid maps" 23 | authors = ["jannikmi "] 24 | include = [ 25 | "LICENSE", 26 | ".editorconfig", 27 | ".pre-commit-config.yaml", 28 | "CHANGELOG.rst", 29 | "CONTRIBUTING.rst", 30 | "Makefile", 31 | "README.rst", 32 | "tox.ini", 33 | "tests/*.py", 34 | "example.json", 35 | ] 36 | #exclude = ["my_package/excluded.py"] 37 | 38 | [tool.poetry.scripts] 39 | extremitypathfinder = "extremitypathfinder.command_line:main" 40 | 41 | [tool.poetry.dependencies] 42 | python = ">=3.8,<4" 43 | networkx = "^3" 44 | numpy = [ 45 | { version = ">=1.21,<2", python = "<3.9" }, 46 | { version = ">=1.23,<2", python = ">=3.9" } 47 | ] 48 | numba = [ 49 | { version = ">=0.56,<1", python = "<3.12", optional = true }, 50 | { version = ">=0.59,<1", python = ">=3.12", optional = true } 51 | ] 52 | # required for jit of np.linalg.solve with numba 53 | scipy = [ 54 | { version = ">=1.9,<2", python = "<3.12", optional = true }, 55 | { version = ">=1.10,<2", python = ">=3.12", optional = true } 56 | ] 57 | 58 | [tool.poetry.group.dev.dependencies] 59 | pre-commit = "*" 60 | pytest = "*" 61 | tox = "*" 62 | 63 | [tool.poetry.group.plot.dependencies] 64 | # also required for runnin the tests 65 | matplotlib = "*" 66 | 67 | [tool.poetry.group.docs.dependencies] 68 | Sphinx = "*" 69 | sphinx-rtd-theme = "*" 70 | 71 | [tool.poetry.group.plot] 72 | optional = true 73 | 74 | [tool.poetry.group.docs] 75 | optional = true 76 | 77 | 78 | [tool.poetry.extras] 79 | numba = ["numba", "scipy"] 80 | 81 | [build-system] 82 | requires = ["poetry-core>=1.5", "poetry>=1.4"] 83 | build-backend = "poetry.core.masonry.api" 84 | 85 | [tool.tox] 86 | legacy_tox_ini = """ 87 | [tox] 88 | isolated_build = true 89 | envlist = 90 | docs,py{38,39,310,311,312}{,-numba} 91 | 92 | [gh-actions] 93 | python = 94 | 3.8: py38{,-numba} 95 | 3.9: py39{,-numba} 96 | 3.10: py310{,-numba} 97 | 3.11: py311{,-numba} 98 | 3.12: py312{,-numba} 99 | 100 | 101 | [testenv:docs] 102 | description = build documentation 103 | basepython = python3.12 104 | allowlist_externals = poetry,sphinx-build 105 | commands = 106 | poetry install -v --with docs 107 | sphinx-build -d "{envtmpdir}{/}doctree" docs "{toxworkdir}{/}docs_out" --color -b html 108 | python -c 'print(r"documentation available under file://{toxworkdir}{/}docs_out{/}index.html")' 109 | 110 | [testenv] 111 | allowlist_externals = poetry 112 | commands = 113 | !numba: poetry install -v --with dev,plot 114 | numba: poetry install -v --with dev,plot --extras numba 115 | poetry run pytest {posargs} 116 | """ 117 | -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 6 | 7 | 8 | # -- Path setup -------------------------------------------------------------- 9 | 10 | # If extensions (or modules to document with autodoc) are in another directory, 11 | # add these directories to sys.path here. If the directory is relative to the 12 | # documentation root, use os.path.abspath to make it absolute, like shown here. 13 | 14 | import os 15 | import subprocess 16 | import sys 17 | 18 | # Get the project root dir, which is the parent dir of this 19 | 20 | cwd = os.getcwd() 21 | project_root = os.path.dirname(cwd) 22 | 23 | # Insert the project root dir as the first element in the PYTHONPATH. 24 | # This ensures that the source package is importable 25 | sys.path.insert(0, os.path.join(project_root)) 26 | 27 | # needed for auto document, ATTENTION: must then be installed during online build! 28 | import extremitypathfinder # noqa: E402 Module level import not at top of file 29 | 30 | print(extremitypathfinder) 31 | 32 | # -- Project information ----------------------------------------------------- 33 | 34 | project = "extremitypathfinder" 35 | copyright = "2018, Jannik Michelfeit" 36 | author = "Jannik Michelfeit" 37 | 38 | # The full version, including alpha/beta/rc tags. 39 | release = subprocess.getoutput("poetry version -s") 40 | print("release version:", release) 41 | 42 | # -- General configuration --------------------------------------------------- 43 | 44 | # Add any Sphinx extension module names here, as strings. They can be 45 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 46 | # ones. 47 | extensions = [ 48 | "sphinx.ext.autodoc", # automatically document with docstring 49 | "sphinx.ext.viewcode", 50 | # 'sphinx.ext.intersphinx', # to auto link to other online documentations 51 | ] 52 | 53 | autodoc_default_options = { 54 | "members": "__all__", 55 | "member-order": "bysource", 56 | "special-members": "__init__", 57 | "undoc-members": True, 58 | "exclude-members": "__weakref__", 59 | "show-inheritance": True, 60 | "inherited-members": True, 61 | } 62 | 63 | # Add any paths that contain templates here, relative to this directory. 64 | templates_path = ["_templates"] 65 | 66 | # The suffix of source filenames. 67 | source_suffix = ".rst" 68 | 69 | # The master toctree document. 70 | master_doc = "index" 71 | 72 | # List of patterns, relative to source directory, that match files and 73 | # directories to ignore when looking for source files. 74 | # This pattern also affects html_static_path and html_extra_path. 75 | exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] 76 | 77 | # The name of the Pygments (syntax highlighting) style to use. 78 | pygments_style = "sphinx" 79 | 80 | # The reST default role (used for this markup: `text`) to use for all 81 | # documents. 82 | # default_role = None 83 | 84 | # If true, '()' will be appended to :func: etc. cross-reference text. 85 | # add_function_parentheses = True 86 | 87 | # If true, the current module name will be prepended to all description 88 | # unit titles (such as .. function::). 89 | # add_module_names = False 90 | 91 | # If true, sectionauthor and moduleauthor directives will be shown in the 92 | # output. They are ignored by default. 93 | # show_authors = False 94 | 95 | # A list of ignored prefixes for module index sorting. 96 | # modindex_common_prefix = ["multivar_horner."] 97 | 98 | # If true, keep warnings as "system message" paragraphs in the built 99 | # documents. 100 | # keep_warnings = False 101 | 102 | # -- Options for HTML output ------------------------------------------------- 103 | 104 | # The theme to use for HTML and HTML Help pages. See the documentation for 105 | # a list of builtin themes. 106 | 107 | html_theme = "sphinx_rtd_theme" 108 | 109 | # Add any paths that contain custom static files (such as style sheets) here, 110 | # relative to this directory. They are copied after the builtin static files, 111 | # so a file named "default.css" will overwrite the builtin "default.css". 112 | html_static_path = ["_static"] 113 | 114 | # https://github.com/adamchainz/django-mysql/blob/master/docs/conf.py 115 | # -- Options for LaTeX output ------------------------------------------ 116 | 117 | # -- Options for manual page output ------------------------------------ 118 | 119 | # -- Options for Texinfo output ---------------------------------------- 120 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | Changelog 2 | ========= 3 | 4 | 5 | 2.7.2 (2024-04-10) 6 | ------------------- 7 | 8 | - added support for python 3.12 9 | 10 | internal: 11 | 12 | - added tests for python 3.12 13 | - use ruff pre-commit hooks 14 | - made dependency groups docs and plot optional 15 | - added tox tes for documentation build 16 | 17 | 18 | 19 | 2.7.1 (2023-05-16) 20 | ------------------- 21 | 22 | internal: 23 | 24 | - JIT compile more utility functions (including numpy.linalg.solve) 25 | - add scipy dependency for JIT compiling numpy.linalg.solve 26 | - updated supported python versions to ">=3.8,<3.12" (required by scipy) 27 | - remove debug print statements and assertions 28 | 29 | 30 | 2.7.0 (2023-06-08) 31 | ------------------- 32 | 33 | - optional Numba JIT compilation offering significant speedup 34 | - extra: `pip install extremitypathfinder[numba]` 35 | 36 | 37 | 2.6.0 (2023-06-04) 38 | ------------------- 39 | 40 | internal: 41 | 42 | * implemented an optimised visibility graph algorithm: sort edges and candidates after their representation to always only check the relevant fraction of candidates for each edge. Runtime complexity O(n^2 log_2 n). 43 | * added visibility computation tests 44 | * automatically skip GitHub actions publishing when the version already exists. useful for minor improvements without publishing a version. build would always fail otherwise 45 | * updated pinned dependencies to fix security alerts 46 | * minor code refactoring 47 | 48 | 49 | 2.5.0 (2023-05-05) 50 | ------------------- 51 | 52 | * removed need for separate ``.prepare()`` call. Storing environment boundary data automatically triggers the preparation of the visibility graph. This is a non-breaking change. The ``.prepare()`` method is still available, but it is not needed anymore. 53 | 54 | internal: 55 | 56 | * updated dependency specification: networkx>=3, relaxed development dependency version requirements 57 | * included tests for python 3.11 58 | * minor code refactoring 59 | 60 | 61 | 2.4.1 (2022-08-22) 62 | ------------------- 63 | 64 | * bugfix: catch the case where no path is possible in the graph in the ``networkx`` A* implementation 65 | * added speed benchmarks and performance section in the documentation with benchmark results 66 | 67 | internal: 68 | 69 | * optimisation: checking edges with the biggest angle range first 70 | * optimisation: skipping visibility checks for the last extremity 71 | * using optimised point in polygon check algorithm 72 | * using undirected Graph: The precomputed graph usually makes up the majority of the visibility graph (in comparison to the temporarily added edges for query start and goal nodes) and this precomputed part has to be undirected. Use undirected graph everywhere. 73 | * added test cases 74 | 75 | 76 | 2.4.0 (2022-08-18) 77 | ------------------- 78 | 79 | * A* and graph representation based on ``networkx`` library -> new dependency 80 | 81 | 82 | 83 | 2.3.0 (2022-08-18) 84 | ------------------- 85 | 86 | * major overhaul of all functionality from OOP to functional programming/numpy based 87 | 88 | internal: 89 | 90 | * added test cases 91 | 92 | 93 | 94 | 95 | 2.2.3 (2022-10-11) 96 | ------------------- 97 | 98 | * reverting changes of version ``2.2.2`` 99 | 100 | 101 | 2.2.2 (2022-07-10) 102 | ------------------- 103 | 104 | * [DEPRECATED] 105 | 106 | 107 | 2.2.1 (2022-07-10) 108 | ------------------- 109 | 110 | * packaging completely based on ``pyproject.toml`` (poetry) 111 | * CI/CD: automatic publishing based on GitHub Actions 112 | 113 | 2.2.0 (2021-01-25) 114 | ------------------- 115 | 116 | * Included a command line interface 117 | * Improved testing routines and codestyle 118 | 119 | 120 | 2.1.0 (2021-01-07) 121 | ------------------ 122 | 123 | IMPORTANT BUGFIX: in some cases the visibility computation was faulty (fix #23) 124 | 125 | * added new test case 126 | 127 | 2.0.0 (2020-12-22) 128 | ------------------ 129 | 130 | * IMPROVEMENT: Different polygons may now intersect each other. Thanks to `Georg Hess `__! 131 | * BUGFIX: Fixed a bug that caused "dangling" extremities in the graph to be left out 132 | * ``TypeError`` and ``ValueError`` are being raised instead of ``AssertionError`` in case of invalid input parameters with ``validate=True``. Thanks to `Andrew Costello `__! 133 | 134 | 1.5.0 (2020-06-18) 135 | ------------------ 136 | 137 | * BUGFIX: fix #16. introduce unique ordering of A* search heap queue with custom class ``SearchState`` (internal) 138 | 139 | 140 | 1.4.0 (2020-05-25) 141 | ------------------ 142 | 143 | * BUGFIX: fix clockwise polygon numbering test (for input data validation, mentioned in #12) 144 | 145 | 146 | 147 | 1.3.0 (2020-05-19) 148 | ------------------ 149 | 150 | * FIX #11: added option ``verify`` to ``find_shortest_path()`` for skipping the 'within map' test for goal and start points 151 | 152 | 153 | 154 | 1.2.0 (2020-05-18) 155 | ------------------ 156 | 157 | * supporting only python 3.7+ 158 | * fix #10: Memory leak in DirectedHeuristicGraph 159 | * fix BUG where "dangling" extremities in the visibility graph would be deleted 160 | * using generators to refer to the polygon properties (vertices,...) of an environment (save memory and remove redundancy) 161 | * enabled plotting the test results, at the same time this is testing the plotting functionality 162 | * added typing 163 | 164 | internal: 165 | 166 | * added sphinx documentation, included auto api documentation, improved docstrings 167 | * added contribution guidelines 168 | * add sponsor button 169 | * updated publishing routine 170 | * split up requirement files (basic, tests) 171 | * specific tags for supported python versions in wheel 172 | * testing all different python versions with tox 173 | * added coverage tests 174 | * added editorconfig 175 | * specify version in VERSION file 176 | * added new tests 177 | 178 | 179 | 1.1.0 (2018-10-17) 180 | ------------------ 181 | 182 | * optimised A*-algorithm to not visit all neighbours of the current node before continuing 183 | 184 | 185 | 186 | 1.0.0 (2018-10-07) 187 | ------------------ 188 | 189 | * first stable public version 190 | 191 | 192 | 193 | 0.0.1 (2018-09-27) 194 | ------------------ 195 | 196 | * birth of this package 197 | -------------------------------------------------------------------------------- /docs/1_usage.rst: -------------------------------------------------------------------------------- 1 | .. _usage: 2 | 3 | ===== 4 | Usage 5 | ===== 6 | 7 | .. note:: 8 | 9 | Also check out the :ref:`API documentation ` or the `code `__. 10 | 11 | 12 | .. _init: 13 | 14 | Initialisation 15 | -------------- 16 | 17 | 18 | Create a new instance of the :ref:`PolygonEnvironment class ` to allow fast consequent timezone queries: 19 | 20 | .. code-block:: python 21 | 22 | from extremitypathfinder import PolygonEnvironment 23 | 24 | environment = PolygonEnvironment() 25 | 26 | 27 | 28 | Store environment 29 | ----------------- 30 | 31 | 32 | **Required data format:** 33 | Ensure that all the following conditions on the polygons are fulfilled: 34 | 35 | - numpy or python array of coordinate tuples: ``[(x1,y1), (x2,y2,)...]`` 36 | - the first point is NOT repeated at the end 37 | - must at least contain 3 vertices 38 | - no consequent vertices with identical coordinates in the polygons (same coordinates in general are allowed) 39 | - a polygon must NOT have self intersections 40 | - different polygons may intersect each other 41 | - edge numbering has to follow this convention (for easier computations): 42 | - outer boundary polygon: counter clockwise 43 | - holes: clockwise 44 | 45 | 46 | .. code-block:: python 47 | 48 | # counter clockwise vertex numbering! 49 | boundary_coordinates = [(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)] 50 | 51 | # clockwise numbering! 52 | list_of_holes = [ 53 | [ 54 | (3.0, 7.0), 55 | (5.0, 9.0), 56 | (4.5, 7.0), 57 | (5.0, 4.0), 58 | ], 59 | ] 60 | environment.store(boundary_coordinates, list_of_holes, validate=False) 61 | 62 | 63 | .. note:: 64 | 65 | Pass ``validate=True`` in order to check the condition on the data. 66 | Raises ``TypeError`` if the input has the wrong type and ``ValueError`` if the input is invalid. 67 | 68 | 69 | .. note:: 70 | 71 | If two Polygons have vertices with identical coordinates (this is allowed), paths through these vertices are theoretically possible! 72 | When the paths should be blocked, use a single polygon with multiple identical vertices instead (also allowed). 73 | 74 | 75 | .. figure:: _static/map_plot.png 76 | 77 | polygon environment with extremities marked in red 78 | 79 | 80 | Visibility Graph Pre-computation 81 | -------------------------------- 82 | 83 | Storing the map properties automatically computes the :ref:`visibility graph ` of the environment once. 84 | 85 | 86 | .. figure:: _static/prepared_map_plot.png 87 | 88 | polygon environment with optimised visibility graph overlay in red 89 | 90 | 91 | Query 92 | ----- 93 | 94 | 95 | .. code-block:: python 96 | 97 | start_coordinates = (4.5, 1.0) 98 | goal_coordinates = (4.0, 8.5) 99 | path, length = environment.find_shortest_path(start_coordinates, goal_coordinates) 100 | 101 | 102 | If any start and goal point should be accepted without checking if they lie within the map, set ``verify=False``. 103 | This is required if points lie really close to polygon edges and 104 | "point in polygon" algorithms might return an unexpected result due to rounding errors. 105 | 106 | .. code-block:: python 107 | 108 | path, length = environment.find_shortest_path( 109 | start_coordinates, goal_coordinates, verify=False 110 | ) 111 | 112 | 113 | .. figure:: _static/graph_path_plot.png 114 | 115 | polygon environment with optimised visibility graph overlay. visualised edges added to the visibility graph in yellow, found shortest path in green. 116 | 117 | 118 | 119 | Converting and storing a grid world 120 | ----------------------------------- 121 | 122 | 123 | .. code-block:: python 124 | 125 | size_x, size_y = 19, 10 126 | obstacle_iter = [ # (x,y), 127 | # obstacles changing boundary 128 | (0, 1), 129 | (1, 1), 130 | (2, 1), 131 | (3, 1), 132 | (17, 9), 133 | (17, 8), 134 | (17, 7), 135 | (17, 5), 136 | (17, 4), 137 | (17, 3), 138 | (17, 2), 139 | (17, 1), 140 | (17, 0), 141 | # hole 1 142 | (5, 5), 143 | (5, 6), 144 | (6, 6), 145 | (6, 7), 146 | (7, 7), 147 | # hole 2 148 | (7, 5), 149 | ] 150 | environment.store_grid_world( 151 | size_x, size_y, obstacle_iter, simplify=False, validate=False 152 | ) 153 | 154 | 155 | 156 | .. figure:: _static/grid_map_plot.png 157 | 158 | grid-like environment converted to a polygon environment with "extremities" marked in red 159 | 160 | 161 | **Note:** As mentioned in 162 | `[1, Ch. III 6.3] `__ 163 | in "chessboard-like grid worlds" (many small obstacles have a lot of extremities!) 164 | it can be better to use A* right away (implemented in ``graph_search.py``). 165 | 166 | 167 | Cache and import the environment 168 | -------------------------------- 169 | 170 | 171 | .. code-block:: python 172 | 173 | environment.export_pickle(path="./pickle_file.pickle") 174 | 175 | from extremitypathfinder.extremitypathfinder import load_pickle 176 | 177 | environment = load_pickle(path="./pickle_file.pickle") 178 | 179 | 180 | 181 | Plotting 182 | -------- 183 | 184 | 185 | The class ``PlottingEnvironment`` automatically generates plots for every step in the path finding process: 186 | 187 | .. code-block:: python 188 | 189 | from extremitypathfinder.plotting import PlottingEnvironment 190 | 191 | environment = PlottingEnvironment(plotting_dir="path/to/plots") 192 | environment.store(boundary_coordinates, list_of_holes, validate=True) 193 | path, distance = environment.find_shortest_path(start, end) 194 | 195 | 196 | Other functions in ``plotting.py`` can be utilised to plot specific parts of an environment (extremities, edges, ...) 197 | 198 | 199 | 200 | Calling extremitypathfinder from the command line 201 | ------------------------------------------------- 202 | 203 | A command line script is being installed as part of this package. 204 | 205 | **Command Line Syntax**: 206 | 207 | :: 208 | 209 | extremitypathfinder -s -g 210 | 211 | The ```` and ```` arguments must be passed as two separate float values. 212 | 213 | **Example**: 214 | 215 | :: 216 | 217 | extremitypathfinder ./example.json -s 2.5 3.2 -g 7.9 6.8 218 | 219 | This returns ``[(2.5, 3.2), (5.0, 4.0), (7.9, 6.8)] 6.656009823830612`` 220 | 221 | Please note that this might be significantly slower than using the package directly from within python. 222 | -------------------------------------------------------------------------------- /extremitypathfinder/plotting.py: -------------------------------------------------------------------------------- 1 | import time 2 | from os import makedirs 3 | from os.path import abspath, exists, join 4 | 5 | import matplotlib.pyplot as plt 6 | from matplotlib.patches import Polygon 7 | 8 | from extremitypathfinder import types as t 9 | from extremitypathfinder.extremitypathfinder import PolygonEnvironment 10 | 11 | EXPORT_RESOLUTION = 200 # dpi 12 | EXPORT_SIZE_X = 19.0 # inch 13 | EXPORT_SIZE_Y = 11.0 # inch 14 | 15 | POLYGON_SETTINGS = { 16 | "edgecolor": "black", 17 | "fill": False, 18 | "linewidth": 1.0, 19 | } 20 | 21 | SHOW_PLOTS = False 22 | PLOTTING_DIR = "all_plots" 23 | PLOT_FILE_ENDING = ".svg" 24 | 25 | 26 | def get_plot_name(file_name="plot"): 27 | return abspath( 28 | join(PLOTTING_DIR, file_name + "_" + str(time.time())[:-7] + PLOT_FILE_ENDING) 29 | ) 30 | 31 | 32 | def export_plot(fig, file_name): 33 | fig.set_size_inches(EXPORT_SIZE_X, EXPORT_SIZE_Y, forward=True) 34 | plt.savefig(get_plot_name(file_name), dpi=EXPORT_RESOLUTION) 35 | plt.close() 36 | 37 | 38 | def mark_points(vertex_iter, **kwargs): 39 | try: 40 | coordinates = [v.tolist() for v in vertex_iter] 41 | except AttributeError: 42 | coordinates = list(vertex_iter) 43 | coords_zipped = list(zip(*coordinates)) 44 | if coords_zipped: # there might be no vertices at all 45 | plt.scatter(*coords_zipped, **kwargs) 46 | 47 | 48 | def draw_edge(v1, v2, c, alpha, **kwargs): 49 | x1, y1 = v1 50 | x2, y2 = v2 51 | plt.plot([x1, x2], [y1, y2], color=c, alpha=alpha, **kwargs) 52 | 53 | 54 | def draw_polygon(ax, coords, **kwargs): 55 | kwargs.update(POLYGON_SETTINGS) 56 | polygon = Polygon(coords, **kwargs) 57 | ax.add_patch(polygon) 58 | 59 | 60 | def draw_boundaries(map, ax): 61 | # TODO outside dark grey 62 | # TODO fill holes light grey 63 | draw_polygon(ax, map.boundary_polygon) 64 | for h in map.holes: 65 | draw_polygon(ax, h, facecolor="grey", fill=True) 66 | 67 | mark_points(map.all_vertices, c="black", s=15) 68 | mark_points(map.all_extremities, c="red", s=50) 69 | 70 | 71 | def draw_internal_graph(map: PolygonEnvironment, ax): 72 | graph = map.graph 73 | coords = map.coords 74 | for n in graph.nodes: 75 | start = coords[n] 76 | all_goals = [coords[i] for i in graph.neighbors(n)] 77 | for goal in all_goals: 78 | draw_edge(start, goal, c="red", alpha=0.2, linewidth=2) 79 | 80 | 81 | def set_limits(map, ax): 82 | ax.set_xlim( 83 | ( 84 | min(map.boundary_polygon[:, 0]) - 1, 85 | max(map.boundary_polygon[:, 0]) + 1, 86 | ) 87 | ) 88 | ax.set_ylim( 89 | ( 90 | min(map.boundary_polygon[:, 1]) - 1, 91 | max(map.boundary_polygon[:, 1]) + 1, 92 | ) 93 | ) 94 | 95 | 96 | def draw_path(vertex_path): 97 | # start, path and goal in green 98 | if not vertex_path: 99 | return 100 | mark_points(vertex_path, c="g", alpha=0.9, s=50) 101 | mark_points([vertex_path[0], vertex_path[-1]], c="g", s=100) 102 | v1 = vertex_path[0] 103 | for v2 in vertex_path[1:]: 104 | draw_edge(v1, v2, c="g", alpha=1.0) 105 | v1 = v2 106 | 107 | 108 | def draw_loaded_map(map): 109 | fig, ax = plt.subplots() 110 | 111 | draw_boundaries(map, ax) 112 | set_limits(map, ax) 113 | export_plot(fig, "map_plot") 114 | if SHOW_PLOTS: 115 | plt.show() 116 | 117 | 118 | def draw_prepared_map(map): 119 | fig, ax = plt.subplots() 120 | 121 | draw_boundaries(map, ax) 122 | draw_internal_graph(map, ax) 123 | set_limits(map, ax) 124 | export_plot(fig, "prepared_map_plot") 125 | if SHOW_PLOTS: 126 | plt.show() 127 | 128 | 129 | def draw_with_path(map, graph: t.Graph, vertex_path): 130 | fig, ax = plt.subplots() 131 | 132 | coords = map._coords_tmp 133 | all_nodes = graph.nodes 134 | draw_boundaries(map, ax) 135 | draw_internal_graph(map, ax) 136 | set_limits(map, ax) 137 | 138 | if len(vertex_path) > 0: 139 | # additionally draw: 140 | # new edges yellow 141 | start, goal = vertex_path[0], vertex_path[-1] 142 | goal_idx = map._idx_goal_tmp 143 | start_idx = map._idx_start_tmp 144 | 145 | if start_idx is not None: 146 | for n_idx in graph.neighbors(start_idx): 147 | n = coords[n_idx] 148 | draw_edge(start, n, c="y", alpha=0.7) 149 | 150 | if goal_idx is not None: 151 | # edges only run towards goal 152 | for n_idx in all_nodes: 153 | if goal_idx in graph.neighbors(n_idx): 154 | n = coords[n_idx] 155 | draw_edge(n, goal, c="y", alpha=0.7) 156 | 157 | # start, path and goal in green 158 | draw_path(vertex_path) 159 | 160 | export_plot(fig, "graph_path_plot") 161 | if SHOW_PLOTS: 162 | plt.show() 163 | 164 | 165 | def draw_only_path(map, vertex_path, start_coordinates, goal_coordinates): 166 | fig, ax = plt.subplots() 167 | 168 | draw_boundaries(map, ax) 169 | set_limits(map, ax) 170 | draw_path(vertex_path) 171 | mark_points([start_coordinates, goal_coordinates], c="g", s=100) 172 | 173 | export_plot(fig, "path_plot") 174 | if SHOW_PLOTS: 175 | plt.show() 176 | 177 | 178 | def draw_graph(map, graph: t.Graph): 179 | fig, ax = plt.subplots() 180 | 181 | nodes = graph.nodes 182 | coords = map._coords_tmp 183 | all_nodes = [coords[i] for i in nodes] 184 | mark_points(all_nodes, c="black", s=30) 185 | 186 | for i in nodes: 187 | x, y = coords[i] 188 | neighbour_idxs = graph.neighbors(i) 189 | for n2_idx in neighbour_idxs: 190 | x2, y2 = coords[n2_idx] 191 | dx, dy = x2 - x, y2 - y 192 | plt.arrow( 193 | x, 194 | y, 195 | dx, 196 | dy, 197 | head_width=0.15, 198 | head_length=0.5, 199 | head_starts_at_zero=False, 200 | shape="full", 201 | length_includes_head=True, 202 | ) 203 | 204 | set_limits(map, ax) 205 | 206 | export_plot(fig, "graph_plot") 207 | if SHOW_PLOTS: 208 | plt.show() 209 | 210 | 211 | class PlottingEnvironment(PolygonEnvironment): 212 | """Extends PolygonEnvironment. In addition to the base functionality it 213 | plots graphs of the polygons, the visibility graph and the computed path. 214 | Stores all graphs in the folder defined by plotting_dir parameter.""" 215 | 216 | def __init__(self, plotting_dir=PLOTTING_DIR): 217 | global PLOTTING_DIR 218 | PLOTTING_DIR = plotting_dir 219 | if not exists(plotting_dir): 220 | makedirs(plotting_dir) 221 | super().__init__() 222 | 223 | def store(self, *args, **kwargs): 224 | """In addition to storing, also plots a graph of the input polygons.""" 225 | super().store(*args, **kwargs) 226 | draw_loaded_map(self) 227 | 228 | def prepare(self): 229 | """Also draws a prepared map with the computed visibility graph.""" 230 | super().prepare() 231 | draw_prepared_map(self) 232 | 233 | def find_shortest_path(self, start_coordinates, goal_coordinates, *args, **kwargs): 234 | """Also draws the computed shortest path.""" 235 | # important to not delete the temp graph! for plotting 236 | vertex_path, distance = super().find_shortest_path( 237 | start_coordinates, goal_coordinates, *args, free_space_after=False, **kwargs 238 | ) 239 | 240 | draw_only_path(self, vertex_path, start_coordinates, goal_coordinates) 241 | if ( 242 | self.temp_graph 243 | ): # in some cases (e.g. direct path possible) no graph is being created! 244 | draw_graph(self, self.temp_graph) 245 | draw_with_path(self, self.temp_graph, vertex_path) 246 | del self.temp_graph # free the memory 247 | 248 | # extract the coordinates from the path 249 | return vertex_path, distance 250 | -------------------------------------------------------------------------------- /tests/main_test.py: -------------------------------------------------------------------------------- 1 | import itertools 2 | 3 | import pytest 4 | 5 | from extremitypathfinder import utils 6 | from extremitypathfinder.extremitypathfinder import PolygonEnvironment 7 | from extremitypathfinder.plotting import PlottingEnvironment 8 | from tests.helpers import other_edge_intersects 9 | from tests.test_cases import ( 10 | GRID_ENV_PARAMS, 11 | INVALID_DESTINATION_DATA, 12 | OVERLAP_POLY_ENV_PARAMS, 13 | POLY_ENV_PARAMS, 14 | POLYGON_ENVS, 15 | SEPARATED_ENV, 16 | TEST_DATA_GRID_ENV, 17 | TEST_DATA_OVERLAP_POLY_ENV, 18 | TEST_DATA_POLY_ENV, 19 | TEST_DATA_SEPARATE_ENV, 20 | ) 21 | 22 | # PLOT_TEST_RESULTS = True 23 | PLOT_TEST_RESULTS = False 24 | TEST_PLOT_OUTPUT_FOLDER = "plots" 25 | 26 | if PLOT_TEST_RESULTS: 27 | print("plotting test environment enabled.") 28 | ENVIRONMENT_CLASS = PlottingEnvironment 29 | CONSTRUCTION_KWARGS = {"plotting_dir": TEST_PLOT_OUTPUT_FOLDER} 30 | else: 31 | ENVIRONMENT_CLASS = PolygonEnvironment 32 | CONSTRUCTION_KWARGS = {} 33 | 34 | 35 | # TODO pytest parameterize 36 | 37 | 38 | def try_test_cases(environment, test_cases): 39 | def validate(start_coordinates, goal_coordinates, expected_output): 40 | output = environment.find_shortest_path( 41 | start_coordinates, goal_coordinates, verify=True 42 | ) 43 | path, length = output 44 | assert isinstance(path, list), "path should be a list" 45 | expected_path, expected_length = expected_output 46 | if expected_length is None: 47 | correct_result = length is None and path == expected_path 48 | else: 49 | correct_result = path == expected_path and length == pytest.approx( 50 | expected_length 51 | ) 52 | if correct_result: 53 | status_str = "OK" 54 | else: 55 | status_str = "XX" 56 | print(f"{status_str} input: {(start_coordinates, goal_coordinates)} ") 57 | assert correct_result, f"unexpected result (path, length): got {output} instead of {expected_output} " 58 | 59 | print("testing if path and distance are correct:") 60 | for (start_coordinates, goal_coordinates), expected_output in test_cases: 61 | validate(start_coordinates, goal_coordinates, expected_output) 62 | # automatically test reversed! 63 | path, length = expected_output 64 | expected_output_reversed = list(reversed(path)), length 65 | validate(goal_coordinates, start_coordinates, expected_output_reversed) 66 | 67 | 68 | def test_grid_env(): 69 | grid_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS) 70 | grid_env.store_grid_world(*GRID_ENV_PARAMS, simplify=False, validate=False) 71 | nr_extremities = len(grid_env.all_extremities) 72 | assert nr_extremities == 17, "extremities do not get detected correctly!" 73 | nr_graph_nodes = len(grid_env.graph.nodes) 74 | assert nr_graph_nodes == 16, "identical nodes should get joined in the graph!" 75 | 76 | # test if points outside the map are being rejected 77 | for start_coordinates, goal_coordinates in INVALID_DESTINATION_DATA: 78 | with pytest.raises(ValueError): 79 | grid_env.find_shortest_path(start_coordinates, goal_coordinates) 80 | 81 | print("testing grid environment") 82 | try_test_cases(grid_env, TEST_DATA_GRID_ENV) 83 | 84 | # when the deep copy mechanism works correctly 85 | # even after many queries the internal graph should have the same structure as before 86 | # otherwise the temporarily added vertices during a query stay stored 87 | # nr_graph_nodes = len(grid_env.graph.nodes) 88 | # TODO 89 | # assert nr_graph_nodes == 16, "the graph should stay unchanged by shortest path queries!" 90 | # nr_nodes_env1_old = len(grid_env.graph.nodes) 91 | 92 | 93 | def test_poly_env(): 94 | poly_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS) 95 | poly_env.store(*POLY_ENV_PARAMS, validate=True) 96 | nr_exp_extremities = 4 97 | assert ( 98 | len(list(poly_env.all_extremities)) == nr_exp_extremities 99 | ), f"the environment should detect all {nr_exp_extremities} extremities!" 100 | # nr_nodes_env2 = len(poly_env.graph.nodes) 101 | # TODO 102 | # assert nr_nodes_env2 == nr_exp_extremities, ( 103 | # f"the visibility graph should store all {nr_exp_extremities} extremities {list(poly_env.all_extremities)}!" 104 | # f"\n found: {poly_env.graph.nodes}" 105 | # ) 106 | 107 | # nr_nodes_env1_new = len(grid_env.graph.nodes) 108 | # assert ( 109 | # nr_nodes_env1_new == nr_nodes_env1_old 110 | # ), "node amount of an grid_env should not change by creating another grid_env!" 111 | # assert grid_env.graph is not poly_env.graph, "different environments share the same graph object" 112 | # assert ( 113 | # grid_env.graph.nodes is not poly_env.graph.nodes 114 | # ), "different environments share the same set of nodes" 115 | 116 | print("\ntesting polygon environment") 117 | try_test_cases(poly_env, TEST_DATA_POLY_ENV) 118 | 119 | # TODO test: When the paths should be blocked, use a single polygon with multiple identical 120 | # vertices instead (also allowed?! change data requirements in doc!). 121 | 122 | # TODO test graph construction 123 | # when two nodes have the same angle representation there should only be an edge to the closer node! 124 | # test if property 1 is being properly exploited 125 | # (extremities lying in front of each other need not be connected) 126 | 127 | 128 | def test_overlapping_polygon(): 129 | overlap_poly_env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS) 130 | overlap_poly_env.store(*OVERLAP_POLY_ENV_PARAMS) 131 | print("\ntesting polygon environment with overlapping polygons") 132 | try_test_cases(overlap_poly_env, TEST_DATA_OVERLAP_POLY_ENV) 133 | 134 | 135 | def test_separated_environment(): 136 | env = ENVIRONMENT_CLASS(**CONSTRUCTION_KWARGS) 137 | env.store(*SEPARATED_ENV) 138 | print("\ntesting polygon environment with two separated areas") 139 | try_test_cases(env, TEST_DATA_SEPARATE_ENV) 140 | 141 | 142 | @pytest.mark.parametrize( 143 | "env_data", 144 | POLYGON_ENVS, 145 | ) 146 | def test_extremity_neighbour_connection(env_data): 147 | # if two extremities are direct neighbours in a polygon, they also must be connected in the prepared graph 148 | # exception: there is another polygon edge intersecting that 149 | print("\ntesting if all two direct extremity neighbour are connected") 150 | env = PolygonEnvironment() 151 | env.store(*env_data) 152 | coords = env.coords 153 | graph = env.graph 154 | extremities = env.extremity_indices 155 | edge_vertex_idxs = env.edge_vertex_idxs 156 | 157 | def connection_as_expected(i1: int, i2: int): 158 | if i2 not in extremities: 159 | return 160 | should_be_connected = not other_edge_intersects( 161 | i1, i2, edge_vertex_idxs, coords 162 | ) 163 | graph_neighbors_e = set(graph.neighbors(i1)) 164 | are_connected = i2 in graph_neighbors_e 165 | assert should_be_connected == are_connected 166 | 167 | for e in extremities: 168 | n1, n2 = utils.get_neighbour_idxs(e, env.vertex_edge_idxs, edge_vertex_idxs) 169 | connection_as_expected(e, n1) 170 | connection_as_expected(e, n2) 171 | 172 | 173 | @pytest.mark.parametrize( 174 | "env_data", 175 | POLYGON_ENVS, 176 | ) 177 | def test_all_coords_work_as_input(env_data): 178 | # if two extremities are direct neighbours in a polygon, they also must be connected in the prepared graph 179 | # exception: there is another polygon edge intersecting that 180 | print("\ntesting if all two direct extremity neighbour are connected") 181 | env = PolygonEnvironment() 182 | env.store(*env_data) 183 | coords = env.coords 184 | nr_vertices = env.nr_vertices 185 | 186 | for start, goal in itertools.product(range(nr_vertices), repeat=2): 187 | coords_start = coords[start] 188 | coords_goal = coords[goal] 189 | 190 | if not env.within_map(coords_start): 191 | continue 192 | if not env.within_map(coords_goal): 193 | continue 194 | 195 | env.find_shortest_path(coords_start, coords_goal, verify=True) 196 | -------------------------------------------------------------------------------- /tests/helper_fcts_test.py: -------------------------------------------------------------------------------- 1 | """ 2 | TODO test find_visible(), ... 3 | TODO test if relation is really bidirectional (y in find_visible(x,y) <=> x in find_visible(y,x)) 4 | TODO test input data validation 5 | """ 6 | 7 | from os.path import abspath, join, pardir 8 | from typing import Dict, Set 9 | 10 | import numpy as np 11 | import pytest 12 | 13 | from extremitypathfinder import PolygonEnvironment, configs 14 | from extremitypathfinder.utils import ( 15 | _cmp_extremity_mask, 16 | _compute_repr_n_dist, 17 | _has_clockwise_numbering, 18 | _inside_polygon, 19 | read_json, 20 | ) 21 | from tests.helpers import proto_test_case 22 | from tests.test_find_visible import _clean_visibles 23 | 24 | 25 | def test_inside_polygon(): 26 | polygon_test_case = np.array( 27 | [(-1.0, -1.0), (1.0, -1.0), (1.0, 1.0), (-1.0, 1.0)], dtype=configs.DTYPE_FLOAT 28 | ) 29 | 30 | for border_value in [True, False]: 31 | 32 | def test_fct(input): 33 | p = np.array(input, dtype=configs.DTYPE_FLOAT) 34 | return _inside_polygon(p, polygon_test_case, border_value) 35 | 36 | p_test_cases = [ 37 | # (x,y), 38 | # inside 39 | (0.0, 0.0), 40 | # # outside 41 | (-2.0, 2.0), 42 | (0, 2.0), 43 | (2.0, 2.0), 44 | (-2.0, 0), 45 | (2.0, 0), 46 | (-2.0, -2.0), 47 | (0, -2.0), 48 | (2.0, -2.0), 49 | # on the line test cases 50 | (-1.0, -1.0), 51 | (1.0, -1.0), 52 | (1.0, 1.0), 53 | (-1.0, 1.0), 54 | (0.0, 1), 55 | (0, -1), 56 | (1, 0), 57 | (-1, 0), 58 | ] 59 | expected_results = [ 60 | True, 61 | False, 62 | False, 63 | False, 64 | False, 65 | False, 66 | False, 67 | False, 68 | False, 69 | # on the line test cases 70 | border_value, 71 | border_value, 72 | border_value, 73 | border_value, 74 | border_value, 75 | border_value, 76 | border_value, 77 | border_value, 78 | ] 79 | 80 | proto_test_case(list(zip(p_test_cases, expected_results)), test_fct) 81 | 82 | 83 | def test_read_json(): 84 | path2json_file = abspath(join(__file__, pardir, pardir, "example.json")) 85 | boundary_coordinates, list_of_holes = read_json(path2json_file) 86 | assert len(boundary_coordinates) == 5 87 | assert len(boundary_coordinates[0]) == 2 88 | assert len(list_of_holes) == 2 89 | first_hole = list_of_holes[0] 90 | assert len(first_hole) == 4 91 | assert len(first_hole[0]) == 2 92 | environment = PolygonEnvironment() 93 | environment.store(boundary_coordinates, list_of_holes, validate=True) 94 | 95 | 96 | @pytest.mark.parametrize( 97 | "visible_idxs, cand_idx2repr, vert_idx2dist, expected", 98 | [ 99 | (set(), {}, {}, set()), 100 | ({0}, {0: 0.0}, {0: 0.0}, {0}), 101 | # different repr -> keep both 102 | ({0, 1}, {0: 0.0, 1: 1.0}, {0: 0.0, 1: 0.0}, {0, 1}), 103 | ({0, 1}, {0: 0.5, 1: 1.0}, {0: 0.0, 1: 1.0}, {0, 1}), 104 | ({0, 1}, {0: 0.5, 1: 1.0}, {0: 1.0, 1: 1.0}, {0, 1}), 105 | # same repr -> keep one the one with the lower dist 106 | ({0, 1}, {0: 0.0, 1: 0.0}, {0: 0.0, 1: 1.0}, {0}), 107 | ({0, 1}, {0: 0.0, 1: 0.0}, {0: 0.0, 1: 1.1}, {0}), 108 | ({0, 1}, {0: 0.0, 1: 0.0}, {0: 1.0, 1: 0.0}, {1}), 109 | ({0, 1}, {0: 0.0, 1: 0.0}, {0: 1.1, 1: 0.0}, {1}), 110 | ], 111 | ) 112 | def test_clean_visible_idxs( 113 | visible_idxs: Set[int], 114 | cand_idx2repr: Dict[int, float], 115 | vert_idx2dist: Dict[int, float], 116 | expected: Set[int], 117 | ): 118 | res = _clean_visibles(visible_idxs, cand_idx2repr, vert_idx2dist) 119 | assert res == expected 120 | 121 | 122 | @pytest.mark.parametrize( 123 | "coords, expected", 124 | [ 125 | ( 126 | [ 127 | (0, 0), 128 | (10, 0), 129 | (9, 5), 130 | (10, 10), 131 | (0, 10), 132 | ], 133 | {2}, 134 | ), 135 | ( 136 | [ 137 | (0, 0), 138 | (10, 0), 139 | (10, 10), 140 | (0, 10), 141 | ], 142 | set(), 143 | ), 144 | ( 145 | [ 146 | (0, 0), 147 | (-2, -2), 148 | (-3, -2.5), 149 | (-3, -4), 150 | (2, -3), 151 | (1, 2.5), 152 | (0, -1), 153 | ], 154 | {6, 1}, 155 | ), 156 | ], 157 | ) 158 | def test_compute_extremity_idxs(coords, expected): 159 | coords = np.array(coords, dtype=configs.DTYPE_FLOAT) 160 | extremity_mask = _cmp_extremity_mask(coords) 161 | res = list(np.where(extremity_mask)[0]) 162 | assert set(res) == expected 163 | 164 | 165 | @pytest.mark.parametrize( 166 | "input, expected", 167 | [ 168 | # clockwise numbering! 169 | ([(3.0, 7.0), (5.0, 9.0), (5.0, 7.0)], True), 170 | ([(3.0, 7.0), (5.0, 9.0), (4.5, 7.0), (5.0, 4.0)], True), 171 | ([(0.0, 0.0), (0.0, 1.0), (1.0, 0.0)], True), 172 | # # counter clockwise edge numbering! 173 | ([(0.0, 0.0), (1.0, 0.0), (0.0, 1.0)], False), 174 | ([(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)], False), 175 | ([(0.0, 0.0), (10.0, 0.0), (10.0, 5.0), (10.0, 10.0), (0.0, 10.0)], False), 176 | ([(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)], False), 177 | ], 178 | ) 179 | def test_clockwise_numering(input, expected): 180 | def clockwise_test_fct(input): 181 | inp = np.array(input, dtype=configs.DTYPE_FLOAT) 182 | return _has_clockwise_numbering(inp) 183 | 184 | assert clockwise_test_fct(input) == expected 185 | 186 | 187 | @pytest.mark.parametrize( 188 | "input, expected", 189 | [ 190 | ([0.0, -5.0], (3.0, 5.0)), 191 | ([-5.0, 0.0], (2.0, 5.0)), 192 | ([-1.0, 0.0], (2.0, 1.0)), 193 | ([1.0, 0.0], (0.0, 1.0)), 194 | ([0.0, 1.0], (1.0, 1.0)), 195 | ([-6.0, -5.0], (2.64018439966448, 7.810249675906654)), 196 | ([-5.0, -6.0], (2.768221279597376, 7.810249675906654)), 197 | ], 198 | ) 199 | def test_compute_repr_n_dist(input, expected): 200 | def test_fct(input): 201 | inp = np.array(input, dtype=configs.DTYPE_FLOAT) 202 | return _compute_repr_n_dist(inp) 203 | 204 | assert test_fct(input) == expected 205 | 206 | 207 | @pytest.mark.parametrize( 208 | "input, expected", 209 | [ 210 | ([1.0, 0.0], 0.0), 211 | ([0.0, 1.0], 1.0), 212 | ([-1.0, 0.0], 2.0), 213 | ([0.0, -1.0], 3.0), 214 | ([2.0, 0.0], 0.0), 215 | ([0.0, 2.0], 1.0), 216 | ([-2.0, 0.0], 2.0), 217 | ([0.0, -2.0], 3.0), 218 | ], 219 | ) 220 | def test_angle_representation(input, expected): 221 | def func(input): 222 | inp = np.array(input, dtype=configs.DTYPE_FLOAT) 223 | repr, dist = _compute_repr_n_dist(inp) 224 | return repr 225 | 226 | assert func(input) == expected 227 | 228 | 229 | @pytest.mark.parametrize( 230 | "input, expected", 231 | [ 232 | ([1.0, 0.0], 0.0), 233 | ([0.0, 1.0], 1.0), 234 | ([-1.0, 0.0], 2.0), 235 | ([0.0, -1.0], 3.0), 236 | ([2.0, 0.0], 0.0), 237 | ([0.0, 2.0], 1.0), 238 | ([-2.0, 0.0], 2.0), 239 | ([0.0, -2.0], 3.0), 240 | ([1.0, 1.0], 0.0), 241 | ([-1.0, 1.0], 1.0), 242 | ([-1.0, -1.0], 2.0), 243 | ([1.0, -1.0], 3.0), 244 | ([1.0, 0.00001], 0.0), 245 | ([0.00001, 1.0], 0.0), 246 | ([-1.0, 0.00001], 1.0), 247 | ([0.00001, -1.0], 3.0), 248 | ([1.0, -0.00001], 3.0), 249 | ([-0.00001, 1.0], 1.0), 250 | ([-1.0, -0.00001], 2.0), 251 | ([-0.00001, -1.0], 2.0), 252 | ], 253 | ) 254 | def test_angle_repr_quadrant(input, expected): 255 | def func(input): 256 | inp = np.array(input, dtype=configs.DTYPE_FLOAT) 257 | repr, dist = _compute_repr_n_dist(inp) 258 | return repr 259 | 260 | res = func(input) 261 | assert res >= expected 262 | assert res < expected + 1 263 | -------------------------------------------------------------------------------- /extremitypathfinder/utils_numba.py: -------------------------------------------------------------------------------- 1 | import math 2 | from typing import Optional, Tuple 3 | 4 | import numpy as np 5 | 6 | from extremitypathfinder import configs 7 | 8 | try: 9 | from numba import b1, f8, i8, njit, typeof, void 10 | except ImportError: 11 | using_numba = False 12 | # replace Numba functionality with "transparent" implementations 13 | from extremitypathfinder.numba_replacements import b1, f8, i8, njit, typeof, void 14 | 15 | FloatTuple = typeof((1.0, 1.0)) 16 | 17 | 18 | @njit(FloatTuple(f8[:]), cache=True) 19 | def _compute_repr_n_dist(np_vector: np.ndarray) -> Tuple[float, float]: 20 | """computing representation for the angle from the origin to a given vector 21 | 22 | value in [0.0 : 4.0[ 23 | every quadrant contains angle measures from 0.0 to 1.0 24 | there are 4 quadrants (counterclockwise numbering) 25 | 0 / 360 degree -> 0.0 26 | 90 degree -> 1.0 27 | 180 degree -> 2.0 28 | 270 degree -> 3.0 29 | ... 30 | Useful for comparing angles without actually computing expensive trigonometrical functions 31 | This representation does not grow directly proportional to its represented angle, 32 | but it its bijective and monotonous: 33 | rep(p1) > rep(p2) <=> angle(p1) > angle(p2) 34 | rep(p1) = rep(p2) <=> angle(p1) = angle(p2) 35 | angle(p): counterclockwise angle between the two line segments (0,0)'--(1,0)' and (0,0)'--p 36 | with (0,0)' being the vector representing the origin 37 | 38 | :param np_vector: 39 | :return: 40 | """ 41 | dx, dy = np_vector 42 | distance = math.sqrt(dx**2 + dy**2) # l-2 norm 43 | if distance == 0.0: 44 | angle_measure = np.nan 45 | else: 46 | # 2D vector: (dx, dy) = np_vector 47 | dx_positive = dx >= 0 48 | dy_positive = dy >= 0 49 | 50 | if dx_positive and dy_positive: 51 | quadrant = 0.0 52 | angle_measure = dy 53 | 54 | elif not dx_positive and dy_positive: 55 | quadrant = 1.0 56 | angle_measure = -dx 57 | 58 | elif not dx_positive and not dy_positive: 59 | quadrant = 2.0 60 | angle_measure = -dy 61 | 62 | else: 63 | quadrant = 3.0 64 | angle_measure = dx 65 | 66 | # normalise angle measure to [0; 1] 67 | angle_measure /= distance 68 | angle_measure += quadrant 69 | 70 | return angle_measure, distance 71 | 72 | 73 | @njit(cache=True) 74 | def _angle_rep_inverse(repr: Optional[float]) -> Optional[float]: 75 | if repr is None: 76 | repr_inv = None 77 | else: 78 | repr_inv = (repr + 2.0) % 4.0 79 | return repr_inv 80 | 81 | 82 | @njit(b1(f8[:], f8[:, :], b1), cache=True) 83 | def _inside_polygon(p: np.ndarray, coords: np.ndarray, border_value: bool) -> bool: 84 | # should return the border value for point equal to any polygon vertex 85 | # TODO overflow possible with large values when comparing slopes, change procedure 86 | # and if the point p lies on any polygon edge 87 | p1 = coords[-1, :] 88 | for p2 in coords[:]: 89 | if np.array_equal(p2, p): 90 | return border_value 91 | rep_p1_p, _ = _compute_repr_n_dist(p1 - p) 92 | rep_p2_p, _ = _compute_repr_n_dist(p2 - p) 93 | if abs(rep_p1_p - rep_p2_p) == 2.0: 94 | return border_value 95 | p1 = p2 96 | 97 | # regular point in polygon algorithm: ray casting 98 | x, y = p 99 | x_coords = coords[:, 0] 100 | y_coords = coords[:, 1] 101 | nr_coords = len(x_coords) 102 | inside = False 103 | 104 | # the edge from the last to the first point is checked first 105 | y1 = y_coords[-1] 106 | y_gt_y1 = y > y1 107 | for i in range(nr_coords): 108 | y2 = y_coords[i] 109 | y_gt_y2 = y > y2 110 | if y_gt_y1 ^ y_gt_y2: # XOR 111 | # [p1-p2] crosses horizontal line in p 112 | x1 = x_coords[i - 1] 113 | x2 = x_coords[i] 114 | # only count crossings "right" of the point ( >= x) 115 | x_le_x1 = x <= x1 116 | x_le_x2 = x <= x2 117 | if x_le_x1 or x_le_x2: 118 | if x_le_x1 and x_le_x2: 119 | # p1 and p2 are both to the right -> valid crossing 120 | inside = not inside 121 | else: 122 | # compare the slope of the line [p1-p2] and [p-p2] 123 | # depending on the position of p2 this determines whether 124 | # the polygon edge is right or left of the point 125 | # to avoid expensive division the divisors (of the slope dy/dx) are brought to the other side 126 | # ( dy/dx > a == dy > a * dx ) 127 | # only one of the points is to the right 128 | slope1 = (y2 - y) * (x2 - x1) 129 | slope2 = (y2 - y1) * (x2 - x) 130 | # NOTE: accept slope equality to also detect if p lies directly on an edge 131 | if y_gt_y1: 132 | if slope1 <= slope2: 133 | inside = not inside 134 | elif slope1 >= slope2: # NOT y_gt_y1 135 | inside = not inside 136 | 137 | # next point 138 | y1 = y2 139 | y_gt_y1 = y_gt_y2 140 | 141 | return inside 142 | 143 | 144 | @njit(b1(f8[:, :]), cache=True) 145 | def _no_identical_consequent_vertices(coords: np.ndarray) -> bool: 146 | p1 = coords[-1] 147 | for p2 in coords: 148 | # TODO adjust allowed difference: rtol, atol 149 | if np.array_equal(p1, p2): 150 | return False 151 | p1 = p2 152 | 153 | return True 154 | 155 | 156 | @njit(b1(f8[:, :]), cache=True) 157 | def _has_clockwise_numbering(coords: np.ndarray) -> bool: 158 | """tests if a polygon has clockwise vertex numbering 159 | approach: Sum over the edges, (x2 − x1)(y2 + y1). If the result is positive the curve is clockwise. 160 | from: 161 | https://stackoverflow.com/questions/1165647/how-to-determine-if-a-list-of-polygon-points-are-in-clockwise-order 162 | :param coords: the list of (x,y) coordinates representing the polygon to be tested 163 | :return: true if the polygon has been given in clockwise numbering 164 | """ 165 | total_sum = 0.0 166 | p1 = coords[-1] 167 | for p2 in coords: 168 | x1, y1 = p1 169 | x2, y2 = p2 170 | total_sum += (x2 - x1) * (y2 + y1) 171 | p1 = p2 172 | return total_sum > 0 173 | 174 | 175 | @njit(void(f8[:, :], b1[:]), cache=True) 176 | def _fill_extremity_mask(coordinates, extremity_mask): 177 | nr_coordinates = len(extremity_mask) 178 | p1 = coordinates[-2] 179 | p2 = coordinates[-1] 180 | for i, p3 in enumerate(coordinates): 181 | # since consequent vertices are not permitted to be equal, 182 | # the angle representation of the difference is well-defined 183 | diff_p3_p2 = p3 - p2 184 | diff_p1_p2 = p1 - p2 185 | repr_p3_p2, _ = _compute_repr_n_dist(diff_p3_p2) 186 | repr_p1_p2, _ = _compute_repr_n_dist(diff_p1_p2) 187 | rep_diff = repr_p3_p2 - repr_p1_p2 188 | if rep_diff % 4.0 < 2.0: # inside angle > 180 degree 189 | # p2 is an extremity 190 | idx_p2 = (i - 1) % nr_coordinates 191 | extremity_mask[idx_p2] = True 192 | 193 | # move to the next point 194 | p1 = p2 195 | p2 = p3 196 | 197 | 198 | # TODO 199 | @njit(void(i8[:, :], i8[:, :]), cache=True) 200 | def _fill_edge_vertex_idxs(edge_vertex_idxs, vertex_edge_idxs): 201 | nr_coords = len(edge_vertex_idxs) 202 | v1 = -1 % nr_coords 203 | # TODO col 1 is just np.arange?! 204 | for edge_idx, v2 in enumerate(range(nr_coords)): 205 | v1_idx = v1 206 | v2_idx = v2 207 | edge_vertex_idxs[edge_idx, 0] = v1_idx 208 | edge_vertex_idxs[edge_idx, 1] = v2_idx 209 | vertex_edge_idxs[v1_idx, 1] = edge_idx 210 | vertex_edge_idxs[v2_idx, 0] = edge_idx 211 | # move to the next vertex/edge 212 | v1 = v2 213 | 214 | 215 | @njit(b1(f8[:], f8[:], f8[:]), cache=True) 216 | def _lies_behind_inner(p1: np.ndarray, p2: np.ndarray, v: np.ndarray) -> bool: 217 | # special case of get_intersection_status() 218 | # solve the set of equations 219 | # (p2-p1) lambda + (p1) = (v) mu 220 | # in matrix form A x = b: 221 | # [(p1-p2) (v)] (lambda, mu)' = (p1) 222 | # because the vertex lies within the angle range between the two edge vertices 223 | # (together with the other conditions on the polygons) 224 | # this set of linear equations is always solvable (the matrix is regular) 225 | A = np.empty((2, 2), dtype=configs.DTYPE_FLOAT) 226 | A[:, 0] = p1 - p2 227 | A[:, 1] = v 228 | # A = np.array([p1 - p2, v]).T 229 | x = np.linalg.solve(A, p1) 230 | # Note: parallel lines are not possible here (singular matrix) 231 | # try: 232 | # x = np.linalg.solve(A, p1) 233 | # except np.linalg.LinAlgError: 234 | # raise Exception("parallel lines") 235 | 236 | # Debug: 237 | # assert np.allclose((p2 - p1) * x[0] + p1, v * x[1]) 238 | # assert np.allclose(np.dot(A, x), b) 239 | 240 | # vertices on the edge are possibly visible! ( < not <=) 241 | return x[1] < 1.0 242 | 243 | 244 | @njit(b1(i8, i8, i8, i8, f8[:, :]), cache=True) 245 | def _lies_behind( 246 | idx_p1: int, idx_p2: int, idx_v: int, idx_orig: int, coords: np.ndarray 247 | ) -> bool: 248 | coords_origin = coords[idx_orig] 249 | coords_p1_rel = coords[idx_p1] - coords_origin 250 | coords_p2_rel = coords[idx_p2] - coords_origin 251 | coords_v_rel = coords[idx_v] - coords_origin 252 | return _lies_behind_inner(coords_p1_rel, coords_p2_rel, coords_v_rel) 253 | -------------------------------------------------------------------------------- /docs/3_about.rst: -------------------------------------------------------------------------------- 1 | ===== 2 | About 3 | ===== 4 | 5 | 6 | .. include:: ./badges.rst 7 | 8 | 9 | python package for fast geometric shortest path computation in 2D multi-polygon or grid environments based on visibility graphs. 10 | 11 | 12 | .. image:: _static/title_demo_plot.png 13 | 14 | 15 | Also see: 16 | `GitHub `__, 17 | `PyPI `__ 18 | 19 | 20 | License 21 | ------- 22 | 23 | ``extremitypathfinder`` is distributed under the terms of the MIT license 24 | (see `LICENSE `__). 25 | 26 | 27 | Basic Idea 28 | ---------------- 29 | 30 | 31 | Well described in `[1, Ch. II 3.2] `__: 32 | 33 | An environment ("world", "map") of a given shortest path problem can be represented by one boundary polygon with holes (themselves polygons). 34 | 35 | **IDEA**: Two categories of vertices/corners can be distinguished in these kind of environments: 36 | 37 | * protruding corners (hereafter called **"Extremities"**) 38 | * all others 39 | 40 | .. figure:: _static/map_plot.png 41 | 42 | polygon environment with extremities marked in red 43 | 44 | 45 | Extremities have an inner angle (facing towards the inside of the environment) of > 180 degree. 46 | As long as there are no obstacles between two points present, it is obviously always best (=shortest) to move to the goal point directly. 47 | When obstacles obstruct the direct path (goal is not directly 'visible' from the start) however, extremities (and only extremities!) have to be visited to reach the areas "behind" them until the goal is directly visible. 48 | 49 | **Improvement:** As described in `[1, Ch. II 4.4.2 "Property One"] `__ during preprocessing time the visibility graph can be reduced further without the loss of guaranteed optimality of the algorithm: 50 | Starting from any point lying "in front of" an extremity ``e``, such that both adjacent edges are visible, one will never visit ``e``, because everything is reachable on a shorter path without ``e`` (except ``e`` itself). An extremity ``e1`` lying in the area "in front of" 51 | extremity ``e`` hence is never the next vertex in a shortest path coming from ``e``. And also in reverse: when coming from ``e1`` everything else than ``e`` itself can be reached faster without visiting ``e1``. -> ``e`` and ``e1`` do not have to be connected in the graph. 52 | 53 | 54 | .. _algorithm: 55 | 56 | Algorithm 57 | ========= 58 | 59 | This package pretty much implements the Visibility Graph Optimized (VGO) Algorithm described in `[1, Ch. II 4.4.2] `__, just with a few computational tweaks: 60 | 61 | 62 | Rough Procedure: 63 | ________________ 64 | 65 | - **1. Preprocessing the environment:** Independently of any query start and goal points the optimized visibility graph is being computed for the static environment once. Later versions might include a faster approach to compute visibility on the fly, for use cases where the environment is changing dynamically. The edges of the precomputed graph between the extremities are shown in red in the following plots. Notice that the extremity on the right is not connected to any other extremity due to the above mentioned optimisation: 66 | 67 | .. figure:: _static/prepared_map_plot.png 68 | 69 | polygon environment with optimised visibility graph overlay in red 70 | 71 | 72 | - **2. Including start and goal:** For each shortest path query the start and goal points are being connected to the internal graph depending on their visibility. Notice that the added edges are directed and also here the optimisation is being used to reduce the amount of edges: 73 | 74 | .. figure:: _static/graph_plot.png 75 | 76 | optimised directed heuristic graph for shortest path computation with added start and goal nodes 77 | 78 | 79 | 80 | - **3. A-star shortest path computation :** Finding the shortest path on graphs is a standard computer science problem. This package uses a modified version of the popular ``A*-Algorithm`` optimized for this special use case. 81 | 82 | .. figure:: _static/graph_path_plot.png 83 | 84 | polygon environment with optimised visibility graph overlay. visualised edges added to the visibility graph in yellow, found shortest path in green. 85 | 86 | 87 | Edge Case: Overlapping Vertices and Edges 88 | _________________________________________ 89 | 90 | 91 | .. warning:: 92 | Overlapping edges and vertices are considered "non-blocking". 93 | Shortest paths can run through either. 94 | Ensure Holes and Boundary Polygons are truly intersecting and not just touching in order to "block" paths. 95 | 96 | .. figure:: _static/path_overlapping_vertices.png 97 | 98 | example of a shortest path running through overlapping vertices 99 | 100 | 101 | .. figure:: _static/path_overlapping_edges.png 102 | 103 | example of a shortest path running along two overlapping edges 104 | 105 | 106 | 107 | 108 | Implementation 109 | ============== 110 | 111 | 112 | Visibility detection: "Angle Range Elimination Algorithm" (AREA, Contribution of this package) 113 | **************************************************************************************************** 114 | 115 | AREA is an algorithm for computing the visibility graph. 116 | 117 | In this use case we are not interested in the full visibility graph, but the visibility of just some points (all extremities, start and goal). 118 | 119 | Simple fundamental idea: points (extremities) are visible when there is no edge running in front "blocking the view". 120 | 121 | Rough procedure: For all edges delete the points lying behind them. Points that remain at the end are visible. 122 | 123 | Optimisations: 124 | 125 | * for each edge only checking the relevant candidates ("within the angle range"): 126 | * By sorting the edges after their angle representation (similar to Lee's algorith, s. below), only the candidates with a bigger representation have to be checked. 127 | * By also sorting the candidates, the candidates with a smaller representation than the edge don't have to be checked. 128 | * angle representations: instead of computing with angles in degree or radians, it is much more efficient and still sufficient to use a representation that is mapping an angle to a range :math:`a \in [0.0 ; 4.0[` (:math:`[0.0 ; 1.0[` in all 4 quadrants). This can be done without computationally expensive trigonometric functions! 129 | * deciding if a point lies behind an edge can often be done without computing intersections by just comparing distances. This can be used to reduce the needed computations. 130 | 131 | 132 | Properties: 133 | 134 | - checking all edges 135 | - checking an edge at most once 136 | - ability to process only a subset of all vertices as possible candidates 137 | - decreasing number of candidates after every checked origin (visibility is a symmetric relation -> only need to check once for every candidate pair!) 138 | - no expensive trigonometric computations 139 | - actual Intersection computation (solving linear scalar equations) only for a fraction of candidates 140 | - could theoretically also work with just lines (this package however currently just allows polygons) 141 | 142 | 143 | Runtime Complexity: 144 | 145 | - :math:`m`: the amount of extremities (candidates) 146 | - :math:`n`: the amount of edges / vertices (since polynomial edges share vertices), with usually :math:`m << n` 147 | - :math:`O(m)` for checking every candidate as origin 148 | - :math:`O(n log_2 n)` for sorting the edges, done once for every origin -> :math:`O(m n log_2 n)` 149 | - :math:`O(n)` for checking every edge -> :math:`O(m (n log_2 n + n)` 150 | - :math:`O(m/n)` (average) for checking the visibility of target candidates. only the fraction relevant for each edge will be checked -> :math:`O(m (n log_2 n + (n m) / n) = O(m (n log_2 n + m) = O(m n log_2 n` 151 | - since :math:`m ~ n` the final complexity is :math:`O(m n log_2 n) = O(n^2 log_2 n)` 152 | 153 | The core visibility algorithm (for one origin) is implemented in ``PolygonEnvironment.get_visible_idxs()`` in ``extremitypathfinder.py`` 154 | 155 | 156 | Comparison: 157 | *********** 158 | 159 | **Lee's visibility graph algorithm**: 160 | 161 | complexity: :math:`O(n^2 log_2 n)` (cf. `these slides `__) 162 | 163 | - Initially all edges are being checked for intersection 164 | - Necessarily checking the visibility of all points (instead of just some) 165 | - Always checking all points in every run 166 | - One intersection computation for most points (always when T is not empty) 167 | - Sorting: all points according to degree on startup, edges in binary tree T 168 | - Can work with just lines (not restricted to polygons) 169 | 170 | 171 | Optimised Pathfinding: 172 | ********************** 173 | 174 | Currently using the default implementation of A* from the `networkx` package. 175 | 176 | Remark: This geometrical property of the specific task (the visibility graph) could be exploited in an optimised (e.g. A*) algorithm: 177 | 178 | - It is always shortest to directly reach a node instead of visiting other nodes first 179 | (there is never an advantage through reduced edge weight). 180 | 181 | Make A* terminate earlier than for general graphs: 182 | 183 | - no need to revisit nodes (path only gets longer) 184 | 185 | - when the goal is directly reachable, there can be no other shorter path to it -> terminate 186 | 187 | - not all neighbours of the current node have to be checked like in vanilla A* before continuing to the next node 188 | 189 | 190 | Comparison to pyvisgraph 191 | ------------------------- 192 | 193 | This package is similar to `pyvisgraph `__ which uses Lee's algorithm. 194 | 195 | 196 | **Pros:** 197 | 198 | - very reduced visibility graph (time and memory!) 199 | - algorithms optimized for path finding 200 | - possibility to convert and use grid worlds 201 | 202 | 203 | **Cons:** 204 | 205 | - parallel computing not supported so far 206 | - no existing speed comparison 207 | 208 | 209 | Contact 210 | -------- 211 | 212 | 213 | Tell me if and how your are using this package. This encourages me to develop and test it further. 214 | 215 | Most certainly there is stuff I missed, things I could have optimized even further or explained more clearly, etc. 216 | I would be really glad to get some feedback. 217 | 218 | If you encounter any bugs, have suggestions etc. do not hesitate to **open an Issue** or **add a Pull Requests** on Git. 219 | Please refer to the :ref:`contribution guidelines ` 220 | 221 | 222 | 223 | 224 | References 225 | ---------------- 226 | 227 | [1] Vinther, Anders Strand-Holm, Magnus Strand-Holm Vinther, and Peyman Afshani. `"Pathfinding in Two-dimensional Worlds" `__. no. June (2015). 228 | 229 | 230 | 231 | Further Reading 232 | ---------------- 233 | 234 | Open source C++ library for 2D floating-point visibility algorithms, path planning: https://karlobermeyer.github.io/VisiLibity1/ 235 | 236 | Python binding of VisiLibity: https://github.com/tsaoyu/PyVisiLibity 237 | 238 | Paper about Lee's algorithm: http://www.dav.ee/papers/Visibility_Graph_Algorithm.pdf 239 | 240 | C implementation of Lee's algorithm: https://github.com/davetcoleman/visibility_graph 241 | 242 | 243 | Acknowledgements 244 | ---------------- 245 | 246 | Thanks to: 247 | 248 | - `Georg Hess `__ for improving the package in order to allow intersecting polygons. 249 | - `Ivan Doria `__ for adding the command line interface. 250 | -------------------------------------------------------------------------------- /tests/test_cases.py: -------------------------------------------------------------------------------- 1 | # size_x, size_y, obstacle_iter 2 | from math import sqrt 3 | 4 | 5 | GRID_ENV_PARAMS = ( 6 | 19, 7 | 10, 8 | [ 9 | # (x,y), 10 | # obstacles changing boundary 11 | (0, 1), 12 | (1, 1), 13 | (2, 1), 14 | (3, 1), 15 | (17, 9), 16 | (17, 8), 17 | (17, 7), 18 | (17, 5), 19 | (17, 4), 20 | (17, 3), 21 | (17, 2), 22 | (17, 1), 23 | (17, 0), 24 | # hole 1 25 | (5, 5), 26 | (5, 6), 27 | (6, 6), 28 | (6, 7), 29 | (7, 7), 30 | # hole 2 31 | (7, 5), 32 | ], 33 | ) 34 | 35 | INVALID_DESTINATION_DATA = [ 36 | # outside of map region 37 | ((-1, 5.0), (17, 0.5)), 38 | ((17, 0.5), (-1, 5.0)), 39 | ((20, 5.0), (17, 0.5)), 40 | ((17, 0.5), (20, 5.0)), 41 | ((1, -5.0), (17, 0.5)), 42 | ((17, 0.5), (1, -5.0)), 43 | ((1, 11.0), (17, 0.5)), 44 | ((17, 0.5), (1, 11.0)), 45 | # outside boundary polygon 46 | ((17.5, 5.0), (17, 0.5)), 47 | ((17, 0.5), (17.5, 5.0)), 48 | ((1, 1.5), (17, 0.5)), 49 | ((17, 0.5), (1, 1.5)), 50 | # inside hole 51 | ((6.5, 6.5), (17, 0.5)), 52 | ((17, 0.5), (6.5, 6.5)), 53 | ] 54 | 55 | TEST_DATA_GRID_ENV = [ 56 | # ((start,goal),(path,distance)) 57 | # shortest paths should be distinct (reverse will automatically be tested) 58 | # identical nodes 59 | (((15, 5), (15, 5)), ([(15, 5), (15, 5)], 0.0)), 60 | # directly reachable 61 | (((15, 5), (15, 6)), ([(15, 5), (15, 6)], 1.0)), 62 | (((15, 5), (16, 6)), ([(15, 5), (16, 6)], sqrt(2))), 63 | # points on the polygon edges (vertices) should be accepted! 64 | # on edge 65 | (((15, 0), (15, 6)), ([(15, 0), (15, 6)], 6.0)), 66 | (((17, 5), (16, 5)), ([(17, 5), (16, 5)], 1.0)), 67 | # on edge of hole 68 | (((7, 8), (7, 9)), ([(7, 8), (7, 9)], 1.0)), 69 | # on vertex 70 | (((4, 2), (4, 3)), ([(4, 2), (4, 3)], 1.0)), 71 | # on vertex of hole 72 | (((6, 8), (6, 9)), ([(6, 8), (6, 9)], 1.0)), 73 | # on two vertices 74 | # coinciding with edge (direct neighbour) 75 | (((4, 2), (4, 1)), ([(4, 2), (4, 1)], 1.0)), 76 | (((5, 5), (5, 7)), ([(5, 5), (5, 7)], 2.0)), 77 | # should have direct connection to all visible extremities! connected in graph 78 | (((6, 8), (5, 7)), ([(6, 8), (5, 7)], sqrt(2))), 79 | (((4, 1), (5, 7)), ([(4, 1), (5, 7)], sqrt(1**2 + 6**2))), 80 | # should have direct connection to all visible extremities! even if not connected in graph! 81 | (((4, 2), (5, 7)), ([(4, 2), (5, 7)], sqrt(1**2 + 5**2))), 82 | # mix of edges and vertices, directly visible 83 | (((2, 2), (5, 7)), ([(2, 2), (5, 7)], sqrt(3**2 + 5**2))), 84 | # also regular points should have direct connection to all visible extremities! 85 | (((10, 3), (17, 6)), ([(10, 3), (17, 6)], sqrt(7**2 + 3**2))), 86 | (((10, 3), (8, 8)), ([(10, 3), (8, 8)], sqrt(2**2 + 5**2))), 87 | # even if the query point lies in front of an extremity! (test if new query vertices are being created!) 88 | (((10, 3), (8, 5)), ([(10, 3), (8, 5)], sqrt(2**2 + 2**2))), 89 | # using a* graph search: 90 | # directly reachable through a single vertex (does not change distance!) 91 | (((5, 1), (3, 3)), ([(5, 1), (4, 2), (3, 3)], sqrt(2**2 + 2**2))), 92 | # If two Polygons have vertices with identical coordinates (this is allowed), 93 | # paths through these vertices are theoretically possible! 94 | ( 95 | ((6.5, 5.5), (7.5, 6.5)), 96 | ([(6.5, 5.5), (7, 6), (7.5, 6.5)], sqrt(1**2 + 1**2)), 97 | ), 98 | # distance should stay the same even if multiple extremities lie on direct path 99 | # test if path is skipping passed extremities 100 | (((8, 4), (8, 8)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8)], 4)), 101 | (((8, 4), (8, 9)), ([(8, 4), (8, 5), (8, 6), (8, 7), (8, 8), (8, 9)], 5)), 102 | # regular examples 103 | ( 104 | ((0.5, 6), (18.5, 0.5)), 105 | ( 106 | [(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (17, 6), (18, 6), (18.5, 0.5)], 107 | 23.18783787537749, 108 | ), 109 | ), 110 | ( 111 | ((0.5, 6), (9, 5.5)), 112 | ([(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (9.0, 5.5)], 8.727806217396338), 113 | ), 114 | ( 115 | ((0.5, 6), (18.5, 9)), 116 | ( 117 | [(0.5, 6.0), (5, 5), (6, 5), (7, 5), (8, 5), (18, 7), (18.5, 9.0)], 118 | 19.869364068640845, 119 | ), 120 | ), 121 | ( 122 | ((6.9, 4), (7, 9)), 123 | ([(6.9, 4.0), (7, 6), (8, 7), (8, 8), (7, 9)], 5.830925564196269), 124 | ), 125 | ( 126 | ((6.5, 4), (7, 9)), 127 | ([(6.5, 4.0), (7, 6), (8, 7), (8, 8), (7, 9)], 5.889979937555021), 128 | ), 129 | # symmetric around the lower boundary obstacle 130 | ( 131 | ((0.5, 0.5), (0.5, 2.5)), 132 | ([(0.5, 0.5), (4, 1), (4, 2), (0.5, 2.5)], 8.071067811865476), 133 | ), 134 | # symmetric around the lower right boundary obstacle 135 | ( 136 | ((16.5, 0.5), (18.5, 0.5)), 137 | ([(16.5, 0.5), (17, 6), (18, 6), (18.5, 0.5)], 12.045361017187261), 138 | ), 139 | # symmetric around the top right boundary obstacle 140 | ( 141 | ((16.5, 9.5), (18.5, 9.5)), 142 | ([(16.5, 9.5), (17, 7), (18, 7), (18.5, 9.5)], 6.0990195135927845), 143 | ), 144 | ] 145 | 146 | POLY_ENV_PARAMS = ( 147 | # boundary_coordinates 148 | [(0.0, 0.0), (10.0, 0.0), (9.0, 5.0), (10.0, 10.0), (0.0, 10.0)], 149 | # list_of_holes 150 | [ 151 | [ 152 | (3.0, 7.0), 153 | (5.0, 9.0), 154 | (4.6, 7.0), 155 | (5.0, 4.0), 156 | ], 157 | ], 158 | ) 159 | 160 | TEST_DATA_POLY_ENV = [ 161 | # # ((start,goal),(path,distance)) 162 | # # identical nodes 163 | # (((1, 1), (1, 1)), ([(1, 1), (1, 1)], 0.0)), 164 | # # directly reachable 165 | # (((1, 1), (1, 2)), ([(1, 1), (1, 2)], 1.0)), 166 | # (((1, 1), (2, 1)), ([(1, 1), (2, 1)], 1.0)), 167 | # # points on the polygon edges (vertices) should be accepted! 168 | # # on edge (boundary polygon) 169 | # (((1, 0), (1, 1)), ([(1, 0), (1, 1)], 1.0)), 170 | # (((9.5, 2.5), (8.5, 2.5)), ([(9.5, 2.5), (8.5, 2.5)], 1.0)), 171 | # (((0, 2), (0, 1)), ([(0, 2), (0, 1)], 1.0)), # both 172 | # (((1, 0), (5, 0)), ([(1, 0), (5, 0)], 4.0)), # both 173 | # # on edge of hole 174 | # (((4, 8), (3, 8)), ([(4, 8), (3, 8)], 1.0)), 175 | # (((4, 8), (4.1, 8.1)), ([(4, 8), (4.1, 8.1)], sqrt(2 * (0.1**2)))), # both 176 | # # on vertex 177 | # (((9, 5), (8, 5)), ([(9, 5), (8, 5)], 1.0)), 178 | # # on vertex of hole 179 | # (((3, 7), (2, 7)), ([(3, 7), (2, 7)], 1.0)), 180 | # # on two vertices 181 | # # coinciding with edge (direct neighbour) 182 | # (((3, 7), (5, 9)), ([(3, 7), (5, 9)], sqrt(8))), 183 | # (((4.6, 7), (5, 9)), ([(4.6, 7), (5, 9)], sqrt((0.4**2) + (2**2)))), 184 | # # should have direct connection to all visible extremities! connected in graph 185 | # (((5, 4), (5, 9)), ([(5, 4), (5, 9)], 5)), 186 | # # should have a direct connection to all visible extremities! even if not connected in graph! 187 | # (((9, 5), (5, 9)), ([(9, 5), (5, 9)], sqrt(2 * (4**2)))), 188 | # using a* graph search: 189 | # directly reachable through a single vertex (does not change distance!) 190 | (((9, 4), (9, 6)), ([(9, 4), (9, 5), (9, 6)], 2)), 191 | # slightly indented, path must go through right boundary extremity 192 | (((9.1, 4), (9.1, 6)), ([(9.1, 4.0), (9.0, 5.0), (9.1, 6.0)], 2.009975124224178)), 193 | # path must go through lower hole extremity 194 | (((4, 4.5), (6, 4.5)), ([(4.0, 4.5), (5.0, 4.0), (6.0, 4.5)], 2.23606797749979)), 195 | # path must go through top hole extremity 196 | (((4, 8.5), (6, 8.5)), ([(4.0, 8.5), (5.0, 9.0), (6.0, 8.5)], 2.23606797749979)), 197 | ] 198 | 199 | OVERLAP_POLY_ENV_PARAMS = ( 200 | # boundary_coordinates 201 | [ 202 | (9.5, 10.5), 203 | (25.5, 10.5), 204 | (25.5, 0.5), 205 | (49.5, 0.5), 206 | (49.5, 49.5), 207 | (0.5, 49.5), 208 | (0.5, 16.5), 209 | (9.5, 16.5), 210 | (9.5, 45.5), 211 | (10.0, 45.5), 212 | (10.0, 30.5), 213 | (35.5, 30.5), 214 | (35.5, 14.5), 215 | (0.5, 14.5), 216 | (0.5, 0.5), 217 | (9.5, 0.5), 218 | ], 219 | # list_of_holes 220 | [ 221 | [ 222 | (40.5, 4.5), 223 | (29.5, 4.5), 224 | (29.5, 15.0), 225 | (40.5, 15.0), 226 | ], 227 | [ 228 | (45.4, 14.5), 229 | (44.6, 14.5), 230 | (43.4, 20.5), 231 | (46.6, 20.5), 232 | ], 233 | # slightly right of the top boundary obstacle 234 | # goal: create an obstacle that obstructs two very close extremities 235 | # to check if visibility is correctly blocked in such cases 236 | [ 237 | (40, 34), 238 | (10.5, 34), 239 | (10.5, 40), 240 | (40, 40), 241 | ], 242 | # on the opposite site close to top boundary obstacle 243 | [ 244 | (9, 34), 245 | (5, 34), 246 | (5, 40), 247 | (9, 40), 248 | ], 249 | [ 250 | (31.5, 5.390098048839718), 251 | (31.5, 10.909901951439679), 252 | (42.5, 13.109901951160282), 253 | (42.5, 7.590098048560321), 254 | ], 255 | ], 256 | ) 257 | 258 | TEST_DATA_OVERLAP_POLY_ENV = [ 259 | # ((start,goal),(path,distance)) 260 | ( 261 | ((1, 1), (5, 20)), 262 | ( 263 | [ 264 | (1, 1), 265 | (9.5, 10.5), 266 | (25.5, 10.5), 267 | (29.5, 4.5), 268 | (40.5, 4.5), 269 | (42.5, 7.590098048560321), 270 | (42.5, 13.109901951160282), 271 | (35.5, 30.5), 272 | (10.5, 34.0), 273 | (10.0, 45.5), 274 | (9.5, 45.5), 275 | (9, 34), 276 | (5, 20), 277 | ], 278 | 138.23115155299263, 279 | ), 280 | ), 281 | ( 282 | ((2, 38), (45, 45)), 283 | ([(2.0, 38.0), (9.5, 45.5), (10.0, 45.5), (45.0, 45.0)], 46.11017296417249), 284 | ), 285 | ( 286 | ((2, 38), (45, 2)), 287 | ( 288 | [ 289 | (2.0, 38.0), 290 | (9.5, 45.5), 291 | (10.0, 45.5), 292 | (10.5, 34.0), 293 | (35.5, 30.5), 294 | (42.5, 13.109901951160282), 295 | (45.0, 2.0), 296 | ], 297 | 77.99506635830616, 298 | ), 299 | ), 300 | ( 301 | ((2, 38), (38, 2)), 302 | ( 303 | [ 304 | (2.0, 38.0), 305 | (9.5, 45.5), 306 | (10.0, 45.5), 307 | (10.5, 34.0), 308 | (35.5, 30.5), 309 | (42.5, 13.109901951160282), 310 | (42.5, 7.590098048560321), 311 | (40.5, 4.5), 312 | (38.0, 2.0), 313 | ], 314 | 79.34355163003127, 315 | ), 316 | ), 317 | ( 318 | ((2, 38), (28, 2)), 319 | ( 320 | [ 321 | (2.0, 38.0), 322 | (9.5, 45.5), 323 | (10.0, 45.5), 324 | (10.5, 34.0), 325 | (35.5, 30.5), 326 | (42.5, 13.109901951160282), 327 | (42.5, 7.590098048560321), 328 | (40.5, 4.5), 329 | (28.0, 2.0), 330 | ], 331 | 88.55556650808049, 332 | ), 333 | ), 334 | ] 335 | 336 | SEPARATED_ENV = ( 337 | [(5, 5), (-5, 5), (-5, -5), (5, -5)], 338 | [ 339 | [(-5.1, 1), (-5.1, 2), (5.1, 2), (5.1, 1)] 340 | ], # intersecting polygons -> no path possible 341 | # [[(-5, 1), (-5, 2), (5, 2), (5, 1)]], # hole lies on the edges -> path possible 342 | ) 343 | 344 | TEST_DATA_SEPARATE_ENV = [ 345 | # ((start,goal),(path,distance)) 346 | (((0, 0), (0, 4)), ([], None)), # unreachable 347 | ] 348 | 349 | # ((start,goal),(path,distance)) 350 | POLYGON_ENVS = [SEPARATED_ENV, OVERLAP_POLY_ENV_PARAMS, POLY_ENV_PARAMS] 351 | -------------------------------------------------------------------------------- /tests/test_find_visible.py: -------------------------------------------------------------------------------- 1 | """tests for visibility detection 2 | 3 | testing against old reference implementation 4 | 5 | current test cases: all vertices of the test polygons 6 | TODO add more test cases, random but valid query points (within polygons) 7 | hypothesis.readthedocs.io/ 8 | """ 9 | 10 | import itertools 11 | from typing import Set, Tuple 12 | 13 | import numpy as np 14 | import pytest 15 | 16 | from extremitypathfinder import PolygonEnvironment, configs, utils 17 | from extremitypathfinder.utils import ( 18 | _find_within_range, 19 | _lies_behind, 20 | get_neighbour_idxs, 21 | ) 22 | from tests.test_cases import GRID_ENV_PARAMS, POLYGON_ENVS 23 | 24 | 25 | def find_candidates_behind( 26 | origin: int, 27 | v1: int, 28 | v2: int, 29 | candidates: Set[int], 30 | distances: np.ndarray, 31 | coords: np.ndarray, 32 | ) -> Set[int]: 33 | dist_v1 = distances[v1] 34 | dist_v2 = distances[v2] 35 | max_distance = max(dist_v1, dist_v2) 36 | idxs_behind = set() 37 | # for all remaining vertices v it has to be tested if the line segment from query point (=origin) to v 38 | # has an intersection with the current edge p1---p2 39 | for idx in candidates: 40 | # if a candidate is farther away from the query point than both vertices of the edge, 41 | # it surely lies behind the edge 42 | # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge, 43 | # it still needs to be checked! 44 | dist2orig = distances[idx] 45 | further_away = dist2orig > max_distance 46 | if further_away or _lies_behind(v1, v2, idx, origin, coords): 47 | idxs_behind.add(idx) 48 | # vertex lies in front of this edge 49 | return idxs_behind 50 | 51 | 52 | def _clean_visibles( 53 | visible_idxs: Set[int], cand_idx2repr: np.ndarray, vert_idx2dist: np.ndarray 54 | ) -> Set[int]: 55 | # in case some vertices have the same representation, only return (link) the closest vertex 56 | if len(visible_idxs) <= 1: 57 | return visible_idxs 58 | 59 | cleaned = set() 60 | visible_idxs_sorted = sorted(visible_idxs, key=lambda i: cand_idx2repr[i]) 61 | min_dist = np.inf 62 | first_idx = visible_idxs_sorted[0] 63 | rep_prev = cand_idx2repr[first_idx] 64 | selected_idx = 0 65 | for i in visible_idxs_sorted: 66 | rep = cand_idx2repr[i] 67 | if rep != rep_prev: 68 | cleaned.add(selected_idx) 69 | min_dist = np.inf 70 | rep_prev = rep 71 | 72 | dist = vert_idx2dist[i] 73 | if dist < min_dist: 74 | selected_idx = i 75 | min_dist = dist 76 | 77 | cleaned.add(selected_idx) 78 | return cleaned 79 | 80 | 81 | def find_visible_reference( 82 | origin: int, 83 | candidates: Set[int], 84 | edges_to_check: Set[int], 85 | coords: np.ndarray, 86 | representations: np.ndarray, 87 | distances: np.ndarray, 88 | edge_vertex_idxs: np.ndarray, 89 | vertex_edge_idxs: np.ndarray, 90 | extremity_mask: np.ndarray, 91 | ) -> Set[int]: 92 | """ 93 | :param origin: the vertex for which the visibility to the other candidates should be checked. 94 | :param candidates: the set of all vertex ids which should be checked for visibility. 95 | IMPORTANT: is being manipulated, so has to be a copy! 96 | IMPORTANT: must not contain any vertices with equal coordinates (e.g. the origin vertex itself)! 97 | :param edges_to_check: the set of edges which determine visibility 98 | :return: a set of all vertices visible from the origin 99 | """ 100 | if len(candidates) == 0: 101 | return candidates 102 | 103 | candidates = candidates.copy() 104 | 105 | # optimisation: check edges with the highest angle range first ("blocking the most view"), 106 | # as they have the highest chance of eliminating candidates early on 107 | # this is especially important for large maps with many candidates 108 | edge_angle_range = {} 109 | samples = {} 110 | edges_to_skip = set() 111 | 112 | def skip_edge(node: int, edge2discard: int) -> Tuple[int, bool, int, int]: 113 | # node identical to origin 114 | # mark this vertex as not visible (would otherwise add 0 distance edge in the graph) 115 | candidates.discard(node) 116 | # no points lie truly "behind" this edge as there is no "direction of sight" defined 117 | # <-> angle representation/range undefined for just this single edge 118 | # however if one considers the point neighbouring in the other direction (<-> two edges) 119 | # these two neighbouring edges define an invisible angle range 120 | # -> simply move the pointer 121 | v1, v2 = get_neighbour_idxs(node, vertex_edge_idxs, edge_vertex_idxs) 122 | range_less_180 = extremity_mask[node] 123 | # do not check the other neighbouring edge of vertex1 in the future (has been considered already) 124 | edge_idx = vertex_edge_idxs[node][edge2discard] 125 | return edge_idx, range_less_180, v1, v2 126 | 127 | for edge in edges_to_check: 128 | if edge in edges_to_skip: 129 | continue 130 | 131 | v1, v2 = edge_vertex_idxs[edge] 132 | 133 | # the "view range" of an edge from a query point (spanned by the two vertices of the edge) 134 | # is always < 180deg 135 | # edge case: edge is running through the query point -> 180 deg 136 | range_less_180 = True 137 | lies_on_edge = False 138 | 139 | if distances[v1] == 0.0: 140 | angle_range = np.inf 141 | # vertex1 of the edge has the same coordinates as the query vertex 142 | # (note: not identical, does not belong to the same polygon!) 143 | # -> the origin lies on the edge 144 | lies_on_edge = True 145 | edge_to_skip, range_less_180, v1, v2 = skip_edge( 146 | node=v1, 147 | edge2discard=0, 148 | ) 149 | edges_to_skip.add(edge_to_skip) 150 | elif distances[v2] == 0.0: 151 | angle_range = np.inf 152 | # same for vertex2 of the edge 153 | # NOTE: it is unsupported that v1 as well as v2 have the same coordinates as the query vertex 154 | # (edge with length 0) 155 | lies_on_edge = True 156 | edge_to_skip, range_less_180, v1, v2 = skip_edge( 157 | node=v2, 158 | edge2discard=1, 159 | ) 160 | edges_to_skip.add(edge_to_skip) 161 | 162 | repr1 = representations[v1] 163 | repr2 = representations[v2] 164 | # case: a 'regular' edge 165 | angle_range = abs(repr1 - repr2) 166 | if angle_range == 2.0: # 180deg -> on the edge 167 | lies_on_edge = True 168 | elif angle_range > 2.0: 169 | # angle range blocked by a single edge is always <180 degree 170 | angle_range = 4 - angle_range 171 | 172 | edge_angle_range[edge] = angle_range 173 | samples[edge] = (v1, v2, repr1, repr2, lies_on_edge, range_less_180) 174 | 175 | # edges with the highest angle range first 176 | edges_prioritised = sorted( 177 | edge_angle_range.keys(), reverse=True, key=lambda e: edge_angle_range[e] 178 | ) 179 | 180 | visibles = set() 181 | # goal: eliminating all vertices lying behind any edge ("blocking the view") 182 | for edge in edges_prioritised: 183 | if len(candidates) == 0: 184 | break 185 | 186 | # for all candidate edges check if there are any candidate vertices (besides the ones belonging to the edge) 187 | # within this angle range 188 | v1, v2, repr1, repr2, lies_on_edge, range_less_180 = samples[edge] 189 | if lies_on_edge: 190 | # the query vertex lies on an edge (or vertex) 191 | # the neighbouring edges are visible for sure 192 | # attention: only add to visible set if vertex was a candidate! 193 | try: 194 | candidates.remove(v1) 195 | visibles.add(v1) 196 | except KeyError: 197 | pass 198 | try: 199 | candidates.remove(v2) 200 | visibles.add(v2) 201 | except KeyError: 202 | pass 203 | idxs_behind = candidates 204 | # all the candidates between the two vertices v1 v2 are not visible for sure 205 | # candidates with the same representation must not be deleted, because they can be visible! 206 | equal_repr_allowed = False 207 | 208 | else: 209 | # case: a 'regular' edge 210 | # eliminate all candidates which are blocked by the edge 211 | # that means inside the angle range spanned by the edge AND actually behind it 212 | idxs_behind = candidates.copy() 213 | # Attention: the vertices belonging to the edge itself (its vertices) must not be checked, 214 | # use discard() instead of remove() to not raise an error (they might not be candidates) 215 | idxs_behind.discard(v1) 216 | idxs_behind.discard(v2) 217 | # candidates with the same representation as v1 or v2 should be considered. 218 | # they may be visible, but must be ruled out if they lie behind any edge! 219 | equal_repr_allowed = True 220 | 221 | idxs_behind = _find_within_range( 222 | repr1, 223 | repr2, 224 | idxs_behind, 225 | range_less_180, 226 | equal_repr_allowed, 227 | representations, 228 | ) 229 | if not lies_on_edge: 230 | # Note: when the origin lies on the edge, all candidates within the angle range lie behind the edge 231 | # -> actual "behind/in front" checks can be skipped! 232 | idxs_behind = find_candidates_behind( 233 | origin, v1, v2, idxs_behind, distances, coords 234 | ) 235 | 236 | # vertices behind any edge are not visible and should not be considered any further 237 | candidates.difference_update(idxs_behind) 238 | 239 | # all edges have been checked 240 | # all remaining vertices were not concealed behind any edge and hence are visible 241 | visibles.update(candidates) 242 | return _clean_visibles(visibles, representations, distances) 243 | 244 | 245 | def compile_boundary_data(env): 246 | boundary, holes = env 247 | boundary = np.array(boundary, dtype=configs.DTYPE_FLOAT) 248 | holes = [np.array(hole, dtype=configs.DTYPE_FLOAT) for hole in holes] 249 | # (coords, extremity_indices, extremity_mask, vertex_edge_idxs, edge_vertex_idxs) 250 | return utils.compile_polygon_datastructs(boundary, holes) 251 | 252 | 253 | def _yield_input_args(boundary_data): 254 | coords, extremity_indices, extremity_mask, vertex_edge_idxs, edge_vertex_idxs = ( 255 | boundary_data 256 | ) 257 | candidates = set(np.where(extremity_mask)[0]) 258 | nr_edges = len(edge_vertex_idxs) 259 | edges_to_check = set(range(nr_edges)) 260 | for origin in range(len(coords)): # all possible vertices as origins 261 | reps_n_distances = utils.cmp_reps_n_distances(origin, coords) 262 | representations, distances = reps_n_distances 263 | 264 | # the origin itself is not a candidate (always visible) 265 | candidates_ = candidates - {origin} 266 | # do not check the 2 edges having the origin as vertex 267 | edges_to_check_ = edges_to_check - set(vertex_edge_idxs[origin]) 268 | yield ( 269 | origin, 270 | candidates_, 271 | edges_to_check_, 272 | coords, 273 | representations, 274 | distances, 275 | edge_vertex_idxs, 276 | vertex_edge_idxs, 277 | extremity_mask, 278 | ) 279 | 280 | 281 | def _yield_reference(boundary_data): 282 | for ( 283 | origin, 284 | candidates, 285 | edges_to_check, 286 | coords, 287 | representations, 288 | distances, 289 | edge_vertex_idxs, 290 | vertex_edge_idxs, 291 | extremity_mask, 292 | ) in _yield_input_args(boundary_data): 293 | visibles_expected = find_visible_reference( 294 | origin, 295 | candidates, 296 | edges_to_check, 297 | coords, 298 | representations, 299 | distances, 300 | edge_vertex_idxs, 301 | vertex_edge_idxs, 302 | extremity_mask, 303 | ) 304 | yield ( 305 | origin, 306 | candidates, 307 | edges_to_check, 308 | coords, 309 | representations, 310 | distances, 311 | edge_vertex_idxs, 312 | vertex_edge_idxs, 313 | extremity_mask, 314 | visibles_expected, 315 | ) 316 | 317 | 318 | grid_env = PolygonEnvironment() 319 | grid_env.store_grid_world(*GRID_ENV_PARAMS, simplify=False, validate=False) 320 | grid_env_polygons = grid_env.boundary_polygon, grid_env.holes 321 | 322 | all_envs = POLYGON_ENVS + [grid_env_polygons] 323 | all_env_boundary_data = [compile_boundary_data(env) for env in all_envs] 324 | test_cases_expected = [list(_yield_reference(env)) for env in all_env_boundary_data] 325 | test_cases_expected = list(itertools.chain.from_iterable(test_cases_expected)) 326 | 327 | 328 | @pytest.mark.parametrize( 329 | "origin,candidates,edges_to_check,coords,representations,distances,edge_vertex_idxs,vertex_edge_idxs,extremity_mask,visibles_expected", 330 | test_cases_expected, 331 | ) 332 | def test_find_visible( 333 | origin, 334 | candidates, 335 | edges_to_check, 336 | coords, 337 | representations, 338 | distances, 339 | edge_vertex_idxs, 340 | vertex_edge_idxs, 341 | extremity_mask, 342 | visibles_expected, 343 | ): 344 | visibles_found = utils.find_visible( 345 | origin, 346 | candidates, 347 | edges_to_check, 348 | coords, 349 | representations, 350 | distances, 351 | edge_vertex_idxs, 352 | vertex_edge_idxs, 353 | extremity_mask, 354 | ) 355 | assert ( 356 | visibles_found == visibles_expected 357 | ), f"expected {visibles_expected} but got {visibles_found}" 358 | -------------------------------------------------------------------------------- /extremitypathfinder/extremitypathfinder.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import warnings 3 | from typing import Dict, Iterable, List, Optional, Set, Tuple 4 | 5 | import networkx as nx 6 | import numpy as np 7 | 8 | from extremitypathfinder import configs 9 | from extremitypathfinder import types as t 10 | from extremitypathfinder import utils 11 | from extremitypathfinder.configs import DEFAULT_PICKLE_NAME 12 | from extremitypathfinder.types import ( 13 | InputCoord, 14 | InputCoordList, 15 | Length, 16 | ObstacleIterator, 17 | Path, 18 | ) 19 | 20 | 21 | class PolygonEnvironment: 22 | """Class allowing to use polygons to represent "2D environments" and use them for path finding. 23 | 24 | Keeps a "loaded" and prepared environment for consecutive path queries. 25 | Internally uses a visibility graph optimised for shortest path finding. 26 | General approach and some optimisations theoretically described in: 27 | [1] Vinther, Anders Strand-Holm, Magnus Strand-Holm Vinther, and Peyman Afshani. 28 | "`Pathfinding in Two-dimensional Worlds 29 | `__" 30 | 31 | TODO document parameters 32 | """ 33 | 34 | nr_edges: int 35 | prepared: bool = False 36 | holes: List[np.ndarray] 37 | extremity_indices: np.ndarray 38 | reprs_n_distances: Dict[int, np.ndarray] 39 | graph: t.Graph 40 | # TODO 41 | temp_graph: Optional[t.Graph] = ( 42 | None # for storing and plotting the graph during a query 43 | ) 44 | boundary_polygon: np.ndarray 45 | coords: np.ndarray 46 | edge_vertex_idxs: np.ndarray 47 | extremity_mask: np.ndarray 48 | vertex_edge_idxs: np.ndarray 49 | 50 | @property 51 | def nr_edges(self) -> int: 52 | return self.nr_vertices 53 | 54 | @property 55 | def all_extremities(self) -> List[Tuple]: 56 | coords = self.coords 57 | return [tuple(coords[i]) for i in self.extremity_indices] 58 | 59 | @property 60 | def all_vertices(self) -> List[Tuple]: 61 | coords = self.coords 62 | return [tuple(coords[i]) for i in range(self.nr_vertices)] 63 | 64 | def store( 65 | self, 66 | boundary_coordinates: InputCoordList, 67 | list_of_hole_coordinates: InputCoordList, 68 | validate: bool = False, 69 | ): 70 | """saves the passed input polygons in the environment 71 | 72 | .. note:: the passed polygons must meet these requirements: 73 | 74 | * given as numpy or python array of coordinate tuples: ``[(x1,y1), (x2,y2,)...]`` 75 | * no repetition of the first point at the end 76 | * at least 3 vertices (no single points or lines allowed) 77 | * no consequent vertices with identical coordinates in the polygons (same coordinates allowed) 78 | * no self intersections 79 | * edge numbering has to follow these conventions: boundary polygon counter clockwise, holes clockwise 80 | 81 | :param boundary_coordinates: array of coordinates with counter clockwise edge numbering 82 | :param list_of_hole_coordinates: array of coordinates with clockwise edge numbering 83 | :param validate: whether the requirements of the data should be tested 84 | 85 | :raises AssertionError: when validate=True and the input is invalid. 86 | """ 87 | self.prepared = False 88 | # loading the map 89 | boundary_coordinates = np.array(boundary_coordinates, dtype=configs.DTYPE_FLOAT) 90 | list_of_hole_coordinates = [ 91 | np.array(hole_coords, dtype=configs.DTYPE_FLOAT) 92 | for hole_coords in list_of_hole_coordinates 93 | ] 94 | if validate: 95 | utils.check_data_requirements( 96 | boundary_coordinates, list_of_hole_coordinates 97 | ) 98 | 99 | # Note: independent copy! 100 | self.holes = list_of_hole_coordinates 101 | self.boundary_polygon = boundary_coordinates 102 | 103 | # TODO redundant data. refactor all functions to only use one format 104 | ( 105 | self.coords, 106 | self.extremity_indices, 107 | self.extremity_mask, 108 | self.vertex_edge_idxs, 109 | self.edge_vertex_idxs, 110 | ) = utils.compile_polygon_datastructs( 111 | boundary_coordinates, list_of_hole_coordinates 112 | ) 113 | 114 | nr_total_pts = self.edge_vertex_idxs.shape[0] 115 | self.nr_vertices = nr_total_pts 116 | self.reprs_n_distances = utils.cmp_reps_n_distance_dict( 117 | self.coords, self.extremity_indices 118 | ) 119 | 120 | # start and goal points will be stored after all polygon coordinates 121 | self.idx_start = nr_total_pts 122 | self.idx_goal = nr_total_pts + 1 123 | 124 | self.prepare() 125 | 126 | def store_grid_world( 127 | self, 128 | size_x: int, 129 | size_y: int, 130 | obstacle_iter: ObstacleIterator, 131 | simplify: bool = True, 132 | validate: bool = False, 133 | ): 134 | """Convert a grid-like into a polygon environment and save it 135 | 136 | Prerequisites: grid world must not have single non-obstacle cells which are surrounded by obstacles 137 | ("white cells in black surrounding" = useless for path planning) 138 | 139 | :param size_x: the horizontal grid world size 140 | :param size_y: the vertical grid world size 141 | :param obstacle_iter: an iterable of coordinate pairs (x,y) representing blocked grid cells (obstacles) 142 | :param validate: whether the input should be validated 143 | :param simplify: whether the polygons should be simplified or not. reduces edge amount, allow diagonal edges 144 | """ 145 | boundary_coordinates, list_of_hole_coordinates = utils.convert_gridworld( 146 | size_x, size_y, obstacle_iter, simplify 147 | ) 148 | self.store(boundary_coordinates, list_of_hole_coordinates, validate) 149 | 150 | def export_pickle(self, path: str = DEFAULT_PICKLE_NAME): 151 | print("storing map class in:", path) 152 | with open(path, "wb") as f: 153 | pickle.dump(self, f) 154 | print("done.\n") 155 | 156 | def prepare(self): 157 | """Computes a visibility graph optimized (=reduced) for path planning and stores it 158 | 159 | Computes all directly reachable extremities based on visibility and their distance to each other 160 | pre-procesing of the map. pre-computation for faster shortest path queries 161 | optimizes graph further at construction time 162 | 163 | NOTE: initialise the graph with all extremities. 164 | even if a node has no edges (visibility to other extremities, dangling node), 165 | it must still be included! 166 | 167 | .. note:: 168 | Multiple polygon vertices might have identical coords_rel. 169 | They must be treated as distinct vertices here, since their attached edges determine visibility. 170 | In the created graph however, these nodes must be merged at the end to avoid ambiguities! 171 | 172 | .. note:: 173 | Pre computing the shortest paths between all directly reachable extremities 174 | and storing them in the graph would not be an advantage, because then the graph is fully connected. 175 | A* would visit every node in the graph at least once (-> disadvantage!). 176 | """ 177 | if self.prepared: # idempotent 178 | warnings.warn( 179 | "called .prepare() on already prepared map. skipping...", stacklevel=1 180 | ) 181 | return 182 | 183 | self.graph = utils.compute_graph( 184 | self.nr_edges, 185 | self.extremity_indices, 186 | self.reprs_n_distances, 187 | self.coords, 188 | self.edge_vertex_idxs, 189 | self.extremity_mask, 190 | self.vertex_edge_idxs, 191 | ) 192 | self.prepared = True 193 | 194 | def within_map(self, coords: np.ndarray) -> bool: 195 | """checks if the given coordinates lie within the boundary polygon and outside of all holes 196 | 197 | :param coords: numerical tuple representing coordinates 198 | :return: whether the given coordinate is a valid query point 199 | """ 200 | return utils.is_within_map(coords, self.boundary_polygon, self.holes) 201 | 202 | def get_visible_idxs( 203 | self, 204 | origin: int, 205 | candidates: Iterable[int], 206 | coords: np.ndarray, 207 | vert_idx2repr: np.ndarray, 208 | vert_idx2dist: np.ndarray, 209 | ) -> Set[int]: 210 | # Note: points with equal coordinates should not be considered visible (will be merged later) 211 | candidates = {i for i in candidates if not vert_idx2dist[i] == 0.0} 212 | edge_idxs2check = set(range(self.nr_edges)) 213 | return utils.find_visible( 214 | origin, 215 | candidates, 216 | edge_idxs2check, 217 | coords, 218 | vert_idx2repr, 219 | vert_idx2dist, 220 | self.edge_vertex_idxs, 221 | self.vertex_edge_idxs, 222 | self.extremity_mask, 223 | ) 224 | 225 | def find_shortest_path( 226 | self, 227 | start_coordinates: InputCoord, 228 | goal_coordinates: InputCoord, 229 | free_space_after: bool = True, 230 | verify: bool = True, 231 | ) -> Tuple[Path, Length]: 232 | """computes the shortest path and its length between start and goal node 233 | 234 | :param start_coordinates: a (x,y) coordinate tuple representing the start node 235 | :param goal_coordinates: a (x,y) coordinate tuple representing the goal node 236 | :param free_space_after: whether the created temporary search graph graph 237 | should be deleted after the query 238 | :param verify: whether it should be checked if start and goal points really lie inside the environment. 239 | if points close to or on polygon edges should be accepted as valid input, set this to ``False``. 240 | :return: a tuple of shortest path and its length. ([], None) if there is no possible path. 241 | """ 242 | # path planning query: 243 | # make sure the map has been loaded and prepared 244 | if self.boundary_polygon is None: 245 | raise ValueError("No Polygons have been loaded into the map yet.") 246 | 247 | coords_start = np.array(start_coordinates, dtype=float) 248 | coords_goal = np.array(goal_coordinates, dtype=float) 249 | if verify and not self.within_map(coords_start): 250 | raise ValueError("start point does not lie within the map") 251 | if verify and not self.within_map(coords_goal): 252 | raise ValueError("goal point does not lie within the map") 253 | 254 | if np.array_equal(coords_start, coords_goal): 255 | # start and goal are identical and can be reached instantly 256 | return [start_coordinates, goal_coordinates], 0.0 257 | 258 | start = self.idx_start 259 | goal = self.idx_goal 260 | # temporarily extend data structure 261 | # Note: start and goal nodes could be identical with one ore more of the vertices 262 | # BUT: this is an edge case -> compute visibility as usual and later try to merge with the graph 263 | coords = np.append(self.coords, (coords_start, coords_goal), axis=0) 264 | self._coords_tmp = coords # for plotting including the start and goal indices 265 | 266 | # check the goal node first (earlier termination possible) 267 | origin = goal 268 | # the visibility of only the graph nodes has to be checked (not all extremities!) 269 | # IMPORTANT: also check if the start node is visible from the goal node! 270 | candidate_idxs: Set[int] = set(self.graph.nodes) 271 | candidate_idxs.add(start) 272 | repr_n_dists = utils.cmp_reps_n_distances(origin, coords) 273 | self.reprs_n_distances[origin] = repr_n_dists 274 | vert_idx2repr, vert_idx2dist = repr_n_dists 275 | visibles_goal = self.get_visible_idxs( 276 | origin, candidate_idxs, coords, vert_idx2repr, vert_idx2dist 277 | ) 278 | if len(visibles_goal) == 0: 279 | # The goal node does not have any neighbours. Hence there is not possible path to the goal. 280 | return [], None 281 | 282 | # IMPORTANT geometrical property of this problem: it is always shortest to directly reach a node 283 | # instead of visiting other nodes first (there is never an advantage through reduced edge weight) 284 | # -> when goal is directly reachable, there can be no other shorter path to it. Terminate 285 | if start in visibles_goal: 286 | d = vert_idx2dist[start] 287 | return [start_coordinates, goal_coordinates], d 288 | 289 | # create temporary graph 290 | # DirectedHeuristicGraph implements __deepcopy__() to not change the original precomputed self.graph 291 | # but to still not create real copies of vertex instances! 292 | graph = self.graph.copy() 293 | # TODO avoid real copy to make make more performant 294 | # graph = self.graph 295 | # nr_edges_before = len(graph.edges) 296 | 297 | # add edges: extremity (i) <-> goal 298 | # Note: also here unnecessary edges in the graph could be deleted 299 | # optimising the graph here however is more expensive than beneficial, 300 | # as the graph is only being used for a single query 301 | for i in visibles_goal: 302 | graph.add_edge(i, goal, weight=vert_idx2dist[i]) 303 | 304 | origin = start 305 | # the visibility of only the graphs nodes have to be checked 306 | # the goal node does not have to be considered, because of the earlier check 307 | repr_n_dists = utils.cmp_reps_n_distances(origin, coords) 308 | self.reprs_n_distances[origin] = repr_n_dists 309 | vert_idx2repr, vert_idx2dist = repr_n_dists 310 | visibles_start = self.get_visible_idxs( 311 | origin, candidate_idxs, coords, vert_idx2repr, vert_idx2dist 312 | ) 313 | 314 | if len(visibles_start) == 0: 315 | # The start node does not have any neighbours. Hence there is no possible path to the goal. 316 | return [], None 317 | 318 | # add edges: start <-> extremity (i) 319 | for i in visibles_start: 320 | graph.add_edge(start, i, weight=vert_idx2dist[i]) 321 | 322 | # apply mapping to start and goal index as well 323 | start_mapped = utils.find_identical_single( 324 | start, graph.nodes, self.reprs_n_distances 325 | ) 326 | if start_mapped != start: 327 | nx.relabel_nodes(graph, {start: start_mapped}, copy=False) 328 | 329 | goal_mapped = utils.find_identical_single( 330 | goal, graph.nodes, self.reprs_n_distances 331 | ) 332 | if goal_mapped != goal_mapped: 333 | nx.relabel_nodes(graph, {goal: goal_mapped}, copy=False) 334 | 335 | self._idx_start_tmp, self._idx_goal_tmp = ( 336 | start_mapped, 337 | goal_mapped, 338 | ) # for plotting 339 | 340 | def l2_distance(n1, n2): 341 | return utils.get_distance(n1, n2, self.reprs_n_distances) 342 | 343 | try: 344 | id_path = nx.astar_path( 345 | graph, start_mapped, goal_mapped, heuristic=l2_distance, weight="weight" 346 | ) 347 | except nx.exception.NetworkXNoPath: 348 | return [], None 349 | 350 | # clean up 351 | # TODO re-use the same graph. need to keep track of all merged edges 352 | # if start_mapped == start: 353 | # graph.remove_node(start) 354 | # if goal_mapped==goal: 355 | # graph.remove_node(goal) 356 | # nr_edges_after = len(graph.edges) 357 | # if not nr_edges_after == nr_edges_before: 358 | # raise ValueError 359 | 360 | if free_space_after: 361 | del graph # free the memory 362 | else: 363 | self.temp_graph = graph 364 | 365 | # compute distance 366 | distance = 0.0 367 | v1 = id_path[0] 368 | for v2 in id_path[1:]: 369 | distance += l2_distance(v1, v2) 370 | v1 = v2 371 | 372 | # extract the coordinates from the path 373 | path = [tuple(coords[i]) for i in id_path] 374 | return path, distance 375 | -------------------------------------------------------------------------------- /extremitypathfinder/utils.py: -------------------------------------------------------------------------------- 1 | import itertools 2 | import json 3 | import pickle 4 | from itertools import combinations 5 | from typing import Dict, Iterable, List, Set, Tuple 6 | 7 | import networkx as nx 8 | import numpy as np 9 | import numpy.linalg 10 | 11 | from extremitypathfinder import configs 12 | from extremitypathfinder import types as t 13 | from extremitypathfinder.configs import ( 14 | BOUNDARY_JSON_KEY, 15 | DEFAULT_PICKLE_NAME, 16 | HOLES_JSON_KEY, 17 | ) 18 | from extremitypathfinder.utils_numba import ( 19 | _angle_rep_inverse, 20 | _compute_repr_n_dist, 21 | _fill_edge_vertex_idxs, 22 | _fill_extremity_mask, 23 | _has_clockwise_numbering, 24 | _inside_polygon, 25 | _lies_behind, 26 | _no_identical_consequent_vertices, 27 | ) 28 | 29 | 30 | def cmp_reps_n_distances(orig_idx: int, coords: np.ndarray) -> np.ndarray: 31 | coords_orig = coords[orig_idx] 32 | coords_translated = coords - coords_orig 33 | repr_n_dists = np.apply_along_axis( 34 | _compute_repr_n_dist, axis=1, arr=coords_translated 35 | ) 36 | return repr_n_dists.T 37 | 38 | 39 | def cmp_reps_n_distance_dict( 40 | coords: np.ndarray, extremity_indices: np.ndarray 41 | ) -> Dict[int, np.ndarray]: 42 | # Note: distance and angle representation relation are symmetric, 43 | # but exploiting this has been found to be slower than using the numpy functionality with slight overhead 44 | reps_n_distance_dict = { 45 | i: cmp_reps_n_distances(i, coords) for i in extremity_indices 46 | } 47 | return reps_n_distance_dict 48 | 49 | 50 | def is_within_map( 51 | p: np.ndarray, boundary: np.ndarray, holes: Iterable[np.ndarray] 52 | ) -> bool: 53 | if not _inside_polygon(p, boundary, border_value=True): 54 | return False 55 | for hole in holes: 56 | if _inside_polygon(p, hole, border_value=False): 57 | return False 58 | return True 59 | 60 | 61 | def _get_intersection_status(p1, p2, q1, q2): 62 | # return: 63 | # 0: no intersection 64 | # 1: intersection in ]p1;p2[ 65 | # TODO support 2 remaining possibilities 66 | # 2: intersection directly in p1 or p2 67 | # 3: intersection directly in q1 or q2 68 | # solve the set of equations 69 | # (p2-p1) lambda + (p1) = (q2-q1) mu + (q1) 70 | # in matrix form A x = b: 71 | # [(p2-p1) (q1-q2)] (lambda, mu)' = (q1-p1) 72 | A = np.array([p2 - p1, q1 - q2]).T 73 | b = np.array(q1 - p1) 74 | try: 75 | x = np.linalg.solve(A, b) 76 | except np.linalg.LinAlgError: 77 | # line segments are parallel (matrix is singular, set of equations is not solvable) 78 | return 0 79 | 80 | # not crossing the line segment is considered to be ok 81 | # so x == 0.0 or x == 1.0 is not considered an intersection 82 | # assert np.allclose((p2 - p1) * x[0] + p1, (q2 - q1) * x[1] + q1) 83 | # assert np.allclose(np.dot(A, x), b) 84 | if x[0] <= 0.0 or x[1] <= 0.0 or x[0] >= 1.0 or x[1] >= 1.0: 85 | return 0 86 | # if np.all(0.0 <= x) and np.all(x <= 1.0): 87 | # return 2 88 | else: 89 | return 1 90 | 91 | 92 | def _no_self_intersection(coords): 93 | polygon_length = len(coords) 94 | # again_check = [] 95 | for index_p1, index_q1 in combinations(range(polygon_length), 2): 96 | # always: index_p1 < index_q1 97 | if index_p1 == index_q1 - 1 or index_p1 == index_q1 + 1: 98 | # neighbouring edges never have an intersection 99 | continue 100 | p1, p2 = coords[index_p1], coords[(index_p1 + 1) % polygon_length] 101 | q1, q2 = coords[index_q1], coords[(index_q1 + 1) % polygon_length] 102 | intersect_status = _get_intersection_status(p1, p2, q1, q2) 103 | if intersect_status == 1: 104 | return False 105 | # if intersect_status == 2: 106 | # TODO 4 different options. check which side the other edge lies on. 107 | # if edge changes sides this is a an intersection 108 | # again_check.append((p1, p2, q1, q2)) 109 | # print(p1, p2, q1, q2) 110 | 111 | # TODO check for intersections across 2 edges! use computed intersection 112 | 113 | return True 114 | 115 | 116 | def _check_polygon(polygon): 117 | """ensures that all the following conditions on the polygons are fulfilled: 118 | - must at least contain 3 vertices 119 | - no consequent vertices with identical coordinates in the polygons! In general might have the same coordinates 120 | - a polygon must not have self intersections (intersections with other polygons are allowed) 121 | """ 122 | if not polygon.shape[0] >= 3: 123 | raise TypeError("Given polygons must at least contain 3 vertices.") 124 | if not polygon.shape[1] == 2: 125 | raise TypeError("Each point of a polygon must consist of two values (x,y).") 126 | if not _no_identical_consequent_vertices(polygon): 127 | raise ValueError("Consequent vertices of a polynomial must not be identical.") 128 | if not _no_self_intersection(polygon): 129 | raise ValueError("The given polygon has self intersections") 130 | 131 | 132 | def check_data_requirements( 133 | boundary_coords: np.ndarray, list_hole_coords: List[np.ndarray] 134 | ): 135 | """ensures that all the following conditions on the polygons are fulfilled: 136 | - basic polygon requirements (s. above) 137 | - edge numbering has to follow this convention (for easier computations): 138 | * outer boundary polygon: counter clockwise 139 | * holes: clockwise 140 | 141 | :param boundary_coords: 142 | :param list_hole_coords: 143 | :return: 144 | """ 145 | _check_polygon(boundary_coords) 146 | if _has_clockwise_numbering(boundary_coords): 147 | raise ValueError( 148 | "Vertex numbering of the boundary polygon must be counter clockwise." 149 | ) 150 | for hole_coords in list_hole_coords: 151 | _check_polygon(hole_coords) 152 | if not _has_clockwise_numbering(hole_coords): 153 | raise ValueError("Vertex numbering of hole polygon must be clockwise.") 154 | 155 | 156 | def _find_within_range( 157 | repr1: float, 158 | repr2: float, 159 | candidate_idxs: Set[int], 160 | angle_range_less_180: bool, 161 | equal_repr_allowed: bool, 162 | representations: np.ndarray, 163 | ) -> Set[int]: 164 | """ 165 | filters out all vertices whose representation lies within the range between 166 | the two given angle representations 167 | which range ('clockwise' or 'counter-clockwise') should be checked is determined by: 168 | - query angle (range) is < 180deg or not (>= 180deg) 169 | :param repr1: 170 | :param repr2: 171 | :param candidate_idxs: 172 | :param angle_range_less_180: whether the angle between repr1 and repr2 is < 180 deg 173 | :param equal_repr_allowed: whether vertices with the same representation should also be returned 174 | :param representations: 175 | :return: 176 | """ 177 | if len(candidate_idxs) == 0: 178 | return set() 179 | 180 | repr_diff = abs(repr1 - repr2) 181 | if repr_diff == 0.0: 182 | return set() 183 | 184 | min_repr = min(repr1, repr2) 185 | max_repr = max(repr1, repr2) # = min_angle + angle_diff 186 | 187 | def repr_within(r): 188 | # Note: vertices with the same representation will NOT be returned! 189 | return min_repr < r < max_repr 190 | 191 | # depending on the angle the included range is clockwise or anti-clockwise 192 | # (from min_repr to max_val or the other way around) 193 | # when the range contains the 0.0 value (transition from 3.99... -> 0.0) 194 | # it is easier to check if a representation does NOT lie within this range 195 | # -> invert filter condition 196 | # special case: angle == 180deg 197 | on_line_inv = repr_diff == 2.0 and repr1 >= repr2 198 | # which range to filter is determined by the order of the points 199 | # since the polygons follow a numbering convention, 200 | # the 'left' side of p1-p2 always lies inside the map 201 | # -> filter out everything on the right side (='outside') 202 | # ^: XOR 203 | inversion_condition = on_line_inv or ((repr_diff < 2.0) ^ angle_range_less_180) 204 | 205 | def within_filter_func(r: float) -> bool: 206 | repr_eq = r == min_repr or r == max_repr 207 | if repr_eq and equal_repr_allowed: 208 | return True 209 | if repr_eq and not equal_repr_allowed: 210 | return False 211 | 212 | res = repr_within(r) 213 | if inversion_condition: 214 | res = not res 215 | return res 216 | 217 | idxs_within = {i for i in candidate_idxs if within_filter_func(representations[i])} 218 | return idxs_within 219 | 220 | 221 | def get_neighbour_idxs( 222 | i: int, vertex_edge_idxs: np.ndarray, edge_vertex_idxs: np.ndarray 223 | ) -> Tuple[int, int]: 224 | edge_idx1, edge_idx2 = vertex_edge_idxs[i] 225 | neigh_idx1 = edge_vertex_idxs[edge_idx1, 0] 226 | neigh_idx2 = edge_vertex_idxs[edge_idx2, 1] 227 | return neigh_idx1, neigh_idx2 228 | 229 | 230 | def find_visible( 231 | origin: int, 232 | candidates: Set[int], 233 | edges_to_check: Set[int], 234 | coords: np.ndarray, 235 | representations: np.ndarray, 236 | distances: np.ndarray, 237 | edge_vertex_idxs: np.ndarray, 238 | vertex_edge_idxs: np.ndarray, 239 | extremity_mask: np.ndarray, 240 | ) -> Set[int]: 241 | """ 242 | TODO 243 | for all origin extremities 244 | precompute all required ranges etc for all edges 245 | sort all edges after their minimum representation 246 | also sort the candidate extremities after their angle representation 247 | for every edge_idx 248 | if the minimum representation of the edge_idx is smaller than the repr of the candidate: 249 | check the next candidate, move start candidate pointer along 250 | if the maximum representation of the edge_idx is bigger than the repr of the candidate: 251 | check the next edge_idx, start at the start candidate index again 252 | check if the edge_idx is blocking the visibility of the node: if yes delete from candidates 253 | check the next node 254 | 255 | optimisation: if flag, invert edge_idx representations and also eliminate 256 | all candidates within range (without lies behind check!) 257 | 258 | 259 | :param origin: the vertex for which the visibility to the other candidates should be checked. 260 | :param candidates: the set of all vertex ids which should be checked for visibility. 261 | IMPORTANT: is being manipulated, so has to be a copy! 262 | IMPORTANT: must not contain any vertices with equal coordinates (e.g. the origin vertex itself)! 263 | :param edges_to_check: the set of edges which determine visibility 264 | :return: a set of all vertices visible from the origin 265 | """ 266 | nr_candidates_total = len(candidates) 267 | if nr_candidates_total == 0: 268 | return candidates 269 | # TODO immutable 270 | # eliminate candidates with equal representations: only keep the closest (min dist) 271 | candidates_sorted = _eliminate_eq_candidates(candidates, distances, representations) 272 | 273 | ( 274 | crossing_edges, 275 | edges_is_crossing, 276 | edges_max_dist, 277 | edges_max_rep, 278 | edges_min_rep, 279 | non_crossing_edges, 280 | edge_vertex_idxs, 281 | ) = _compile_visibility_datastructs( 282 | distances, 283 | edge_vertex_idxs, 284 | edges_to_check, 285 | extremity_mask, 286 | representations, 287 | vertex_edge_idxs, 288 | ) 289 | 290 | # check non-crossing edges 291 | # TODO skipped edges. argsort 292 | # sort after the minimum representation 293 | edge_idxs_sorted = sorted(non_crossing_edges, key=lambda e: edges_min_rep[e]) 294 | # edge_ptr_iter = iter(ptr for ptr in edge_ptrs if not edges_is_crossing[ptr]) 295 | _check_candidates( 296 | candidates_sorted, 297 | edge_idxs_sorted, 298 | edges_max_rep, 299 | edges_min_rep, 300 | origin, 301 | distances, 302 | representations, 303 | coords, 304 | edge_vertex_idxs, 305 | edges_max_dist, 306 | ) 307 | 308 | # when there are no origin-crossing edges, we are done 309 | if len(crossing_edges) == 0: 310 | return set(candidates_sorted) 311 | 312 | # check origin-crossing edges 313 | candidates_sorted, edges_max_rep, edges_min_rep, representations = rotate_crossing( 314 | candidates_sorted, 315 | edges_is_crossing, 316 | edges_max_rep, 317 | edges_min_rep, 318 | representations, 319 | ) 320 | 321 | # start with checking the first candidate again 322 | edge_idxs_sorted = sorted(crossing_edges, key=lambda e: edges_min_rep[e]) 323 | _check_candidates( 324 | candidates_sorted, 325 | edge_idxs_sorted, 326 | edges_max_rep, 327 | edges_min_rep, 328 | origin, 329 | distances, 330 | representations, 331 | coords, 332 | edge_vertex_idxs, 333 | edges_max_dist, 334 | ) 335 | # TODO avoid conversion? 336 | candidates_ = set(candidates_sorted) 337 | return candidates_ 338 | 339 | 340 | def _eliminate_eq_candidates( 341 | candidates: List[int], distances: np.ndarray, representations: np.ndarray 342 | ) -> List[int]: 343 | # sort after angle representation, then after distance ascending 344 | candidates_sorted = sorted( 345 | candidates, key=lambda i: (representations[i], distances[i]) 346 | ) 347 | rep_prev = representations[candidates_sorted[0]] 348 | i = 1 349 | while i < len(candidates_sorted): 350 | candidate = candidates_sorted[i] 351 | rep = representations[candidate] 352 | if np.isnan(rep) or rep == rep_prev: 353 | # the candidate is equal to the origin OR 354 | # two candidates have equal angle representation 355 | # only keep the closest (=1st) 356 | candidates_sorted.pop(i) 357 | else: 358 | rep_prev = rep 359 | i += 1 360 | 361 | return candidates_sorted 362 | 363 | 364 | def _compile_visibility_datastructs( 365 | distances, 366 | edge_vertex_idxs, 367 | edges_to_check, 368 | extremity_mask, 369 | representations, 370 | vertex_edge_idxs, 371 | ): 372 | edges = list(edges_to_check) 373 | nr_edges_total = len(edge_vertex_idxs) 374 | edges_min_rep = np.zeros(nr_edges_total, dtype=float) 375 | edges_max_rep = np.zeros(nr_edges_total, dtype=float) 376 | edges_max_dist = np.zeros(nr_edges_total, dtype=float) 377 | edges_is_crossing = np.zeros(nr_edges_total, dtype=bool) 378 | edges_to_skip = set() 379 | crossing_edges = set() 380 | non_crossing_edges = set() 381 | copied_indices = False 382 | for e in edges: 383 | if e in edges_to_skip: 384 | # Note: edges will be skipped, because they do not appear the sets of crossing or non-crossing edges 385 | continue 386 | 387 | # Attention: introduces new indexing! beware of confusion 388 | i1, i2 = edge_vertex_idxs[e] 389 | r1, r2 = representations[i1], representations[i2] 390 | 391 | identical_node = None 392 | if np.isnan(r1): 393 | identical_node = i1 394 | elif np.isnan(r2): 395 | identical_node = i2 396 | 397 | if identical_node is not None: 398 | # one of the edge vertices is identical to origin 399 | # no points lie truly "behind" this edge as there is no "direction of sight" defined 400 | # <-> angle representation/range undefined for just this single edge 401 | # however if one considers the point neighbouring in the other direction (<-> two edges) 402 | # these two neighbouring edges define an invisible angle range 403 | # -> simply move the pointer 404 | i1, i2 = get_neighbour_idxs( 405 | identical_node, vertex_edge_idxs, edge_vertex_idxs 406 | ) 407 | r1, r2 = representations[i1], representations[i2] 408 | min_rep = min(r1, r2) 409 | max_rep = max(r1, r2) 410 | 411 | # Note: the second edge should not be considered twice 412 | e1, e2 = vertex_edge_idxs[identical_node] 413 | if e1 != e: 414 | edges_to_skip.add(e1) 415 | if e2 != e: 416 | edges_to_skip.add(e2) 417 | # TODO remove 418 | if e1 != e and e2 != e: 419 | raise ValueError() 420 | 421 | if not copied_indices: 422 | edge_vertex_idxs = edge_vertex_idxs.copy() 423 | copied_indices = True 424 | 425 | # point to the neighbouring vertices (used for looking up the coordinates) 426 | edge_vertex_idxs[e] = (i1, i2) 427 | 428 | # the "outside the polygon" angle range should be eliminated 429 | # this angle range is greater than 180 degree if the node identical to the origin is NOT an extremity 430 | deg_gr_180_exp = not extremity_mask[identical_node] 431 | deg_gr_180_actual = max_rep - min_rep > 2.0 432 | # an edge "crosses the origin" (rep: 4.0 -> 0.0) 433 | # when its vertex representation difference is unlike expected 434 | is_crossing = deg_gr_180_actual != deg_gr_180_exp 435 | 436 | # set distance to 0 in order to mark all candidates within range as "lying behind" 437 | max_dist = 0.0 438 | 439 | # mark the identical vertex as not visible (would otherwise add 0 distance edge in the graph) 440 | # # TODO needs to be added and will be combined later?! check in separate function?! 441 | # if i1 != origin: 442 | # candidates.discard(i1) 443 | else: 444 | min_rep = min(r1, r2) 445 | max_rep = max(r1, r2) 446 | rep_diff = max_rep - min_rep 447 | # special case: angle == 180deg <-> lies on the line 448 | on_the_edge = rep_diff == 2.0 449 | if on_the_edge: 450 | # "edge case": origin lies on the edge 451 | # the edge blocks the visibility to the "outside the polygon" 452 | # depending on the vertex numbering, the outside angle range crosses the origin or not 453 | # (from min_repr to max_val or the other way around) 454 | # when the range contains the 0.0 value (transition from 3.99... -> 0.0) 455 | # it is easier to check if a representation does NOT lie within this range 456 | # -> invert filter condition 457 | is_crossing = r1 > r2 458 | # TODO edge case one of the reps is 0?! 459 | # set distance to 0 in order to mark all candidates within range as "lying behind" 460 | max_dist = 0.0 461 | else: 462 | # regular edge 463 | # a single edge_idx can block at most 180 degree 464 | is_crossing = rep_diff > 2.0 465 | max_dist = max(distances[i1], distances[i2]) 466 | 467 | if is_crossing: 468 | crossing_edges.add(e) 469 | else: 470 | non_crossing_edges.add(e) 471 | 472 | edges_min_rep[e] = min_rep 473 | edges_max_rep[e] = max_rep 474 | edges_max_dist[e] = max_dist 475 | edges_is_crossing[e] = is_crossing 476 | 477 | return ( 478 | crossing_edges, 479 | edges_is_crossing, 480 | edges_max_dist, 481 | edges_max_rep, 482 | edges_min_rep, 483 | non_crossing_edges, 484 | edge_vertex_idxs, 485 | ) 486 | 487 | 488 | def rotate_crossing( 489 | candidate_idxs, edges_is_crossing, edges_max_rep, edges_min_rep, representations 490 | ): 491 | if not np.all(edges_min_rep >= 0.0): 492 | raise ValueError 493 | 494 | # TODO refactor 495 | if not np.any(edges_is_crossing): 496 | return set(candidate_idxs) 497 | 498 | # special case: edges cross the origin 499 | # trick: rotate coordinate system to avoid dealing with origin crossings 500 | # -> implementation for regular edges can be reused! 501 | # bring the minimum maximal angle representation ("supremum") of all crossing edges to 0 502 | supremum_cross_edge_rep = np.min(edges_max_rep[edges_is_crossing]) 503 | infimum_to_0 = 4.0 - supremum_cross_edge_rep 504 | # Note: adding an angle < 180deg to the minimum edge_idx representations (quadrant 1 & 2) cannot lead to "overflow" 505 | edges_min_rep[edges_is_crossing] = edges_min_rep[edges_is_crossing] + infimum_to_0 506 | edges_max_rep[edges_is_crossing] = ( 507 | edges_max_rep[edges_is_crossing] + infimum_to_0 508 | ) % 4.0 509 | # Note: all maximum representations were moved to 1. or 2. quadrant 510 | # -> became the smaller that the previously min rep! -> swap 511 | tmp = edges_min_rep 512 | edges_min_rep = edges_max_rep 513 | edges_max_rep = tmp 514 | # apply same transformation also to candidate representations 515 | # TODO avoid large copy. use dict 516 | representations = representations.copy() # IMPORTANT: work on independent copy 517 | representations[candidate_idxs] = ( 518 | representations[candidate_idxs] + infimum_to_0 519 | ) % 4.0 520 | # Note: new sorting is required 521 | candidate_idxs = sorted(candidate_idxs, key=lambda i: representations[i]) 522 | # TODO move to tests 523 | # non_nan_reps = representations[np.logical_not(np.isnan(representations))] 524 | # assert np.all(non_nan_reps >= 0.0) 525 | # assert np.all(non_nan_reps <= 4.0) 526 | # if not np.all(edges_min_rep >= 0.0): 527 | # raise ValueError 528 | # assert np.all(edges_min_rep <= 4.0) 529 | # assert np.min(edges_min_rep[edges_is_crossing]) == 0.0 530 | # assert np.all(edges_max_rep >= 0.0) 531 | # assert np.all(edges_max_rep <= 4.0) 532 | # for r_min, r_max in zip(edges_min_rep[edges_is_crossing], edges_max_rep[edges_is_crossing]): 533 | # if r_min > r_max: 534 | # raise ValueError 535 | return candidate_idxs, edges_max_rep, edges_min_rep, representations 536 | 537 | 538 | def check_candidates_one_edge( 539 | edge_min_rep: float, 540 | edge_max_rep: float, 541 | edge_max_dist: float, 542 | p1: int, 543 | p2: int, 544 | candidate_ptr: int, 545 | origin: int, 546 | candidate_indices: List[int], 547 | coords: np.ndarray, 548 | distances: np.ndarray, 549 | representations: np.ndarray, 550 | ): 551 | # start over at the same candidate than the previous edge 552 | # TODO Note: check within range: edge case, same representation than edge vertex. 553 | # do not include (edge does not block view) 554 | for candidate_idx in candidate_indices[candidate_ptr:]: 555 | candidate_rep = representations[candidate_idx] 556 | # a candidate does not have to be considered, 557 | # when its representation is smaller or equal than the minimum representation of the edge 558 | if candidate_rep >= edge_min_rep: 559 | # candidate has to be considered 560 | break 561 | # this also is the case for all consequent edges (have a larger or equal minimum representation!) 562 | candidate_ptr += 1 563 | 564 | # start at the start candidate index again 565 | candidate_ptr_curr = candidate_ptr 566 | while 1: 567 | # Note: candidate list shrinks during the iteration -> avoid index error 568 | try: 569 | candidate_idx = candidate_indices[candidate_ptr_curr] 570 | except IndexError: 571 | break 572 | 573 | if candidate_idx == p1 or candidate_idx == p2: 574 | # an edge cannot block its own vertices 575 | # move pointer to the next candidate 576 | candidate_ptr_curr += 1 577 | continue 578 | 579 | candidate_rep = representations[candidate_idx] 580 | 581 | if edge_max_rep < candidate_rep: 582 | # the maximum representation of the edge is smaller than the repr of the candidate, 583 | # -> check the next edge 584 | break 585 | 586 | # candidate representation lies between edge_min_rep and edge_max_rep 587 | # assert edge_min_rep <= candidate_rep <= edge_max_rep 588 | 589 | # check if the edge is blocking the visibility of the node: if yes delete from candidates 590 | dist_to_candidate = distances[candidate_idx] 591 | equal_reps = edge_min_rep == candidate_rep or edge_max_rep == candidate_rep 592 | if equal_reps: 593 | # edge of the candidate itself should not block visibility 594 | # distance must be truly larger to block visibility. 595 | visibility_is_blocked = dist_to_candidate > edge_max_dist 596 | else: 597 | # optimisation: if a candidate is farther away from the query point than both vertices of the edge, 598 | # it surely lies behind the edge 599 | # ATTENTION: even if a candidate is closer to the query point than both vertices of the edge, 600 | # it still needs to be checked! 601 | further_away = dist_to_candidate > edge_max_dist 602 | visibility_is_blocked = further_away or _lies_behind( 603 | p1, p2, candidate_idx, origin, coords 604 | ) 605 | 606 | if visibility_is_blocked: 607 | candidate_indices.pop(candidate_ptr_curr) 608 | # Note: keep ptr at the same position (list shrank) 609 | else: 610 | # move pointer to the next candidate 611 | candidate_ptr_curr += 1 612 | 613 | return candidate_ptr 614 | 615 | 616 | def _check_candidates( 617 | candidate_idxs: List[int], 618 | edge_idxs_sorted: List[int], 619 | edges_max_rep: np.ndarray, 620 | edges_min_rep: np.ndarray, 621 | origin: int, 622 | distances: np.ndarray, 623 | representations: np.ndarray, 624 | coords: np.ndarray, 625 | edge_vertex_idxs: np.ndarray, 626 | edges_max_dist: np.ndarray, 627 | ): 628 | candidate_ptr = 0 629 | for edge_idx in edge_idxs_sorted: 630 | if len(candidate_idxs) == 0: 631 | # Note: length is decreasing 632 | break 633 | 634 | edge_min_rep = edges_min_rep[edge_idx] 635 | if candidate_ptr == len(candidate_idxs) - 1: 636 | # pointing to the last candidate (w/ highest representation) 637 | candidate_idx = candidate_idxs[candidate_ptr] 638 | candidate_rep = representations[candidate_idx] 639 | if edge_min_rep > candidate_rep: 640 | # optimisation: the edge has a higher minimum representation that the last candidate 641 | # all following edges have higher minimum representation and hence can't block the visibility 642 | # -> visibility computation is finished 643 | break 644 | 645 | edge_max_rep = edges_max_rep[edge_idx] 646 | edge_max_dist = edges_max_dist[edge_idx] 647 | i1, i2 = edge_vertex_idxs[edge_idx] 648 | 649 | candidate_ptr = check_candidates_one_edge( 650 | edge_min_rep, 651 | edge_max_rep, 652 | edge_max_dist, 653 | i1, 654 | i2, 655 | candidate_ptr, 656 | origin, 657 | candidate_idxs, 658 | coords, 659 | distances, 660 | representations, 661 | ) 662 | 663 | 664 | def _find_visible_and_in_front( 665 | origin: int, 666 | nr_edges: int, 667 | coords: np.ndarray, 668 | candidates: Set[int], 669 | candidates_in_front: Set[int], 670 | extremity_mask: np.ndarray, 671 | distances: np.ndarray, 672 | representations: np.ndarray, 673 | vertex_edge_idxs: np.ndarray, 674 | edge_vertex_idxs: np.ndarray, 675 | ): 676 | # vertices all belong to a polygon 677 | n1, n2 = get_neighbour_idxs(origin, vertex_edge_idxs, edge_vertex_idxs) 678 | n1_repr = representations[n1] 679 | n2_repr = representations[n2] 680 | # as shown in [1, Ch. II 4.4.2 "Property One"]: Starting from any point lying "in front of" an extremity e, 681 | # such that both adjacent edges are visible, one will never visit e, because everything is 682 | # reachable on a shorter path without e (except e itself). 683 | # An extremity e1 lying in the area "in front of" extremity e hence is never the next vertex 684 | # in the shortest path coming from e. 685 | # And also in reverse: when coming from e1 everything else than e itself can be reached faster 686 | # without visiting e. 687 | # -> e1 and e do not have to be connected in the graph. 688 | # IMPORTANT: this condition only holds for building the basic visibility graph without start and goal node! 689 | # When a query point (start/goal) happens to be an extremity, edges to the (visible) extremities in front 690 | # MUST be added to the graph! 691 | # Find extremities which fulfill this condition for the given query extremity 692 | # IMPORTANT: check all extremities here, not just current candidates 693 | # do not check extremities with equal coords_rel (also query extremity itself!) 694 | # and with the same angle representation (those edges must not get deleted from graph!) 695 | candidates_in_front = _find_within_range( 696 | repr1=_angle_rep_inverse(n1_repr), 697 | repr2=_angle_rep_inverse(n2_repr), 698 | candidate_idxs=candidates_in_front, 699 | angle_range_less_180=True, 700 | equal_repr_allowed=False, 701 | representations=representations, 702 | ) 703 | # do not consider points lying in front when looking for visible extremities, 704 | # even if they are actually visible. 705 | candidates.difference_update(candidates_in_front) 706 | # ATTENTION: polygons may intersect -> neighbouring extremities must NOT be visible from each other! 707 | # eliminate all vertices 'behind' the query point from the candidate set 708 | # since the query vertex is an extremity the 'outer' angle is < 180 degree 709 | # then the difference between the angle representation of the two edges has to be < 2.0 710 | # all vertices between the angle of the two neighbouring edges ('outer side') 711 | # are not visible (no candidates!) 712 | # ATTENTION: vertices with the same angle representation might be visible and must NOT be deleted! 713 | idxs_behind = _find_within_range( 714 | n1_repr, 715 | n2_repr, 716 | candidates, 717 | angle_range_less_180=True, 718 | equal_repr_allowed=False, 719 | representations=representations, 720 | ) 721 | # do not consider points found to lie behind 722 | candidates.difference_update(idxs_behind) 723 | 724 | # all edges have to be checked, except the 2 neighbouring edges (handled above!) 725 | # TODO edge set to ignore instead 726 | edge_idxs2check = set(range(nr_edges)) 727 | edge_idxs2check.difference_update(vertex_edge_idxs[origin]) 728 | visible_idxs = find_visible( 729 | origin, 730 | candidates, 731 | edge_idxs2check, 732 | coords, 733 | representations, 734 | distances, 735 | edge_vertex_idxs, 736 | vertex_edge_idxs, 737 | extremity_mask, 738 | ) 739 | return candidates_in_front, visible_idxs 740 | 741 | 742 | def get_distance(n1, n2, reprs_n_distances): 743 | if n2 > n1: 744 | # Note: start and goal nodex get added last -> highest idx 745 | # for the lower idxs the distances to goal and start have not been computed 746 | # -> use the higher indices to access the distances 747 | tmp = n1 748 | n1 = n2 749 | n2 = tmp 750 | _, dists = reprs_n_distances[n1] 751 | distance = dists[n2] 752 | return distance 753 | 754 | 755 | def _find_identical( 756 | candidates: Iterable[int], reprs_n_distances: Dict[int, np.ndarray] 757 | ) -> Dict[int, int]: 758 | # for shortest path computations all graph nodes should be unique 759 | # join all nodes with the same coordinates 760 | merging_mapping = {} 761 | # symmetric relation -> only consider one direction 762 | for n1, n2 in itertools.combinations(candidates, 2): 763 | dist = get_distance(n1, n2, reprs_n_distances) 764 | if dist == 0.0: # same coordinates 765 | merging_mapping[n2] = n1 766 | 767 | return merging_mapping 768 | 769 | 770 | def find_identical_single( 771 | i: int, candidates: Iterable[int], reprs_n_distances: Dict[int, np.ndarray] 772 | ) -> int: 773 | # for shortest path computations all graph nodes should be unique 774 | # join all nodes with the same coordinates 775 | # symmetric relation -> only consider one direction 776 | for n in candidates: 777 | if i == n: 778 | continue 779 | dist = get_distance(i, n, reprs_n_distances) 780 | if dist == 0.0: # same coordinates 781 | return n 782 | return i 783 | 784 | 785 | def compute_graph( 786 | nr_edges: int, 787 | extremity_indices: Iterable[int], 788 | reprs_n_distances: Dict[int, np.ndarray], 789 | coords: np.ndarray, 790 | edge_vertex_idxs: np.ndarray, 791 | extremity_mask: np.ndarray, 792 | vertex_edge_idxs: np.ndarray, 793 | ) -> t.Graph: 794 | graph = t.Graph() 795 | # IMPORTANT: add all extremities (even if they turn out to be dangling in the end), 796 | # adding start and goal nodes at query time might connect them! 797 | graph.add_nodes_from(extremity_indices) 798 | 799 | # optimisation: no not check the last extremity as no other candidates will remain (cf. below) 800 | for extr_ptr, origin_idx in enumerate(extremity_indices[:-1]): 801 | vert_idx2repr, vert_idx2dist = reprs_n_distances[origin_idx] 802 | # optimisation: extremities are always visible to each other 803 | # (bidirectional relation -> undirected edges in the graph) 804 | # -> do not check extremities which have been checked already 805 | # (must give the same result when algorithms are correct) 806 | # the origin extremity itself must also not be checked when looking for visible neighbours 807 | candidate_idxs = set(extremity_indices[extr_ptr + 1 :]) 808 | # Note: also the nodes previously connected to the current origin must be considered for removal 809 | candidates_in_front = candidate_idxs | set(graph.neighbors(origin_idx)) 810 | idxs_in_front, visible_idxs = _find_visible_and_in_front( 811 | origin_idx, 812 | nr_edges, 813 | coords, 814 | candidate_idxs, 815 | candidates_in_front, 816 | extremity_mask, 817 | vert_idx2dist, 818 | vert_idx2repr, 819 | vertex_edge_idxs, 820 | edge_vertex_idxs, 821 | ) 822 | # "thin out" the graph: 823 | # remove already existing edges in the graph to the extremities in front 824 | for i in idxs_in_front: 825 | try: 826 | graph.remove_edge(origin_idx, i) 827 | except nx.exception.NetworkXError: 828 | pass 829 | 830 | for i in visible_idxs: 831 | graph.add_edge(origin_idx, i, weight=vert_idx2dist[i]) 832 | 833 | merge_mapping = _find_identical(graph.nodes, reprs_n_distances) 834 | if len(merge_mapping) > 0: 835 | nx.relabel_nodes(graph, merge_mapping, copy=False) 836 | 837 | return graph 838 | 839 | 840 | def _try_extraction(json_data, key): 841 | try: 842 | extracted_data = json_data[key] 843 | except KeyError as e: 844 | raise ValueError(f"The expected key {key} was not found in the JSON file:\n{e}") 845 | return extracted_data 846 | 847 | 848 | def _convert2polygon(json_list): 849 | return [tuple(coord_pair_list) for coord_pair_list in json_list] 850 | 851 | 852 | def read_json(path2json_file): 853 | """ 854 | Parse data from a JSON file and save as lists of tuples for both boundary and holes. 855 | NOTE: The format of the JSON file is explained in the command line script (argparse definition) 856 | 857 | :param path2json_file: The path to the input json file 858 | :return: The parsed lists of boundaries and holes 859 | """ 860 | # parse data from the input file 861 | with open(path2json_file) as json_file: 862 | json_data = json_file.read() 863 | json_loaded = json.loads(json_data) 864 | boundary_data = _try_extraction(json_loaded, BOUNDARY_JSON_KEY) 865 | holes_data = _try_extraction(json_loaded, HOLES_JSON_KEY) 866 | boundary_coordinates = _convert2polygon(boundary_data) 867 | list_of_holes = [_convert2polygon(hole_data) for hole_data in holes_data] 868 | return boundary_coordinates, list_of_holes 869 | 870 | 871 | def convert_gridworld( 872 | size_x: int, size_y: int, obstacle_iter: iter, simplify: bool = True 873 | ) -> (list, list): 874 | """ 875 | prerequisites: grid world must not have non-obstacle cells which are surrounded by obstacles 876 | ("single white cell in black surrounding" = useless for path planning) 877 | :param size_x: the horizontal grid world size 878 | :param size_y: the vertical grid world size 879 | :param obstacle_iter: an iterable of coordinate pairs (x,y) representing blocked grid cells (obstacles) 880 | :param simplify: whether the polygons should be simplified or not. reduces edge amount, allow diagonal edges 881 | :return: a boundary polygon (counterclockwise numbering) and a list of hole polygons (clockwise numbering) 882 | NOTE: convert grid world into polygons in a way that coordinates coincide with grid! 883 | -> no conversion of obtained graphs needed! 884 | the origin of the polygon coordinate system is (-0.5,-0.5) in the grid cell system (= corners of the grid world) 885 | """ 886 | 887 | assert size_x > 0 and size_y > 0 888 | 889 | if len(obstacle_iter) == 0: 890 | # there are no obstacles. return just the simple boundary rectangle 891 | return [ 892 | np.array(x, y) 893 | for x, y in [(0, 0), (size_x, 0), (size_x, size_y), (0, size_y)] 894 | ], [] 895 | 896 | # convert (x,y) into np.arrays 897 | # obstacle_iter = [np.array(o) for o in obstacle_iter] 898 | obstacle_iter = np.array(obstacle_iter) 899 | 900 | def within_grid(pos): 901 | return 0 <= pos[0] < size_x and 0 <= pos[1] < size_y 902 | 903 | def is_equal(pos1, pos2): 904 | return np.all(pos1 == pos2) 905 | 906 | def pos_in_iter(pos, iter): 907 | for i in iter: 908 | if is_equal(pos, i): 909 | return True 910 | return False 911 | 912 | def is_obstacle(pos): 913 | return pos_in_iter(pos, obstacle_iter) 914 | 915 | def is_blocked(pos): 916 | return not within_grid(pos) or is_obstacle(pos) 917 | 918 | def is_unblocked(pos): 919 | return within_grid(pos) and not is_obstacle(pos) 920 | 921 | def find_start(start_pos, boundary_detect_fct, **kwargs): 922 | # returns the lowest and leftmost unblocked grid cell from the start position 923 | # for which the detection function evaluates to True 924 | start_x, start_y = start_pos 925 | for y in range(start_y, size_y): 926 | for x in range(start_x, size_x): 927 | pos = np.array([x, y]) 928 | if boundary_detect_fct(pos, **kwargs): 929 | return pos 930 | 931 | # north, east, south, west 932 | directions = np.array([[0, 1], [1, 0], [0, -1], [-1, 0]], dtype=int) 933 | # the correct offset to determine where nodes should be added. 934 | offsets = np.array([[0, 1], [1, 1], [1, 0], [0, 0]], dtype=int) 935 | 936 | def construct_polygon(start_pos, boundary_detect_fct, cntr_clockwise_wanted: bool): 937 | current_pos = start_pos.copy() 938 | # (at least) the west and south are blocked 939 | # -> there has to be a polygon node at the current position (bottom left corner of the cell) 940 | edge_list = [start_pos] 941 | forward_index = 0 # start with moving north 942 | forward_vect = directions[forward_index] 943 | left_index = (forward_index - 1) % 4 944 | # left_vect = directions[(forward_index - 1) % 4] 945 | just_turned = True 946 | 947 | # follow the border between obstacles and free cells ("wall") until one 948 | # reaches the start position again 949 | while True: 950 | # left has to be checked first 951 | # do not check if just turned left or right (-> the left is blocked for sure) 952 | # left_pos = current_pos + left_vect 953 | if not ( 954 | just_turned or boundary_detect_fct(current_pos + directions[left_index]) 955 | ): 956 | # print('< turn left') 957 | forward_index = left_index 958 | left_index = (forward_index - 1) % 4 959 | forward_vect = directions[forward_index] 960 | just_turned = True 961 | 962 | # add a new node at the correct position 963 | # decrease the index first! 964 | edge_list.append(current_pos + offsets[forward_index]) 965 | 966 | # move forward (previously left, there is no obstacle) 967 | current_pos += forward_vect 968 | else: 969 | forward_pos = current_pos + forward_vect 970 | if boundary_detect_fct(forward_pos): 971 | node_pos = current_pos + offsets[forward_index] 972 | # there is a node in the bottom left corner of the start position (offset= (0,0) ) 973 | if is_equal(node_pos, start_pos): 974 | # check and terminate if this node does already exist 975 | break 976 | 977 | # add a new node at the correct position 978 | edge_list.append(node_pos) 979 | # print('> turn right') 980 | left_index = forward_index 981 | forward_index = (forward_index + 1) % 4 982 | forward_vect = directions[forward_index] 983 | just_turned = True 984 | # print(direction_index,forward_vect,just_turned,edge_list,) 985 | else: 986 | # print('^ move forward') 987 | current_pos += forward_vect 988 | just_turned = False 989 | 990 | if cntr_clockwise_wanted: 991 | # make edge numbering counterclockwise! 992 | edge_list.reverse() 993 | return np.array(edge_list, dtype=float) 994 | 995 | # build the boundary polygon 996 | # start at the lowest and leftmost unblocked grid cell 997 | start_pos = find_start(start_pos=(0, 0), boundary_detect_fct=is_unblocked) 998 | # print(start_pos+directions[3]) 999 | # raise ValueError 1000 | boundary_edges = construct_polygon( 1001 | start_pos, boundary_detect_fct=is_blocked, cntr_clockwise_wanted=True 1002 | ) 1003 | 1004 | if simplify: 1005 | # TODO 1006 | raise NotImplementedError() 1007 | 1008 | # detect which of the obstacles have to be converted into holes 1009 | # just the obstacles inside the boundary polygon are part of holes 1010 | # shift coordinates by +(0.5,0.5) for correct detection 1011 | # the border value does not matter here 1012 | 1013 | def get_unchecked_obstacles( 1014 | obstacles: Iterable, poly: np.ndarray, required_val: bool = True 1015 | ) -> List: 1016 | unchecked_obstacles = [] 1017 | for o in obstacles: 1018 | p = o + 0.5 1019 | if _inside_polygon(p, poly, border_value=True) == required_val: 1020 | unchecked_obstacles.append(o) 1021 | 1022 | return unchecked_obstacles 1023 | 1024 | unchecked_obstacles = get_unchecked_obstacles(obstacle_iter, boundary_edges) 1025 | 1026 | hole_list = [] 1027 | while len(unchecked_obstacles) > 0: 1028 | start_pos = find_start( 1029 | start_pos=(0, 0), boundary_detect_fct=pos_in_iter, iter=unchecked_obstacles 1030 | ) 1031 | hole = construct_polygon( 1032 | start_pos, boundary_detect_fct=is_unblocked, cntr_clockwise_wanted=False 1033 | ) 1034 | 1035 | # detect which of the obstacles still do not belong to any hole: 1036 | # delete the obstacles which are included in the just constructed hole 1037 | unchecked_obstacles = get_unchecked_obstacles( 1038 | unchecked_obstacles, hole, required_val=False 1039 | ) 1040 | 1041 | if simplify: 1042 | # TODO 1043 | pass 1044 | 1045 | hole_list.append(hole) 1046 | 1047 | return boundary_edges, hole_list 1048 | 1049 | 1050 | def _cmp_extremity_mask(coordinates: np.ndarray) -> np.ndarray: 1051 | """identify all protruding points = vertices with an inside angle of > 180 degree ('extremities') 1052 | expected edge numbering: 1053 | outer boundary polygon: counterclockwise 1054 | holes: clockwise 1055 | 1056 | basic idea: 1057 | - translate the coordinate system to have p2 as origin 1058 | - compute the angle representations of both vectors representing the edges 1059 | - "rotate" the coordinate system (equal to deducting) so that the p1p2 representation is 0 1060 | - check in which quadrant the p2p3 representation lies 1061 | %4 because the quadrant has to be in [0,1,2,3] (representation in [0:4[) 1062 | if the representation lies within quadrant 0 or 1 (<2.0), the inside angle 1063 | (for boundary polygon inside, for holes outside) between p1p2p3 is > 180 degree 1064 | then p2 = extremity 1065 | :param coordinates: 1066 | :return: 1067 | """ 1068 | extremity_mask = np.full(len(coordinates), False, dtype=bool) 1069 | _fill_extremity_mask(coordinates, extremity_mask) 1070 | return extremity_mask 1071 | 1072 | 1073 | def _cmp_extremities( 1074 | list_of_polygons, coords, boundary_coordinates, list_of_hole_coordinates 1075 | ): 1076 | extremity_masks = [ 1077 | _cmp_extremity_mask(coords_poly) for coords_poly in list_of_polygons 1078 | ] 1079 | extremity_mask = np.concatenate(extremity_masks, axis=0, dtype=bool) 1080 | # Attention: since polygons are allowed to overlap, only consider extremities that are actually within the map 1081 | for extremity_idx in np.where(extremity_mask)[0]: 1082 | extrimity_coords = coords[extremity_idx] 1083 | if not is_within_map( 1084 | extrimity_coords, boundary_coordinates, list_of_hole_coordinates 1085 | ): 1086 | extremity_mask[extremity_idx] = False 1087 | extremity_indices = np.where(extremity_mask)[0] 1088 | return extremity_indices, extremity_mask 1089 | 1090 | 1091 | def _cmp_edge_vertex_idxs(coordinates: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: 1092 | nr_coords = len(coordinates) 1093 | edge_vertex_idxs = np.empty((nr_coords, 2), dtype=int) 1094 | vertex_edge_idxs = np.empty((nr_coords, 2), dtype=int) 1095 | _fill_edge_vertex_idxs(edge_vertex_idxs, vertex_edge_idxs) 1096 | return edge_vertex_idxs, vertex_edge_idxs 1097 | 1098 | 1099 | def _cmp_edge_and_vertex_idxs(list_of_polygons): 1100 | # compute edge and vertex indices from polygon data structure 1101 | edge_and_vertex_indices = [ 1102 | _cmp_edge_vertex_idxs(coords_poly) for coords_poly in list_of_polygons 1103 | ] 1104 | offset = 0 1105 | for edge_vertex_idxs, vertex_edge_idxs in edge_and_vertex_indices: 1106 | edge_vertex_idxs += offset 1107 | vertex_edge_idxs += offset 1108 | offset += len(edge_vertex_idxs) 1109 | edge_vertex_idxs, vertex_edge_idxs = zip(*edge_and_vertex_indices) 1110 | edge_vertex_idxs = np.concatenate(edge_vertex_idxs, axis=0) 1111 | vertex_edge_idxs = np.concatenate(vertex_edge_idxs, axis=0) 1112 | return edge_vertex_idxs, vertex_edge_idxs 1113 | 1114 | 1115 | def compile_polygon_datastructs( 1116 | boundary_coordinates: np.ndarray, list_of_hole_coordinates: List[np.ndarray] 1117 | ): 1118 | list_of_polygons = [boundary_coordinates] + list_of_hole_coordinates 1119 | coords = np.concatenate(list_of_polygons, axis=0, dtype=configs.DTYPE_FLOAT) 1120 | edge_vertex_idxs, vertex_edge_idxs = _cmp_edge_and_vertex_idxs(list_of_polygons) 1121 | extremity_indices, extremity_mask = _cmp_extremities( 1122 | list_of_polygons, coords, boundary_coordinates, list_of_hole_coordinates 1123 | ) 1124 | return coords, extremity_indices, extremity_mask, vertex_edge_idxs, edge_vertex_idxs 1125 | 1126 | 1127 | def load_pickle(path=DEFAULT_PICKLE_NAME): 1128 | print("loading map from:", path) 1129 | with open(path, "rb") as f: 1130 | return pickle.load(f) 1131 | --------------------------------------------------------------------------------