diff --git a/.clang-format b/.clang-format index 332b39c..b05423f 100644 --- a/.clang-format +++ b/.clang-format @@ -7,6 +7,7 @@ PointerAlignment: Left UseTab: ForIndentation IndentWidth: 4 TabWidth: 4 +# ColumnLimit: 120 Cpp11BracedListStyle: true AlwaysBreakTemplateDeclarations: Yes AlwaysBreakAfterReturnType: All diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d163863 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 742690b..dea0e84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,13 +1,17 @@ cmake_minimum_required(VERSION 3.13) project(WarthogJPS - VERSION 0.2.0 + VERSION 0.5.0 LANGUAGES CXX C) +set_property(GLOBAL PROPERTY WARTHOG_warthog-jps ON) + set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED TRUE) -include(cmake/submodule.cmake) +include(cmake/warthog.cmake) +warthog_module_declare(warthog-core v0.5.0) +warthog_module(warthog-core) add_library(warthog_libjps) add_library(warthog::libjps ALIAS warthog_libjps) @@ -26,4 +30,3 @@ target_link_libraries(warthog_jps PUBLIC warthog::libjps) add_subdirectory(src) add_subdirectory(apps) -warthog_submodule(warthog-core) diff --git a/LICENSE b/LICENSE index 2da8ca8..265f6da 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2024 Shortest Path Lab @ Monash University +Copyright (c) 2024-2025 Shortest Path Lab @ Monash University Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/README.md b/README.md index 2504437..2e58d3a 100644 --- a/README.md +++ b/README.md @@ -2,4 +2,73 @@ Warthog is an optimised C++ library for pathfinding search. Warthog-JPS is an extension to Warthog with JPS support. -See [warthog-core](https://github.com/ShortestPathLab/warthog-core) for extended README. +See [warthog-core](https://github.com/ShortestPathLab/warthog-core) for more details on Warthog setup. + +If you find this work useful, please cite our paper as follows: + + @article{Harabor_Grastien_2014, + title={Improving Jump Point Search}, + volume={24}, + url={https://ojs.aaai.org/index.php/ICAPS/article/view/13633}, + DOI={10.1609/icaps.v24i1.13633}, + number={1}, + journal={Proceedings of the International Conference on Automated Planning and Scheduling}, + author={Harabor, Daniel and Grastien, Alban}, + year={2014}, + pages={128-135} + } + +# Using JPS + +## Compile + +Program `warthog-jps` uses cmake for the compile process. +Simply run the following code below: + +``` +cmake -DCMAKE_BUILD_TYPE=Release -B build +cmake --build build +./build/warthog-jps --alg jps --scen example/arena2.map.scen +``` + +The example map is from the MovingAI benchmarks under Dragon Age Origins. +Use the `build.sh` or `run.sh` for quick compile/usage of this scenario. + +For more evaluation data, please refer to existing benchmarks from the community, which offer a range of different challenges across a variety of maps/domains; links found in the resource section. + +JPS requires `warthog-core` repo to compile, and by default will fetch the repo. + +## JPS Variants + +JPS comes with several variants. +All variants use block-based symmetry breaking for finding jump-point locations, +making them at least JPS (B). +The list is as follows: + +- `jps` JPS (B) like original with block-based symmetry +- `jpsP` or `jps2` JPS (B+P) with intercardinal pruning +- `jps+` JPS+ (B) is original with offline jump points +- `jpsP+` or `jps2+` JPS+ (B+P) offline with pruning + +These jump points are found in namespace `jps::jump`, while `jps::search` uses provided `jps::jump` locators +to produce successors used in warthog-core. + +The algorithms provided to `--alg` parameter, along with their `jps::search` and `jps::jump` classes presented +in the table below: + +| `--alg` | `jps::search` | `jps::jump` | +|--------------------|--------------------------------|------------------------| +| `jps` | `jps_expansion_policy<>` | `jump_point_online` | +| `jpsP` or `jps2` | `jps_prune_expansion_policy<>` | `jump_point_online` | +| `jps+` | `jps_expansion_policy` | `jump_point_offline<>` | +| `jpsP+` or `jps2+` | `jps_prune_expansion_policy<>` | `jump_point_offline<>` | + +## Use in project + +If elements of warthog is used in a project, follow guides from `warthog-core` to set up the project structure. + +# Resources + +- [Moving AI Lab](https://movingai.com/): pathfinding benchmark and tools +- [Posthoc](https://posthoc-app.pathfinding.ai/): visualiser and debugger for pathfinding +- [Pathfinding Benchmarks](https://benchmarks.pathfinding.ai/): git repo for benchmarks diff --git a/apps/jps.cpp b/apps/jps.cpp index 4493c6e..2d8d29d 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -8,11 +8,6 @@ // #include -#include -#include -#include -#include -#include #include #include #include @@ -21,7 +16,10 @@ #include #include -#include +#include +#include +#include +#include #include "cfg.h" #include @@ -70,8 +68,7 @@ help(std::ostream& out) << "Invoking the program this way solves all instances in [scen " "file] with algorithm [alg]\n" << "Currently recognised values for [alg]:\n" - << "\tjps, jps+, jps2, jps2+\n" - << "\tjpsV2, jpsV2-cardinal, jpsV2-prune\n"; + << "\tjps, jpsP or jps2, jps+, jpsP+ or jps2+\n"; // << "" // << "The following are valid parameters for GENERATING instances:\n" // << "\t --gen [map file (required)]\n" @@ -90,20 +87,12 @@ check_optimality( if(fabs(delta - epsilon) > epsilon) { - std::stringstream strpathlen; - strpathlen << std::fixed << std::setprecision(exp->precision()); - strpathlen << sol.sum_of_edge_costs_; - - std::stringstream stroptlen; - stroptlen << std::fixed << std::setprecision(exp->precision()); - stroptlen << exp->distance(); - - std::cerr << std::setprecision(exp->precision()); + std::cerr << std::setprecision(15); std::cerr << "optimality check failed!" << std::endl; std::cerr << std::endl; - std::cerr << "optimal path length: " << stroptlen.str() + std::cerr << "optimal path length: " << exp->distance() << " computed length: "; - std::cerr << strpathlen.str() << std::endl; + std::cerr << sol.sum_of_edge_costs_ << std::endl; std::cerr << "precision: " << precision << " epsilon: " << epsilon << std::endl; std::cerr << "delta: " << delta << std::endl; @@ -253,39 +242,26 @@ main(int argc, char** argv) using namespace jps::search; if(alg == "jps") { - return run_jps(scenmgr, mapfile, alg); - } - else if(alg == "jps+") - { - return run_jps(scenmgr, mapfile, alg); - } - else if(alg == "jps2") - { - return run_jps(scenmgr, mapfile, alg); - } - else if(alg == "jps2+") - { - return run_jps(scenmgr, mapfile, alg); + using jump_point = jps::jump::jump_point_online; + return run_jps>( + scenmgr, mapfile, alg); } - else if(alg == "jpsV2") + else if(alg == "jpsP" || alg == "jps2") { - using jump_point - = jps::jump::jump_point_online; - return run_jps>( + using jump_point = jps::jump::jump_point_online; + return run_jps>( scenmgr, mapfile, alg); } - else if(alg == "jpsV2-cardinal") + else if(alg == "jps+") { - using jump_point = jps::jump::jump_point_online< - jps::JpsFeature::STORE_CARDINAL_JUMP>; - return run_jps>( + using jump_point = jps::jump::jump_point_offline<>; + return run_jps>( scenmgr, mapfile, alg); } - else if(alg == "jpsV2-prune") + else if(alg == "jpsP+" || alg == "jps2+") { - using jump_point = jps::jump::jump_point_online< - jps::JpsFeature::PRUNE_INTERCARDINAL>; - return run_jps>( + using jump_point = jps::jump::jump_point_offline<>; + return run_jps>( scenmgr, mapfile, alg); } else diff --git a/cmake/headers.cmake b/cmake/headers.cmake index 02f876b..50b5884 100644 --- a/cmake/headers.cmake +++ b/cmake/headers.cmake @@ -3,16 +3,16 @@ cmake_minimum_required(VERSION 3.13) # find include/jps -type f -name '*.h' | sort target_sources(warthog_libjps PUBLIC include/jps/forward.h -include/jps/jump/four_connected_jps_locator.h -include/jps/jump/offline_jump_point_locator.h -include/jps/jump/offline_jump_point_locator2.h -include/jps/jump/online_jump_point_locator.h -include/jps/jump/online_jump_point_locator2.h -include/jps/search/jps2_expansion_policy.h -include/jps/search/jps.h -include/jps/search/jps2plus_expansion_policy.h -include/jps/search/jps4c_expansion_policy.h +include/jps/domain/rotate_gridmap.h + +include/jps/jump/block_online.h +include/jps/jump/jump.h +include/jps/jump/jump_point_offline.h +include/jps/jump/jump_point_online.h + +include/jps/search/jps_expansion_policy_base.h include/jps/search/jps_expansion_policy.h -include/jps/search/jpsplus_expansion_policy.h +include/jps/search/jps.h +include/jps/search/jps_prune_expansion_policy.h ) diff --git a/cmake/submodule.cmake b/cmake/submodule.cmake deleted file mode 100644 index da47f29..0000000 --- a/cmake/submodule.cmake +++ /dev/null @@ -1,35 +0,0 @@ -cmake_minimum_required(VERSION 3.13) - -# include this file to check submodule include - -set(WARTHOG_SUBMODULE_ROOT_ONLY ON CACHE BOOL "add submodule if present only allowed for root project") - -function(warthog_top_level) -set(_is_top OFF) -if(${CMAKE_VERSION} VERSION_GREATER_EQUAL 3.21) - if(${PROJECT_IS_TOP_LEVEL}) - set(_is_top ON) - endif() -else() - if(${CMAKE_SOURCE_DIR} STREQUAL ${PROJECT_SOURCE_DIR}) - set(_is_top ON) - endif() -endif() -set(_is_top ${_is_top} PARENT_SCOPE) -endfunction() - -function(warthog_submodule dirname) -set(_is_top OFF) -if(NOT ${WARTHOG_SUBMODULE_ROOT_ONLY}) - set(_is_top ON) -else() - warthog_top_level() -endif() -if(${_is_top}) - if(NOT EXISTS "${PROJECT_SOURCE_DIR}/extern/${dirname}/.git") - message(SEND_ERROR "Failed to find submodule ${dirname}") - else() - add_subdirectory("${PROJECT_SOURCE_DIR}/extern/${dirname}" EXCLUDE_FROM_ALL) - endif() -endif() -endfunction() diff --git a/cmake/warthog.cmake b/cmake/warthog.cmake new file mode 100644 index 0000000..635f050 --- /dev/null +++ b/cmake/warthog.cmake @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.13) + +# include this file to check submodule include + +# set here as only +set(warthog_https_warthog-core "https://github.com/ShortestPathLab/warthog-core.git" CACHE INTERNAL "") +set(warthog_https_warthog-jps "https://github.com/ShortestPathLab/warthog-jps.git" CACHE INTERNAL "") + +if(COMMAND warthog_module) + return() # do not redefine +endif() + +include(FetchContent) + +set(WARTHOG_MODULE_ROOT_ONLY ON CACHE BOOL "add submodule if present only allowed for root project") + +function(warthog_top_level) +set(_is_top OFF) +if(${CMAKE_VERSION} VERSION_GREATER_EQUAL 3.21) + if(${PROJECT_IS_TOP_LEVEL}) + set(_is_top ON) + endif() +else() + if(${CMAKE_SOURCE_DIR} STREQUAL ${PROJECT_SOURCE_DIR}) + set(_is_top ON) + endif() +endif() +set(_warthog_is_top ${_is_top} PARENT_SCOPE) +endfunction() + +# warthog_module module [override_top] +# adds a module to warthog +# has checks to insure module is only added once +# if called from the top level project (or optional variable override_top is true) +# then if extern/${module}/CMakeLists.txt exists, adds that version instead +# intended for use with git module/subtree. +# otherwise, fetchcontent ${module} is made available +# user must add FetchContent_Declare +function(warthog_module module) +get_property(_module_added GLOBAL PROPERTY WARTHOG_${module} SET) +if(${_module_added}) # module already added + return() +endif() +if((${ARGC} GREATER 1) AND (${ARGV1})) + set(is_top_level ON) +else() + warthog_top_level() + set(is_top_level ${_warthog_is_top}) +endif() +if(${is_top_level}) + if(EXISTS "${PROJECT_SOURCE_DIR}/extern/${module}/CMakeLists.txt") + # module or subtree, include and exit + add_subdirectory("${PROJECT_SOURCE_DIR}/extern/${module}") + set_property(GLOBAL PROPERTY WARTHOG_${module} ON) + return() + endif() +endif() +# failed to add module, fetch if able +warthog_module_declare(${module}) +FetchContent_MakeAvailable(${module}) +set_property(GLOBAL PROPERTY WARTHOG_${module} ON) +endfunction() + +function(warthog_module_declare module) +if(NOT DEFINED warthog_https_${module}) + return() +endif() +if((${ARGC} GREATER 1)) + set(git_tag ${ARGV1}) +else() + set(git_tag "main") +endif() +FetchContent_Declare(${module} + GIT_REPOSITORY ${warthog_https_${module}} + GIT_TAG ${git_tag}) +endfunction() diff --git a/example/arena2.map b/example/arena2.map new file mode 100644 index 0000000..686c35f --- /dev/null +++ b/example/arena2.map @@ -0,0 +1,213 @@ +type octile +height 209 +width 281 +map +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT............TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT.....TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT.....TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............................T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTT............................TT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTT............................TT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............TTT.............TTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............................T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............TTT.............TTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............................................T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...........TTTTT...........TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT...........TTTTT...........TTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............................................TT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............................................TT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT............TTTT............TTTT............TTTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............................................T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...........TTTTT...........TTTT.............TTT.............TTT.............TTTT...........TTTTT...........TTTTT...........TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT...........TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTT............TTTTTTTTTTTTTTTTTT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT..TTTTTTTT.TTTTT...........TTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@TT@@@@@@@@@@@@@@@@TT............TT@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT......TTTTT...........TTTTT...........TTTT............TTTTT...........TTTTT...........TTTTT...........TTTT............TTTTT...........TTTTT......TTT@@@@@@@@@@@@@@@@@@@T@@@@@@@@@@@@@@@@TT.............T@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTT........TTT.............TTT.............TT.............TTTTT...........TTTTT...........TTTTT............................TTT.............TTT........TTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTT.........................................TT..............TTT.............TTT.............TTT.........................................................TTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTT..........................................TT...........................................................................................................TTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTT..........................................TT...........................................................................................................TTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTT...........................................TT............................................................................................................TTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTT...............................................TT................................................................................................................TTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTT................................................TT.................................................................................................................TTTT@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT..................................................TT...................................................................................................................TT@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT..................................................TT....................................................................................................................TT@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT...................................................TT....................................................................................................................TT@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT...................................................TT....................................................................................................................TT@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT....................................................TT.....................................................................................................................TT@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT....................................TTT.............TTT.............................................................TTT.............TTT....................................TTT@@@@@@T@@@@@@@@@@@@@@@@TT.............T@@@@@@@@@@@@@@@@ +.................@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTT..................................TT@@@@@@@@@@@@@@@@TTT...........................................................TTTTT...........TTTTT..................................TTTTTTTTTTTTTTTTTTTTTTTTTTTTT............TT@@@@@@@@@@@@@@@@ +........................TTTTTTTTTTTTTTTTTTTTTTTTTTTTT..................................TTTTT...........TTTTT..........................................................TTTTTTTTTTTTTTTTTTTTTT..................................TTTTTTTTTTTTTTTTTTTTTTTTTTTTT............TT@@@@@@@@@@@@@@@@ +.................TTTTTTTTTTTTTTTT......TTTT......TTTT..................................TTTT............TTTT............................................................TTTT............TTTT...................................TTTT......TTT.............TTT.............T@@@@@@@@@@@@@@@@ +.........TTTTT...TTTTTTTTT.........TT...TTT.......T......................................................................................................................TT..............T...............................................TT..............................@@@@@@@@@@@@@@@@ +.........TTTTTTTTTTTTTTTTT...........T...................................................................................................................................TT..............................................................TT..............................@@@@@@@@@@@@@@@@ +...............TTTTTTTTTTT...........TT..................................................................................................................................TT..............................................................TT..............................@@@@@@@@@@@@@@@@ +.....................TTTTT...........TT..................................................................................................................................TT..............................................................TT..............................@@@@@@@@@@@@@@@@ +.....................................TT..................................................................................................................................TT..............................................................................................@@@@@@@@@@@@@@@@ +......................................T..................................................................................................................................TT..............................................................................................@@@@@@@@@@@@@@@@ +.....................................TT..................................................................................................................................TT..............................................................................................@@@@@@@@@@@@@@@@ +.....................................TT..................................................................................................................................TT..............................................................................................@@@@@@@@@@@@@@@@ +.....................TTTTT..........TTT..................................................................................................................................TT..............................................................................................@@@@@@@@@@@@@@@@ +....................TTTTTT.TTTTTTTTTTTT..................................................................................................................................TT..............................................................TT..............................@@@@@@@@@@@@@@@@ +..............TTTTTTTTTTTTTTTTTTTTTTTT...................................................................................................................................TT..............................................................TT..............................@@@@@@@@@@@@@@@@ +.........TTTTTTTTTTTTTTTTTTTTTTTTTTTT...TTT..............................................................................................................................TT..............................................................TT..............................@@@@@@@@@@@@@@@@ +.........TT......TTTTTTTTTTTTTTTTTTT...TTTT......TTTT...................................TTT.............TTT.............................................................TTT.............TTT...................................TTTT......TTT.............TTT.............T@@@@@@@@@@@@@@@@ +.................@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTT..................................TTTTT...........TTTTT...........................................................TTTTTTTTTTTTTTTTTTTTT..................................TTTTTTTTTTTTTTTTTTTTTTTTTTTTT............TT@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTT..................................TTTTTTTTTTTTTTTTTTTTTT..........................................................TTTTT..TTTTTTTT.TTTTT..................................TTTTTTTTTTT@@@@@@@@@@@@@@@@TT............TT@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...................................TTTT............TTTT............................................................TTTT............TTTT....................................TTT@@@@@@T@@@@@@@@@@@@@@@@TT.............T@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT....................................................TT.....................................................................................................................TT@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT...................................................TT....................................................................................................................TT@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT...................................................TT....................................................................................................................TT@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT...................................................TT...................................................................................................................TTT@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT..................................................TT...................................................................................................................TT@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT.................................................TT..................................................................................................................TTT@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTT...............................................TT................................................................................................................TTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTT.T............................................TT............................................................................................................TTTTTT@T@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTT...........................................TT...........................................................................................................TTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTT..........................................TT...........................................................................................................TTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTT..........................................TT..............TTT.............TTT.............TTT..........................................................TTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTT........................................TT.............TTTTT...........TTTTT...........TTTTT.......................................................TTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT......TTTTT...........TTTTT...........TTTT............TTTTT...........TTTTT...........TTTTT............TTT............TTTTT...........TTTTT......TTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............T@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............TTTTTTTTTTTTTTTTTT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT...........TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............TTTTTTTTTTTTTTTTTT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...........TTTTT...........TTTT............TTTT............TTTT............TTTTT...........TTTTT...........TTTTT...........TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TT...............................................TT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T..............................TT...............................................TT..............................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............TTT.............TTT.............TTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............................T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT............................TT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...........TTTTT...........TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT...........TTTTT...........TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT............................TT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............TTT.............TTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............................T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............TTT.............TTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............................TTT.............TTT.............T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............................TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTT...........TTTTT...........TTTTT...........TTTTT...........TTTTT...........TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............................TT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................................................................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T...............................@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............TTT.............TTT.............T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............................T@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............................TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............................TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............................TTT.............TTT.............T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............................................T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTT............................................TT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT............................................TT +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............................................T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@T............................................... +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TT.............TTT.............TTT.............T +@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT diff --git a/example/arena2.map.scen b/example/arena2.map.scen new file mode 100644 index 0000000..ec335fd --- /dev/null +++ b/example/arena2.map.scen @@ -0,0 +1,911 @@ +version 1 +0 arena2.map 281 209 99 159 101 162 3.82842712 +0 arena2.map 281 209 112 130 109 131 3.41421356 +0 arena2.map 281 209 101 94 103 93 2.41421356 +0 arena2.map 281 209 111 58 110 60 2.41421356 +0 arena2.map 281 209 93 83 95 80 3.82842712 +0 arena2.map 281 209 256 124 258 125 2.41421356 +0 arena2.map 281 209 262 48 259 46 3.82842712 +0 arena2.map 281 209 270 39 268 40 2.41421356 +0 arena2.map 281 209 237 101 237 102 1.00000000 +0 arena2.map 281 209 274 178 273 178 1.00000000 +1 arena2.map 281 209 102 99 98 104 6.65685425 +1 arena2.map 281 209 69 113 69 117 4.00000000 +1 arena2.map 281 209 256 138 256 131 7.00000000 +1 arena2.map 281 209 118 123 112 126 7.24264069 +1 arena2.map 281 209 141 128 147 129 6.41421356 +1 arena2.map 281 209 140 40 137 44 5.24264069 +1 arena2.map 281 209 239 53 245 50 7.24264069 +1 arena2.map 281 209 164 143 160 143 4.00000000 +1 arena2.map 281 209 170 73 174 69 5.65685425 +1 arena2.map 281 209 264 123 258 122 6.41421356 +2 arena2.map 281 209 82 91 81 101 10.41421356 +2 arena2.map 281 209 108 98 116 106 11.31370850 +2 arena2.map 281 209 194 139 185 132 11.89949493 +2 arena2.map 281 209 196 90 189 98 10.89949493 +2 arena2.map 281 209 268 144 277 141 10.24264069 +2 arena2.map 281 209 263 91 263 100 9.82842712 +2 arena2.map 281 209 122 87 123 97 10.41421356 +2 arena2.map 281 209 168 136 178 136 10.00000000 +2 arena2.map 281 209 150 43 144 51 10.48528137 +2 arena2.map 281 209 130 39 133 31 10.41421356 +3 arena2.map 281 209 192 113 180 106 14.89949493 +3 arena2.map 281 209 221 166 227 153 15.48528137 +3 arena2.map 281 209 161 49 147 45 15.65685425 +3 arena2.map 281 209 270 74 274 60 15.65685425 +3 arena2.map 281 209 126 157 121 170 15.07106781 +3 arena2.map 281 209 202 95 206 82 14.65685425 +3 arena2.map 281 209 183 132 196 128 14.65685425 +3 arena2.map 281 209 121 167 133 169 12.82842712 +3 arena2.map 281 209 90 164 105 165 15.41421356 +3 arena2.map 281 209 256 189 266 196 12.89949493 +4 arena2.map 281 209 129 49 138 36 16.72792206 +4 arena2.map 281 209 254 154 256 136 18.82842712 +4 arena2.map 281 209 95 50 107 38 16.97056274 +4 arena2.map 281 209 161 72 175 78 16.48528137 +4 arena2.map 281 209 117 92 136 94 19.82842712 +4 arena2.map 281 209 264 67 251 81 19.38477631 +4 arena2.map 281 209 128 18 114 13 16.07106781 +4 arena2.map 281 209 167 135 184 139 18.65685425 +4 arena2.map 281 209 82 95 99 102 19.89949493 +4 arena2.map 281 209 128 67 116 77 16.14213562 +5 arena2.map 281 209 169 149 183 131 23.79898987 +5 arena2.map 281 209 172 75 194 79 23.65685425 +5 arena2.map 281 209 167 154 156 137 21.55634918 +5 arena2.map 281 209 200 122 179 117 23.07106781 +5 arena2.map 281 209 105 150 83 153 23.24264069 +5 arena2.map 281 209 179 124 161 114 22.14213562 +5 arena2.map 281 209 156 159 139 151 20.31370850 +5 arena2.map 281 209 84 105 77 85 22.89949493 +5 arena2.map 281 209 95 41 105 58 21.14213562 +5 arena2.map 281 209 238 65 258 74 23.72792206 +6 arena2.map 281 209 172 88 153 104 25.62741699 +6 arena2.map 281 209 257 207 267 186 25.14213562 +6 arena2.map 281 209 190 96 181 119 27.89949493 +6 arena2.map 281 209 95 140 95 164 24.00000000 +6 arena2.map 281 209 70 94 92 87 24.89949493 +6 arena2.map 281 209 96 121 69 122 27.41421356 +6 arena2.map 281 209 62 104 75 126 27.38477631 +6 arena2.map 281 209 262 82 238 79 26.07106781 +6 arena2.map 281 209 202 88 178 96 27.31370850 +6 arena2.map 281 209 96 147 117 163 27.62741699 +7 arena2.map 281 209 135 70 107 68 28.82842712 +7 arena2.map 281 209 245 101 219 95 31.31370850 +7 arena2.map 281 209 265 138 254 114 28.55634918 +7 arena2.map 281 209 123 73 123 102 29.82842712 +7 arena2.map 281 209 135 38 105 35 31.24264069 +7 arena2.map 281 209 177 100 182 81 31.31370850 +7 arena2.map 281 209 192 102 199 75 29.89949493 +7 arena2.map 281 209 141 105 158 126 28.04163055 +7 arena2.map 281 209 226 100 201 110 29.14213562 +7 arena2.map 281 209 191 129 199 104 28.31370850 +8 arena2.map 281 209 103 103 136 102 33.41421356 +8 arena2.map 281 209 276 179 242 175 35.65685425 +8 arena2.map 281 209 151 114 136 88 32.21320343 +8 arena2.map 281 209 85 115 56 104 33.55634918 +8 arena2.map 281 209 171 60 145 36 35.94112549 +8 arena2.map 281 209 192 116 207 87 35.21320343 +8 arena2.map 281 209 184 102 198 130 33.79898987 +8 arena2.map 281 209 197 82 172 62 33.28427124 +8 arena2.map 281 209 99 82 113 56 34.14213562 +8 arena2.map 281 209 277 70 253 45 34.94112549 +9 arena2.map 281 209 146 53 112 61 37.31370850 +9 arena2.map 281 209 183 123 144 123 39.00000000 +9 arena2.map 281 209 165 136 142 166 39.52691193 +9 arena2.map 281 209 66 104 101 100 36.65685425 +9 arena2.map 281 209 106 120 84 100 39.21320343 +9 arena2.map 281 209 65 120 70 85 37.07106781 +9 arena2.map 281 209 93 107 129 108 36.41421356 +9 arena2.map 281 209 148 41 110 37 39.65685425 +9 arena2.map 281 209 92 170 120 150 36.28427124 +9 arena2.map 281 209 128 73 160 88 38.21320343 +10 arena2.map 281 209 101 77 124 49 42.79898987 +10 arena2.map 281 209 162 124 126 109 42.21320343 +10 arena2.map 281 209 119 99 150 129 43.42640686 +10 arena2.map 281 209 94 13 128 25 42.48528137 +10 arena2.map 281 209 178 64 199 99 43.69848480 +10 arena2.map 281 209 139 135 143 93 43.65685425 +10 arena2.map 281 209 234 167 266 194 43.18376617 +10 arena2.map 281 209 101 105 94 79 40.79898987 +10 arena2.map 281 209 118 150 158 144 42.48528137 +10 arena2.map 281 209 140 95 147 134 41.89949493 +11 arena2.map 281 209 97 97 56 106 44.72792206 +11 arena2.map 281 209 163 79 182 95 44.89949493 +11 arena2.map 281 209 151 61 114 44 44.04163055 +11 arena2.map 281 209 149 119 118 86 45.84062042 +11 arena2.map 281 209 91 75 99 112 45.28427124 +11 arena2.map 281 209 115 151 155 162 45.97056274 +11 arena2.map 281 209 118 110 82 93 44.21320343 +11 arena2.map 281 209 155 90 119 116 46.76955261 +11 arena2.map 281 209 166 103 143 67 45.52691193 +11 arena2.map 281 209 134 159 95 175 45.62741699 +12 arena2.map 281 209 60 121 97 88 50.66904755 +12 arena2.map 281 209 145 122 152 73 51.89949493 +12 arena2.map 281 209 184 76 201 118 50.21320343 +12 arena2.map 281 209 251 136 260 88 51.72792206 +12 arena2.map 281 209 125 146 174 152 51.48528137 +12 arena2.map 281 209 102 43 85 84 48.04163055 +12 arena2.map 281 209 78 57 67 101 48.55634918 +12 arena2.map 281 209 78 147 66 102 49.97056274 +12 arena2.map 281 209 130 17 109 60 51.69848480 +12 arena2.map 281 209 103 107 137 71 50.08326111 +13 arena2.map 281 209 114 109 66 121 52.97056274 +13 arena2.map 281 209 161 119 191 90 53.14213562 +13 arena2.map 281 209 150 131 143 78 55.89949493 +13 arena2.map 281 209 126 112 93 120 54.31370850 +13 arena2.map 281 209 97 98 125 139 52.59797974 +13 arena2.map 281 209 167 159 112 157 55.82842712 +13 arena2.map 281 209 102 118 100 84 55.79898987 +13 arena2.map 281 209 128 74 178 84 54.97056274 +13 arena2.map 281 209 119 55 82 88 54.18376617 +13 arena2.map 281 209 166 101 129 138 54.08326111 +14 arena2.map 281 209 137 163 93 135 56.76955261 +14 arena2.map 281 209 95 93 113 42 58.45584412 +14 arena2.map 281 209 60 116 106 148 59.25483398 +14 arena2.map 281 209 159 92 195 122 57.79898987 +14 arena2.map 281 209 102 146 64 105 57.32590179 +14 arena2.map 281 209 198 109 171 154 56.18376617 +14 arena2.map 281 209 198 71 152 94 56.69848480 +14 arena2.map 281 209 112 132 127 80 58.21320343 +14 arena2.map 281 209 89 62 103 103 58.07106781 +14 arena2.map 281 209 107 117 66 93 56.69848480 +15 arena2.map 281 209 150 137 183 111 63.97056274 +15 arena2.map 281 209 243 67 227 102 61.38477631 +15 arena2.map 281 209 92 115 100 65 61.79898987 +15 arena2.map 281 209 260 117 278 65 60.62741699 +15 arena2.map 281 209 208 103 168 140 60.01219330 +15 arena2.map 281 209 143 82 134 140 61.72792206 +15 arena2.map 281 209 184 70 179 125 60.38477631 +15 arena2.map 281 209 105 59 93 110 61.76955261 +15 arena2.map 281 209 96 84 111 127 63.35533905 +15 arena2.map 281 209 154 52 176 84 62.18376617 +16 arena2.map 281 209 145 89 171 77 67.89949493 +16 arena2.map 281 209 139 116 188 78 67.66904755 +16 arena2.map 281 209 269 61 250 121 67.87005768 +16 arena2.map 281 209 119 115 101 78 66.84062042 +16 arena2.map 281 209 77 97 104 44 64.18376617 +16 arena2.map 281 209 98 109 99 159 66.21320343 +16 arena2.map 281 209 187 136 133 168 67.25483398 +16 arena2.map 281 209 72 85 17 106 65.35533905 +16 arena2.map 281 209 95 34 69 90 66.76955261 +16 arena2.map 281 209 74 75 30 104 66.35533905 +17 arena2.map 281 209 138 100 194 73 69.52691193 +17 arena2.map 281 209 105 62 54 105 68.81118317 +17 arena2.map 281 209 86 105 141 73 68.25483398 +17 arena2.map 281 209 165 158 196 99 71.84062042 +17 arena2.map 281 209 205 110 258 84 68.45584412 +17 arena2.map 281 209 133 42 96 89 68.18376617 +17 arena2.map 281 209 263 131 212 102 68.28427124 +17 arena2.map 281 209 159 139 95 146 70.07106781 +17 arena2.map 281 209 181 54 195 117 68.79898987 +17 arena2.map 281 209 152 120 162 132 69.89949493 +18 arena2.map 281 209 179 69 152 69 72.79898987 +18 arena2.map 281 209 199 116 131 128 72.97056274 +18 arena2.map 281 209 260 132 272 61 75.97056274 +18 arena2.map 281 209 206 86 137 78 72.31370850 +18 arena2.map 281 209 161 133 150 121 72.89949493 +18 arena2.map 281 209 132 118 176 111 73.48528137 +18 arena2.map 281 209 149 138 199 101 73.52691193 +18 arena2.map 281 209 131 70 193 77 72.11269836 +18 arena2.map 281 209 253 128 204 87 75.35533905 +18 arena2.map 281 209 77 80 106 69 74.97056274 +19 arena2.map 281 209 96 10 127 63 79.31370850 +19 arena2.map 281 209 131 102 59 117 78.21320343 +19 arena2.map 281 209 78 143 145 168 77.35533905 +19 arena2.map 281 209 171 86 100 106 79.28427124 +19 arena2.map 281 209 163 108 157 78 77.21320343 +19 arena2.map 281 209 147 127 161 136 77.14213562 +19 arena2.map 281 209 99 130 128 86 76.74011536 +19 arena2.map 281 209 266 181 275 134 79.59797974 +19 arena2.map 281 209 253 115 250 40 76.24264069 +19 arena2.map 281 209 254 81 249 155 76.07106781 +20 arena2.map 281 209 86 130 139 91 80.28427124 +20 arena2.map 281 209 131 45 200 74 81.01219330 +20 arena2.map 281 209 51 105 96 168 83.98275604 +20 arena2.map 281 209 175 79 128 83 80.07106781 +20 arena2.map 281 209 110 81 80 146 82.69848480 +20 arena2.map 281 209 228 190 258 121 81.42640686 +20 arena2.map 281 209 131 104 90 67 80.72792206 +20 arena2.map 281 209 156 49 85 73 81.52691193 +20 arena2.map 281 209 130 71 84 129 83.49747467 +20 arena2.map 281 209 220 90 150 122 83.25483398 +21 arena2.map 281 209 190 142 126 113 84.21320343 +21 arena2.map 281 209 146 66 177 61 85.45584412 +21 arena2.map 281 209 90 78 129 67 87.11269836 +21 arena2.map 281 209 237 184 251 104 85.79898987 +21 arena2.map 281 209 86 85 154 87 86.97056274 +21 arena2.map 281 209 168 152 142 113 85.66904755 +21 arena2.map 281 209 135 10 79 56 84.42640686 +21 arena2.map 281 209 171 90 163 142 84.62741699 +21 arena2.map 281 209 134 45 82 101 85.74011536 +21 arena2.map 281 209 256 75 201 123 86.01219330 +22 arena2.map 281 209 187 92 166 171 88.52691193 +22 arena2.map 281 209 182 104 134 148 90.52691193 +22 arena2.map 281 209 209 94 266 62 90.59797974 +22 arena2.map 281 209 249 53 263 136 88.79898987 +22 arena2.map 281 209 257 63 210 84 91.35533905 +22 arena2.map 281 209 165 110 78 118 90.31370850 +22 arena2.map 281 209 131 136 188 77 89.63961029 +22 arena2.map 281 209 97 174 49 110 90.32590179 +22 arena2.map 281 209 97 165 112 119 91.04163055 +22 arena2.map 281 209 63 112 133 160 89.88225098 +23 arena2.map 281 209 119 67 78 67 93.28427124 +23 arena2.map 281 209 185 70 138 138 95.81118317 +23 arena2.map 281 209 196 103 141 167 93.22539673 +23 arena2.map 281 209 222 99 155 38 93.43860016 +23 arena2.map 281 209 74 123 145 175 92.53910522 +23 arena2.map 281 209 212 92 277 59 95.98275604 +23 arena2.map 281 209 181 96 255 126 95.11269836 +23 arena2.map 281 209 158 119 87 84 95.69848480 +23 arena2.map 281 209 165 108 92 123 94.72792206 +23 arena2.map 281 209 129 59 92 116 93.69848480 +24 arena2.map 281 209 72 101 143 45 99.46803741 +24 arena2.map 281 209 137 51 59 97 97.05382385 +24 arena2.map 281 209 147 62 228 98 96.74011536 +24 arena2.map 281 209 174 115 262 122 98.11269836 +24 arena2.map 281 209 252 139 193 131 98.59797974 +24 arena2.map 281 209 176 65 163 136 98.04163055 +24 arena2.map 281 209 175 130 87 155 98.35533905 +24 arena2.map 281 209 78 124 167 109 98.14213562 +24 arena2.map 281 209 98 170 82 79 97.62741699 +24 arena2.map 281 209 136 123 171 62 97.19595947 +25 arena2.map 281 209 261 85 178 121 100.25483398 +25 arena2.map 281 209 159 51 238 108 102.61017303 +25 arena2.map 281 209 78 55 13 104 102.18376617 +25 arena2.map 281 209 192 132 153 52 101.42640686 +25 arena2.map 281 209 78 106 175 96 102.79898987 +25 arena2.map 281 209 75 159 133 113 101.21320343 +25 arena2.map 281 209 144 94 240 109 102.21320343 +25 arena2.map 281 209 251 107 267 194 100.69848480 +25 arena2.map 281 209 205 122 240 152 103.52691193 +25 arena2.map 281 209 101 124 162 99 103.69848480 +26 arena2.map 281 209 263 78 179 96 104.18376617 +26 arena2.map 281 209 111 133 110 55 104.15432892 +26 arena2.map 281 209 100 151 128 75 105.63961029 +26 arena2.map 281 209 139 157 208 94 105.05382385 +26 arena2.map 281 209 95 156 128 134 104.01219330 +26 arena2.map 281 209 155 153 166 81 107.08326111 +26 arena2.map 281 209 147 131 159 74 106.49747467 +26 arena2.map 281 209 60 96 159 116 107.28427124 +26 arena2.map 281 209 94 111 133 30 104.36753235 +26 arena2.map 281 209 134 157 107 114 106.42640686 +27 arena2.map 281 209 267 45 208 113 109.42640686 +27 arena2.map 281 209 178 96 139 168 111.56854248 +27 arena2.map 281 209 257 121 173 146 108.88225098 +27 arena2.map 281 209 106 115 205 92 108.52691193 +27 arena2.map 281 209 199 86 242 154 110.84062042 +27 arena2.map 281 209 115 53 169 90 110.25483398 +27 arena2.map 281 209 206 111 228 151 109.55634918 +27 arena2.map 281 209 97 108 149 167 109.56854248 +27 arena2.map 281 209 231 100 249 191 109.62741699 +27 arena2.map 281 209 138 135 166 172 110.49747467 +28 arena2.map 281 209 166 110 257 85 113.11269836 +28 arena2.map 281 209 128 164 95 77 112.81118317 +28 arena2.map 281 209 170 93 109 63 113.18376617 +28 arena2.map 281 209 179 109 119 162 113.08326111 +28 arena2.map 281 209 119 52 206 106 115.22539673 +28 arena2.map 281 209 84 109 166 135 114.42640686 +28 arena2.map 281 209 162 44 132 99 112.39696960 +28 arena2.map 281 209 144 150 141 139 113.84062042 +28 arena2.map 281 209 134 81 158 143 114.36753235 +28 arena2.map 281 209 160 82 56 93 113.52691193 +29 arena2.map 281 209 150 148 51 100 118.88225098 +29 arena2.map 281 209 174 59 66 83 118.76955261 +29 arena2.map 281 209 136 169 226 99 118.99494934 +29 arena2.map 281 209 124 23 64 115 119.33809509 +29 arena2.map 281 209 114 77 121 164 118.95331879 +29 arena2.map 281 209 156 67 123 110 117.74011536 +29 arena2.map 281 209 138 120 140 162 118.32590179 +29 arena2.map 281 209 262 96 268 191 116.01219330 +29 arena2.map 281 209 240 57 190 78 119.56854248 +29 arena2.map 281 209 155 104 99 150 116.28427124 +30 arena2.map 281 209 125 83 158 39 121.08326111 +30 arena2.map 281 209 145 168 132 126 120.98275604 +30 arena2.map 281 209 208 102 115 155 120.22539673 +30 arena2.map 281 209 114 126 130 46 122.12489166 +30 arena2.map 281 209 276 130 178 91 120.25483398 +30 arena2.map 281 209 149 45 147 126 123.50966797 +30 arena2.map 281 209 173 91 94 71 121.62741699 +30 arena2.map 281 209 127 137 167 57 120.99494934 +30 arena2.map 281 209 154 113 92 55 120.28427124 +30 arena2.map 281 209 171 84 267 131 120.74011536 +31 arena2.map 281 209 146 95 135 165 126.92388153 +31 arena2.map 281 209 118 172 127 129 124.12489166 +31 arena2.map 281 209 75 117 132 13 127.61017303 +31 arena2.map 281 209 186 76 94 9 126.78174591 +31 arena2.map 281 209 174 88 250 149 125.52691193 +31 arena2.map 281 209 147 114 94 170 125.38477631 +31 arena2.map 281 209 118 42 205 112 125.95331879 +31 arena2.map 281 209 248 101 146 67 125.35533905 +31 arena2.map 281 209 152 58 115 78 126.01219330 +31 arena2.map 281 209 268 47 191 119 127.32590179 +32 arena2.map 281 209 173 130 265 142 128.22539673 +32 arena2.map 281 209 95 100 109 3 131.61017303 +32 arena2.map 281 209 260 125 163 159 129.50966797 +32 arena2.map 281 209 186 135 268 155 129.88225098 +32 arena2.map 281 209 138 158 120 132 131.98275604 +32 arena2.map 281 209 262 66 252 186 129.11269836 +32 arena2.map 281 209 189 123 130 27 128.05382385 +32 arena2.map 281 209 261 90 145 109 128.84062042 +32 arena2.map 281 209 132 67 101 166 130.71067810 +32 arena2.map 281 209 133 137 118 169 131.61017303 +33 arena2.map 281 209 116 117 247 109 135.97056274 +33 arena2.map 281 209 179 89 278 55 134.63961029 +33 arena2.map 281 209 177 134 74 105 133.39696960 +33 arena2.map 281 209 93 91 198 81 132.11269836 +33 arena2.map 281 209 158 122 240 79 134.94112549 +33 arena2.map 281 209 114 119 151 171 134.29646454 +33 arena2.map 281 209 159 58 110 114 132.49747467 +33 arena2.map 281 209 247 178 194 98 133.04163055 +33 arena2.map 281 209 79 86 197 110 135.59797974 +33 arena2.map 281 209 279 39 193 99 132.91168823 +34 arena2.map 281 209 124 44 133 65 138.85281372 +34 arena2.map 281 209 136 75 13 108 138.32590179 +34 arena2.map 281 209 261 104 132 124 137.28427124 +34 arena2.map 281 209 194 128 223 163 138.56854248 +34 arena2.map 281 209 115 124 168 46 137.50966797 +34 arena2.map 281 209 69 118 136 7 138.75230865 +34 arena2.map 281 209 117 118 151 175 137.36753235 +34 arena2.map 281 209 78 121 167 71 137.08326111 +34 arena2.map 281 209 83 131 165 61 139.69848480 +34 arena2.map 281 209 114 69 165 37 136.98275604 +35 arena2.map 281 209 132 172 101 57 142.61017303 +35 arena2.map 281 209 258 65 154 85 143.01219330 +35 arena2.map 281 209 168 172 111 104 142.05382385 +35 arena2.map 281 209 172 103 96 53 140.42640686 +35 arena2.map 281 209 206 106 73 96 141.28427124 +35 arena2.map 281 209 123 78 253 105 141.18376617 +35 arena2.map 281 209 253 137 161 159 140.61017303 +35 arena2.map 281 209 182 79 272 49 140.71067810 +35 arena2.map 281 209 158 166 252 78 142.16652222 +35 arena2.map 281 209 227 155 196 142 143.25483398 +36 arena2.map 281 209 1 110 112 67 146.28427124 +36 arena2.map 281 209 230 175 180 108 146.35533905 +36 arena2.map 281 209 206 108 93 84 145.76955261 +36 arena2.map 281 209 111 72 156 152 145.06601715 +36 arena2.map 281 209 260 199 207 102 144.18376617 +36 arena2.map 281 209 122 10 158 98 144.16652222 +36 arena2.map 281 209 95 174 115 43 146.74011536 +36 arena2.map 281 209 260 100 127 74 146.25483398 +36 arena2.map 281 209 276 46 223 159 144.32590179 +36 arena2.map 281 209 86 74 168 151 146.69848480 +37 arena2.map 281 209 238 56 159 124 150.74011536 +37 arena2.map 281 209 116 69 251 101 149.91168823 +37 arena2.map 281 209 167 141 39 99 150.95331879 +37 arena2.map 281 209 134 2 193 116 148.53910522 +37 arena2.map 281 209 251 86 124 108 149.52691193 +37 arena2.map 281 209 218 108 127 3 150.30865784 +37 arena2.map 281 209 76 151 127 23 149.12489166 +37 arena2.map 281 209 242 202 236 62 149.94112549 +37 arena2.map 281 209 106 14 64 118 148.75230865 +37 arena2.map 281 209 133 60 150 143 149.05382385 +38 arena2.map 281 209 255 89 134 46 153.92388153 +38 arena2.map 281 209 235 175 181 82 153.22539673 +38 arena2.map 281 209 256 41 242 191 155.79898987 +38 arena2.map 281 209 111 65 163 169 155.62236633 +38 arena2.map 281 209 183 51 280 150 155.23759003 +38 arena2.map 281 209 267 137 151 60 154.33809509 +38 arena2.map 281 209 164 46 276 135 155.89444427 +38 arena2.map 281 209 213 90 75 76 155.39696960 +38 arena2.map 281 209 176 61 233 159 155.26702728 +38 arena2.map 281 209 129 51 174 157 154.98275604 +39 arena2.map 281 209 259 205 270 62 158.32590179 +39 arena2.map 281 209 125 102 115 4 157.19595947 +39 arena2.map 281 209 246 201 194 87 159.59797974 +39 arena2.map 281 209 139 138 12 98 157.46803741 +39 arena2.map 281 209 93 129 225 109 159.88225098 +39 arena2.map 281 209 188 129 125 4 158.71067810 +39 arena2.map 281 209 116 100 254 86 157.11269836 +39 arena2.map 281 209 265 131 135 74 158.88225098 +39 arena2.map 281 209 129 170 183 50 159.05382385 +39 arena2.map 281 209 212 85 278 185 157.25483398 +40 arena2.map 281 209 106 169 242 108 162.09545441 +40 arena2.map 281 209 131 8 235 108 161.23759003 +40 arena2.map 281 209 238 106 98 139 162.05382385 +40 arena2.map 281 209 144 167 145 58 163.19595947 +40 arena2.map 281 209 86 65 173 136 163.59797974 +40 arena2.map 281 209 270 59 149 79 163.81118317 +40 arena2.map 281 209 127 89 241 75 163.69848480 +40 arena2.map 281 209 147 148 273 142 163.33809509 +40 arena2.map 281 209 148 175 167 40 161.09545441 +40 arena2.map 281 209 250 154 138 115 161.97056274 +41 arena2.map 281 209 200 132 83 63 164.91168823 +41 arena2.map 281 209 245 203 269 47 165.94112549 +41 arena2.map 281 209 195 127 108 13 165.85281372 +41 arena2.map 281 209 80 150 227 103 166.46803741 +41 arena2.map 281 209 166 34 236 66 166.16652222 +41 arena2.map 281 209 127 132 244 75 166.91168823 +41 arena2.map 281 209 105 6 125 103 165.61017303 +41 arena2.map 281 209 245 158 161 53 164.09545441 +41 arena2.map 281 209 77 71 177 159 165.74011536 +41 arena2.map 281 209 141 119 124 14 165.03657989 +42 arena2.map 281 209 165 39 280 67 168.13708496 +42 arena2.map 281 209 154 165 128 54 171.02438660 +42 arena2.map 281 209 173 103 258 191 168.94112549 +42 arena2.map 281 209 275 51 148 99 168.39696960 +42 arena2.map 281 209 57 112 218 99 170.52691193 +42 arena2.map 281 209 177 64 244 186 171.95331879 +42 arena2.map 281 209 109 134 111 2 170.99494934 +42 arena2.map 281 209 265 68 139 155 171.99494934 +42 arena2.map 281 209 160 53 115 147 170.98275604 +42 arena2.map 281 209 268 142 140 70 168.95331879 +43 arena2.map 281 209 94 74 241 110 173.61017303 +43 arena2.map 281 209 242 77 122 138 175.56854248 +43 arena2.map 281 209 116 164 146 35 175.65180359 +43 arena2.map 281 209 261 71 129 146 174.43860016 +43 arena2.map 281 209 231 165 159 65 175.09545441 +43 arena2.map 281 209 175 150 218 163 172.26702728 +43 arena2.map 281 209 139 162 236 79 174.50966797 +43 arena2.map 281 209 146 170 275 72 174.86500702 +43 arena2.map 281 209 146 68 242 58 172.91168823 +43 arena2.map 281 209 144 173 245 66 173.13708496 +44 arena2.map 281 209 151 73 280 55 177.95331879 +44 arena2.map 281 209 136 78 103 13 179.68124084 +44 arena2.map 281 209 135 125 265 158 179.08326111 +44 arena2.map 281 209 253 204 188 138 177.22539673 +44 arena2.map 281 209 247 157 127 121 179.69848480 +44 arena2.map 281 209 265 35 246 207 179.87005768 +44 arena2.map 281 209 88 126 240 103 177.28427124 +44 arena2.map 281 209 266 69 121 105 176.15432892 +44 arena2.map 281 209 148 146 259 47 178.61017303 +44 arena2.map 281 209 178 158 93 57 179.49747467 +45 arena2.map 281 209 257 142 109 116 181.45584412 +45 arena2.map 281 209 233 158 142 75 183.63961029 +45 arena2.map 281 209 257 95 97 148 181.95331879 +45 arena2.map 281 209 127 30 253 133 180.96551208 +45 arena2.map 281 209 261 39 280 189 183.76955261 +45 arena2.map 281 209 112 76 94 3 183.26702728 +45 arena2.map 281 209 144 131 265 44 181.05382385 +45 arena2.map 281 209 128 86 275 151 183.88225098 +45 arena2.map 281 209 277 137 140 44 180.79393921 +45 arena2.map 281 209 184 53 100 164 182.98275604 +46 arena2.map 281 209 139 147 274 156 185.33809509 +46 arena2.map 281 209 265 141 110 88 186.91168823 +46 arena2.map 281 209 125 20 264 120 185.10764770 +46 arena2.map 281 209 239 148 136 156 185.95331879 +46 arena2.map 281 209 131 150 245 152 184.12489166 +46 arena2.map 281 209 280 68 125 79 184.71067810 +46 arena2.map 281 209 264 182 164 91 184.59797974 +46 arena2.map 281 209 277 186 273 39 184.49747467 +46 arena2.map 281 209 182 157 98 44 185.56854248 +46 arena2.map 281 209 104 158 262 89 186.58073578 +47 arena2.map 281 209 146 163 229 153 190.92388153 +47 arena2.map 281 209 116 94 250 51 188.79898987 +47 arena2.map 281 209 123 75 263 151 188.46803741 +47 arena2.map 281 209 140 105 225 161 188.49747467 +47 arena2.map 281 209 171 159 258 185 191.26702728 +47 arena2.map 281 209 114 48 264 131 189.65180359 +47 arena2.map 281 209 55 109 235 99 191.45584412 +47 arena2.map 281 209 114 142 270 73 189.85281372 +47 arena2.map 281 209 255 50 128 138 189.81118317 +47 arena2.map 281 209 111 11 111 160 188.23759003 +48 arena2.map 281 209 275 188 166 95 195.25483398 +48 arena2.map 281 209 279 67 112 89 192.98275604 +48 arena2.map 281 209 220 182 161 153 195.13708496 +48 arena2.map 281 209 160 38 234 174 195.96551208 +48 arena2.map 281 209 120 59 255 59 192.53910522 +48 arena2.map 281 209 115 87 264 55 193.66904755 +48 arena2.map 281 209 82 78 253 106 195.85281372 +48 arena2.map 281 209 251 180 149 78 194.08326111 +48 arena2.map 281 209 11 111 163 76 192.43860016 +48 arena2.map 281 209 255 53 120 71 193.56854248 +49 arena2.map 281 209 263 107 100 82 199.88225098 +49 arena2.map 281 209 116 37 260 140 198.96551208 +49 arena2.map 281 209 115 97 234 150 199.11269836 +49 arena2.map 281 209 165 127 92 8 196.40916290 +49 arena2.map 281 209 232 149 128 151 199.29646454 +49 arena2.map 281 209 268 151 125 55 196.82337646 +49 arena2.map 281 209 261 195 162 76 196.36753235 +49 arena2.map 281 209 258 134 104 163 198.33809509 +49 arena2.map 281 209 260 48 135 44 196.82337646 +49 arena2.map 281 209 239 197 165 60 198.68124084 +50 arena2.map 281 209 182 55 254 207 201.19595947 +50 arena2.map 281 209 156 140 221 190 202.33809509 +50 arena2.map 281 209 109 12 133 147 203.50966797 +50 arena2.map 281 209 264 149 123 35 203.45079346 +50 arena2.map 281 209 237 167 131 65 201.43860016 +50 arena2.map 281 209 85 94 248 78 201.87005768 +50 arena2.map 281 209 264 40 126 78 201.39696960 +50 arena2.map 281 209 119 153 99 9 203.23759003 +50 arena2.map 281 209 154 87 264 195 201.63961029 +50 arena2.map 281 209 89 118 261 128 202.42640686 +51 arena2.map 281 209 134 68 227 170 204.92388153 +51 arena2.map 281 209 269 201 161 132 204.19595947 +51 arena2.map 281 209 226 151 117 107 205.52691193 +51 arena2.map 281 209 183 159 7 97 207.58073578 +51 arena2.map 281 209 133 112 226 181 205.15432892 +51 arena2.map 281 209 131 32 237 153 206.96551208 +51 arena2.map 281 209 99 146 256 143 205.29646454 +51 arena2.map 281 209 99 119 264 100 205.35533905 +51 arena2.map 281 209 222 170 129 107 205.39696960 +51 arena2.map 281 209 252 123 107 13 204.62236633 +52 arena2.map 281 209 264 146 125 19 210.35028839 +52 arena2.map 281 209 250 182 141 66 209.22539673 +52 arena2.map 281 209 118 55 234 147 209.78174591 +52 arena2.map 281 209 263 43 129 43 209.48023071 +52 arena2.map 281 209 240 191 138 102 208.49747467 +52 arena2.map 281 209 117 91 265 35 210.42640686 +52 arena2.map 281 209 250 40 113 73 211.49747467 +52 arena2.map 281 209 259 180 132 112 208.69848480 +52 arena2.map 281 209 256 72 94 74 211.43860016 +52 arena2.map 281 209 243 161 125 41 208.89444427 +53 arena2.map 281 209 125 77 242 180 213.39696960 +53 arena2.map 281 209 124 175 260 47 214.20815277 +53 arena2.map 281 209 143 166 115 11 215.79393921 +53 arena2.map 281 209 247 54 108 154 212.43860016 +53 arena2.map 281 209 125 105 243 187 215.01219330 +53 arena2.map 281 209 135 166 111 10 212.20815277 +53 arena2.map 281 209 143 106 259 200 213.74011536 +53 arena2.map 281 209 91 5 118 158 213.96551208 +53 arena2.map 281 209 61 95 262 117 214.25483398 +53 arena2.map 281 209 152 156 110 10 212.55129852 +54 arena2.map 281 209 257 153 101 143 216.12489166 +54 arena2.map 281 209 91 10 169 157 219.06601715 +54 arena2.map 281 209 250 191 125 89 219.42640686 +54 arena2.map 281 209 118 51 242 166 217.16652222 +54 arena2.map 281 209 259 199 140 126 216.56854248 +54 arena2.map 281 209 273 139 96 54 217.48023071 +54 arena2.map 281 209 212 114 4 105 218.35533905 +54 arena2.map 281 209 124 2 252 141 217.79393921 +54 arena2.map 281 209 238 60 133 13 216.45079346 +54 arena2.map 281 209 239 60 129 12 217.03657989 +55 arena2.map 281 209 110 110 222 172 223.98275604 +55 arena2.map 281 209 159 175 219 190 220.52186127 +55 arena2.map 281 209 222 146 104 109 220.45584412 +55 arena2.map 281 209 77 54 253 72 223.78174591 +55 arena2.map 281 209 125 153 271 34 221.65180359 +55 arena2.map 281 209 268 68 78 113 220.98275604 +55 arena2.map 281 209 129 47 237 179 222.89444427 +55 arena2.map 281 209 256 144 93 172 222.23759003 +55 arena2.map 281 209 128 90 231 191 220.56854248 +55 arena2.map 281 209 254 104 40 106 222.28427124 +56 arena2.map 281 209 100 170 280 142 225.86500702 +56 arena2.map 281 209 262 186 118 115 226.94112549 +56 arena2.map 281 209 218 157 129 37 226.37972565 +56 arena2.map 281 209 274 55 132 12 226.17871551 +56 arena2.map 281 209 253 50 85 102 224.52691193 +56 arena2.map 281 209 73 114 236 74 226.01219330 +56 arena2.map 281 209 239 79 102 114 227.94112549 +56 arena2.map 281 209 138 168 245 194 225.99494934 +56 arena2.map 281 209 0 105 214 107 225.59797974 +56 arena2.map 281 209 99 111 228 160 224.42640686 +57 arena2.map 281 209 246 61 70 94 230.59797974 +57 arena2.map 281 209 250 135 55 102 231.35533905 +57 arena2.map 281 209 235 72 82 125 229.25483398 +57 arena2.map 281 209 262 134 59 103 230.08326111 +57 arena2.map 281 209 246 172 110 166 230.75230865 +57 arena2.map 281 209 81 102 235 55 229.32590179 +57 arena2.map 281 209 148 144 267 197 228.46803741 +57 arena2.map 281 209 111 10 275 65 231.14927826 +57 arena2.map 281 209 67 111 240 66 230.66904755 +57 arena2.map 281 209 148 141 270 202 230.53910522 +58 arena2.map 281 209 235 64 112 2 232.90663757 +58 arena2.map 281 209 263 57 100 131 232.09545441 +58 arena2.map 281 209 100 169 274 157 234.45079346 +58 arena2.map 281 209 235 156 101 174 235.72287140 +58 arena2.map 281 209 268 41 113 174 234.10764770 +58 arena2.map 281 209 229 181 125 175 235.52186127 +58 arena2.map 281 209 68 87 251 59 235.42640686 +58 arena2.map 281 209 92 137 269 55 234.48023071 +58 arena2.map 281 209 76 51 254 64 234.43860016 +58 arena2.map 281 209 280 73 117 2 233.07821045 +59 arena2.map 281 209 74 62 276 69 238.33809509 +59 arena2.map 281 209 245 170 100 68 237.61017303 +59 arena2.map 281 209 132 136 280 186 239.63961029 +59 arena2.map 281 209 62 90 235 78 237.18376617 +59 arena2.map 281 209 246 206 138 167 237.16652222 +59 arena2.map 281 209 261 54 84 83 238.42640686 +59 arena2.map 281 209 274 195 136 149 238.75230865 +59 arena2.map 281 209 97 88 263 58 236.32590179 +59 arena2.map 281 209 244 170 99 51 239.33809509 +59 arena2.map 281 209 142 157 273 205 239.20815277 +60 arena2.map 281 209 107 13 260 159 243.93607483 +60 arena2.map 281 209 270 53 83 66 242.16652222 +60 arena2.map 281 209 109 144 258 178 241.46803741 +60 arena2.map 281 209 99 163 251 35 242.50966797 +60 arena2.map 281 209 108 9 280 59 242.63455963 +60 arena2.map 281 209 127 75 273 189 240.12489166 +60 arena2.map 281 209 98 85 270 56 242.58073578 +60 arena2.map 281 209 127 92 277 205 243.71067810 +60 arena2.map 281 209 251 54 103 3 243.10764770 +60 arena2.map 281 209 75 151 273 146 240.58073578 +61 arena2.map 281 209 260 45 94 119 245.49747467 +61 arena2.map 281 209 84 120 231 167 247.15432892 +61 arena2.map 281 209 103 154 224 176 245.89444427 +61 arena2.map 281 209 70 122 278 153 246.78174591 +61 arena2.map 281 209 111 171 277 34 245.59292908 +61 arena2.map 281 209 250 57 55 104 247.94112549 +61 arena2.map 281 209 274 155 76 125 244.71067810 +61 arena2.map 281 209 101 152 220 171 247.82337646 +61 arena2.map 281 209 243 167 93 46 244.82337646 +61 arena2.map 281 209 275 198 135 156 244.89444427 +62 arena2.map 281 209 220 153 79 109 249.52691193 +62 arena2.map 281 209 103 40 230 175 250.69343414 +62 arena2.map 281 209 104 4 264 53 248.07821045 +62 arena2.map 281 209 101 103 257 194 250.15432892 +62 arena2.map 281 209 98 161 249 179 250.09545441 +62 arena2.map 281 209 113 70 254 202 251.95331879 +62 arena2.map 281 209 244 197 127 28 249.03657989 +62 arena2.map 281 209 249 186 102 143 249.53910522 +62 arena2.map 281 209 240 183 105 164 248.40916290 +62 arena2.map 281 209 230 160 95 130 249.50966797 +63 arena2.map 281 209 58 122 240 150 254.42640686 +63 arena2.map 281 209 75 118 269 40 254.46803741 +63 arena2.map 281 209 269 38 75 116 255.63961029 +63 arena2.map 281 209 109 81 238 206 255.39696960 +63 arena2.map 281 209 277 182 120 151 253.19595947 +63 arena2.map 281 209 250 37 69 100 252.28427124 +63 arena2.map 281 209 112 146 257 200 253.85281372 +63 arena2.map 281 209 265 38 93 40 252.55129852 +63 arena2.map 281 209 275 38 95 161 252.62236633 +63 arena2.map 281 209 227 169 93 95 255.63961029 +64 arena2.map 281 209 232 166 77 129 257.63961029 +64 arena2.map 281 209 120 164 272 192 257.72287140 +64 arena2.map 281 209 84 129 267 41 257.56854248 +64 arena2.map 281 209 67 121 237 169 259.98275604 +64 arena2.map 281 209 251 127 23 106 258.25483398 +64 arena2.map 281 209 83 122 224 168 256.98275604 +64 arena2.map 281 209 76 130 257 41 257.74011536 +64 arena2.map 281 209 91 4 240 57 256.24978332 +64 arena2.map 281 209 242 182 101 35 256.79393921 +64 arena2.map 281 209 224 163 101 79 256.33809509 +65 arena2.map 281 209 221 184 102 137 262.65180359 +65 arena2.map 281 209 274 39 83 77 262.37972565 +65 arena2.map 281 209 94 4 252 45 261.10764770 +65 arena2.map 281 209 66 95 262 35 262.66904755 +65 arena2.map 281 209 276 206 130 172 260.83556976 +65 arena2.map 281 209 93 60 259 183 263.85281372 +65 arena2.map 281 209 44 110 248 52 263.94112549 +65 arena2.map 281 209 76 89 245 183 263.32590179 +65 arena2.map 281 209 98 99 245 207 261.18376617 +65 arena2.map 281 209 127 12 256 191 262.35028839 +66 arena2.map 281 209 90 51 226 179 264.79393921 +66 arena2.map 281 209 258 206 118 36 266.69343414 +66 arena2.map 281 209 227 190 103 46 264.45079346 +66 arena2.map 281 209 111 9 238 181 265.24978332 +66 arena2.map 281 209 279 201 126 168 264.10764770 +66 arena2.map 281 209 219 161 114 2 265.32085113 +66 arena2.map 281 209 236 200 107 162 264.23759003 +66 arena2.map 281 209 229 152 92 13 267.27922058 +66 arena2.map 281 209 227 171 80 68 264.30865784 +66 arena2.map 281 209 106 61 268 197 265.23759003 +67 arena2.map 281 209 235 199 96 144 269.68124084 +67 arena2.map 281 209 254 195 95 138 268.50966797 +67 arena2.map 281 209 110 165 258 206 269.30865784 +67 arena2.map 281 209 81 100 264 180 268.01219330 +67 arena2.map 281 209 108 10 240 183 269.00714264 +67 arena2.map 281 209 100 44 252 198 270.89444427 +67 arena2.map 281 209 260 99 3 110 270.66904755 +67 arena2.map 281 209 90 172 248 186 268.06601715 +67 arena2.map 281 209 264 190 82 98 271.15432892 +67 arena2.map 281 209 118 6 239 191 270.83556976 +68 arena2.map 281 209 91 172 245 193 273.65180359 +68 arena2.map 281 209 227 185 100 116 275.39696960 +68 arena2.map 281 209 95 40 259 193 275.45079346 +68 arena2.map 281 209 62 95 280 36 273.12489166 +68 arena2.map 281 209 79 105 237 196 272.49747467 +68 arena2.map 281 209 86 86 230 187 272.39696960 +68 arena2.map 281 209 93 101 275 200 275.71067810 +68 arena2.map 281 209 93 151 241 202 273.61017303 +68 arena2.map 281 209 74 76 239 179 275.63961029 +68 arena2.map 281 209 261 182 95 93 274.25483398 +69 arena2.map 281 209 245 176 47 108 279.94112549 +69 arena2.map 281 209 90 70 249 201 278.50966797 +69 arena2.map 281 209 108 174 271 200 276.17871551 +69 arena2.map 281 209 89 68 250 201 279.09545441 +69 arena2.map 281 209 67 101 244 191 277.42640686 +69 arena2.map 281 209 98 168 272 187 279.30865784 +69 arena2.map 281 209 279 204 116 36 279.49242401 +69 arena2.map 281 209 269 194 134 8 278.42135620 +69 arena2.map 281 209 94 132 268 181 277.85281372 +69 arena2.map 281 209 88 147 253 203 278.78174591 +70 arena2.map 281 209 123 20 280 203 283.22034607 +70 arena2.map 281 209 222 191 73 94 281.78174591 +70 arena2.map 281 209 274 36 48 101 283.81118317 +70 arena2.map 281 209 18 104 260 142 281.15432892 +70 arena2.map 281 209 240 54 27 100 283.08326111 +70 arena2.map 281 209 79 133 263 181 283.92388153 +70 arena2.map 281 209 93 162 235 203 281.65180359 +70 arena2.map 281 209 241 190 66 89 281.98275604 +70 arena2.map 281 209 71 115 242 200 282.42640686 +70 arena2.map 281 209 16 98 257 86 281.08326111 +71 arena2.map 281 209 271 199 93 55 284.55129852 +71 arena2.map 281 209 276 186 91 68 285.75230865 +71 arena2.map 281 209 33 107 258 157 286.25483398 +71 arena2.map 281 209 25 104 263 56 285.15432892 +71 arena2.map 281 209 48 107 243 182 286.18376617 +71 arena2.map 281 209 280 206 109 173 286.24978332 +71 arena2.map 281 209 16 111 261 84 286.81118317 +71 arena2.map 281 209 263 90 21 97 286.74011536 +71 arena2.map 281 209 99 70 271 207 286.96551208 +71 arena2.map 281 209 40 109 224 154 284.94112549 +72 arena2.map 281 209 260 179 94 11 289.52186127 +72 arena2.map 281 209 72 114 278 178 290.18376617 +72 arena2.map 281 209 71 106 262 205 291.98275604 +72 arena2.map 281 209 12 101 265 144 291.63961029 +72 arena2.map 281 209 20 102 269 61 288.22539673 +72 arena2.map 281 209 276 201 99 44 288.93607483 +72 arena2.map 281 209 246 79 13 111 291.08326111 +72 arena2.map 281 209 249 60 12 103 289.59797974 +72 arena2.map 281 209 265 199 80 129 291.46803741 +72 arena2.map 281 209 20 105 251 52 289.59797974 +73 arena2.map 281 209 63 124 254 198 293.81118317 +73 arena2.map 281 209 31 102 227 152 295.25483398 +73 arena2.map 281 209 81 143 276 193 295.75230865 +73 arena2.map 281 209 267 199 76 65 295.89444427 +73 arena2.map 281 209 55 92 233 187 292.05382385 +73 arena2.map 281 209 269 186 76 60 292.09545441 +73 arena2.map 281 209 252 197 100 6 292.66399689 +73 arena2.map 281 209 279 194 90 157 292.65180359 +73 arena2.map 281 209 77 103 275 203 292.95331879 +73 arena2.map 281 209 235 65 12 106 293.39696960 +74 arena2.map 281 209 276 40 36 100 299.05382385 +74 arena2.map 281 209 276 181 79 50 298.16652222 +74 arena2.map 281 209 276 62 14 108 297.19595947 +74 arena2.map 281 209 62 99 266 199 297.05382385 +74 arena2.map 281 209 277 207 89 89 298.02438660 +74 arena2.map 281 209 6 112 238 71 296.98275604 +74 arena2.map 281 209 83 71 277 190 297.65180359 +74 arena2.map 281 209 220 170 37 111 299.39696960 +74 arena2.map 281 209 50 102 228 188 298.46803741 +74 arena2.map 281 209 18 108 242 50 298.56854248 +75 arena2.map 281 209 7 101 245 149 300.66904755 +75 arena2.map 281 209 256 64 0 112 302.56854248 +75 arena2.map 281 209 254 204 99 3 302.73506470 +75 arena2.map 281 209 46 108 224 187 300.63961029 +75 arena2.map 281 209 98 12 263 204 303.73506470 +75 arena2.map 281 209 49 107 269 180 303.76955261 +75 arena2.map 281 209 35 102 225 164 301.39696960 +75 arena2.map 281 209 104 12 276 195 301.73506470 +75 arena2.map 281 209 274 150 12 107 302.19595947 +75 arena2.map 281 209 265 149 6 104 302.22539673 +76 arena2.map 281 209 85 77 279 201 304.69343414 +76 arena2.map 281 209 39 108 229 188 306.56854248 +76 arena2.map 281 209 265 144 14 111 306.71067810 +76 arena2.map 281 209 268 205 62 117 304.71067810 +76 arena2.map 281 209 226 153 24 105 304.32590179 +76 arena2.map 281 209 272 40 23 104 306.88225098 +76 arena2.map 281 209 272 205 79 74 304.69343414 +76 arena2.map 281 209 279 178 61 120 304.66904755 +76 arena2.map 281 209 43 106 244 195 304.18376617 +76 arena2.map 281 209 280 199 106 8 307.04877319 +77 arena2.map 281 209 263 153 1 102 310.39696960 +77 arena2.map 281 209 12 108 252 43 308.25483398 +77 arena2.map 281 209 26 100 241 180 309.91168823 +77 arena2.map 281 209 7 110 237 53 309.46803741 +77 arena2.map 281 209 256 185 34 99 310.91168823 +77 arena2.map 281 209 23 97 246 66 308.08326111 +77 arena2.map 281 209 15 108 245 169 311.56854248 +77 arena2.map 281 209 279 198 61 95 311.71067810 +77 arena2.map 281 209 278 200 64 121 309.36753235 +77 arena2.map 281 209 10 105 278 52 310.78174591 +78 arena2.map 281 209 29 106 220 164 312.05382385 +78 arena2.map 281 209 253 46 4 106 312.84062042 +78 arena2.map 281 209 26 99 228 180 314.88225098 +78 arena2.map 281 209 240 147 11 111 312.49747467 +78 arena2.map 281 209 257 50 2 98 313.32590179 +78 arena2.map 281 209 229 163 16 108 314.71067810 +78 arena2.map 281 209 35 101 256 183 312.49747467 +78 arena2.map 281 209 41 109 249 203 313.35533905 +78 arena2.map 281 209 22 97 262 63 312.56854248 +78 arena2.map 281 209 40 106 261 196 313.56854248 +79 arena2.map 281 209 245 187 25 104 317.91168823 +79 arena2.map 281 209 279 58 0 99 316.02438660 +79 arena2.map 281 209 97 4 279 206 319.60512237 +79 arena2.map 281 209 59 104 280 206 316.78174591 +79 arena2.map 281 209 276 203 57 88 317.68124084 +79 arena2.map 281 209 27 107 248 185 316.74011536 +79 arena2.map 281 209 8 108 229 146 316.49747467 +79 arena2.map 281 209 256 152 22 97 317.32590179 +79 arena2.map 281 209 33 98 265 181 318.25483398 +79 arena2.map 281 209 32 107 235 186 319.39696960 +80 arena2.map 281 209 21 105 226 180 323.19595947 +80 arena2.map 281 209 46 101 280 183 320.66904755 +80 arena2.map 281 209 4 104 258 40 320.08326111 +80 arena2.map 281 209 41 101 252 206 320.08326111 +80 arena2.map 281 209 19 104 251 182 321.15432892 +80 arena2.map 281 209 7 102 236 168 320.63961029 +80 arena2.map 281 209 245 205 36 111 320.35533905 +80 arena2.map 281 209 233 187 28 106 321.88225098 +80 arena2.map 281 209 219 147 11 109 323.49747467 +80 arena2.map 281 209 92 5 278 201 321.11984100 +81 arena2.map 281 209 255 195 36 106 326.56854248 +81 arena2.map 281 209 11 108 238 176 325.46803741 +81 arena2.map 281 209 272 39 5 109 327.95331879 +81 arena2.map 281 209 26 107 220 184 326.09545441 +81 arena2.map 281 209 20 97 252 42 327.42640686 +81 arena2.map 281 209 224 148 8 112 324.91168823 +81 arena2.map 281 209 275 158 15 111 325.85281372 +81 arena2.map 281 209 8 109 265 36 325.05382385 +81 arena2.map 281 209 6 97 245 173 326.15432892 +81 arena2.map 281 209 264 194 32 101 325.46803741 +82 arena2.map 281 209 37 98 271 201 328.53910522 +82 arena2.map 281 209 270 204 36 98 330.95331879 +82 arena2.map 281 209 15 105 243 186 328.15432892 +82 arena2.map 281 209 237 191 19 102 331.22539673 +82 arena2.map 281 209 14 105 256 180 330.74011536 +82 arena2.map 281 209 13 103 241 184 328.15432892 +82 arena2.map 281 209 228 186 21 106 328.78174591 +82 arena2.map 281 209 263 196 32 104 328.88225098 +82 arena2.map 281 209 218 182 20 102 329.68124084 +82 arena2.map 281 209 40 101 278 196 330.05382385 +83 arena2.map 281 209 28 102 269 189 332.22539673 +83 arena2.map 281 209 30 107 270 182 333.32590179 +83 arena2.map 281 209 275 49 23 97 332.95331879 +83 arena2.map 281 209 278 206 40 107 332.53910522 +83 arena2.map 281 209 14 104 223 182 333.02438660 +83 arena2.map 281 209 15 103 260 182 333.74011536 +83 arena2.map 281 209 240 197 20 105 335.39696960 +83 arena2.map 281 209 228 172 4 111 333.85281372 +83 arena2.map 281 209 9 108 219 169 334.19595947 +83 arena2.map 281 209 224 157 12 111 333.05382385 +84 arena2.map 281 209 1 101 220 162 337.05382385 +84 arena2.map 281 209 8 105 249 188 337.15432892 +84 arena2.map 281 209 16 106 222 187 337.26702728 +84 arena2.map 281 209 15 98 232 173 336.71067810 +84 arena2.map 281 209 8 106 256 186 339.63961029 +84 arena2.map 281 209 220 156 13 112 337.22539673 +84 arena2.map 281 209 14 111 238 177 339.29646454 +84 arena2.map 281 209 234 171 18 97 337.29646454 +84 arena2.map 281 209 31 104 264 206 339.71067810 +84 arena2.map 281 209 237 199 20 106 339.05382385 +85 arena2.map 281 209 247 184 15 99 342.74011536 +85 arena2.map 281 209 220 167 0 111 342.61017303 +85 arena2.map 281 209 17 102 268 187 342.22539673 +85 arena2.map 281 209 23 106 272 191 342.71067810 +85 arena2.map 281 209 18 103 254 200 340.39696960 +85 arena2.map 281 209 22 105 271 193 343.12489166 +85 arena2.map 281 209 241 198 16 106 340.39696960 +85 arena2.map 281 209 13 105 229 191 340.95331879 +85 arena2.map 281 209 21 104 235 202 341.05382385 +85 arena2.map 281 209 16 96 225 171 340.61017303 +86 arena2.map 281 209 11 105 266 186 346.22539673 +86 arena2.map 281 209 227 189 10 109 344.43860016 +86 arena2.map 281 209 18 107 270 189 345.29646454 +86 arena2.map 281 209 12 104 264 194 346.12489166 +86 arena2.map 281 209 243 203 13 107 347.98275604 +86 arena2.map 281 209 274 190 22 106 345.29646454 +86 arena2.map 281 209 274 203 30 105 344.02438660 +86 arena2.map 281 209 8 111 260 180 344.39696960 +86 arena2.map 281 209 272 182 17 107 345.39696960 +86 arena2.map 281 209 18 102 263 201 345.53910522 +87 arena2.map 281 209 33 105 280 204 349.19595947 +87 arena2.map 281 209 6 107 220 188 349.50966797 +87 arena2.map 281 209 220 184 0 103 349.85281372 +87 arena2.map 281 209 20 107 261 205 348.53910522 +87 arena2.map 281 209 264 196 10 107 350.19595947 +87 arena2.map 281 209 9 103 267 190 350.05382385 +87 arena2.map 281 209 3 101 248 198 351.74011536 +87 arena2.map 281 209 18 106 274 188 348.46803741 +87 arena2.map 281 209 12 109 270 188 351.71067810 +87 arena2.map 281 209 260 188 6 112 349.53910522 +88 arena2.map 281 209 0 111 224 186 353.50966797 +88 arena2.map 281 209 16 107 272 197 352.61017303 +88 arena2.map 281 209 247 204 5 101 355.32590179 +88 arena2.map 281 209 265 200 8 103 354.95331879 +88 arena2.map 281 209 268 191 8 110 355.95331879 +88 arena2.map 281 209 13 107 275 183 352.81118317 +88 arena2.map 281 209 1 100 245 198 353.74011536 +88 arena2.map 281 209 272 185 9 102 353.39696960 +88 arena2.map 281 209 241 197 7 112 352.05382385 +88 arena2.map 281 209 12 103 259 206 354.46803741 +89 arena2.map 281 209 222 189 14 96 356.68124084 +89 arena2.map 281 209 10 109 276 184 358.05382385 +89 arena2.map 281 209 19 97 240 192 356.81118317 +89 arena2.map 281 209 256 190 19 97 356.46803741 +89 arena2.map 281 209 7 105 260 202 356.71067810 +89 arena2.map 281 209 21 97 224 183 356.43860016 +89 arena2.map 281 209 242 206 8 109 357.22539673 +89 arena2.map 281 209 251 207 8 99 358.98275604 +89 arena2.map 281 209 5 99 267 191 356.12489166 +89 arena2.map 281 209 273 197 10 104 358.36753235 +90 arena2.map 281 209 258 203 6 97 360.71067810 +90 arena2.map 281 209 268 190 1 112 362.78174591 +90 arena2.map 281 209 10 96 272 184 360.22539673 +90 arena2.map 281 209 279 196 13 106 361.78174591 +90 arena2.map 281 209 10 108 280 187 362.88225098 +90 arena2.map 281 209 270 197 6 111 362.26702728 +90 arena2.map 281 209 267 178 16 112 360.98275604 +90 arena2.map 281 209 11 107 273 207 363.92388153 +90 arena2.map 281 209 268 198 4 101 360.19595947 +90 arena2.map 281 209 5 112 275 181 362.05382385 diff --git a/example/build.sh b/example/build.sh new file mode 100755 index 0000000..5761b12 --- /dev/null +++ b/example/build.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +SRC_DIR=$(dirname -- "${BASH_SOURCE[0]}") +cmake -B "$SRC_DIR/build" -S "$SRC_DIR/.." +cmake --build "$SRC_DIR/build" diff --git a/example/run.sh b/example/run.sh new file mode 100755 index 0000000..881f4d8 --- /dev/null +++ b/example/run.sh @@ -0,0 +1,19 @@ +#!/bin/bash + +SRC_DIR=$(dirname -- "${BASH_SOURCE[0]}") + +if [ $# -gt 0 ]; then + ALG="$1" +else + ALG="jpsP" +fi + +if [ $# -gt 1 ]; then + SCEN="$2" +else + SCEN="$SRC_DIR/arena2.map.scen" +fi + +# command + +"$SRC_DIR/build/warthog-jps" --alg "$ALG" --scen "$SCEN" --checkopt diff --git a/extern/warthog-core b/extern/warthog-core index 0f31e1a..f379ef2 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 0f31e1a30148744f43adcc8914794e70f6a42674 +Subproject commit f379ef257335bb8bb6645c6efaeef031763273ea diff --git a/include/jps/domain/rotate_gridmap.h b/include/jps/domain/rotate_gridmap.h new file mode 100644 index 0000000..ae4688b --- /dev/null +++ b/include/jps/domain/rotate_gridmap.h @@ -0,0 +1,631 @@ +#ifndef WARTHOG_DOMAIN_ROTATE_GRIDMAP_H +#define WARTHOG_DOMAIN_ROTATE_GRIDMAP_H + +// +// jps/domain/rotate_gridmap.h +// +// An extended domain of gridmap that uses 2 gridmaps, one rotated +// for use in JPS block-based jump-point location. +// +// Class rotate_gridmap is the main implementation. +// It supports pointing to a user-supplied gridmap and rotated gridmap, +// or just a user provided gridmap, which it would generate and own the +// rotated gridmap. +// +// Small copyable storage of these maps are provided in utility structs: +// gridmap_rotate_ptr, gridmap_rotate_ptr_convs: 2 pointers to gridmap, +// accessing the grid would require 2-level of indirects (gridmap->data[i]) +// gridmap_rotate_table, gridmap_rotate_table_convs: 2 gridmap::bitarray, +// accessing the grid is only a single redirect +// Use of these datatypes do not copy the tables themselves, and are quite +// compact. The non _convs variants should be 2-pointer sized (16-bytes). The +// _convs variants are supplied with width and height, allowing for x/y and +// grid_id and rgrid_id conversions between the two grid (24-bytes). +// +// @author Ryan Hechenberger +// @created 2025-11-20 +// + +#include +#include +#include +#include +#include +#include +#include + +namespace jps::domain +{ + +namespace details +{ + +template +struct direction_grid_id; +template<> +struct direction_grid_id +{ + using type = rgrid_id; + static constexpr size_t map_id = 1; + static constexpr bool east = true; +}; +template<> +struct direction_grid_id +{ + using type = grid_id; + static constexpr size_t map_id = 0; + static constexpr bool east = true; +}; +template<> +struct direction_grid_id +{ + using type = rgrid_id; + static constexpr size_t map_id = 1; + static constexpr bool east = false; +}; +template<> +struct direction_grid_id +{ + using type = grid_id; + static constexpr size_t map_id = 0; + static constexpr bool east = false; +}; +template<> +struct direction_grid_id +{ + using type = rgrid_id; + static constexpr size_t map_id = 1; + static constexpr bool east = true; +}; +template<> +struct direction_grid_id +{ + using type = grid_id; + static constexpr size_t map_id = 0; + static constexpr bool east = true; +}; +template<> +struct direction_grid_id +{ + using type = rgrid_id; + static constexpr size_t map_id = 1; + static constexpr bool east = false; +}; +template<> +struct direction_grid_id +{ + using type = grid_id; + static constexpr size_t map_id = 0; + static constexpr bool east = false; +}; + +template +struct index_grid_id; +template<> +struct index_grid_id<0> +{ + using type = grid_id; +}; +template<> +struct index_grid_id<1> +{ + using type = rgrid_id; +}; + +template +struct grid_identity; +template<> +struct grid_identity +{ + using type = grid_id; + static constexpr size_t index = 0; +}; +template<> +struct grid_identity +{ + using type = rgrid_id; + static constexpr size_t index = 1; +}; + +} // namespace details + +/// @brief returns id type for cardinal direction, {EAST,EAST_ID,WEST,WEST_ID} +/// = grid_id; {NORTH,NORTH_ID,SOUTH,SOUTH_ID} = rgrid_id; +/// @tparam D value in direction or direction_id +template +using grid_id_dir_t = typename details::direction_grid_id::type; +/// @brief returns id type of map index I +template +using grid_id_index_t = typename details::index_grid_id::type; +/// @brief returns id type for cardinal direction, {EAST,EAST_ID,WEST,WEST_ID} +/// = grid_id; {NORTH,NORTH_ID,SOUTH,SOUTH_ID} = rgrid_id; +/// @tparam D value in direction or direction_id +template +using grid_identity = details::grid_identity>; + +template +constexpr inline size_t rgrid_index = details::direction_grid_id::map_id; +template +constexpr inline bool rgrid_east = details::direction_grid_id::east; +template +constexpr inline bool rgrid_hori = rgrid_index == 0; + +using ::warthog::domain::gridmap; + +struct grid_pair_id +{ + grid_id g; ///< grid_id for horizontal id + rgrid_id r; ///< grid_id for vertical id +}; +/// @return id.g for I==0, id.r for I==1 +template +constexpr auto +get(const grid_pair_id& id) noexcept +{ + static_assert(I < 2, "0 = grid_id, 1 = rgird_id"); + if constexpr(I == 0) { return id.g; } + else { return id.r; } +} +/// @return id.g for I==0, id.r for I==1 +template +constexpr auto& +get(grid_pair_id& id) noexcept +{ + static_assert(I < 2, "0 = grid_id, 1 = rgird_id"); + if constexpr(I == 0) { return id.g; } + else { return id.r; } +} +/// @return id.g for grid_id, id.r for rgrid_id +template +constexpr T +get(const grid_pair_id& id) noexcept +{ + return get::index>(id); +} +/// @return id.g for grid_id, id.r for rgrid_id +template +constexpr T& +get(grid_pair_id& id) noexcept +{ + return get::index>(id); +} +/// @return get(id) with associated horizontal/vertical of D +template +constexpr grid_id_dir_t +get_d(const grid_pair_id& id) noexcept +{ + return get>(id); +} +/// @return get(id) with associated horizontal/vertical of D +template +constexpr grid_id_dir_t& +get_d(grid_pair_id& id) noexcept +{ + return get>(id); +} +/// @return get(id) with associated horizontal/vertical of D +template +constexpr grid_id_dir_t +get_d(const grid_pair_id& id) noexcept +{ + return get>(id); +} +/// @return get(id) with associated horizontal/vertical of D +template +constexpr grid_id_dir_t& +get_d(grid_pair_id& id) noexcept +{ + return get>(id); +} + +/// @brief class with functions to handle conversions from/to grid and rgrid, +/// designed to be inherited by other classes for functionality. +struct rgridmap_point_conversions +{ + static constexpr uint16_t XADJ + = uint16_t(-1u); // width adjustment to rotate for height + static constexpr uint16_t YADJ + = gridmap::PADDED_ROWS; // height adjustment to rotate + std::array map_height_ = {}; // height + XADJ + std::array map_width_ = {}; + // uint16_t map_width_ = 0; + // uint16_t map_height_m1p_ = 0; + // uint16_t rmap_width_ = 0; + // uint16_t rmap_height_ = 0; + + void + conv_assign(gridmap::bittable map, gridmap::bittable rmap) noexcept + { + map_height_ + = {static_cast(map.height() + XADJ), + static_cast(rmap.height() + XADJ)}; + map_width_ + = {static_cast(map.width()), + static_cast(rmap.width())}; + } + + /// @return returns the padded width of map + uint16_t + width() const noexcept + { + return map_width_[0]; + } + /// @return returns the padded height of map + uint16_t + height() const noexcept + { + return uint16_t(map_height_[0] - XADJ); + } + + /// @return returns the padded width of map + uint16_t + rwidth() const noexcept + { + return map_width_[1]; + } + /// @return returns the padded height of map + uint16_t + rheight() const noexcept + { + return uint16_t(map_height_[1] - XADJ); + } + + point + point_to_rpoint(point p) const noexcept + { + return { + static_cast(map_height_[0] - p.y), + static_cast(p.x + YADJ)}; + } + point + rpoint_to_point(point p) const noexcept + { + return { + static_cast(p.y - YADJ), + static_cast(map_height_[0] - p.x)}; + } + grid_id + point_to_id(point p) const noexcept + { + return grid_id{ + static_cast(p.y) * map_width_[0] + + static_cast(p.x)}; + } + rgrid_id + rpoint_to_rid(point p) const noexcept + { + return rgrid_id{ + static_cast(p.y) * map_width_[1] + + static_cast(p.x)}; + } + point + id_to_point(grid_id p) const noexcept + { + return { + static_cast(p.id % map_width_[0]), + static_cast(p.id / map_width_[0])}; + } + point + rid_to_rpoint(rgrid_id p) const noexcept + { + return { + static_cast(p.id % map_width_[1]), + static_cast(p.id / map_width_[1])}; + } + rgrid_id + id_to_rid(grid_id mapid) const noexcept + { + assert(!mapid.is_none()); + return rpoint_to_rid(point_to_rpoint(id_to_point(mapid))); + } + grid_id + rid_to_id(rgrid_id mapid) const noexcept + { + assert(!mapid.is_none()); + return point_to_id(rpoint_to_point(rid_to_rpoint(mapid))); + } + grid_pair_id + point_to_pair_id(point loc) const noexcept + { + return grid_pair_id{ + point_to_id(loc), rpoint_to_rid(point_to_rpoint(loc))}; + } + + template + requires std::same_as + || std::same_as + grid_id_dir_t + to_id_d(GridId id) const noexcept + { + using res_type = grid_id_dir_t; + if constexpr(std::same_as) + { + return id; // is same as output, do nothing + } + else if constexpr(std::same_as) + { + return rid_to_id(id); + } + else { return id_to_rid(id); } + } + + template + grid_id_dir_t + point_to_id_d(point loc) const noexcept + { + using res_type = grid_id_dir_t; + if constexpr(std::same_as) + { + return point_to_id(loc); + } + else { return rpoint_to_rid(point_to_rpoint(loc)); } + } + + template + grid_id_dir_t + rpoint_to_id_d(point loc) const noexcept + { + using res_type = grid_id_dir_t; + if constexpr(std::same_as) + { + return point_to_id(rpoint_to_point(loc)); + } + else { return rpoint_to_rid(loc); } + } +}; + +/// @brief a copy-by-value class pointing to grid/rgrid +struct gridmap_rotate_ptr : std::array +{ + gridmap_rotate_ptr() : array{} { } + gridmap_rotate_ptr( + domain::gridmap& l_map, domain::gridmap& l_rmap) noexcept + : array{&l_map, &l_rmap} + { } + domain::gridmap& + map() noexcept + { + return *(*this)[0]; + } + const domain::gridmap& + map() const noexcept + { + return *(*this)[0]; + } + domain::gridmap& + rmap() noexcept + { + return *(*this)[1]; + } + const domain::gridmap& + rmap() const noexcept + { + return *(*this)[1]; + } + operator bool() const noexcept { return (*this)[0]; } +}; +/// @brief a copy-by-value class pointing to grid/rgrid with id conversions +/// functions +struct gridmap_rotate_ptr_convs : gridmap_rotate_ptr, + rgridmap_point_conversions +{ + gridmap_rotate_ptr_convs() = default; + gridmap_rotate_ptr_convs( + domain::gridmap& l_map, domain::gridmap& l_rmap) noexcept + : gridmap_rotate_ptr(l_map, l_rmap) + { + conv_assign(l_map, l_rmap); + } + gridmap_rotate_ptr_convs(gridmap_rotate_ptr maps) noexcept + : gridmap_rotate_ptr(maps) + { + if(*this) { conv_assign(map(), rmap()); } + } +}; +/// @brief a copy-by-value class for fast access to grid/rgrid +/// differs from gridmap_rotate_ptr with direct pointer to bit array, +/// instead of pointer to gridmap to bit array +struct gridmap_rotate_table : std::array +{ + gridmap_rotate_table() : array{} { } + gridmap_rotate_table( + domain::gridmap& l_map, domain::gridmap& l_rmap) noexcept + : array{l_map, l_rmap} + { } + gridmap_rotate_table( + domain::gridmap::bittable& l_map, + domain::gridmap::bittable& l_rmap) noexcept + : array{l_map, l_rmap} + { } + domain::gridmap::bitarray + map() const noexcept + { + return (*this)[0]; + } + domain::gridmap::bitarray + rmap() const noexcept + { + return (*this)[1]; + } + operator bool() const noexcept { return (*this)[0].data(); } +}; +/// @brief a copy-by-value class for fast access to grid/rgrid with id +/// conversions functions +struct gridmap_rotate_table_convs : gridmap_rotate_table, + rgridmap_point_conversions +{ + gridmap_rotate_table_convs() = default; + gridmap_rotate_table_convs( + domain::gridmap& l_map, domain::gridmap& l_rmap) noexcept + : gridmap_rotate_table(l_map, l_rmap) + { + conv_assign(l_map, l_rmap); + } + gridmap_rotate_table_convs( + gridmap_rotate_table maps, rgridmap_point_conversions conv) noexcept + : gridmap_rotate_table(maps), rgridmap_point_conversions{conv} + { } + domain::gridmap::bittable + table() noexcept + { + return domain::gridmap::bittable((*this)[0], width(), height()); + } + domain::gridmap::bittable + rtable() noexcept + { + return domain::gridmap::bittable((*this)[1], rwidth(), rheight()); + } + domain::gridmap::bittable + table(size_t i) noexcept + { + return domain::gridmap::bittable( + (*this)[i], map_width_[i], uint16_t(map_height_[i] - XADJ)); + } +}; + +/// The main rotate gridmap class. +/// Stores a pointer to a gridmap and rotated gridmap. +/// The rotated gridmap memory may be created and owned by this class, or +/// provided and controlled by the user. +/// +/// Supports static_cast operations for all gridmap_rotate_(ptr|table)(_conv)?. +/// They will act as valid small pointer classes (16-24 bytes) with various +/// levels if indirection to the grid data. +class rotate_gridmap : public rgridmap_point_conversions +{ +private: + std::unique_ptr rmap_obj; + gridmap_rotate_ptr maps = {}; + +public: + rotate_gridmap() = default; + rotate_gridmap(domain::gridmap& map, domain::gridmap* rmap = nullptr) + { + if(rmap != nullptr) + { + maps[0] = ↦ + maps[1] = rmap; + conv_assign(map, *rmap); + } + else { create_rmap(map); } + } + + void + link(gridmap_rotate_ptr rmap) + { + rmap_obj = nullptr; + if(rmap) + { + maps = rmap; + conv_assign(*maps[0], *maps[1]); + } + else { clear(); } + } + void + clear() + { + rmap_obj = nullptr; + maps = {}; + static_cast(*this) = {}; + } + void + create_rmap(domain::gridmap& map) + { + maps[0] = ↦ + + const uint32_t maph = map.height(); + const uint32_t maphmp1 = static_cast( + maph + XADJ); // cast required to handle signed value in uint16_t + const uint32_t mapw = map.width(); + auto tmap = std::make_unique(mapw, maph); + + for(uint32_t y = 0; y < maph; y++) + { + for(uint32_t x = 0; x < mapw; x++) + { + bool label = map.get_label(map.to_padded_id_from_padded(x, y)); + uint32_t rx = maphmp1 - y; + uint32_t ry = static_cast(x + YADJ); + tmap->set_label(tmap->to_padded_id_from_padded(rx, ry), label); + } + } + + // set values + rmap_obj = std::move(tmap); + maps[1] = rmap_obj.get(); + conv_assign(*maps[0], *maps[1]); + +#ifndef NDEBUG + for(uint32_t y = 0; y < maph; y++) + { + for(uint32_t x = 0; x < mapw; x++) + { + auto p = this->point_to_rpoint(point(x, y)); + assert( + maps[0]->get_label(this->point_to_id(point(x, y))) + == maps[1]->get_label( + static_cast(this->rpoint_to_rid( + this->point_to_rpoint(point(x, y)))))); + } + } +#endif // NDEBUG + } + + domain::gridmap& + map() noexcept + { + assert(maps[0] != nullptr); + return *maps[0]; + } + const domain::gridmap& + map() const noexcept + { + assert(maps[0] != nullptr); + return *maps[0]; + } + domain::gridmap& + rmap() noexcept + { + assert(maps[1] != nullptr); + return *maps[1]; + } + const domain::gridmap& + rmap() const noexcept + { + assert(maps[1] != nullptr); + return *maps[1]; + } + + operator bool() const noexcept { return maps[0] != nullptr; } + operator gridmap_rotate_ptr() const noexcept + { + assert(maps[0] != nullptr && maps[1] != nullptr); + return maps; + } + operator gridmap_rotate_ptr_convs() const noexcept + { + assert(maps[0] != nullptr && maps[1] != nullptr); + return gridmap_rotate_ptr_convs(maps); + } + operator gridmap_rotate_table() const noexcept + { + assert(maps[0] != nullptr && maps[1] != nullptr); + return gridmap_rotate_table(*maps[0], *maps[1]); + } + operator gridmap_rotate_table_convs() const noexcept + { + assert(maps[0] != nullptr && maps[1] != nullptr); + return gridmap_rotate_table_convs( + *this, static_cast(*this)); + } + + size_t + mem() const noexcept + { + return rmap_obj != nullptr ? rmap_obj->mem() : 0; + } +}; + +} // namespace warthog::grid + +#endif // WARTHOG_DOMAIN_GRIDMAP_H diff --git a/include/jps/forward.h b/include/jps/forward.h index 9107288..c4ea966 100644 --- a/include/jps/forward.h +++ b/include/jps/forward.h @@ -1,7 +1,15 @@ #ifndef JPS_FORWARD_H #define JPS_FORWARD_H -#include +// +// jps/forward.h +// +// Global namespace types include. +// +// @author Ryan Hechenberger +// @created 2025-11-20 +// + #include #include #include @@ -10,39 +18,17 @@ namespace jps { -using warthog::pack_id; -using warthog::pad_id; -using jps_id = warthog::pad32_id; +using namespace ::warthog::grid; +using ::warthog::pad_id; +// using jps_id = grid_id; struct rmap_id_tag { }; -using jps_rid = warthog::identity_base; +using rgrid_id = warthog::identity_base; using warthog::cost_t; -using namespace warthog::grid; - -using vec_jps_id = std::vector; -using vec_jps_cost = std::vector; - -struct alignas(uint32_t) point -{ - uint16_t x; - uint16_t y; -}; - -enum class JpsFeature : uint8_t -{ - DEFAULT = 0, // uses block-based jumping - PRUNE_INTERCARDINAL = 1 << 0, - STORE_CARDINAL_JUMP = 1 << 1, // if not PRUNE_INTERCARDINAL, then store - // cardinal results in intercandial jump -}; -inline JpsFeature -operator|(JpsFeature a, JpsFeature b) noexcept -{ - return static_cast( - static_cast(a) | static_cast(b)); -} - } // namespace jps +#include +#include + #endif // JPS_SEARCH_FORWARD_H diff --git a/include/jps/jump/block_online.h b/include/jps/jump/block_online.h new file mode 100644 index 0000000..685b688 --- /dev/null +++ b/include/jps/jump/block_online.h @@ -0,0 +1,677 @@ +#ifndef JPS_JUMP_BLOCK_ONLINE_H +#define JPS_JUMP_BLOCK_ONLINE_H + +// +// jps/jump/block_online.h +// +// Block-based jump-point location fuctions, used in jump_point_online. +// Handles low-level block jumps, scanning up to 56 cells (bits) at a time. +// +// The jump_point_online_hori function locates jump-points accross a maps hori; +// providing the rotated map gives a vert version. +// +// IntercardinalWalker is an intercardinal (NE/NW/SE/SW) jump-point locator +// utility structure. Gives low-level operations so users easily create their +// own fast intercardinal expanders. +// The BasicIntercardinalWalker is a non-templated version of +// IntercardinalWalker. +// +// @author dharabor & Ryan Hechenberger +// @created 2025-11-20 +// + +#include "jump.h" +#include +#include +#include +#include +#include + +namespace jps::jump +{ + +/// @brief find first jump-point horizontal east (if East) or west on map. +/// Will land of target if Target is true. +/// @tparam East Jump east (true) or west (false) +/// @tparam Target if true, consider target as a jump-point as well. +/// @param map a map of the grid, only the width parameter is required +/// @param node the starting location +/// @param target the target location, only used if Target == true +/// @return positive distance to jump-point or target, otherwise negated +/// distance to wall blocker +template +jump_distance +jump_point_online_hori( + ::warthog::domain::gridmap::bittable map, uint32_t node, + uint32_t target [[maybe_unused]] = std::numeric_limits::max()) +{ + assert(map.data() != nullptr); + assert(map.size() == 0 || node < map.size()); + assert(map.get(grid_id(node))); + assert(Target != (target == std::numeric_limits::max())); + // read tiles from the grid: + // - along the row of node_id + // - from the row above node_id + // - from the row below node_id + // NB: the jump direction corresponds to moving from the + // low bit of the tileset and towards the high bit (EAST) + // or high bit to low bit (WEST) + auto nei_slider + = ::warthog::domain::gridmap_slider::from_bittable(map, pad_id{node}); + if constexpr(!East) + { + nei_slider.adj_bytes(-7); // west is opposite side + nei_slider.width8_bits = 7u - nei_slider.width8_bits; + } + assert(nei_slider.width8_bits < 8); + // width8_bits is how many bits in on word current node is at, from lsb + // EAST and from msb WEST + // 7 - width8_bits == 63 - (width8_bits + 7 * 8) + + // order going east is stored as least significant bit to most significant + // bit + + // first loop gets neis as 8 <= width8_bits < 16 + // other loops will have width8_bits = 7 + std::array neis = nei_slider.get_neighbours_64bit_le(); + // the rest will only jump 7 + // shift above and below 2 points east + // mask out to trav(1) before node location + assert(nei_slider.width8_bits < 8); + uint64_t tmp = East ? ~(~0ull << nei_slider.width8_bits) + : ~(~0ull >> nei_slider.width8_bits); + neis[0] |= tmp; + neis[1] |= tmp; + neis[2] |= tmp; + + jump_distance jump_count + = 7u - static_cast(nei_slider.width8_bits); + + while(true) + { + // find first jump point, is +1 location past blocker above or below + if constexpr(East) + { + tmp = ((~neis[1] << 1) + & neis[1]) // above row: block(zero) trailing trav(one) + | ((~neis[2] << 1) + & neis[2]); // below row: block(zero) trailing trav(one) + } + else + { + tmp = ((~neis[1] >> 1) + & neis[1]) // above row: block(zero) trailing trav(one) + | ((~neis[2] >> 1) + & neis[2]); // below row: block(zero) trailing trav(one) + } + // append for dead-end check + tmp = tmp | ~neis[0]; + if(tmp) + { + int stop_pos = East + ? std::countr_zero(tmp) + : std::countl_zero(tmp); // the location to stop at + // v is blocker location, prune unless target is present + // 10111111 + // dead end takes president as jump point can't pass a blocker + // first jump may not skip 7, this is adjusted for on init + jump_count += static_cast(stop_pos) - 7; + uint32_t target_jump [[maybe_unused]] + = Target ? (East ? target - node : node - target) : 0; + // if blocked: pos + jump_count = first block + // otherwise: jump_count = trav cell after turn (jump point + // location) + // check for target with target_jump (dist) <= jump_count, as if < + // then target is reachable, if equal then trav pos is the target + // if greater, then target is further or another row or behind (as + // unsigned) + assert(jump_count >= 0); + // must be checked as unsigned for: + // 1. target.is_none(): will not fit int32_t + // 2. underflow means target is in opposite direction, desirable + if(Target && (target_jump <= static_cast(jump_count))) + { + // target reached + jump_count = static_cast(target_jump); + } + else if( + East ? !(neis[0] & (static_cast(1) << stop_pos)) + : !(neis[0] + & (static_cast( + std::numeric_limits::min()) + >> stop_pos))) // deadend + { + // deadend, return negative jump + assert(jump_count > 0); + jump_count = -(jump_count - 1); + } + return jump_count; + } + + // failed, goto next 56 bits + jump_count += static_cast(63 - 7); + // nei_slider.width8_bits = 7; + nei_slider.adj_bytes(East ? 7 : -7); + + // get next neis at end of loop + neis = nei_slider.get_neighbours_64bit_le(); + // mask out to trav(1) is not nessesary on loop, as we know it is clear + // constexpr uint64_t neis_mask = East ? ~(~0ull << 7) : ~(~0ull >> 7); + // neis[0] |= neis_mask; + // neis[1] |= neis_mask; + // neis[2] |= neis_mask; + } +} + +/// @brief find first jump-point horizontal east (if East) or west on map. +/// Will land of target if Target is true. +/// @tparam East Jump east (true) or west (false) +/// @tparam Target if true, consider target as a jump-point as well. +/// @param map a map of the grid, only the width parameter is required +/// @param node the starting location +/// @param target the target location, only used if Target == true +/// @return positive distance to jump-point or target, otherwise negated +/// distance to wall blocker +template +jump_distance +jump_point_online_hori_target( + ::warthog::domain::gridmap::bittable map, uint32_t node, uint32_t target) +{ + assert(map.data() != nullptr); + // read tiles from the grid: + // - along the row of node_id + // - from the row above node_id + // - from the row below node_id + // NB: the jump direction corresponds to moving from the + // low bit of the tileset and towards the high bit (EAST) + // or high bit to low bit (WEST) + auto nei_slider + = ::warthog::domain::gridmap_slider::from_bittable(map, pad_id{node}); + if constexpr(!East) + { // adjust to last byte for west + nei_slider.adj_bytes(-7); + nei_slider.width8_bits = 7u - nei_slider.width8_bits; + } + + // order going east is stored as least significant bit to most significant + // bit + + const uint32_t target_jump = East ? target - node : node - target; + assert(nei_slider.width8_bits < 8); + jump_distance jump_count + = -static_cast(nei_slider.width8_bits); + // setup jump block, negate so trav is 0, mask out points before start + // just mask + uint64_t jump_block = East ? (~0ull << nei_slider.width8_bits) + : (~0ull >> nei_slider.width8_bits); + // negate and mask + jump_block = ~nei_slider.get_block_64bit_le() & jump_block; + + while(true) + { + if(jump_block) + { + int stop_pos = East + ? std::countr_zero(jump_block) + : std::countl_zero(jump_block); // the location to stop at + // v is blocker location, prune unless target is present + // 10111111 + // dead end takes president as jump point can't pass a blocker + jump_count += static_cast(stop_pos); + // if blocked: pos + jump_count = first block + // otherwise: jump_count = trav cell after turn (jump point + // location) + // check for target with target_jump (dist) <= jump_count, as if < + // then target is reachable, if equal then trav pos is the target + // if greater, then target is further or another row or behind (as + // unsigned) + assert(jump_count >= 0); + if(static_cast(jump_count) >= target_jump) + return target_jump; // found target + else + return static_cast( + -(jump_count - 1)); // no target + } + + // no blockers, check for target + jump_count += static_cast(64); + if(static_cast(jump_count) >= target_jump) + return target_jump; // found target + nei_slider.adj_bytes(East ? 8 : -8); + jump_block = ~nei_slider.get_block_64bit_le(); + } +} + +struct BasicIntercardinalWalker +{ + using map_type = ::warthog::domain::gridmap::bitarray; + /// @brief map and rmap (as bit array for small memory size) + map_type map; + /// @brief location of current node on map and rmap + uint32_t node_at; + /// @brief map and rmap value to adjust node_at for each row + uint32_t adj_width; + /// @brief row scan + union + { + uint8_t row_[2]; ///< stores 3 bits at node_at[0]+-1, 0=prev, + ///< 1=current; high order bits 3..7 are not zero'd + uint16_t row_i_; + }; + uint16_t row_mask_; + + template + requires InterCardinalId + void + set_map(map_type map, uint32_t width) noexcept + { + this->map = map; + this->adj_width = dir_id_adj(D, width); + if constexpr(D == NORTHEAST_ID || D == SOUTHEAST_ID) + { + row_mask_ = std::endian::native == std::endian::little + ? 0b0000'0011'0000'0110 + : 0b0000'0110'0000'0011; + } + else + { // NORTHWEST_ID and SOUTHWEST_ID + row_mask_ = std::endian::native == std::endian::little + ? 0b0000'0110'0000'0011 + : 0b0000'0011'0000'0110; + } + } + + void + set_map(direction_id d, map_type map, uint32_t width) noexcept + { + assert(is_intercardinal_id(d)); + warthog::util::choose_integer_sequence>(d, [&, map, width](auto dv) { + set_map(map, width); + }); + } + + void + next_index() noexcept + { + node_at += adj_width; + } + /// @brief call for first row, then call next_row + void + first_row() noexcept + { + row_[1] = get_row(); + } + /// @brief update index to next row and update + void + next_row() noexcept + { + next_index(); + row_[0] = row_[1]; + row_[1] = get_row(); + } + + /// @brief return get node_at-1..node_at+1 bits. CAUTION return + /// bits 3..7 may not all be 0. + uint8_t + get_row() const noexcept + { + return static_cast(map.get_span<3>(pad_id{node_at - 1})); + } + /// @brief get node id for location node + dist * adj_width + grid_id + adj_id(uint32_t node, int32_t dist) const noexcept + { + return grid_id{static_cast( + node + static_cast(dist) * adj_width)}; + } + + /// @brief the current locations row is a valid intercardinal move (i.e. + /// 2x2 is free) + bool + valid_row() const noexcept + { + return (row_i_ & row_mask_) == row_mask_; + } + template + requires InterCardinalId + bool + valid_row() const noexcept + { + // east | west differernce + // north/south does not make a difference + if constexpr(D == NORTHEAST_ID || D == SOUTHEAST_ID) + { + // we want from grid + // .xx == row[0] = 0bxx. + // xx. == row[1] = 0b.xx + // all[x] = 1 + constexpr uint16_t mask + = std::endian::native == std::endian::little + ? 0b0000'0011'0000'0110 + : 0b0000'0110'0000'0011; + return (row_i_ & mask) == mask; + } + else + { + // we want from grid + // xx. == row[0] = 0b.xx + // .xx == row[1] = 0bxx. + // all[x] = 1 + constexpr uint16_t mask + = std::endian::native == std::endian::little + ? 0b0000'0110'0000'0011 + : 0b0000'0011'0000'0110; + return (row_i_ & mask) == mask; + } + } +}; + +template + requires InterCardinalId +struct IntercardinalWalker +{ + static_assert( + D == NORTHEAST_ID || D == NORTHWEST_ID || D == SOUTHEAST_ID + || D == SOUTHWEST_ID, + "Must be intercardinal direction"); + union LongJumpRes + { + jump_distance dist[2]; /// distance hori/vert of D a jump is valid to + + operator bool() const noexcept { return dist[0] > 0 || dist[1] > 0; } + }; + using map_type = ::warthog::domain::gridmap::bitarray; + /// @brief map and rmap (as bit array for small memory size) + std::array map; + /// @brief location of current node on map and rmap + std::array node_at; + /// @brief map and rmap value to adjust node_at for each row + std::array adj_width; + // /// @brief map and rmap target locations + // uint32_t target[2]; + /// @brief row scan + union + { + uint8_t row_[2]; ///< stores 3 bits at node_at[0]+-1, 0=prev, + ///< 1=current; high order bits 3..7 are not zero'd + uint16_t row_i_; + }; + + /// @brief convert map width to a map adj_width variable suited to + /// intercardinal D2 + static constexpr uint32_t + to_map_adj_width(uint32_t width) noexcept + { + return dir_id_adj(D, width); + } + /// @brief convert rmap width to a rmap adj_width variable suited to + /// intercardinal D2 + static constexpr uint32_t + to_rmap_adj_width(uint32_t width) noexcept + { + return dir_id_adj(dir_id_cw90(D), width); + } + + /// @brief convert map adj_width to map width, reciprocal to + /// to_map_adj_width + static constexpr uint32_t + from_map_adj_width(uint32_t adj_width) noexcept + { + return dir_id_adj_inv_intercardinal(D, adj_width); + } + /// @brief convert rmap adj_width to rmap width, reciprocal to + /// to_rmap_adj_width + static constexpr uint32_t + from_rmap_adj_width(uint32_t adj_width) noexcept + { + return dir_id_adj_inv_intercardinal(dir_id_cw90(D), adj_width); + } + + /// @brief set map width + void + map_width(uint32_t width) noexcept + { + adj_width[0] = to_map_adj_width(width); + } + /// @brief get map width + uint32_t + map_width() const noexcept + { + return from_map_adj_width(adj_width[0]); + } + /// @brief set rmap width + void + rmap_width(uint32_t width) noexcept + { + adj_width[1] = to_rmap_adj_width(width); + } + /// @brief get rmap width + uint32_t + rmap_width() const noexcept + { + return from_rmap_adj_width(adj_width[1]); + } + + static jump_distance + jump_east(map_type map, uint32_t width, uint32_t node) + { + jump_distance d = jump_point_online_hori( + ::warthog::domain::gridmap::bittable(map, width, 0), node); + // for LongJumpRes, must return 0 for deadend + return d; + } + static jump_distance + jump_east(map_type map, uint32_t width, uint32_t node, uint32_t target) + { + jump_distance d = jump_point_online_hori( + ::warthog::domain::gridmap::bittable(map, width, 0), node, target); + // for LongJumpRes, must return 0 for deadend + return d; + } + static jump_distance + jump_west(map_type map, uint32_t width, uint32_t node) + { + jump_distance d = jump_point_online_hori( + ::warthog::domain::gridmap::bittable(map, width, 0), node); + // for LongJumpRes, must return 0 for deadend + return d; + } + static jump_distance + jump_west(map_type map, uint32_t width, uint32_t node, uint32_t target) + { + jump_distance d = jump_point_online_hori( + ::warthog::domain::gridmap::bittable(map, width, 0), node, target); + // for LongJumpRes, must return 0 for deadend + return d; + } + jump_distance + jump_hori() + { + if constexpr(D == NORTHEAST_ID) + { + return jump_east(map[0], map_width(), node_at[0]); // east + } + else if constexpr(D == SOUTHEAST_ID) + { + return jump_east(map[0], map_width(), node_at[0]); // east + } + else if constexpr(D == SOUTHWEST_ID) + { + return jump_west(map[0], map_width(), node_at[0]); // west + } + else if constexpr(D == NORTHWEST_ID) + { + return jump_west(map[0], map_width(), node_at[0]); // west + } + } + jump_distance + jump_hori(grid_id target) + { + if constexpr(D == NORTHEAST_ID) + { + return jump_east( + map[0], map_width(), node_at[0], + static_cast(target)); // east + } + else if constexpr(D == SOUTHEAST_ID) + { + return jump_east( + map[0], map_width(), node_at[0], + static_cast(target)); // east + } + else if constexpr(D == SOUTHWEST_ID) + { + return jump_west( + map[0], map_width(), node_at[0], + static_cast(target)); // west + } + else if constexpr(D == NORTHWEST_ID) + { + return jump_west( + map[0], map_width(), node_at[0], + static_cast(target)); // west + } + } + jump_distance + jump_vert() + { + if constexpr(D == NORTHEAST_ID) + { + return jump_east(map[1], rmap_width(), node_at[1]); // north + } + else if constexpr(D == SOUTHEAST_ID) + { + return jump_west(map[1], rmap_width(), node_at[1]); // south + } + else if constexpr(D == SOUTHWEST_ID) + { + return jump_west(map[1], rmap_width(), node_at[1]); // south + } + else if constexpr(D == NORTHWEST_ID) + { + return jump_east(map[1], rmap_width(), node_at[1]); // north + } + } + jump_distance + jump_vert(rgrid_id target) + { + if constexpr(D == NORTHEAST_ID) + { + return jump_east( + map[1], rmap_width(), node_at[1], + static_cast(target)); // north + } + else if constexpr(D == SOUTHEAST_ID) + { + return jump_west( + map[1], rmap_width(), node_at[1], + static_cast(target)); // south + } + else if constexpr(D == SOUTHWEST_ID) + { + return jump_west( + map[1], rmap_width(), node_at[1], + static_cast(target)); // south + } + else if constexpr(D == NORTHWEST_ID) + { + return jump_east( + map[1], rmap_width(), node_at[1], + static_cast(target)); // north + } + } + + void + next_index() noexcept + { + node_at[0] += adj_width[0]; + node_at[1] += adj_width[1]; + } + + /// @brief return get node_at[0]-1..node_at[0]+1 bits. CAUTION return + /// bits 3..7 may not all be 0. + uint8_t + get_row() const noexcept + { + return static_cast( + map[0].get_span<3>(pad_id{node_at[0] - 1})); + } + /// @brief call for first row, then call next_row + void + first_row() noexcept + { + row_[1] = get_row(); + } + /// @brief update index to next row and update + void + next_row() noexcept + { + next_index(); + row_[0] = row_[1]; + row_[1] = get_row(); + } + /// @brief get node id for location node + dist(EAST/WEST of D) + grid_id + adj_hori(uint32_t node, uint32_t dist) const noexcept + { + if constexpr(D == NORTHEAST_ID || D == SOUTHEAST_ID) + { + return grid_id{node + dist}; + } + else { return grid_id{node - dist}; } + } + /// @brief get node id for location node + dist(NORTH/SOUTH of D) + rgrid_id + adj_vert(uint32_t node, uint32_t dist) const noexcept + { + if constexpr(D == NORTHEAST_ID || D == SOUTHEAST_ID) + { + return rgrid_id{node + (adj_width[0] - 1) * dist}; + } + else { return rgrid_id{node + (adj_width[0] + 1) * dist}; } + } + /// @brief the current locations row is a valid intercardinal move (i.e. + /// 2x2 is free) + bool + valid_row() const noexcept + { + // east | west differernce + // north/south does not make a difference + if constexpr(D == NORTHEAST_ID || D == SOUTHEAST_ID) + { + // we want from grid + // .xx == row[0] = 0bxx. + // xx. == row[1] = 0b.xx + // all[x] = 1 + constexpr uint16_t mask + = std::endian::native == std::endian::little + ? 0b0000'0011'0000'0110 + : 0b0000'0110'0000'0011; + return (row_i_ & mask) == mask; + } + else + { + // we want from grid + // xx. == row[0] = 0b.xx + // .xx == row[1] = 0bxx. + // all[x] = 1 + constexpr uint16_t mask + = std::endian::native == std::endian::little + ? 0b0000'0110'0000'0011 + : 0b0000'0011'0000'0110; + return (row_i_ & mask) == mask; + } + } + // {hori,vert} of jump hori/vert of D, from node_at. + LongJumpRes + long_jump() + { + return {jump_hori(), jump_vert()}; + } +}; + +} // namespace jps::jump + +#endif // JPS_JUMP_BLOCK_ONLINE_H diff --git a/include/jps/jump/four_connected_jps_locator.h b/include/jps/jump/four_connected_jps_locator.h deleted file mode 100644 index 504ab4f..0000000 --- a/include/jps/jump/four_connected_jps_locator.h +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef JPS_JUMP_FOUR_CONNECTED_JPS_LOCATOR_H -#define JPS_JUMP_FOUR_CONNECTED_JPS_LOCATOR_H - -// jps/four_connected_jps_locator.h -// -// Implements grid scanning operations for -// Jump Point Search in 4-connected gridmaps. -// -// NB: based on the class online_jump_point_locator -// -// @author: dharabor -// @created: 2019-11-13 -// - -#include -#include - -namespace jps::jump -{ - -class four_connected_jps_locator -{ -public: - four_connected_jps_locator(warthog::domain::gridmap* map); - ~four_connected_jps_locator(); - - void - jump( - direction d, jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - double& jumpcost); - - size_t - mem() const noexcept - { - return sizeof(this); - } - - // private: - void - jump_north( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_south( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_east( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_west( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - - // these versions can be passed a map parameter to - // use when jumping. they allow switching between - // map_ and rmap_ (a rotated counterpart). - void - jump_east_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost, - warthog::domain::gridmap* mymap); - void - jump_west_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost, - warthog::domain::gridmap* mymap); - void - jump_north_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost, - warthog::domain::gridmap* mymap); - void - jump_south_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost, - warthog::domain::gridmap* mymap); - - warthog::domain::gridmap* map_; - // uint32_t jumplimit_; -}; - -} // namespace jps::jump - -#endif // JPS_JUMP_FOUR_CONNECTED_JPS_LOCATOR_H diff --git a/include/jps/jump/jump.h b/include/jps/jump/jump.h new file mode 100644 index 0000000..ba01363 --- /dev/null +++ b/include/jps/jump/jump.h @@ -0,0 +1,93 @@ +#ifndef JPS_JUMP_JUMP_H +#define JPS_JUMP_JUMP_H + +// +// jps/jump/jump.h +// +// Global jump header. +// Include types and basic utility functions. +// +// @author Ryan Hechenberger +// @created 2025-11-20 +// + +#include + +// Common types for jump + +namespace jps::jump +{ + +using jump_distance = int16_t; + +// functions for interpreting a jump point type from distance +inline constexpr bool +is_jump_point(jump_distance d) noexcept +{ + return d > 0; +} +inline constexpr bool +is_deadend(jump_distance d) noexcept +{ + return d < 0; +} +inline constexpr bool +is_blocked(jump_distance d) noexcept +{ + return d == 0; +} + +// simple cast to unsigned jump distance, care as it is both widening and +// changing sign +inline constexpr uint32_t +to_unsigned_jump_distance(jump_distance d) noexcept +{ + return static_cast(static_cast(d)); +} + +/// get the horizontal direction of an intercardinal +inline constexpr direction_id +get_hori_from_intercardinal(direction_id d) noexcept +{ + constexpr uint32_t map = (static_cast(EAST_ID) + << 4 * static_cast(NORTHEAST_ID)) + | (static_cast(WEST_ID) + << 4 * static_cast(NORTHWEST_ID)) + | (static_cast(EAST_ID) + << 4 * static_cast(SOUTHEAST_ID)) + | (static_cast(WEST_ID) + << 4 * static_cast(SOUTHWEST_ID)); + return static_cast( + (map >> 4 * static_cast(d)) & 0b1111); +} +/// get the vertical direction of an intercardinal +inline constexpr direction_id +get_vert_from_intercardinal(direction_id d) noexcept +{ + constexpr uint32_t map = (static_cast(NORTH_ID) + << 4 * static_cast(NORTHEAST_ID)) + | (static_cast(NORTH_ID) + << 4 * static_cast(NORTHWEST_ID)) + | (static_cast(SOUTH_ID) + << 4 * static_cast(SOUTHEAST_ID)) + | (static_cast(SOUTH_ID) + << 4 * static_cast(SOUTHWEST_ID)); + return static_cast( + (map >> 4 * static_cast(d)) & 0b1111); +} + +/// @brief standard result type of an intercardianl jump. +/// Supports the inter-cardinal distance, as well as the jump-point +/// distance of the hori/vert from the inter (if exists) +struct intercardinal_jump_result +{ + jump_distance inter; ///< intercardinal distance (NE/NW/SE/SW) + jump_distance hori; ///< hori distance from inter (i.e. + ///< get_hori_from_intercardinal(D)) + jump_distance vert; ///< vert distance from inter (i.e. + ///< get_vert_from_intercardinal(D)) +}; + +} // namespace jps::jump + +#endif // JPS_JUMP_JUMP_H diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h new file mode 100644 index 0000000..c37edc3 --- /dev/null +++ b/include/jps/jump/jump_point_offline.h @@ -0,0 +1,655 @@ +#ifndef JPS_JUMP_JUMP_POINT_OFFLINE_H +#define JPS_JUMP_JUMP_POINT_OFFLINE_H + +// +// jps/jump/jump_point_offline.h +// +// Offline level jump-point locator. +// Contains two components, the jump_point_table which handles how jump-points +// are stored in all 8-directions. +// The jump_point_offline is given a templated jump_point_table and an online +// jump-point locator. +// +// The table will precompute the jump-point distances from an online locator +// and store them into a jump_point_table. +// +// @author dharabor & Ryan Hechenberger +// @created 2025-11-20 +// + +#include "jump_point_online.h" +#include +#include +#include +#include + +namespace jps::jump +{ + +/// @brief Store, set and access offline jump-point results in all +/// 8-directions. +/// @tparam ChainJump if true: store jumps as 1-byte, chaining for long jumps; +/// false: stores in 2-bytes the full jump distance +/// @tparam DeadEnd if true: track deadend as negative, false: deadend not +/// recorded +template +struct jump_point_table +{ + using direction_id = warthog::grid::direction_id; + using jump_res_width = std::conditional_t; + using jump_res = std::conditional_t< + DeadEnd, std::make_signed_t, jump_res_width>; + using length = int32_t; + struct alignas(int64_t) cell : std::array + { }; + + static consteval bool + chain_jump() noexcept + { + return ChainJump; + } + static consteval bool + dead_end() noexcept + { + return DeadEnd; + } + + std::unique_ptr db; + uint32_t width = 0; + uint32_t cells = 0; + static constexpr uint32_t + node_count(uint32_t width, uint32_t height) noexcept + { + return static_cast(width) * static_cast(height); + } + constexpr size_t + mem() noexcept + { + return cells * sizeof(jump_res); + } + + /// @return the value that indicates to chain + static consteval jump_res + chain_value() noexcept + { + return DeadEnd ? std::numeric_limits::min() + : std::numeric_limits::max(); + } + /// @return the length to jump after chain_value + static consteval length + chain_stride() noexcept + { + constexpr length acv = std::abs(static_cast(chain_value())); + return acv - 2; // direct jump max at chain_stride()+1, but chain only + // chain_stride() as we must never reach 0 + } + static_assert( + !chain_jump() + || std::abs(int32_t(std::make_signed_t(chain_value()))) + > int32_t(chain_stride()), + "absolute value of chain_value() must be greater than chain_length()"); + + /// @brief setup db, init to zero + void + init(uint32_t width, uint32_t height) + { + this->cells = node_count(width, height); + this->width = width; + this->db = std::make_unique(cells); + } + /// @brief sets precomputed jump-point along line [loc...loc+len) + /// when len reaches 0, final cell is not set + /// @param d direction of line + /// @param loc location + /// @param len length to cover, exclusive end + void + set_line(direction_id d, grid_id loc, length len) noexcept + { + // no negative for deadend + assert(d < 8); + assert(db != nullptr); + assert(loc.id < cells); + assert(DeadEnd || len >= 0); + assert(std::abs(len) < std::numeric_limits::max()); + uint32_t id = loc.id; + const uint32_t id_adj = warthog::grid::dir_id_adj(d, width); + const length len_adj + = DeadEnd ? static_cast(len >= 0 ? -1 : 1) : -1; + while(len != 0) + { + jump_res value; + if constexpr(!ChainJump) + { + // just use value + value = static_cast(len); + } + else + { + // if len <= chain_length(), use len, otherwise use + // chain_value() to force a chain lookup + if constexpr(DeadEnd) + { + // seperate DeadEnd code to remove abs + static_assert(std::is_signed_v); + value = std::abs(len) <= chain_stride() + 1 + ? static_cast(len) + : chain_value(); + } + else + { + value = len <= chain_stride() + 1 + ? static_cast(len) + : chain_value(); + } + } + db[id][d] = value; + id += id_adj; + len += len_adj; + } + } + /// @brief get pre-computed jump at location in direction + /// @param d direction + /// @param loc location + /// @return jump from loc in d + length + get_jump(direction_id d, grid_id loc) noexcept + { + // no negative for deadend + assert(d < 8); + assert(db != nullptr); + assert(loc.id < cells); + if constexpr(!ChainJump) { return static_cast(db[loc.id][d]); } + else + { + jump_res u = static_cast(db[loc.id][d]); + if(u != chain_value()) { return static_cast(u); } + else { return chain_jump(d, loc); } + } + } + /// @brief get mutable cell + /// @param loc location + /// @return cell entry for loc + cell& + get(grid_id loc) noexcept + { + assert(db != nullptr); + assert(loc.id < cells); + return db[loc.id]; + } + /// @brief get immutable cell + /// @param loc location + /// @return cell entry for loc + cell + get(grid_id loc) const noexcept + { + assert(db != nullptr); + assert(loc.id < cells); + return db[loc.id]; + } + /// @brief perform a chain jump, assumes loc is chain value + /// assumes user manually checked db value at loc and value was + /// chain_value() + /// @param d direction of jump + /// @param loc location + /// @return jump length, at least chain_length()+1 + /// @pre db[loc.id][d] == chain_value(), loc must be chained + length + chain_jump(direction_id d, grid_id loc) noexcept + requires(ChainJump) + { + assert(db != nullptr); + assert(loc.id < cells); + assert(db[loc.id][d] == chain_value()); + const uint32_t id_adj = static_cast(chain_stride()) + * warthog::grid::dir_id_adj(d, width); + length len = 0; + jump_res j; + do + { + // continue from previous jump + loc.id += id_adj; + len += chain_stride(); + j = static_cast(db[loc.id][d]); + } while(j == chain_value()); + assert(j != 0); + if constexpr(DeadEnd) + { + // check if block and negate, j is signed in this version + len = j >= 0 ? (len + j) : -(len - j); + } + else + { + // j is unsigned + len += static_cast>(j); + } + return len; + } + + /// @brief get jump cell + /// @param loc location + /// @return cell + cell + operator[](grid_id loc) const noexcept + { + assert(db != nullptr); + assert(loc.id < cells); + return db[loc.id]; + } + + std::ostream& + print(std::ostream& out) + { + std::array buffer; + for(uint32_t i = 0; i < cells;) + { + for(uint32_t j = 0; j < width; ++j, ++i) + { + // fill buffer with cell + char *at = buffer.begin(), *end = buffer.end(); + for(int k = 0; k < 8; ++k) + { + jump_distance dist + = get_jump(static_cast(k), grid_id{i}); + auto res = std::to_chars(at, end, dist); + if(res.ec != std::errc{}) + { + // error, exit + out.setstate(std::ios::failbit); + return out; + } + if(at == end) + { + // out of space, should not happen + out.setstate(std::ios::failbit); + return out; + } + *at++ = k + 1 < 8 ? ',' : '\n'; + } + out << buffer.data() << (j + 1 < width ? '\t' : '\n'); + } + } + } +}; + +template< + typename JumpTable = jump_point_table<>, + typename OnlinePoint = jump_point_online> +class jump_point_offline : public OnlinePoint +{ +public: + using jump_point_online::jump_point_online; + using typename OnlinePoint::bittable; + using typename OnlinePoint::rotate_grid; + + template + requires CardinalId + jump_distance + jump_cardinal_next(domain::grid_pair_id node_id) + { + return static_cast( + jump_table_.get_jump(D, get(node_id))); + } + + template + requires InterCardinalId + std::pair + jump_intercardinal_many( + domain::grid_pair_id node_id, intercardinal_jump_result* result, + uint16_t result_size, + jump_distance max_distance = std::numeric_limits::max()) + { + constexpr direction_id Dhori = dir_intercardinal_hori(D); + constexpr direction_id Dvert = dir_intercardinal_vert(D); + grid_id node = get(node_id); + const uint32_t node_adj = dir_id_adj(D, this->map_.width()); + std::pair res{0, 0}; + for(/*res.first*/; res.first < result_size;) + { + jump_distance dist + = static_cast(jump_table_.get_jump(D, node)); + if(dist <= 0) + { + res.second = static_cast(-res.second + dist); + break; + } + res.first += 1; + res.second += dist; + node.id += dist * node_adj; + // found point + intercardinal_jump_result resi; + resi.inter = res.second; + resi.hori = static_cast( + jump_table_.get_jump(Dhori, node)); + resi.vert = static_cast( + jump_table_.get_jump(Dvert, node)); + *(result++) = resi; + if(res.second > max_distance) break; + } + return res; + } + + /// @brief test jump directly to target point is visible or blocked + /// i.e. has line-of-sight or is blocked and where + /// @param node_id the id pairs for grid and rgrid (at loc) + /// @param loc x/y location (node_id points here) + /// @param target target point to check visiblity to + /// @return pair , if both >= 0 + /// then target is visible, + /// first<0 means intercardinal reaches blocker at -first distance + /// (second will be -1) first>=0 second<0 means cardinal blocker at + /// -second distance away second<0 mean target is blocked in + /// general + std::pair + jump_target(domain::grid_pair_id node_id, point loc, point target) + { + if constexpr(!JumpTable::dead_end()) + { + // dead end locations are not stored, so may not be able to find + // target, use online version + return OnlinePoint::jump_target(node_id, loc, target); + } + else + { + // direction_id real_d = d != 255 ? static_cast(d) : + // warthog::grid::point_to_direction_id(loc, target); + if(loc == target) return {0, 0}; + jump_distance inter_len = 0; + jump_distance cardinal_len = 0; + struct + { + uint32_t v; + uint32_t adj; + } id; + id.v = get(node_id).id; + direction_id d = point_to_direction_id(loc, target); + id.adj = dir_id_adj(d, this->map_.width()); + auto [xd, yd] = warthog::grid::point_signed_diff( + loc, target); // outside for cardinal pass + jump_distance jump_to; + if(is_intercardinal_id(d)) + { + jump_to = static_cast( + std::min(std::abs(xd), std::abs(yd))); + uint32_t startv = id.v; + do + { + jump_distance dist = static_cast( + jump_table_.get_jump(d, grid_id(id.v))); + if(dist <= 0) + { + // hit wall, final check + inter_len += -dist; + id.v += to_unsigned_jump_distance(-dist) * id.adj; + if(inter_len < jump_to) // failed to reach target + return {-inter_len, -1}; + break; + } + inter_len += dist; + id.v += to_unsigned_jump_distance(dist) * id.adj; + } while(inter_len < jump_to); + inter_len = jump_to; + id.v = startv + + jump_to * id.adj; // correct for over jumping jump_to + if(std::abs(xd) > std::abs(yd)) + { + // east/west + jump_to + = static_cast(std::abs(xd) - jump_to); + d = xd > 0 ? EAST_ID : WEST_ID; + } + else if(std::abs(xd) < std::abs(yd)) + { + // noth/south + jump_to + = static_cast(std::abs(yd) - jump_to); + d = yd > 0 ? SOUTH_ID : NORTH_ID; + } + else + { + // target reached + return {inter_len, 0}; + } + id.adj = dir_id_adj(d, this->map_.width()); + } + else + { + // target is cardinal to loc, max is correct jump_to + jump_to = static_cast( + std::max(std::abs(xd), std::abs(yd))); + } + // jump_to and d are correct for the cardinal jump + do + { + jump_distance dist = static_cast( + jump_table_.get_jump(d, grid_id(id.v))); + if(dist <= 0) + { + // hit wall, final check + cardinal_len += -dist; + id.v += to_unsigned_jump_distance(-dist) * id.adj; + if(cardinal_len < jump_to) + { // failed to reach target + if(cardinal_len == 0) // corner case + return {-inter_len, -1}; + else + return {inter_len, -cardinal_len}; + } + break; + } + cardinal_len += dist; + id.v += to_unsigned_jump_distance(dist) * id.adj; + } while(cardinal_len < jump_to); + return {inter_len, jump_to}; + } + } + + /// @brief set the underlying map, re-computing offline jump table + void + set_map(const rotate_grid& map) + { + OnlinePoint::set_map(map); + // compute offline jump-point table + compute_jump_table(); + } + /// @brief computes the jump table. Linear complexity to size of grid O(WH) + void + compute_jump_table() + { + const uint32_t width = this->map_.width(); + const uint32_t height = this->map_.height(); + jump_table_.init(width, height); + auto&& point_in_range + = [=](point p) noexcept { return p.x < width && p.y < height; }; + + // handle cardinal scans + + // compute jump table in N,S,E,W + // preset how to scan in each direction to reduce code to a for-loop + struct CardinalScan + { + direction_id d; + point start; + spoint row_adj; + }; + const std::array scans{ + {{NORTH_ID, point(0, height - 1), spoint(1, 0)}, + {SOUTH_ID, point(0, 0), spoint(1, 0)}, + {EAST_ID, point(0, 0), spoint(0, 1)}, + {WEST_ID, point(width - 1, 0), spoint(0, 1)}}}; + + // esseintally compile-type for-each N,S,E,W + // computes each direction and stores result in table in linear time on + // grid size + using jump_cardinal_type = jump_distance(OnlinePoint*, uint32_t); + warthog::util::for_each_integer_sequence>([&](auto iv) { + constexpr direction_id di = decltype(iv)::value; + constexpr int map_id = domain::rgrid_index; + const auto map = this->map_[map_id]; + CardinalScan s + = *std::find_if(scans.begin(), scans.end(), [di](auto& s) { + return s.d == di; + }); + // start scan + point node = s.start; + const spoint adj = dir_unit_point(s.d); + // for each directional row + for(point node = s.start; point_in_range(node); + node = node + s.row_adj) + { + point current_node = node; + // current_node loops column + while(point_in_range(current_node)) + { + auto current_id + = this->map_.point_to_pair_id(current_node); + if(map.get(grid_id(get(current_id)))) + { + jump_distance d + = OnlinePoint::template jump_cardinal_next( + current_id); + if(d == 0) [[unlikely]] + { + // immediently blocked + // jump_table_.get(row_node)[di] = 0; + current_node = current_node + + adj; // we know the next cell is a blocker + // assert(!this->map_.map().get(grid_id{node.id + + // col_adj})); + } + else + { + // store result + jump_table_.set_line( + di, get(current_id), d); + current_node = current_node + + std::abs(d) + * adj; // next cell is the reached cell + assert(point_in_range( + current_node)); // j should never jump past + // edge + // assert(this->map_.map().get(grid_id{node.id + j + // * col_adj})); + } + } + else + { + current_node = current_node + + adj; // is invalid cell, check the next + } + } + } + }); + + // + // InterCardinal scans + // + + // compute jump table in NE,NW,SE,SW + // preset how to scan in each direction to reduce code to a for-loop + struct ICardinalScan + { + direction_id d; + point start; // start location + }; + const std::array Iscans{ + {{NORTHEAST_ID, point(0, height - 1)}, + {NORTHWEST_ID, point(width - 1, height - 1)}, + {SOUTHEAST_ID, point(0, 0)}, + {SOUTHWEST_ID, point(width - 1, 0)}}}; + + for(auto s : Iscans) + { + const direction_id dh = dir_intercardinal_hori(s.d); + const direction_id dv = dir_intercardinal_vert(s.d); + const spoint adj = dir_unit_point(s.d); + BasicIntercardinalWalker walker; + walker.set_map(s.d, this->map_.map(), this->map_.width()); + for(int axis = 0; axis < 2; ++axis) + { + // 0 = follow hori edge, then follow vert edge + point start = s.start; + if(axis == 0) // adjust 1 axis to get diagonal along the start + start.x -= static_cast(adj.x); + while(true) + { // start location + if(axis == 0) + start.x += static_cast(adj.x); + else + start.y += static_cast(adj.y); + if(!point_in_range(start)) + break; // out of bounds, do nothing + // now scan along the diagonal + point loc = start; + while(true) + { + if(!point_in_range(loc)) break; // out of bounds, end + if(!this->map_.map().get(this->map_.point_to_id(loc))) + { + // blocker + loc = loc + adj; + continue; + } + // calc distance + point currentloc = loc; + jump_distance dist = 0; + // walk from current loc + walker.node_at = static_cast( + this->map_.point_to_id(currentloc)); + walker.first_row(); + while(true) + { + point nextloc = currentloc + adj; + bool valid = point_in_range(nextloc); + if(valid) + { + walker.next_row(); + valid = walker.valid_row(); + } + // loc in grid and non-corner cutting dia is clear + if(valid) + { + // traversable tile + dist += 1; + const auto& cell + = jump_table_[this->map_.point_to_id( + nextloc)]; + if(auto id = this->map_.point_to_id(nextloc); + jump_table_.get_jump(dh, id) > 0 + || jump_table_.get_jump(dv, id) > 0) + { + // turning point here + jump_table_.set_line( + s.d, this->map_.point_to_id(loc), + dist); + loc = nextloc; + break; // done with this line + } + } + else + { + // reached blocker or edge of map + if(dist != 0) + { + jump_table_.set_line( + s.d, this->map_.point_to_id(loc), + -dist); + } + loc = nextloc; + break; // done with this line + } + currentloc = nextloc; + } + } + } + } + } + } + +protected: + JumpTable jump_table_; +}; + +} // namespace jps::jump + +#endif // JPS_JUMP_JUMP_POINT_OFFLINE_H diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index 0a2dcb3..6b2dd0d 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -1,802 +1,170 @@ #ifndef JPS_JUMP_JUMP_POINT_ONLINE_H #define JPS_JUMP_JUMP_POINT_ONLINE_H -// A class wrapper around some code that finds, online, jump point -// successors of an arbitrary nodes in a uniform-cost grid map. // -// For theoretical details see: -// [Harabor D. and Grastien A, 2011, -// Online Graph Pruning Pathfinding on Grid Maps, AAAI] +// jps/jump/jump_point_offline.h +// +// Online jump-point locator. +// Contains utility functions for online jump-point locators +// +// The table will precompute the jump-point distances from an online locator +// and store them into a jump_point_table. +// +// @author dharabor & Ryan Hechenberger +// @created 2025-11-20 // +#include "block_online.h" +#include "jump.h" #include +#include #include #include -#include +#include namespace jps::jump { -namespace details -{ - -/// @brief -/// @tparam East Jump east (true) or west (false) -/// @param map a map of the grid, only the width parameter is required -/// @param node the starting location -/// @param goal the goal location -/// @return -template -uint32_t -jump_point_online_hori( - ::warthog::domain::gridmap::bittable map, uint32_t node, uint32_t goal) -{ - // read tiles from the grid: - // - along the row of node_id - // - from the row above node_id - // - from the row below node_id - // NB: the jump direction corresponds to moving from the - // low bit of the tileset and towards the high bit (EAST) - // or high bit to low bit (WEST) - auto nei_slider - = ::warthog::domain::gridmap_slider::from_bittable(map, pad_id{node}); - nei_slider.adj_bytes( - East ? -1 : -6); // current location is 1 byte from boundrary - // width8_bits is how many bits in on word current node is at, from lsb - // EAST and from msb WEST - nei_slider.width8_bits - = East ? nei_slider.width8_bits + 8 : 15 - nei_slider.width8_bits; - // 15 - width8_bits == 63 - (width8_bits + 6 * 8) - uint32_t jump_count = 0; - - // order going east is stored as least significant bit to most significant - // bit - - while(true) - { - std::array neis = nei_slider.get_neighbours_64bit_le(); - assert(nei_slider.width8_bits < 16); - uint64_t tmp = East ? ~(~0ull << nei_slider.width8_bits) - : ~(~0ull >> nei_slider.width8_bits); - // shift above and below 2 points east - // mask out to trav(1) before node - neis[0] |= tmp; - neis[1] |= tmp; - neis[2] |= tmp; - // v & will make this the least sig bit, and is where the jump point - // is located - // 10100011 : 0 - // 10111000 : <<1~ - if constexpr(East) - { - tmp = ((~neis[1] << 1) - & neis[1]) // above row: block(zero) trailing trav(one) - | ((~neis[2] << 1) - & neis[2]); // below row: block(zero) trailing trav(one) - } - else - { - tmp = ((~neis[1] >> 1) - & neis[1]) // above row: block(zero) trailing trav(one) - | ((~neis[2] >> 1) - & neis[2]); // below row: block(zero) trailing trav(one) - } - tmp = tmp | ~neis[0]; - if(tmp) - { - int stop_pos = East - ? std::countr_zero(tmp) - : std::countl_zero(tmp); // the location to stop at - // v is blocker location, prune unless target is present - // 10111111 - // dead end takes president as jump point can't pass a blocker - jump_count - += static_cast(stop_pos) - nei_slider.width8_bits; - uint32_t goal_jump = East ? goal - node : node - goal; - // if blocked: pos + jump_count = first block - // otherwise: jump_count = trav cell after turn (jump point - // location) - // check for goal with goal_jump (dist) <= jump_count, as if < than - // goal is reachable, if equal then trav pos is the goal if - // greater, than goal is further or another row or behind (as - // unsigned) - if(goal_jump <= jump_count) - { - // goal reached - jump_count = goal_jump; - } - else if( - East ? !(neis[0] & (static_cast(1) << stop_pos)) - : !(neis[0] - & (static_cast( - std::numeric_limits::min()) - >> stop_pos))) // deadend - { - jump_count = 0; - } - return jump_count; - } - - // failed, goto next 56 bits - jump_count += 63 - nei_slider.width8_bits; - nei_slider.adj_bytes(East ? 7 : -7); - nei_slider.width8_bits = 7; - } -} - -} // namespace details - -struct intercardinal_jump_result -{ - jps_id node; - jps_rid rnode; - uint32_t dist; -}; -template -struct IntercardinalWalker -{ - static_assert( - D == NORTHEAST || D == NORTHWEST || D == SOUTHEAST || D == SOUTHWEST, - "Must be intercardinal direction"); - union LongJumpRes - { - uint32_t dist[2]; /// distance hori/vert of D a jump is valid to - uint64_t joint; - }; - using map_type = ::warthog::domain::gridmap::bitarray; - /// @brief map and rmap (as bit array for small memory size) - map_type map[2]; - /// @brief location of current node on map and rmap - uint32_t node_at[2]; - /// @brief map and rmap value to adjust node_at for each row - int32_t adj_width[2]; - /// @brief map and rmap goal locations - uint32_t goal[2]; - /// @brief row scan - union - { - uint8_t row_[2]; ///< stores 3 bits at node_at[0]+-1, 0=prev, - ///< 1=current; high order bits 3..7 are not zero'd - uint16_t row_i_; - }; - - /// @brief convert map width to a map adj_width variable suited to - /// intercardinal D2 - template - static constexpr int32_t - to_map_adj_width(uint32_t width) noexcept - { - static_assert( - D2 == NORTHEAST || D2 == NORTHWEST || D2 == SOUTHEAST - || D2 == SOUTHWEST, - "Must be intercardinal direction"); - assert(width > 0); - if constexpr(D2 == NORTHEAST) - { - return -static_cast(width - 1); // - (mapW-1) - } - else if constexpr(D2 == SOUTHEAST) - { - return static_cast(width + 1); // + (mapW+1) - } - else if constexpr(D2 == SOUTHWEST) - { - return static_cast(width - 1); // + (mapW-1) - } - else - { // NORTHWEST - return -static_cast(width + 1); // - (mapW+1) - } - } - /// @brief convert rmap width to a rmap adj_width variable suited to - /// intercardinal D2 - template - static constexpr int32_t - to_rmap_adj_width(uint32_t width) noexcept - { - return to_map_adj_width(width); - } - - /// @brief convert map adj_width to map width, reciprocal to - /// to_map_adj_width - template - static constexpr uint32_t - from_map_adj_width(int32_t adj_width) noexcept - { - static_assert( - D2 == NORTHEAST || D2 == NORTHWEST || D2 == SOUTHEAST - || D2 == SOUTHWEST, - "Must be intercardinal direction"); - if constexpr(D2 == NORTHEAST) - { - return static_cast(-adj_width + 1); - } - else if constexpr(D2 == SOUTHEAST) - { - return static_cast(adj_width - 1); - } - else if constexpr(D2 == SOUTHWEST) - { - return static_cast(adj_width + 1); - } - else - { // NORTHWEST - return static_cast(-adj_width - 1); - } - } - /// @brief convert rmap adj_width to rmap width, reciprocal to - /// to_rmap_adj_width - template - static constexpr uint32_t - from_rmap_adj_width(int32_t adj_width) noexcept - { - return from_map_adj_width(adj_width); - } - - /// @brief set map width` - void - map_width(uint32_t width) noexcept - { - adj_width[0] = to_map_adj_width(width); - } - /// @brief get map width - uint32_t - map_width() const noexcept - { - return from_map_adj_width(adj_width[0]); - } - /// @brief set rmap width - void - rmap_width(uint32_t width) noexcept - { - adj_width[1] = to_rmap_adj_width(width); - } - /// @brief get rmap width - uint32_t - rmap_width() const noexcept - { - return from_rmap_adj_width(adj_width[1]); - } - - static uint32_t - jump_east(map_type map, uint32_t width, uint32_t node, uint32_t goal) - { - return details::jump_point_online_hori( - ::warthog::domain::gridmap::bittable(map, width, 0), node, goal); - } - static uint32_t - jump_west(map_type map, uint32_t width, uint32_t node, uint32_t goal) - { - return details::jump_point_online_hori( - ::warthog::domain::gridmap::bittable(map, width, 0), node, goal); - } - uint32_t - jump_hori() - { - if constexpr(D == NORTHEAST) - { - return jump_east(map[0], map_width(), node_at[0], goal[0]); // east - } - else if constexpr(D == SOUTHEAST) - { - return jump_east(map[0], map_width(), node_at[0], goal[0]); // east - } - else if constexpr(D == SOUTHWEST) - { - return jump_west(map[0], map_width(), node_at[0], goal[0]); // west - } - else if constexpr(D == NORTHWEST) - { - return jump_west(map[0], map_width(), node_at[0], goal[0]); // west - } - } - uint32_t - jump_vert() - { - if constexpr(D == NORTHEAST) - { - return jump_east( - map[1], rmap_width(), node_at[1], goal[1]); // north - } - else if constexpr(D == SOUTHEAST) - { - return jump_west( - map[1], rmap_width(), node_at[1], goal[1]); // south - } - else if constexpr(D == SOUTHWEST) - { - return jump_west( - map[1], rmap_width(), node_at[1], goal[1]); // south - } - else if constexpr(D == NORTHWEST) - { - return jump_east( - map[1], rmap_width(), node_at[1], goal[1]); // north - } - } - - void - next_index() noexcept - { - node_at[0] = static_cast( - static_cast(node_at[0]) + adj_width[0]); - node_at[1] = static_cast( - static_cast(node_at[1]) + adj_width[1]); - } - - /// @brief return get node_at[0]-1..node_at[0]+1 bits. CAUTION: return - /// bits 3..7 may not all be 0. - uint8_t - get_row() const noexcept - { - return static_cast( - map[0].get_span<3>(pad_id{node_at[0] - 1})); - } - /// @brief call for first row, then call next_row - void - first_row() noexcept - { - row_[1] = get_row(); - } - /// @brief update index to next row and update - void - next_row() noexcept - { - next_index(); - row_[0] = row_[1]; - row_[1] = get_row(); - } - /// @brief get node id for location node + dist(EAST/WEST of D) - jps_id - adj_hori(uint32_t node, uint32_t dist) const noexcept - { - if constexpr(D == NORTHEAST || D == SOUTHEAST) - { - return jps_id{node + dist}; - } - else { return jps_id{node - dist}; } - } - /// @brief get node id for location node + dist(NORTH/SOUTH of D) - jps_id - adj_vert(uint32_t node, uint32_t dist) const noexcept - { - if constexpr(D == NORTHEAST || D == SOUTHEAST) - { - return jps_id{node + (adj_width[0] - 1) * dist}; - } - else { return jps_id{node + (adj_width[0] + 1) * dist}; } - } - /// @brief the current locations row is a valid intercardinal move (i.e. - /// 2x2 is free) - bool - valid_row() const noexcept - { - // east | west differernce - // north/south does not make a difference - if constexpr(D == NORTHEAST || D == SOUTHEAST) - { - // we want from grid - // .xx == row[0] = 0bxx. - // xx. == row[1] = 0b.xx - // all[x] = 1 - constexpr uint16_t mask - = std::endian::native == std::endian::little - ? 0b0000'0011'0000'0110 - : 0b0000'0110'0000'0011; - return (row_i_ & mask) == mask; - } - else - { - // we want from grid - // xx. == row[0] = 0b.xx - // .xx == row[1] = 0bxx. - // all[x] = 1 - constexpr uint16_t mask - = std::endian::native == std::endian::little - ? 0b0000'0110'0000'0011 - : 0b0000'0011'0000'0110; - return (row_i_ & mask) == mask; - } - } - // {hori,vert} of jump hori/vert of D, from node_at. - LongJumpRes - long_jump() - { - return {jump_hori(), jump_vert()}; - } -}; - -template class jump_point_online { public: - using gridmap = warthog::domain::gridmap; - using map_type = gridmap::bittable; - jump_point_online(); - jump_point_online(gridmap* map) - { - if(map != nullptr) set_map(*map); - } - ~jump_point_online() = default; + using bittable = ::warthog::domain::gridmap::bittable; + using rotate_grid = domain::gridmap_rotate_table_convs; - static consteval bool - feature_prune_intercardinal() noexcept - { - return (static_cast(Feature) - & static_cast(JpsFeature::PRUNE_INTERCARDINAL)) - != 0; - } - static consteval bool - feature_store_cardinal() noexcept - { - return (static_cast(Feature) - & static_cast(JpsFeature::STORE_CARDINAL_JUMP)) - != 0; - } + jump_point_online() = default; + jump_point_online(const rotate_grid& map) { set_map(map); } + ~jump_point_online() = default; void - set_map(gridmap& map); - void - set_goal(jps_id goal_id) noexcept; - void - set_goal(point goal_id) noexcept; - - /// @brief avoid modifying these grids accidentally - gridmap::bittable - get_map() const noexcept - { - return map_; - } - /// @brief care should be taken to avoid modifying these grids - gridmap::bittable - get_rmap() noexcept - { - return rmap_; - } + set_map(const rotate_grid& map); + // void + // set_target(jps_id target_id) noexcept; + // void + // set_target(point target_id) noexcept; + + // /// @brief avoid modifying these grids accidentally + // bittable + // map() const noexcept + // { + // return map_; + // } + // /// @brief care should be taken to avoid modifying these grids + // bittable + // rmap() noexcept + // { + // return rmap_; + // } - /** - * @returns pair first: steps to reach node (or 0 if no node), second: id - * of jump point (or node_id if none) - */ - std::pair - jump_cardinal(direction d, jps_id node_id, jps_rid rnode_id); - intercardinal_jump_result - jump_intercardinal( - direction d, jps_id node_id, jps_rid rnode_id, jps_id* result_node, - cost_t* result_cost, uint32_t result_size = 0); - - uint32_t - jump_north(jps_rid rnode) - { - return jump_east(rmap_, rnode.id, rgoal_.id); - } - uint32_t - jump_east(jps_id node) - { - return jump_east(map_, node.id, goal_.id); - } - uint32_t - jump_south(jps_rid rnode) - { - return jump_west(rmap_, rnode.id, rgoal_.id); - } - uint32_t - jump_west(jps_id node) - { - return jump_west(map_, node.id, goal_.id); - } - intercardinal_jump_result - jump_northeast( - jps_id node, jps_rid rnode, jps_id* result_node, cost_t* result_cost, - uint32_t result_size = 0) - { - return jump_intercardinal( - node, rnode, result_node, result_cost, result_size); - } - intercardinal_jump_result - jump_southeast( - jps_id node, jps_rid rnode, jps_id* result_node, cost_t* result_cost, - uint32_t result_size) - { - return jump_intercardinal( - node, rnode, result_node, result_cost, result_size); - } - intercardinal_jump_result - jump_southwest( - jps_id node, jps_rid rnode, jps_id* result_node, cost_t* result_cost, - uint32_t result_size) - { - return jump_intercardinal( - node, rnode, result_node, result_cost, result_size); - } - intercardinal_jump_result - jump_northwest( - jps_id node, jps_rid rnode, jps_id* result_node, cost_t* result_cost, - uint32_t result_size) - { - return jump_intercardinal( - node, rnode, result_node, result_cost, result_size); - } + /// @brief same as jump_cardinal_next(point) but is given the correct + /// grid_id type + /// @tparam D cardinal direction_id (NORTH_ID,SOUTH_ID,EAST_ID,WEST_ID) + /// @param node_id the id pairs for grid and rgrid (at loc) + /// @return >0: jump point n steps away, <=0: blocker -n steps away + template + requires CardinalId + jump_distance + jump_cardinal_next(domain::grid_pair_id node_id); + + /// @brief returns all intercardinal jump points up to max_distance + /// (default inf) + /// and max of results_size + /// @tparam D intercardinal direction_id + /// (NORTHEAST_ID,NORTHWEST_ID,SOUTHEAST_ID,SOUTHWEST_ID) + /// @param node_id the id pairs for grid and rgrid + /// @param result_size maximum number of results that can fit in result + /// @param result pointer to results storage of at least size result_size + /// @param max_distance the maximum intercardinal distance to scan to + /// @return pair first: number of results returned. second: the end + /// distance + /// + /// This function is designed to efficiently find all jump points + /// intercardinally. The returned point is either on the final result, the + /// max_distance location (loc + max_distance) or the point in-front of the + /// blocker(deadend). This function is intended to be run multiple times by + /// passing the return loc into the next row, until either reaching a + /// deadend or some algorithmic specific stopping criteria. + template + requires InterCardinalId + std::pair + jump_intercardinal_many( + domain::grid_pair_id node_id, intercardinal_jump_result* result, + uint16_t result_size, + jump_distance max_distance + = std::numeric_limits::max()); + + /// @brief shoot ray to target point + /// @param node_id the id pairs for grid and rgrid (at loc) + /// @param loc shoot from loc + /// @param target shoot to target + /// @return pair , if both >= 0 + /// then target is visible, + /// first<0 means intercardinal reaches blocker at -first distance + /// (second will be -1) first>=0 second<0 means cardinal blocker at + /// -second distance away second<0 mean target is blocked in + /// general + std::pair + jump_target(domain::grid_pair_id node_id, point loc, point target); size_t mem() { - return sizeof(this) + (rmap_data_ ? rmap_data_->mem() : 0); + return sizeof(this); } - point - point_to_rpoint(point p) const noexcept - { - return { - static_cast(map_unpad_height_m1_ - p.y), - static_cast(p.x)}; - } - point - rpoint_to_point(point p) const noexcept - { - return { - static_cast(p.y), - static_cast(map_unpad_height_m1_ - p.x)}; - } - jps_id - point_to_id(point p) const noexcept - { - return jps_id{ - static_cast(p.y + gridmap::PADDED_ROWS) - * map_.width() - + static_cast(p.x)}; - } - jps_rid - rpoint_to_rid(point p) const noexcept - { - return jps_rid{ - static_cast(p.y + gridmap::PADDED_ROWS) - * rmap_.width() - + static_cast(p.x)}; - } - point - id_to_point(jps_id p) const noexcept - { - return { - static_cast(p.id % map_.width()), - static_cast(p.id / map_.width() - gridmap::PADDED_ROWS)}; - } - point - rid_to_rpoint(jps_rid p) const noexcept - { - return { - static_cast(p.id % rmap_.width()), - static_cast( - p.id / rmap_.width() - gridmap::PADDED_ROWS)}; - } - jps_rid - id_to_rid(jps_id mapid) +protected: + static int32_t + jump_east(bittable map, uint32_t node) { - assert(!mapid.is_none()); - return rpoint_to_rid(point_to_rpoint(id_to_point(mapid))); + return jump_point_online_hori(map, node); } - jps_id - rid_to_id(jps_rid mapid) + static int32_t + jump_west(bittable map, uint32_t node) { - assert(!mapid.is_none()); - return point_to_id(rpoint_to_point(rid_to_rpoint(mapid))); + return jump_point_online_hori(map, node); } protected: - static uint32_t - jump_east(map_type map, uint32_t node, uint32_t goal); - static uint32_t - jump_west(map_type map, uint32_t node, uint32_t goal); - - /** - * Jumps on the intercardinal. - * The result_* variables store the cardinal jump results (if enabled) - * result_count only used for PRUNE_INTERCARDINAL. - * result_* must be big enough to store: - * !PRUNE_INTERCARDINAL & !STORE_CARDINAL_JUMP => 0 (should be nullptr) - * !PRUNE_INTERCARDINAL & STORE_CARDINAL_JUMP => 2 - * PRUNE_INTERCARDINAL => result_cost (min 4) - * - * if !PRUNE_INTERCARDINAL & STORE_CARDINAL_JUMP: - * results[0] = east/west result or jps_id::none() if none - * results[1] = north/south result or jps_id::none() if none - * - * The return intercardinal_jump_result is as follows: - * node: return end point id, or jps_id::none() if no more successors. - * rnode: return end rpoint id, or jps_rid::none() if no more successors. - * dist: !PRUNE_INTERCARDINAL => distance jumped (0 = no jump) - * PRUNE_INTERCARDINAL => the number of elements pushed on the - * result node - */ - template - intercardinal_jump_result - jump_intercardinal( - jps_id node, jps_rid rnode, jps_id* result_node, cost_t* result_cost, - uint32_t result_size = 0); - - // jps_id point_to_jps_id(point p) noexcept - // { - // return - // } - -protected: - /** - * Rotate 90 clockwise - * NORTH -> EAST - * EAST -> SOUTH - * SOUTH -> WEST - * WEST -> NORTH - * NORTHEAST -> SOUTHEAST - * SOUTHEAST -> SOUTHWEST - * SOUTHWEST -> NORTHWEST - * NORTHWEST -> NORTHEAST - * - * unpadded (x,y) -> (y, Rh-1-x) - */ - void - create_rotate_(const gridmap& orig); - -protected: - std::unique_ptr rmap_data_; - map_type map_ = {}; - map_type rmap_ = {}; - // uint32_t map_width_ = 0; - // uint32_t rmap_width_ = 0; - uint32_t map_unpad_height_m1_ = 0; - jps_id goal_ = {}; - jps_rid rgoal_ = {}; + rotate_grid map_; }; -template void -jump_point_online::set_map(gridmap& orig) +jump_point_online::set_map(const rotate_grid& orig) { map_ = orig; - create_rotate_(orig); } -template -void -jump_point_online::set_goal(jps_id p) noexcept -{ - goal_ = p; - rgoal_ = id_to_rid(p); -} -template -void -jump_point_online::set_goal(point p) noexcept +template + requires CardinalId +jump_distance +jump_point_online::jump_cardinal_next(domain::grid_pair_id node_id) { - goal_ = point_to_id(p); - rgoal_ = rpoint_to_rid(point_to_rpoint(p)); -} - -template -void -jump_point_online::create_rotate_(const gridmap& orig) -{ - const uint32_t maph = orig.header_height(); - const uint32_t mapw = orig.header_width(); - auto tmap = std::make_unique(mapw, maph); - - for(uint32_t y = 0; y < maph; y++) + if constexpr(D == NORTH_ID || D == EAST_ID) { - for(uint32_t x = 0; x < mapw; x++) - { - bool label = orig.get_label(orig.to_padded_id_from_unpadded(x, y)); - uint32_t rx = (maph - 1) - y; - uint32_t ry = x; - tmap->set_label(tmap->to_padded_id_from_unpadded(rx, ry), label); - } + return jump_east( + map_.table(domain::rgrid_index), + static_cast(get_d(node_id))); } - - // set values - rmap_data_ = std::move(tmap); - rmap_ = *rmap_data_; - // map_.width() = map_->width(); - // rmap_.width() = rmap_->width(); - map_unpad_height_m1_ = maph - 1; -} - -template -std::pair -jump_point_online::jump_cardinal( - direction d, jps_id node_id, jps_rid rnode_id) -{ - std::pair node; - switch(d) + else if constexpr(D == SOUTH_ID || D == WEST_ID) { - case NORTH: - node.first = jump_north(rnode_id); - node.second.id = node_id.id - map_.width() * node.first; - break; - case SOUTH: - node.first = jump_south(rnode_id); - node.second.id = node_id.id + map_.width() * node.first; - break; - case EAST: - node.first = jump_east(node_id); - node.second.id = node_id.id + node.first; - break; - case WEST: - node.first = jump_west(node_id); - node.second.id = node_id.id - node.first; - break; - default: - assert(false); - node = {0, node_id}; + return jump_west( + map_.table(domain::rgrid_index), + static_cast(get_d(node_id))); } - return node; -} -template -intercardinal_jump_result -jump_point_online::jump_intercardinal( - direction d, jps_id node_id, jps_rid rnode_id, jps_id* result_node, - cost_t* result_cost, uint32_t result_size) -{ - intercardinal_jump_result node; - switch(d) + else { - case NORTHEAST: - node = jump_intercardinal( - node_id, rnode_id, result_node, result_cost, result_size); - break; - case NORTHWEST: - node = jump_intercardinal( - node_id, rnode_id, result_node, result_cost, result_size); - break; - case SOUTHEAST: - node = jump_intercardinal( - node_id, rnode_id, result_node, result_cost, result_size); - break; - case SOUTHWEST: - node = jump_intercardinal( - node_id, rnode_id, result_node, result_cost, result_size); - break; - default: assert(false); - node = {jps_id::none(), jps_rid::none(), 0}; + return 0; } - return node; } -template -uint32_t -jump_point_online::jump_east( - map_type map, uint32_t node, uint32_t goal) +template + requires InterCardinalId +std::pair +jump_point_online::jump_intercardinal_many( + domain::grid_pair_id node_id, intercardinal_jump_result* result, + uint16_t result_size, jump_distance max_distance) { - return details::jump_point_online_hori(map, node, goal); -} - -template -uint32_t -jump_point_online::jump_west( - map_type map, uint32_t node, uint32_t goal) -{ - return details::jump_point_online_hori(map, node, goal); -} - -template -template -intercardinal_jump_result -jump_point_online::jump_intercardinal( - jps_id node, jps_rid rnode, jps_id* result_node [[maybe_unused]], - cost_t* result_cost [[maybe_unused]], - uint32_t result_size [[maybe_unused]]) -{ - static_assert( - D == NORTHEAST || D == NORTHWEST || D == SOUTHEAST || D == SOUTHWEST, - "D must be inter-cardinal."); - - assert(!node.is_none() && !rnode.is_none()); - - // precondition - assert( - !(feature_prune_intercardinal() - || feature_store_cardinal()) // both not enabled = fine - || (result_node != nullptr - && result_cost != nullptr) // must be set if enabled - ); - + assert(result_size > 0); /* map: NW N NE @@ -836,141 +204,161 @@ jump_point_online::jump_intercardinal( IntercardinalWalker walker; // class to walk // setup the walker members // 0 = map, 1 = rmap - walker.map[0] = map_; - walker.map[1] = rmap_; + walker.map = {map_[0], map_[1]}; walker.map_width(map_.width()); - walker.rmap_width(rmap_.width()); - walker.node_at[0] = static_cast(node); - walker.node_at[1] = static_cast(rnode); - walker.goal[0] = goal_.id; - walker.goal[1] = rgoal_.id; + walker.rmap_width(map_.rwidth()); + walker.node_at[0] = static_cast(get(node_id)); + walker.node_at[1] = static_cast(get(node_id)); + assert( + map_.map().get(grid_id(walker.node_at[0])) + && map_.rmap().get(grid_id(walker.node_at[1]))); - // pre-set cardinal results to none, in case no successors are found - if constexpr(feature_store_cardinal()) - { - result_node[0] = result_node[1] = jps_id::none(); + // JPS, stop at the first intercardinal turning point + uint16_t results_count = 0; + jump_distance walk_count = 0; + walker.first_row(); + while(walk_count < max_distance) // if equal, max distance is reached + { + walker.next_row(); // walk_count adjusted at end of loop + if(!walker.valid_row()) [[unlikely]] // no successors + return {results_count, static_cast(-walk_count)}; + walk_count += 1; + // + // handle hori/vert long jump + // + auto res = walker.long_jump(); + if(res) + { // at least one jump has a turning point + assert(results_count < result_size); + *(result++) = {walk_count, res.dist[0], res.dist[1]}; + if(++results_count == result_size) break; + } } - // JPS, stop at the first intercardinal turning point - if constexpr(!feature_prune_intercardinal()) - { - uint32_t walk_count = 1; - walker.first_row(); - while(true) - { - walker.next_row(); // walk_count adjusted at end of loop - if(!walker.valid_row()) // no successors - return {jps_id::none(), jps_rid::none(), 0}; - // check if intercardinal is passing over goal - if(walker.node_at[0] == walker.goal[0]) [[unlikely]] + return {results_count, walk_count}; +} + +std::pair +jump_point_online::jump_target( + domain::grid_pair_id node_id, point loc, point target) +{ + // direction_id real_d = d != 255 ? static_cast(d) : + // warthog::grid::point_to_direction_id(loc, target); + auto [xd, yd] = warthog::grid::point_signed_diff(loc, target); + jump_distance inter_len; + jump_distance cardinal_len; + direction_id d; + if(xd != 0 && yd != 0) + { + // diagonal + BasicIntercardinalWalker walker; + if(xd > 0) + { // east + if(yd > 0) + { // south + d = SOUTHEAST_ID; + walker.set_map(map_[0], map_.width()); + } + else { - // reached goal - intercardinal_jump_result result; - result.node = jps_id{walker.node_at[0]}; - result.rnode = jps_rid::none(); // walker.get_last_rrow(); - result.dist = walk_count; - return result; + d = NORTHEAST_ID; + walker.set_map(map_[0], map_.width()); + } + } + else + { // west + if(yd > 0) + { // south + d = SOUTHWEST_ID; + walker.set_map(map_[0], map_.width()); } - // - // handle hori/vert long jump - // - auto res = walker.long_jump(); - if(res.joint != 0) - { // at least one jump has a turning point - intercardinal_jump_result result; - if(!feature_store_cardinal()) - { - // do not store cardinal results, just return the - // intercardinal point - result.node = jps_id{walker.node_at[0]}; - result.rnode = jps_rid::none(); // walker.get_last_rrow(); - result.dist = walk_count; - } - else - { - cost_t current_cost = walk_count * warthog::DBL_ROOT_TWO; - // check hori/vert jump result and store their values - if(res.dist[0] != 0) - { - result_node[0] - = walker.adj_hori(walker.node_at[0], res.dist[0]); - result_cost[0] - = current_cost + res.dist[0] * warthog::DBL_ONE; - } - else { result_node[0] = jps_id::none(); } - if(res.dist[1] != 0) - { - result_node[1] - = walker.adj_vert(walker.node_at[0], res.dist[1]); - result_cost[1] - = current_cost + res.dist[1] * warthog::DBL_ONE; - } - else { result_node[1] = jps_id::none(); } - // store next row location as we do not need to keep the - // current intercardinal - walker.next_row(); - result.node = jps_id{walker.node_at[0]}; - result.rnode = jps_rid::none(); // walker.get_last_rrow(); - result.dist = walker.valid_row() ? walk_count + 1 : 0; - } - // found jump point, return - return result; + else + { + d = NORTHWEST_ID; + walker.set_map(map_[0], map_.width()); } - walk_count += 1; } - } - else - { - // prunes intercardinal, progress and add successors to count - assert(result_size > 2); - result_size - -= 1; // ensure there is always space for at least 2 results - uint32_t walk_count = 1; - uint32_t result_count = 0; + walker.node_at = static_cast(get(node_id)); + inter_len + = static_cast(std::min(std::abs(xd), std::abs(yd))); walker.first_row(); - // only continue if there is room to store results - while(result_count < result_size) + for(jump_distance i = 0; i < inter_len; ++i) { walker.next_row(); if(!walker.valid_row()) - return {jps_id::none(), jps_rid::none(), result_count}; - if(walker.node_at[0] == walker.goal[0]) [[unlikely]] { - // reached goal - result_count += 1; - *(result_node++) = jps_id{walker.node_at[0]}; - *(result_cost++) = walk_count * warthog::DBL_ROOT_TWO; - break; - } - auto res = walker.long_jump(); - cost_t current_cost = walk_count * warthog::DBL_ROOT_TWO; - if(res.dist[0] != 0) - { // east/west - result_count += 1; - *(result_node++) - = walker.adj_hori(walker.node_at[0], res.dist[0]); - *(result_cost++) - = current_cost + res.dist[0] * warthog::DBL_ONE; - } - if(res.dist[1] != 0) - { // north/south - result_count += 1; - // NORTH/SOUTH handles the correct sing, adjust for EAST/WEST - // diff - *(result_node++) - = walker.adj_vert(walker.node_at[0], res.dist[1]); - *(result_cost++) - = current_cost + res.dist[1] * warthog::DBL_ONE; + // diagonal blocked before reaching turn, report + return {static_cast(-i), -1}; } - walk_count += 1; } - // not enough buffer, return result and start again - return { - jps_id{walker.node_at[0]}, jps_rid{walker.node_at[1]}, - result_count}; + // update loc + spoint dia_unit = dir_unit_point(d); + dia_unit.x *= inter_len; + dia_unit.y *= inter_len; + loc = loc + dia_unit; + xd -= dia_unit.x; + yd -= dia_unit.y; } + else + { + // no diagonal + inter_len = 0; + } + assert(xd == 0 || yd == 0); + if(yd == 0) + { + // horizontal ray + if(xd == 0) [[unlikely]] + { + // at target + return {inter_len, 0}; + } + else if(xd > 0) + { + // east + cardinal_len + = jump_point_online_hori_target>( + map_.table(domain::rgrid_index), + static_cast(map_.point_to_id_d(loc)), + static_cast( + map_.point_to_id_d(target))); + } + else + { + // west + cardinal_len + = jump_point_online_hori_target>( + map_.table(domain::rgrid_index), + static_cast(map_.point_to_id_d(loc)), + static_cast( + map_.point_to_id_d(target))); + } + } + else if(yd > 0) + { + // south + cardinal_len + = jump_point_online_hori_target>( + map_.table(domain::rgrid_index), + static_cast(map_.point_to_id_d(loc)), + static_cast(map_.point_to_id_d(target))); + } + else + { + // north + cardinal_len + = jump_point_online_hori_target>( + map_.table(domain::rgrid_index), + static_cast(map_.point_to_id_d(loc)), + static_cast(map_.point_to_id_d(target))); + } + if(cardinal_len == 0) // corner case, do not return {>0,0} value at + // blocker, instead return {<,-1} + return {-inter_len, -1}; + else + return {inter_len, cardinal_len}; } -} +} // namespace jps::jump #endif // JPS_JUMP_JUMP_POINT_ONLINE_H diff --git a/include/jps/jump/offline_jump_point_locator.h b/include/jps/jump/offline_jump_point_locator.h deleted file mode 100644 index c272b78..0000000 --- a/include/jps/jump/offline_jump_point_locator.h +++ /dev/null @@ -1,83 +0,0 @@ -#ifndef JPS_JUMP_OFFLINE_JUMP_POINT_LOCATOR_H -#define JPS_JUMP_OFFLINE_JUMP_POINT_LOCATOR_H - -// offline_jump_point_locator.h -// -// Identifies jump points using a pre-computed database that stores -// distances from each node to jump points in every direction. -// -// @author: dharabor -// @created: 05/05/2013 -// - -#include -#include - -namespace jps::jump -{ - -class offline_jump_point_locator -{ -public: - offline_jump_point_locator(warthog::domain::gridmap* map); - ~offline_jump_point_locator(); - - void - jump( - direction d, jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - double& jumpcost); - - uint32_t - mem() const noexcept - { - return sizeof(this) + sizeof(*db_) * dbsize_; - } - -private: - void - preproc(); - - bool - load(const char* filename); - - void - save(const char* filename); - - void - jump_northwest( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_northeast( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_southwest( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_southeast( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_north( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_south( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_east( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - void - jump_west( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost); - - warthog::domain::gridmap* map_; - uint32_t dbsize_; - uint16_t* db_; - - // uint32_t jumppoints_[3]; - // double costs_[3]; - uint32_t max_; - uint32_t current_; -}; - -} - -#endif // JPS_JUMP_OFFLINE_JUMP_POINT_LOCATOR_H diff --git a/include/jps/jump/offline_jump_point_locator2.h b/include/jps/jump/offline_jump_point_locator2.h deleted file mode 100644 index f9f6c72..0000000 --- a/include/jps/jump/offline_jump_point_locator2.h +++ /dev/null @@ -1,90 +0,0 @@ -#ifndef JPS_JUMP_OFFLINE_JUMP_POINT_LOCATOR2_H -#define JPS_JUMP_OFFLINE_JUMP_POINT_LOCATOR2_H - -// offline_jump_point_locator2.h -// -// Variant of warthog::offline_jump_point_locator. -// Jump points are identified using a pre-computed database that stores -// distances from each node to jump points in every direction. -// This version additionally prunes all jump points that do not have at -// least one forced neighbour. -// -// @author: dharabor -// @created: 05/05/2013 -// - -#include -#include -#include - -namespace jps::jump -{ - -class offline_jump_point_locator2 -{ -public: - offline_jump_point_locator2(warthog::domain::gridmap* map); - ~offline_jump_point_locator2(); - - void - jump( - direction d, jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs); - - uint32_t - mem() const noexcept - { - return sizeof(this) + sizeof(*db_) * dbsize_; - } - -private: - void - preproc(); - - bool - load(const char* filename); - - void - save(const char* filename); - - void - jump_northwest( - jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs); - void - jump_northeast( - jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs); - void - jump_southwest( - jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs); - void - jump_southeast( - jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs); - void - jump_north( - jps_id node_id, jps_id goal_id, double cost_to_node_id, - vec_jps_id& neighbours, vec_jps_cost& costs); - void - jump_south( - jps_id node_id, jps_id goal_id, double cost_to_node_id, - vec_jps_id& neighbours, vec_jps_cost& costs); - void - jump_east( - jps_id node_id, jps_id goal_id, double cost_to_node_id, - vec_jps_id& neighbours, vec_jps_cost& costs); - void - jump_west( - jps_id node_id, jps_id goal_id, double cost_to_node_id, - vec_jps_id& neighbours, vec_jps_cost& costs); - - warthog::domain::gridmap* map_; - uint32_t dbsize_; - uint16_t* db_; -}; - -} - -#endif // JPS_JUMP_OFFLINE_JUMP_POINT_LOCATOR2_H diff --git a/include/jps/jump/online_jump_point_locator.h b/include/jps/jump/online_jump_point_locator.h deleted file mode 100644 index 83373cc..0000000 --- a/include/jps/jump/online_jump_point_locator.h +++ /dev/null @@ -1,130 +0,0 @@ -#ifndef JPS_JUMP_ONLINE_JUMP_POINT_LOCATOR_H -#define JPS_JUMP_ONLINE_JUMP_POINT_LOCATOR_H - -// online_jump_point_locator.h -// -// A class wrapper around some code that finds, online, jump point -// successors of an arbitrary nodes in a uniform-cost grid map. -// -// For theoretical details see: -// [Harabor D. and Grastien A, 2011, -// Online Graph Pruning Pathfinding on Grid Maps, AAAI] -// -// @author: dharabor -// @created: 03/09/2012 -// - -#include -#include - -namespace jps::jump -{ - -class online_jump_point_locator -{ -public: - online_jump_point_locator(warthog::domain::gridmap* map); - ~online_jump_point_locator(); - - void - jump( - direction d, jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost); - - size_t - mem() const noexcept - { - return sizeof(this) + rmap_->mem(); - } - -private: - void - jump_northwest( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost); - void - jump_northeast( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost); - void - jump_southwest( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost); - void - jump_southeast( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost); - void - jump_north( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost); - void - jump_south( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost); - void - jump_east( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost); - void - jump_west( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost); - - // these versions can be passed a map parameter to - // use when jumping. they allow switching between - // map_ and rmap_ (a rotated counterpart). - void - jump_east_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); - void - jump_west_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); - void - jump_north_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); - void - jump_south_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); - - inline jps_id - map_id_to_rmap_id(jps_id mapid) - { - if(mapid.is_none()) { return jps_id::none(); } - - uint32_t x, y; - uint32_t rx, ry; - map_->to_unpadded_xy(mapid, x, y); - ry = x; - rx = map_->header_height() - y - 1; - return jps_id(rmap_->to_padded_id_from_unpadded(rx, ry)); - } - - inline jps_id - rmap_id_to_map_id(jps_id rmapid) - { - if(rmapid.is_none()) { return jps_id::none(); } - - uint32_t x, y; - uint32_t rx, ry; - rmap_->to_unpadded_xy(rmapid, rx, ry); - x = ry; - y = rmap_->header_width() - rx - 1; - return jps_id(map_->to_padded_id_from_unpadded(x, y)); - } - - warthog::domain::gridmap* - create_rmap(); - - warthog::domain::gridmap* map_; - warthog::domain::gridmap* rmap_; - // uint32_t jumplimit_; -}; - -} - -#endif // JPS_JUMP_ONLINE_JUMP_POINT_LOCATOR_H diff --git a/include/jps/jump/online_jump_point_locator2.h b/include/jps/jump/online_jump_point_locator2.h deleted file mode 100644 index 2e2db02..0000000 --- a/include/jps/jump/online_jump_point_locator2.h +++ /dev/null @@ -1,156 +0,0 @@ -#ifndef JPS_JUMP_ONLINE_JUMP_POINT_LOCATOR2_H -#define JPS_JUMP_ONLINE_JUMP_POINT_LOCATOR2_H - -// online_jump_point_locator2.h -// -// A class wrapper around some code that finds, online, jump point -// successors of an arbitrary nodes in a uniform-cost grid map. -// -// For theoretical details see: -// [Harabor D. and Grastien A, 2011, -// Online Graph Pruning Pathfinding on Grid Maps, AAAI] -// -// @author: dharabor -// @created: 03/09/2012 -// - -#include -#include -#include - -namespace jps::jump -{ - -class online_jump_point_locator2 -{ -public: - online_jump_point_locator2(warthog::domain::gridmap* map); - ~online_jump_point_locator2(); - - void - jump( - direction d, jps_id node_id, jps_id goal_id, vec_jps_id& jpoints, - vec_jps_cost& costs); - - size_t - mem() const noexcept - { - return sizeof(this) + rmap_->mem(); - } - -private: - void - jump_north(vec_jps_id& jpoints, vec_jps_cost& costs); - void - jump_south(vec_jps_id& jpoints, vec_jps_cost& costs); - void - jump_east(vec_jps_id& jpoints, vec_jps_cost& costs); - void - jump_west(vec_jps_id& jpoints, vec_jps_cost& costs); - void - jump_northeast(vec_jps_id& jpoints, vec_jps_cost& costs); - void - jump_northwest(vec_jps_id& jpoints, vec_jps_cost& costs); - void - jump_southeast(vec_jps_id& jpoints, vec_jps_cost& costs); - void - jump_southwest(vec_jps_id& jpoints, vec_jps_cost& costs); - - // these versions can be passed a map parameter to - // use when jumping. they allow switching between - // map_ and rmap_ (a rotated counterpart). - void - jump_north_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); - void - jump_south_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); - void - jump_east_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); - void - jump_west_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); - - // these versions perform a single diagonal jump, returning - // the intermediate diagonal jump point and the straight - // jump points that caused the jumping process to stop - void - jump_northeast_( - jps_id& node_id, jps_id& rnode_id, jps_id goal_id, jps_id rgoal_id, - jps_id& jumpnode_id, warthog::cost_t& jumpcost, jps_id& jp1_id, - warthog::cost_t& jp1_cost, jps_id& jp2_id, warthog::cost_t& jp2_cost); - void - jump_northwest_( - jps_id& node_id, jps_id& rnode_id, jps_id goal_id, jps_id rgoal_id, - jps_id& jumpnode_id, warthog::cost_t& jumpcost, jps_id& jp1_id, - warthog::cost_t& jp1_cost, jps_id& jp2_id, warthog::cost_t& jp2_cost); - void - jump_southeast_( - jps_id& node_id, jps_id& rnode_id, jps_id goal_id, jps_id rgoal_id, - jps_id& jumpnode_id, warthog::cost_t& jumpcost, jps_id& jp1_id, - warthog::cost_t& jp1_cost, jps_id& jp2_id, warthog::cost_t& jp2_cost); - void - jump_southwest_( - jps_id& node_id, jps_id& rnode_id, jps_id goal_id, jps_id rgoal_id, - jps_id& jumpnode_id, warthog::cost_t& jumpcost, jps_id& jp1_id, - warthog::cost_t& jp1_cost, jps_id& jp2_id, warthog::cost_t& jp2_cost); - - // functions to convert map indexes to rmap indexes - inline jps_id - map_id_to_rmap_id(jps_id mapid) - { - if(mapid.is_none()) { return jps_id::none(); } - - uint32_t x, y; - uint32_t rx, ry; - map_->to_unpadded_xy(mapid, x, y); - ry = x; - rx = map_->header_height() - y - 1; - return jps_id(rmap_->to_padded_id_from_unpadded(rx, ry)); - } - - // convert rmap indexes to map indexes - inline jps_id - rmap_id_to_map_id(jps_id rmapid) - { - if(rmapid.is_none()) { return jps_id::none(); } - - uint32_t x, y; - uint32_t rx, ry; - rmap_->to_unpadded_xy(rmapid, rx, ry); - x = ry; - y = rmap_->header_width() - rx - 1; - return jps_id(map_->to_padded_id_from_unpadded(x, y)); - } - - warthog::domain::gridmap* - create_rmap(); - - warthog::domain::gridmap* map_; - warthog::domain::gridmap* rmap_; - // uint32_t jumplimit_; - - jps_id current_goal_id_; - jps_id current_rgoal_id_; - jps_id current_node_id_; - jps_id current_rnode_id_; - - // these function pointers allow us to switch between forward jumping - // and backward jumping (i.e. with the parent direction reversed) - void (online_jump_point_locator2::*jump_east_fp)( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); - - void (online_jump_point_locator2::*jump_west_fp)( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap); -}; - -} // namespace jps::jump - -#endif // JPS_JUMP_ONLINE_JUMP_POINT_LOCATOR2_H diff --git a/include/jps/search/jps.h b/include/jps/search/jps.h index e0f7590..2ac7c2d 100644 --- a/include/jps/search/jps.h +++ b/include/jps/search/jps.h @@ -29,7 +29,7 @@ namespace jps::search // // @return one of direction::{N, S, E, W} inline direction -from_direction(jps_id n1_id_p, jps_id n2_id_p, uint32_t map_width_p) +from_direction(grid_id n1_id_p, grid_id n2_id_p, uint32_t map_width_p) { if(uint32_t{n1_id_p} == warthog::domain::GRID_ID_MAX) { return NONE; } @@ -81,7 +81,7 @@ from_direction(jps_id n1_id_p, jps_id n2_id_p, uint32_t map_width_p) //} inline direction -from_direction_4c(jps_id n1_xy_id, jps_id n2_xy_id, uint32_t mapwidth) +from_direction_4c(grid_id n1_xy_id, grid_id n2_xy_id, uint32_t mapwidth) { if(n1_xy_id.is_none()) { return NONE; } @@ -191,29 +191,6 @@ compute_successors_4c(direction d, uint32_t tiles) return retval; } -#if 0 -// creates a warthog::graph::xy_graph which contains only -// nodes that are jump points and edges which represent valid jumps, -// from one jump point to another. -// -// @param gm: the input grid -// @param id_map: a key/value set that maps the grid id of -// of each jump points to a corresponding id in the graph (optional) -// -// -// @return the jump point graph -warthog::graph::xy_graph* -create_jump_point_graph(warthog::domain::gridmap* gm); -#endif - -// given an input grid, create a new map where every (x, y) location -// is labeled as a corner point or not. -// -// @param: gm; the input grid -// @return the corner gridmap -warthog::domain::gridmap* -create_corner_map(warthog::domain::gridmap* gm); - } // namespace jps::search #endif // JPS_SEARCH_JPS_H diff --git a/include/jps/search/jps2_expansion_policy.h b/include/jps/search/jps2_expansion_policy.h deleted file mode 100644 index ac70afb..0000000 --- a/include/jps/search/jps2_expansion_policy.h +++ /dev/null @@ -1,66 +0,0 @@ -#ifndef JPS_SEARCH_JPS2_EXPANSION_POLICY_H -#define JPS_SEARCH_JPS2_EXPANSION_POLICY_H - -// jps2_expansion_policy.h -// -// An experimental variation of warthog::jps2_expansion_policy, -// this version works with a modified version of online JPS -// which skips intermediate jump points (i.e. those jps -// that do not have any forced neighbours) -// -// @author: dharabor -// @created: 06/01/2010 - -#include "jps.h" -#include -#include -#include -#include -#include -#include - -namespace jps::search -{ - -class jps2_expansion_policy - : public warthog::search::gridmap_expansion_policy_base -{ -public: - jps2_expansion_policy(warthog::domain::gridmap* map); - virtual ~jps2_expansion_policy(); - - void - expand( - warthog::search::search_node*, - warthog::search::search_problem_instance*) override; - - size_t - mem() override - { - return expansion_policy::mem() + sizeof(*this) + map_->mem() - + jpl_->mem(); - } - - warthog::search::search_node* - generate_start_node(warthog::search::search_problem_instance* pi) override; - - warthog::search::search_node* - generate_target_node( - warthog::search::search_problem_instance* pi) override; - - // this function gets called whenever a successor node is relaxed. at that - // point we set the node currently being expanded (==current) as the - // parent of n and label node n with the direction of travel, - // from current to n - // void - // update_parent_direction(warthog::search::search_node* n); - -private: - jump::online_jump_point_locator2* jpl_; - vec_jps_id jp_ids_; - vec_jps_cost jp_costs_; -}; - -} - -#endif // JPS_SEARCH_JPS2_EXPANSION_POLICY_H diff --git a/include/jps/search/jps2plus_expansion_policy.h b/include/jps/search/jps2plus_expansion_policy.h deleted file mode 100644 index 4058812..0000000 --- a/include/jps/search/jps2plus_expansion_policy.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef JPS_SEARCH_JPS2PLUS_EXPANSION_POLICY_H -#define JPS_SEARCH_JPS2PLUS_EXPANSION_POLICY_H - -// jps2plus_expansion_policy.h -// -// An experimental variation of warthog::jps_expansion_policy, -// this version is designed for efficient offline jps. -// -// @author: dharabor -// @created: 06/01/2010 - -#include "jps.h" -#include -#include -#include -#include -#include -#include - -#include - -namespace jps::search -{ - -class jps2plus_expansion_policy - : public warthog::search::gridmap_expansion_policy_base -{ -public: - jps2plus_expansion_policy(warthog::domain::gridmap* map); - virtual ~jps2plus_expansion_policy(); - - void - expand( - warthog::search::search_node*, - warthog::search::search_problem_instance*) override; - - size_t - mem() override - { - return expansion_policy::mem() + sizeof(*this) + map_->mem() - + jpl_->mem(); - } - - warthog::search::search_node* - generate_start_node(warthog::search::search_problem_instance* pi) override; - - warthog::search::search_node* - generate_target_node( - warthog::search::search_problem_instance* pi) override; - -private: - jump::offline_jump_point_locator2* jpl_; - vec_jps_cost costs_; - vec_jps_id jp_ids_; -}; - -} - -#endif // JPS_SEARCH_JPS2PLUS_EXPANSION_POLICY_H diff --git a/include/jps/search/jps4c_expansion_policy.h b/include/jps/search/jps4c_expansion_policy.h deleted file mode 100644 index ebcaf94..0000000 --- a/include/jps/search/jps4c_expansion_policy.h +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef JPS_SEARCH_JPS4C_EXPANSION_POLICY_H -#define JPS_SEARCH_JPS4C_EXPANSION_POLICY_H - -// jps/jps4c_expansion_policy.h -// -// Successor generating functions for Jump Point Search on 4-connected gridmaps -// -// @author: dharabor -// @created: 2019-11-13 -// - -#include "jps.h" -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace jps::search -{ - -class jps4c_expansion_policy - : public warthog::search::gridmap_expansion_policy_base -{ -public: - jps4c_expansion_policy(warthog::domain::gridmap* map); - virtual ~jps4c_expansion_policy(); - - void - expand( - warthog::search::search_node*, - warthog::search::search_problem_instance*) override; - - warthog::search::search_node* - generate_start_node(warthog::search::search_problem_instance* pi) override; - - warthog::search::search_node* - generate_target_node( - warthog::search::search_problem_instance* pi) override; - - size_t - mem() override - { - return expansion_policy::mem() + sizeof(*this) + map_->mem() - + jpl_->mem(); - } - -private: - jump::four_connected_jps_locator* jpl_; -}; - -} - -#endif // JPS_SEARCH_JPS4C_EXPANSION_POLICY_H diff --git a/include/jps/search/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index cede0ca..39714bf 100644 --- a/include/jps/search/jps_expansion_policy.h +++ b/include/jps/search/jps_expansion_policy.h @@ -1,6 +1,7 @@ #ifndef JPS_SEARCH_JPS_EXPANSION_POLICY_H #define JPS_SEARCH_JPS_EXPANSION_POLICY_H +// // jps_expansion_policy.h // // This expansion policy reduces the branching factor @@ -8,38 +9,58 @@ // could be reached by an equivalent (or shorter) path that visits // the parent of n but not n itself. // -// An extension of this idea is to generate jump nodes located in the -// same direction as the remaining neighbours. +// Used with jump_point_online gives JPS (B) algorithm. +// Used with jump_point_offline gives JPS+ (B) algorithm. // // Theoretical details: // [Harabor D. and Grastien A., 2011, Online Node Pruning for Pathfinding // On Grid Maps, AAAI] // -// @author: dharabor +// @author: dharabor & Ryan Hechenberger // @created: 06/01/2010 +// -#include "jps.h" -#include -#include +#include "jps_expansion_policy_base.h" +#include #include -#include -#include -#include +#include namespace jps::search { -class jps_expansion_policy - : public warthog::search::gridmap_expansion_policy_base +/// @brief generates successor nodes for use in warthog-core search algorithm +/// @tparam JpsJump the jump-point locator to use +/// +/// JPS expansion policy that pushes the first cardinal and first intercardinal +/// jump points for all taut directions. +/// The given JpsJump expects a class from jps::jump namespace, +/// such as jump_point_online for online JPS (B), or jump_point_offline<> for +/// offline JPS+ (B). +template +class jps_expansion_policy : public jps_expansion_policy_base { public: - jps_expansion_policy(warthog::domain::gridmap* map); - virtual ~jps_expansion_policy(); + /// @brief sets the policy to use with map + /// @param map point to gridmap, if null map will need to be set later; + /// otherwise sets map and creates a rotated gridmap. + /// Use set_map to provide a map at a later stage. + jps_expansion_policy(warthog::domain::gridmap* map) + : jps_expansion_policy_base(map) + { + if(map != nullptr) + { + jpl_.set_map(rmap_); + map_width_ = rmap_.map().width(); + } + } + ~jps_expansion_policy() = default; + + using jump_point = JpsJump; void expand( - warthog::search::search_node*, - warthog::search::search_problem_instance*) override; + warthog::search::search_node* current, + warthog::search::search_problem_instance* pi) override; warthog::search::search_node* generate_start_node(warthog::search::search_problem_instance* pi) override; @@ -51,14 +72,161 @@ class jps_expansion_policy size_t mem() override { - return expansion_policy::mem() + sizeof(*this) + map_->mem() - + jpl_->mem(); + return jps_expansion_policy_base::mem() + + (sizeof(jps_expansion_policy) + - sizeof(jps_expansion_policy_base)); + } + + jump_point& + get_jump_point() noexcept + { + return jpl_; + } + const jump_point& + get_jump_point() const noexcept + { + return jpl_; + } + +protected: + void + set_rmap_(domain::rotate_gridmap& rmap) override + { + jps_expansion_policy_base::set_rmap_(rmap); + jpl_.set_map(rmap); + map_width_ = rmap.map().width(); } private: - jump::online_jump_point_locator* jpl_; + JpsJump jpl_; + point target_loc_ = {}; + grid_id target_id_ = {}; + uint32_t map_width_ = 0; }; +template +void +jps_expansion_policy::expand( + warthog::search::search_node* current, + warthog::search::search_problem_instance* instance) +{ + reset(); + + // compute the direction of travel used to reach the current node. + const grid_id current_id = grid_id(current->get_id()); + const point loc = rmap_.id_to_point(current_id); + const domain::grid_pair_id pair_id{ + current_id, rmap_.rpoint_to_rid(rmap_.point_to_rpoint(loc))}; + assert( + rmap_.map().get_label(get(pair_id)) + && rmap_.rmap().get_label( + grid_id(get(pair_id)))); // loc must be trav on map + // const jps_rid current_rid = jpl_.id_to_rid(current_id); + // const cost_t current_cost = current->get_g(); + const direction dir_c = from_direction( + grid_id(current->get_parent()), current_id, rmap_.map().width()); + const direction_id target_d + = warthog::grid::point_to_direction_id(loc, target_loc_); + + // get the tiles around the current node c + uint32_t c_tiles; + map_->get_neighbours(current_id, (uint8_t*)&c_tiles); + + // look for jump points in the direction of each natural + // and forced neighbour + uint32_t succ_dirs = compute_successors(dir_c, c_tiles); + if(succ_dirs & static_cast(warthog::grid::to_dir(target_d))) + { + // target in successor direction, check + if(auto target_dist = jpl_.jump_target(pair_id, loc, target_loc_); + target_dist.second >= 0) + { + // target is visible, push + warthog::search::search_node* jp_succ = this->generate(target_id_); + add_neighbour( + jp_succ, + target_dist.first * warthog::DBL_ROOT_TWO + + target_dist.second * warthog::DBL_ONE); + return; // no other successor required + } + } + + // cardinal directions + ::warthog::util::for_each_integer_sequence>([&](auto iv) { + constexpr direction_id di = decltype(iv)::value; + if(succ_dirs & warthog::grid::to_dir(di)) + { + auto jump_result = jpl_.template jump_cardinal_next(pair_id); + if(jump_result > 0) // jump point + { + // successful jump + pad_id node{static_cast( + current_id.id + + warthog::grid::dir_id_adj(di, map_width_) + * jump_result)}; + assert(rmap_.map().get(node)); // successor must be traversable + warthog::search::search_node* jp_succ = this->generate(node); + add_neighbour(jp_succ, jump_result * warthog::DBL_ONE); + } + } + }); + // intercardinal directions + ::warthog::util::for_each_integer_sequence>( + [&](auto iv) { + constexpr direction_id di = decltype(iv)::value; + if(succ_dirs & warthog::grid::to_dir(di)) + { + jump::intercardinal_jump_result res; + auto jump_result = jpl_.template jump_intercardinal_many( + pair_id, &res, 1); + if(jump_result.first > 0) // jump point + { + // successful jump + pad_id node{pad_id(static_cast( + current_id.id + + warthog::grid::dir_id_adj(di, map_width_) + * res.inter))}; + assert(rmap_.map().get( + node)); // successor must be traversable + warthog::search::search_node* jp_succ + = this->generate(node); + add_neighbour(jp_succ, res.inter * warthog::DBL_ROOT_TWO); + } + } + }); } +template +warthog::search::search_node* +jps_expansion_policy::generate_start_node( + warthog::search::search_problem_instance* pi) +{ + uint32_t max_id = map_->width() * map_->height(); + if(static_cast(pi->start_) >= max_id) { return nullptr; } + pad_id padded_id = pad_id(pi->start_); + if(map_->get_label(padded_id) == 0) { return nullptr; } + target_id_ = grid_id(pi->target_); + uint32_t x, y; + rmap_.map().to_padded_xy(target_id_, x, y); + target_loc_.x = static_cast(x); + target_loc_.y = static_cast(y); + return generate(padded_id); +} + +template +warthog::search::search_node* +jps_expansion_policy::generate_target_node( + warthog::search::search_problem_instance* pi) +{ + uint32_t max_id = map_->width() * map_->height(); + if(static_cast(pi->target_) >= max_id) { return nullptr; } + pad_id padded_id = pad_id(pi->target_); + if(map_->get_label(padded_id) == 0) { return nullptr; } + return generate(padded_id); +} + +} // namespace jps::search + #endif // JPS_SEARCH_JPS_EXPANSION_POLICY_H diff --git a/include/jps/search/jps_expansion_policy2.h b/include/jps/search/jps_expansion_policy2.h deleted file mode 100644 index a49becc..0000000 --- a/include/jps/search/jps_expansion_policy2.h +++ /dev/null @@ -1,244 +0,0 @@ -#ifndef JPS_SEARCH_JPS_EXPANSION_POLICY2_H -#define JPS_SEARCH_JPS_EXPANSION_POLICY2_H - -// jps_expansion_policy.h -// -// This expansion policy reduces the branching factor -// of a node n during search by ignoring any neighbours which -// could be reached by an equivalent (or shorter) path that visits -// the parent of n but not n itself. -// -// An extension of this idea is to generate jump nodes located in the -// same direction as the remaining neighbours. -// -// Theoretical details: -// [Harabor D. and Grastien A., 2011, Online Node Pruning for Pathfinding -// On Grid Maps, AAAI] -// -// @author: dharabor -// @created: 06/01/2010 - -#include "jps.h" -#include -#include - -namespace jps::search -{ - -template -class jps_expansion_policy2 - : public warthog::search::gridmap_expansion_policy_base -{ -public: - jps_expansion_policy2(warthog::domain::gridmap* map, bool setup_map = true) - : gridmap_expansion_policy_base(map), jpl_(setup_map ? map : nullptr) - { } - virtual ~jps_expansion_policy2() = default; - - using jump_point = JpsJump; - - static consteval bool - feature_prune_intercardinal() noexcept - { - return jump_point::feature_prune_intercardinal(); - } - static consteval bool - feature_store_cardinal() noexcept - { - return jump_point::feature_store_cardinal(); - } - - void - expand( - warthog::search::search_node*, - warthog::search::search_problem_instance*) override; - - warthog::search::search_node* - generate_start_node(warthog::search::search_problem_instance* pi) override; - - warthog::search::search_node* - generate_target_node( - warthog::search::search_problem_instance* pi) override; - - size_t - mem() override - { - return expansion_policy::mem() + sizeof(*this) + map_->mem() - + jpl_.mem(); - } - - jump_point& - get_jump_point() noexcept - { - return jpl_; - } - const jump_point& - get_jump_point() const noexcept - { - return jpl_; - } - -private: - JpsJump jpl_; -}; - -template -void -jps_expansion_policy2::expand( - warthog::search::search_node* current, - warthog::search::search_problem_instance* instance) -{ - reset(); - - // compute the direction of travel used to reach the current node. - const jps_id current_id = jps_id(current->get_id()); - const jps_rid current_rid = jpl_.id_to_rid(current_id); - // const cost_t current_cost = current->get_g(); - const direction dir_c = from_direction( - jps_id(current->get_parent()), current_id, map_->width()); - - // get the tiles around the current node c - uint32_t c_tiles; - map_->get_neighbours(current_id, (uint8_t*)&c_tiles); - - // look for jump points in the direction of each natural - // and forced neighbour - uint32_t succ_dirs = compute_successors(dir_c, c_tiles); - - // cardinal directions - for(uint32_t i = 0; i < 4; i++) - { - direction d = (direction)(1 << i); - if(succ_dirs & d) - { - auto jump_result = jpl_.jump_cardinal(d, current_id, current_rid); - if(jump_result.first != 0) - { - // successful jump - warthog::search::search_node* jp_succ - = this->generate(jump_result.second); - // if(jp_succ->get_searchid() != search_id) { - // jp_succ->reset(search_id); } - add_neighbour(jp_succ, jump_result.first * warthog::DBL_ONE); - } - } - } - // intercardinal - constexpr size_t buffer_size = feature_prune_intercardinal() ? 1024 - : feature_store_cardinal() ? 4 - : 1; - std::array id_array [[maybe_unused]]; - std::array cost_array [[maybe_unused]]; - for(uint32_t i = 4; i < 8; i++) - { - direction d = (direction)(1 << i); - if(succ_dirs & d) - { - jump::intercardinal_jump_result jump_result{ - current_id, current_rid, 0}; - uint32_t jump_span [[maybe_unused]] = 0; - while(true) - { - // exit conditions: do while - // => feature_prune_intercardinal(): jump_span.dist != 0 # - // if buffer_size is not large enough, do multiple - // iterations - // => otherwise: always break, only single run required - if constexpr( - feature_prune_intercardinal() || feature_store_cardinal()) - { - jump_result = jpl_.jump_intercardinal( - d, jump_result.node, jump_result.rnode, - id_array.data(), cost_array.data(), id_array.size()); - } - else - { - jump_result = jpl_.jump_intercardinal( - d, jump_result.node, jump_result.rnode, nullptr, - nullptr); - } - // add successor from id_array (if any) - if constexpr(feature_prune_intercardinal()) - { - if(jump_result.dist != 0) - { - // successful jump - for(uint32_t j = 0; j < jump_result.dist; ++j) - { - warthog::search::search_node* jp_succ - = this->generate(id_array[j]); - // if(jp_succ->get_searchid() != search_id) { - // jp_succ->reset(search_id); } - add_neighbour( - jp_succ, - jump_span * warthog::DBL_ROOT_TWO - + cost_array[j]); - } - } - } - else if constexpr(feature_store_cardinal()) - { - for(uint32_t j = 0; j < 2; ++j) - { - if(auto expand_id = id_array[j]; !expand_id.is_none()) - { - warthog::search::search_node* jp_succ - = this->generate(expand_id); - // if(jp_succ->get_searchid() != search_id) { - // jp_succ->reset(search_id); } - add_neighbour(jp_succ, cost_array[j]); - } - } - } - // add successor return from function - if constexpr(!feature_prune_intercardinal()) - { - if(jump_result.dist != 0) - { - warthog::search::search_node* jp_succ - = this->generate(jump_result.node); - // if(jp_succ->get_searchid() != search_id) { - // jp_succ->reset(search_id); } - add_neighbour( - jp_succ, jump_result.dist * warthog::DBL_ROOT_TWO); - } - break; // do not loop - } - else - { - if(jump_result.node.is_none()) - break; // exit loop after reaching end - } - } - } - } -} - -template -warthog::search::search_node* -jps_expansion_policy2::generate_start_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - if(static_cast(pi->start_) >= max_id) { return nullptr; } - jps_id padded_id = jps_id(pi->start_); - if(map_->get_label(padded_id) == 0) { return nullptr; } - jpl_.set_goal(jps_id(pi->target_)); - return generate(padded_id); -} - -template -warthog::search::search_node* -jps_expansion_policy2::generate_target_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - if(static_cast(pi->target_) >= max_id) { return nullptr; } - jps_id padded_id = jps_id(pi->target_); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -} - -#endif // JPS_SEARCH_JPS_EXPANSION_POLICY2_H diff --git a/include/jps/search/jps_expansion_policy_base.h b/include/jps/search/jps_expansion_policy_base.h new file mode 100644 index 0000000..f799dff --- /dev/null +++ b/include/jps/search/jps_expansion_policy_base.h @@ -0,0 +1,77 @@ +#ifndef JPS_SEARCH_JPS_EXPANSION_POLICY_BASE_H +#define JPS_SEARCH_JPS_EXPANSION_POLICY_BASE_H + +// +// jps_expansion_policy_base.h +// +// The base expansion policy for jps-base algorithm. +// Adds interface for setting the domain, rotate_gridmap. +// +// +// @author: Ryan Hechenberger +// @created: 06/01/2010 +// + +#include "jps.h" +#include +#include + +namespace jps::search +{ + +class jps_expansion_policy_base + : public warthog::search::gridmap_expansion_policy_base +{ +public: + /// @brief sets the policy to use with map + /// @param map point to gridmap, if null map will need to be set later; + /// otherwise sets map and creates a rotated gridmap. + /// Use set_map to provide a map at a later stage. + jps_expansion_policy_base(warthog::domain::gridmap* map) + : gridmap_expansion_policy_base(map) + { + if(map != nullptr) { rmap_.create_rmap(*map); } + } + + size_t + mem() override + { + return gridmap_expansion_policy_base::mem() + + (sizeof(jps_expansion_policy_base) + - sizeof(gridmap_expansion_policy_base)) + + rmap_.mem(); + } + + /// @brief Sets the map, creating the rotated gridmap from map at current + /// state. + /// @param map the gridmap to use and rotate + void + set_map(warthog::domain::gridmap& map) + { + rmap_.create_rmap(map); + gridmap_expansion_policy_base::set_map(map); + set_rmap_(rmap_); + } + /// @brief Gives a user-proved map and rotated map, the expander does not + /// own either resource. + /// @param rmap the rotated_gridmap pair-point struct + void + set_map(domain::gridmap_rotate_ptr rmap) + { + rmap_.link(rmap); + gridmap_expansion_policy_base::set_map(rmap.map()); + set_rmap_(rmap_); + } + +protected: + virtual void + set_rmap_(domain::rotate_gridmap& rmap) + { } + +protected: + domain::rotate_gridmap rmap_; +}; + +} // namespace jps::search + +#endif // JPS_SEARCH_JPS_EXPANSION_POLICY_BASE_H diff --git a/include/jps/search/jps_prune_expansion_policy.h b/include/jps/search/jps_prune_expansion_policy.h new file mode 100644 index 0000000..2af154f --- /dev/null +++ b/include/jps/search/jps_prune_expansion_policy.h @@ -0,0 +1,339 @@ +#ifndef JPS_SEARCH_JPS_PRUNE_EXPANSION_POLICY_H +#define JPS_SEARCH_JPS_PRUNE_EXPANSION_POLICY_H + +// +// jps_prune_expansion_policy.h +// +// This expansion policy reduces the branching factor +// of a node n during search by ignoring any neighbours which +// could be reached by an equivalent (or shorter) path that visits +// the parent of n but not n itself. +// It extends jps_expansion_policy by pruning the intercardinal nodes +// (NE/NW/SE/SE) by expanding these nodes in-place and pushing their +// cardinal successors. +// +// Used with jump_point_online gives JPS (B+P) algorithm. +// Used with jump_point_offline gives JPS+ (B+P) algorithm. +// +// Theoretical details: +// [Harabor D. and Grastien A., 2011, Online Node Pruning for Pathfinding +// On Grid Maps, AAAI] +// +// @author: dharabor & Ryan Hechenberger +// @created: 06/01/2010 +// + +#include "jps_expansion_policy_base.h" +#include +#include +#include + +namespace jps::search +{ + +/// @brief generates successor nodes for use in warthog-core search algorithm, +/// pruning intercardinals +/// @tparam JpsJump the jump-point locator to use +/// @tparam InterLimit max distance the intercardinal can expand to, =0 for +/// run-time set, -1 for no limit. Will push nodes first then check if +/// limit is reached, thus jump_point_offline may exceed this greatly. +/// @tparam InterSize the max amount of intercardinal successors. +/// This is where results to the JpsJump are stored, which are placed +/// on the stack. If this limit is reached, then that intercardinal +/// will be a successor and successors in that direction will cease. +/// The max successors will not exceed to 2*max(W,H). +/// +/// JPS expansion policy that pushes all jump points that have a direct +/// line-of-sight to the expanding node following its intercardinal first, then +/// cardinal. It essentially expands the discovered intercardinal successor of +/// JPS without adding those intercardinal nodes to the queue. +/// +/// The given JpsJump expects a class from jps::jump namespace, +/// such as jump_point_online for online JPS (B+P), or jump_point_offline<> for +/// offline JPS+ (B+P). +template +class jps_prune_expansion_policy : public jps_expansion_policy_base +{ + static_assert(InterSize >= 1, "InterSize must be at least 2."); + +public: + /// @brief sets the policy to use with map + /// @param map point to gridmap, if null map will need to be set later; + /// otherwise sets map and creates a rotated gridmap. + /// Use set_map to provide a map at a later stage. + jps_prune_expansion_policy(warthog::domain::gridmap* map) + : jps_expansion_policy_base(map) + { + if(map != nullptr) + { + jpl_.set_map(rmap_); + map_width_ = rmap_.map().width(); + } + } + using jps_expansion_policy_base::jps_expansion_policy_base; + ~jps_prune_expansion_policy() = default; + + using jump_point = JpsJump; + + void + expand( + warthog::search::search_node* current, + warthog::search::search_problem_instance* pi) override; + + warthog::search::search_node* + generate_start_node(warthog::search::search_problem_instance* pi) override; + + warthog::search::search_node* + generate_target_node( + warthog::search::search_problem_instance* pi) override; + + size_t + mem() override + { + return jps_expansion_policy_base::mem() + + (sizeof(jps_prune_expansion_policy) + - sizeof(jps_expansion_policy_base)); + } + + jump_point& + get_jump_point() noexcept + { + return jpl_; + } + const jump_point& + get_jump_point() const noexcept + { + return jpl_; + } + + bool + set_jump_limit( + jump::jump_distance limit + = std::numeric_limits::max()) noexcept + requires(InterLimit == 0) + { + if(limit < 1) return false; + jump_limit_ = limit; + } + jump::jump_distance + get_jump_limit() const noexcept + requires(InterLimit == 0) + { + return jump_limit_; + } + static constexpr jump::jump_distance + get_jump_limit() noexcept + requires(InterLimit != 0) + { + return InterLimit < 0 ? std::numeric_limits::max() + : static_cast(InterLimit); + } + +protected: + void + set_rmap_(domain::rotate_gridmap& rmap) override + { + jps_expansion_policy_base::set_rmap_(rmap); + jpl_.set_map(rmap); + map_width_ = rmap.map().width(); + } + +private: + JpsJump jpl_; + point target_loc_ = {}; + grid_id target_id_ = {}; + uint32_t map_width_ = 0; + jump::jump_distance jump_limit_ + = std::numeric_limits::max(); +}; + +template +void +jps_prune_expansion_policy::expand( + warthog::search::search_node* current, + warthog::search::search_problem_instance* instance) +{ + reset(); + + // compute the direction of travel used to reach the current node. + const grid_id current_id = grid_id(current->get_id()); + point loc = rmap_.id_to_point(current_id); + domain::grid_pair_id pair_id{ + current_id, rmap_.rpoint_to_rid(rmap_.point_to_rpoint(loc))}; + assert( + rmap_.map().get_label(get(pair_id)) + && rmap_.rmap().get_label( + grid_id(get(pair_id)))); // loc must be trav on map + // const jps_rid current_rid = jpl_.id_to_rid(current_id); + // const cost_t current_cost = current->get_g(); + const direction dir_c = from_direction( + grid_id(current->get_parent()), current_id, rmap_.map().width()); + const direction_id target_d + = warthog::grid::point_to_direction_id(loc, target_loc_); + + // get the tiles around the current node c + uint32_t c_tiles; + map_->get_neighbours(current_id, (uint8_t*)&c_tiles); + + // look for jump points in the direction of each natural + // and forced neighbour + uint32_t succ_dirs = compute_successors(dir_c, c_tiles); + if(succ_dirs & static_cast(warthog::grid::to_dir(target_d))) + { + // target in successor direction, check + if(auto target_dist = jpl_.jump_target(pair_id, loc, target_loc_); + target_dist.second >= 0) + { + // target is visible, push + warthog::search::search_node* jp_succ = this->generate(target_id_); + add_neighbour( + jp_succ, + target_dist.first * warthog::DBL_ROOT_TWO + + target_dist.second * warthog::DBL_ONE); + return; // no other successor required + } + } + + // cardinal directions + ::warthog::util::for_each_integer_sequence>([&](auto iv) { + constexpr direction_id di = decltype(iv)::value; + if(succ_dirs & warthog::grid::to_dir(di)) + { + auto jump_result = jpl_.template jump_cardinal_next(pair_id); + if(jump_result > 0) // jump point + { + // successful jump + pad_id node{static_cast( + current_id.id + + warthog::grid::dir_id_adj(di, map_width_) + * jump_result)}; + assert(rmap_.map().get(node)); // successor must be traversable + warthog::search::search_node* jp_succ = this->generate(node); + add_neighbour(jp_succ, jump_result * warthog::DBL_ONE); + } + } + }); + // intercardinal directions + ::warthog::util::for_each_integer_sequence>( + [&](auto iv) { + constexpr direction_id di = decltype(iv)::value; + if(succ_dirs & warthog::grid::to_dir(di)) + { + const int32_t node_adj_ic + = warthog::grid::dir_id_adj(di, map_width_); + const int32_t node_adj_vert + = warthog::grid::dir_id_adj_vert(di, map_width_); + constexpr int32_t node_adj_hori + = warthog::grid::dir_id_adj_hori(di); + jump::intercardinal_jump_result res[InterSize]; + jump::jump_distance inter_total = 0; + while(true) + { + auto [result_n, dist] + = jpl_.template jump_intercardinal_many( + pair_id, res, InterSize, get_jump_limit()); + for(decltype(result_n) result_i = 0; result_i < result_n; + ++result_i) // jump point + { + // successful jump + const jump::intercardinal_jump_result res_i + = res[result_i]; + assert(res_i.inter > 0); + const uint32_t node + = current_id.id + + static_cast( + node_adj_ic * (inter_total + res_i.inter)); + const auto cost = warthog::DBL_ROOT_TWO + * (inter_total + res_i.inter); + assert(rmap_.map().get( + pad_id{node})); // successor must be traversable + if(res_i.hori > 0) + { + // horizontal + const uint32_t node_j = node + + static_cast(node_adj_hori + * res_i.hori); + const auto cost_j + = cost + warthog::DBL_ONE * res_i.hori; + assert(rmap_.map().get(pad_id{ + node_j})); // successor must be traversable + warthog::search::search_node* jp_succ + = this->generate(pad_id{node_j}); + add_neighbour(jp_succ, cost_j); + } + if(res_i.vert > 0) + { + // horizontal + const uint32_t node_j = node + + static_cast(node_adj_vert + * res_i.vert); + const auto cost_j + = cost + warthog::DBL_ONE * res_i.vert; + assert(rmap_.map().get(pad_id{ + node_j})); // successor must be traversable + warthog::search::search_node* jp_succ + = this->generate(pad_id{node_j}); + add_neighbour(jp_succ, cost_j); + } + } + if(dist <= 0) // hit wall, break + break; + if constexpr(InterLimit < 0) + { + // repeat until all jump points are discovered + inter_total += dist; + loc = loc + dist * dir_unit_point(di); + pair_id = rmap_.point_to_pair_id(loc); + } + else + { + // reach limit, push dia onto queue + const uint32_t node = current_id.id + + static_cast(node_adj_ic * dist); + const auto cost = warthog::DBL_ROOT_TWO * dist; + assert(rmap_.map().get( + pad_id{node})); // successor must be traversable + warthog::search::search_node* jp_succ + = this->generate(pad_id{node}); + add_neighbour(jp_succ, cost); + break; + } + } + } + }); +} + +template +warthog::search::search_node* +jps_prune_expansion_policy:: + generate_start_node(warthog::search::search_problem_instance* pi) +{ + uint32_t max_id = map_->width() * map_->height(); + if(static_cast(pi->start_) >= max_id) { return nullptr; } + pad_id padded_id = pad_id(pi->start_); + if(map_->get_label(padded_id) == 0) { return nullptr; } + target_id_ = grid_id(pi->target_); + uint32_t x, y; + rmap_.map().to_padded_xy(target_id_, x, y); + target_loc_.x = static_cast(x); + target_loc_.y = static_cast(y); + return generate(padded_id); +} + +template +warthog::search::search_node* +jps_prune_expansion_policy:: + generate_target_node(warthog::search::search_problem_instance* pi) +{ + uint32_t max_id = map_->width() * map_->height(); + if(static_cast(pi->target_) >= max_id) { return nullptr; } + pad_id padded_id = pad_id(pi->target_); + if(map_->get_label(padded_id) == 0) { return nullptr; } + return generate(padded_id); +} + +} // namespace jps::search + +#endif // JPS_SEARCH_JPS_PRUNE_EXPANSION_POLICY_H diff --git a/include/jps/search/jpsplus_expansion_policy.h b/include/jps/search/jpsplus_expansion_policy.h deleted file mode 100644 index 512ed0e..0000000 --- a/include/jps/search/jpsplus_expansion_policy.h +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef JPS_SEARCH_JPSPLUS_EXPANSION_POLICY_H -#define JPS_SEARCH_JPSPLUS_EXPANSION_POLICY_H - -// jpsplus_expansion_policy.h -// -// JPS+ is Jump Point Search together with a preprocessed database -// that stores all jump points for every node. -// -// Theoretical details: -// [Harabor and Grastien, 2012, The JPS Pathfinding System, SoCS] -// -// @author: dharabor -// @created: 05/05/2012 - -#include "jps.h" -#include -#include -#include -#include -#include -#include - -#include - -namespace jps::search -{ - -class jpsplus_expansion_policy - : public warthog::search::gridmap_expansion_policy_base -{ -public: - jpsplus_expansion_policy(warthog::domain::gridmap* map); - virtual ~jpsplus_expansion_policy(); - - void - expand( - warthog::search::search_node*, - warthog::search::search_problem_instance*) override; - - size_t - mem() override - { - return expansion_policy::mem() + sizeof(*this) + map_->mem() - + jpl_->mem(); - } - - warthog::search::search_node* - generate_start_node(warthog::search::search_problem_instance* pi) override; - - warthog::search::search_node* - generate_target_node( - warthog::search::search_problem_instance* pi) override; - -private: - jump::offline_jump_point_locator* jpl_; -}; - -} - -#endif // JPS_SEARCH_JPSPLUS_EXPANSION_POLICY_H diff --git a/src/jump/CMakeLists.txt b/src/jump/CMakeLists.txt index 0949bc2..1556217 100644 --- a/src/jump/CMakeLists.txt +++ b/src/jump/CMakeLists.txt @@ -1,10 +1,5 @@ cmake_minimum_required(VERSION 3.13) # ( cd src/jump && echo *.cpp | sort ) -target_sources(warthog_libjps PRIVATE -four_connected_jps_locator.cpp -offline_jump_point_locator.cpp -offline_jump_point_locator2.cpp -online_jump_point_locator.cpp -online_jump_point_locator2.cpp -) +# target_sources(warthog_libjps PRIVATE +# ) diff --git a/src/jump/four_connected_jps_locator.cpp b/src/jump/four_connected_jps_locator.cpp deleted file mode 100644 index 969b01a..0000000 --- a/src/jump/four_connected_jps_locator.cpp +++ /dev/null @@ -1,240 +0,0 @@ -#include -#include - -#include -#include - -namespace jps::jump -{ - -four_connected_jps_locator::four_connected_jps_locator( - warthog::domain::gridmap* map) - : map_(map) //, jumplimit_(UINT32_MAX) -{ } - -four_connected_jps_locator::~four_connected_jps_locator() { } - -// Finds a jump point successor of node (x, y) in Direction d. -// Also given is the location of the goal node (goalx, goaly) for a particular -// search instance. If encountered, the goal node is always returned as a -// jump point successor. -// -// @return: the id of a jump point successor or INF32 if no jp exists. -void -four_connected_jps_locator::jump( - jps::direction d, jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - double& jumpcost) -{ - switch(d) - { - case jps::NORTH: - jump_north(node_id, goal_id, jumpnode_id, jumpcost); - break; - case jps::SOUTH: - jump_south(node_id, goal_id, jumpnode_id, jumpcost); - break; - case jps::EAST: - jump_east(node_id, goal_id, jumpnode_id, jumpcost); - break; - case jps::WEST: - jump_west(node_id, goal_id, jumpnode_id, jumpcost); - break; - default: - break; - } -} - -void -four_connected_jps_locator::jump_north( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint32_t num_steps = 0; - uint32_t mapw = map_->width(); - - jps_id jp_w_id; - jps_id jp_e_id; - double jp_w_cost; - double jp_e_cost; - - jps_id next_id = node_id; - while(true) - { - next_id.id -= mapw; - num_steps++; - - // verify the next location is traversable - if(!map_->get_label(next_id)) - { - next_id = jps_id::none(); - break; - } - - jump_east(next_id, goal_id, jp_e_id, jp_e_cost); - if(!jp_e_id.is_none()) { break; } - jump_west(next_id, goal_id, jp_w_id, jp_w_cost); - if(!jp_w_id.is_none()) { break; } - } - - jumpnode_id = next_id; - jumpcost = num_steps; - - // adjust num_steps if we stopped due to a deadend - // (we return the distance to the last traversable tile) - num_steps = !next_id.is_none() ? num_steps : num_steps - 1; -} - -void -four_connected_jps_locator::jump_south( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint32_t num_steps = 0; - uint32_t mapw = map_->width(); - - jps_id jp_w_id; - jps_id jp_e_id; - double jp_w_cost; - double jp_e_cost; - - jps_id next_id = node_id; - while(true) - { - next_id.id += mapw; - num_steps++; - - // verify the next location is traversable - if(!map_->get_label(next_id)) - { - next_id = jps_id::none(); - break; - } - - jump_east(next_id, goal_id, jp_e_id, jp_e_cost); - if(!jp_e_id.is_none()) { break; } - jump_west(next_id, goal_id, jp_w_id, jp_w_cost); - if(!jp_w_id.is_none()) { break; } - } - jumpnode_id = next_id; - jumpcost = num_steps; - - // adjust num_steps if we stopped due to a deadend - // (we return the distance to the last traversable tile) - num_steps = !next_id.is_none() ? num_steps : num_steps - 1; -} - -void -four_connected_jps_locator::jump_east( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - jumpnode_id = node_id; - - uint32_t neis[3] = {0, 0, 0}; - bool deadend = false; - - jumpnode_id = node_id; - while(true) - { - // read in tiles from 3 adjacent rows. the curent node - // is in the low byte of the middle row - map_->get_neighbours_32bit(jumpnode_id, neis); - - // identify forced neighbours and deadend tiles. - // forced neighbours are found in the top or bottom row. they - // can be identified as a non-obstacle tile that follows - // immediately after an obstacle tile. A dead-end tile is - // an obstacle found on the middle row; - uint32_t forced_bits = (~neis[0] << 1) & neis[0]; - forced_bits |= (~neis[2] << 1) & neis[2]; - uint32_t deadend_bits = ~neis[1]; - - // stop if we found any forced or dead-end tiles - int32_t stop_bits = (int32_t)(forced_bits | deadend_bits); - if(stop_bits) - { - // TODO: remove builtin - uint32_t stop_pos = __builtin_ffs(stop_bits) - 1; // returns idx+1 - jumpnode_id.id += stop_pos; - deadend = deadend_bits & (1 << stop_pos); - break; - } - - // jump to the last position in the cache. we do not jump past the end - // in case the last tile from the row above or below is an obstacle. - // Such a tile, followed by a non-obstacle tile, would yield a forced - // neighbour that we don't want to miss. - jumpnode_id.id += 31; - } - - uint32_t num_steps = uint32_t{jumpnode_id} - uint32_t{node_id}; - uint32_t goal_dist = uint32_t{goal_id} - uint32_t{node_id}; - if(num_steps > goal_dist) - { - jumpnode_id = goal_id; - jumpcost = goal_dist; - return; - } - - if(deadend) - { - // number of steps to reach the deadend tile is not - // correct here since we just inverted neis[1] and then - // looked for the first set bit. need -1 to fix it. - num_steps -= (1 && num_steps); - jumpnode_id = jps_id::none(); - } - jumpcost = num_steps; -} - -void -four_connected_jps_locator::jump_west( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - bool deadend = false; - uint32_t neis[3] = {0, 0, 0}; - - jumpnode_id = node_id; - while(true) - { - // cache 32 tiles from three adjacent rows. - // current tile is in the high byte of the middle row - map_->get_neighbours_upper_32bit(jumpnode_id, neis); - - // identify forced and dead-end nodes - uint32_t forced_bits = (~neis[0] >> 1) & neis[0]; - forced_bits |= (~neis[2] >> 1) & neis[2]; - uint32_t deadend_bits = ~neis[1]; - - // stop if we encounter any forced or deadend nodes - uint32_t stop_bits = (forced_bits | deadend_bits); - if(stop_bits) - { - uint32_t stop_pos = (uint32_t)__builtin_clz(stop_bits); - jumpnode_id.id -= stop_pos; - deadend = deadend_bits & (0x80000000 >> stop_pos); - break; - } - // jump to the end of cache. jumping +32 involves checking - // for forced neis between adjacent sets of contiguous tiles - jumpnode_id.id -= 31; - } - - uint32_t num_steps = uint32_t{node_id} - uint32_t{jumpnode_id}; - uint32_t goal_dist = uint32_t{node_id} - uint32_t{goal_id}; - if(num_steps > goal_dist) - { - jumpnode_id = goal_id; - jumpcost = goal_dist; - return; - } - - if(deadend) - { - // number of steps to reach the deadend tile is not - // correct here since we just inverted neis[1] and then - // counted leading zeroes. need -1 to fix it. - num_steps -= (1 && num_steps); - jumpnode_id = jps_id::none(); - } - jumpcost = num_steps; -} - -} // namespace jps::jump diff --git a/src/jump/offline_jump_point_locator.cpp b/src/jump/offline_jump_point_locator.cpp deleted file mode 100644 index d93a8c9..0000000 --- a/src/jump/offline_jump_point_locator.cpp +++ /dev/null @@ -1,488 +0,0 @@ -#include -#include -#include - -#include -#include -#include - -namespace jps::jump -{ - -offline_jump_point_locator::offline_jump_point_locator( - warthog::domain::gridmap* map) - : map_(map) -{ - preproc(); -} - -offline_jump_point_locator::~offline_jump_point_locator() -{ - delete[] db_; -} - -void -offline_jump_point_locator::preproc() -{ - if(load(map_->filename())) { return; } - - dbsize_ = 8 * map_->padded_mapsize(); - db_ = new uint16_t[dbsize_]; - for(uint32_t i = 0; i < dbsize_; i++) - db_[i] = 0; - - online_jump_point_locator jpl(map_); - for(uint32_t y = 0; y < map_->header_height(); y++) - { - for(uint32_t x = 0; x < map_->header_width(); x++) - { - jps_id mapid = jps_id{map_->to_padded_id_from_unpadded(x, y)}; - // std::cout << mapid << " "; - for(uint32_t i = 0; i < 8; i++) - { - direction dir = (direction)(1 << i); - // std::cout << dir << ": - //"; - jps_id jumpnode_id; - double jumpcost; - jpl.jump(dir, mapid, jps_id::none(), jumpnode_id, jumpcost); - - // convert from cost to number of steps - if(dir > 8) - { - jumpcost = jumpcost * warthog::DBL_ONE_OVER_ROOT_TWO; - } - uint32_t num_steps = (uint16_t)floor((jumpcost + 0.5)); - // std::cout << (jumpnode_id == INF ? 0 : - // num_steps) << " "; - - // set the leading bit if the jump leads to a dead-end - if(jumpnode_id.is_none()) { db_[mapid.id * 8 + i] |= 32768; } - - // truncate jump cost so we can fit the label into a single - // byte - // if(num_steps > 32767) - //{ - // num_steps = 32767; - // jumpnode_id = 0; - //} - - db_[mapid.id * 8 + i] |= num_steps; - - if(num_steps > 32768) - { - std::cerr << "label overflow; maximum jump distance " - "exceeded. aborting\n"; - exit(1); - } - } - // std::cout << std::endl; - } - } - - save(map_->filename()); -} - -bool -offline_jump_point_locator::load(const char* filename) -{ - char fname[256]; - strcpy(fname, filename); - strcat(fname, ".jps+"); - FILE* f = fopen(fname, "rb"); - std::cerr << "loading " << fname << "... "; - if(f == NULL) - { - std::cerr << "no dice. oh well. keep going.\n" << std::endl; - return false; - } - - fread(&dbsize_, sizeof(dbsize_), 1, f); - std::cerr << "#labels=" << dbsize_ << std::endl; - - db_ = new uint16_t[dbsize_]; - fread(db_, sizeof(uint16_t), dbsize_, f); - fclose(f); - return true; -} - -void -offline_jump_point_locator::save(const char* filename) -{ - char fname[256]; - strcpy(fname, filename); - strcat(fname, ".jps+"); - std::cerr << "saving to file " << fname << "; nodes=" << dbsize_ - << " size: " << sizeof(db_[0]) << std::endl; - - FILE* f = fopen(fname, "wb"); - if(f == NULL) - { - std::cerr << "err; cannot write jump-point graph to file " << fname - << ". oh well. try to keep going.\n" - << std::endl; - return; - } - - fwrite(&dbsize_, sizeof(dbsize_), 1, f); - fwrite(db_, sizeof(*db_), dbsize_, f); - fclose(f); - std::cerr << "jump-point graph saved to disk. file=" << fname << std::endl; -} - -void -offline_jump_point_locator::jump( - direction d, jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - double& jumpcost) -{ - current_ = max_ = 0; - switch(d) - { - case NORTH: - jump_north(node_id, goal_id, jumpnode_id, jumpcost); - break; - case SOUTH: - jump_south(node_id, goal_id, jumpnode_id, jumpcost); - break; - case EAST: - jump_east(node_id, goal_id, jumpnode_id, jumpcost); - break; - case WEST: - jump_west(node_id, goal_id, jumpnode_id, jumpcost); - break; - case NORTHEAST: - jump_northeast(node_id, goal_id, jumpnode_id, jumpcost); - break; - case NORTHWEST: - jump_northwest(node_id, goal_id, jumpnode_id, jumpcost); - break; - case SOUTHEAST: - jump_southeast(node_id, goal_id, jumpnode_id, jumpcost); - break; - case SOUTHWEST: - jump_southwest(node_id, goal_id, jumpnode_id, jumpcost); - break; - default: - break; - } -} - -void -offline_jump_point_locator::jump_northwest( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint32_t mapw = map_->width(); - uint16_t label = db_[8 * node_id.id + 5]; - uint16_t num_steps = label & 32767; - - // goal test (so many div ops! and branches! how ugly!) - uint32_t id_delta = (mapw + 1) * num_steps; - if(node_id.id - goal_id.id - < map_->padded_mapsize()) // heading toward the goal? - { - uint32_t gx, gy, nx, ny; - map_->to_padded_xy(goal_id, gx, gy); - map_->to_padded_xy(node_id, nx, ny); - - uint32_t ydelta = (ny - gy); - uint32_t xdelta = (nx - gx); - if(xdelta < mapw && ydelta < map_->height()) - { - jumpnode_id = jps_id::none(); - uint32_t nid, steps_to_nid; - if(ydelta < xdelta && ydelta <= num_steps) - { - steps_to_nid = ydelta; - nid = node_id.id - (mapw + 1) * steps_to_nid; - jump_west(jps_id{nid}, goal_id, jumpnode_id, jumpcost); - if(jumpnode_id == goal_id) - { - jumpnode_id = goal_id; - jumpcost = steps_to_nid * warthog::DBL_ROOT_TWO + jumpcost; - return; - } - } - else if(xdelta <= num_steps) - { - steps_to_nid = xdelta; - nid = node_id.id - (mapw + 1) * steps_to_nid; - jump_north(jps_id{nid}, goal_id, jumpnode_id, jumpcost); - if(jumpnode_id == goal_id) - { - jumpnode_id = goal_id; - jumpcost = steps_to_nid * warthog::DBL_ROOT_TWO + jumpcost; - return; - } - } - } - } - - // return the jump point; but only if it isn't sterile - jumpnode_id = jps_id(node_id.id - id_delta); - jumpcost = num_steps * warthog::DBL_ROOT_TWO; - if(label & 32768) { jumpnode_id = jps_id::none(); } -} - -void -offline_jump_point_locator::jump_northeast( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint16_t label = db_[8 * node_id.id + 4]; - uint16_t num_steps = label & 32767; - uint32_t mapw = map_->width(); - - // goal test (so many div ops! and branches! how ugly!) - uint32_t id_delta = (mapw - 1) * num_steps; - if((node_id.id - goal_id.id) < map_->padded_mapsize()) - { - uint32_t gx, gy, nx, ny; - map_->to_padded_xy(goal_id, gx, gy); - map_->to_padded_xy(node_id, nx, ny); - - uint32_t ydelta = (ny - gy); - uint32_t xdelta = (gx - nx); - if(xdelta < mapw && ydelta < map_->height()) - { - jumpnode_id = jps_id::none(); - uint32_t nid, steps_to_nid; - if(ydelta < xdelta && ydelta <= num_steps) - { - steps_to_nid = ydelta; - nid = node_id.id - (mapw - 1) * steps_to_nid; - jump_east(jps_id(nid), goal_id, jumpnode_id, jumpcost); - if(jumpnode_id == goal_id) - { - jumpnode_id = goal_id; - jumpcost = steps_to_nid * warthog::DBL_ROOT_TWO + jumpcost; - return; - } - } - else if(xdelta <= num_steps) - { - steps_to_nid = xdelta; - nid = node_id.id - (mapw - 1) * steps_to_nid; - jump_north(jps_id(nid), goal_id, jumpnode_id, jumpcost); - if(jumpnode_id == goal_id) - { - jumpnode_id = goal_id; - jumpcost = steps_to_nid * warthog::DBL_ROOT_TWO + jumpcost; - return; - } - } - } - } - - // return the jump point; but only if it isn't sterile - jumpnode_id = jps_id(node_id.id - id_delta); - jumpcost = num_steps * warthog::DBL_ROOT_TWO; - if(label & 32768) { jumpnode_id = jps_id::none(); } -} - -void -offline_jump_point_locator::jump_southwest( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint16_t label = db_[8 * node_id.id + 7]; - uint16_t num_steps = label & 32767; - uint32_t mapw = map_->width(); - - // goal test (so many div ops! and branches! how ugly!) - if((goal_id.id - node_id.id) < map_->padded_mapsize()) - { - uint32_t gx, gy, nx, ny; - map_->to_padded_xy(goal_id, gx, gy); - map_->to_padded_xy(node_id, nx, ny); - - uint32_t ydelta = (gy - ny); - uint32_t xdelta = (nx - gx); - if(xdelta < mapw && ydelta < map_->height()) - { - jumpnode_id = jps_id::none(); - uint32_t nid, steps_to_nid; - if(ydelta < xdelta && ydelta <= num_steps) - { - steps_to_nid = ydelta; - nid = node_id.id + (mapw - 1) * steps_to_nid; - jump_west(jps_id(nid), goal_id, jumpnode_id, jumpcost); - if(jumpnode_id == goal_id) - { - jumpnode_id = goal_id; - jumpcost = steps_to_nid * warthog::DBL_ROOT_TWO + jumpcost; - return; - } - } - else if(xdelta <= num_steps) - { - steps_to_nid = xdelta; - nid = node_id.id + (mapw - 1) * steps_to_nid; - jump_south(jps_id(nid), goal_id, jumpnode_id, jumpcost); - if(jumpnode_id == goal_id) - { - jumpnode_id = goal_id; - jumpcost = steps_to_nid * warthog::DBL_ROOT_TWO + jumpcost; - return; - } - } - } - } - - // return the jump point; but only if it isn't sterile - jumpnode_id = jps_id(node_id.id + (mapw - 1) * num_steps); - jumpcost = num_steps * warthog::DBL_ROOT_TWO; - if(label & 32768) { jumpnode_id = jps_id::none(); } -} - -void -offline_jump_point_locator::jump_southeast( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint16_t label = db_[8 * node_id.id + 6]; - uint16_t num_steps = label & 32767; - uint32_t mapw = map_->width(); - - // goal test (so many div ops! and branches! how ugly!) - if((goal_id.id - node_id.id) < map_->padded_mapsize()) - { - uint32_t gx, gy, nx, ny; - map_->to_padded_xy(goal_id, gx, gy); - map_->to_padded_xy(node_id, nx, ny); - - uint32_t ydelta = (gy - ny); - uint32_t xdelta = (gx - nx); - if(xdelta < mapw && ydelta < map_->height()) - { - uint32_t nid, steps_to_nid; - jumpnode_id = jps_id::none(); - if(ydelta < xdelta && ydelta <= num_steps) - { - steps_to_nid = ydelta; - nid = node_id.id + (mapw + 1) * steps_to_nid; - jump_east(jps_id(nid), goal_id, jumpnode_id, jumpcost); - if(jumpnode_id == goal_id) - { - jumpnode_id = goal_id; - jumpcost = steps_to_nid * warthog::DBL_ROOT_TWO + jumpcost; - return; - } - } - else if(xdelta <= num_steps) - { - steps_to_nid = xdelta; - nid = node_id.id + (mapw + 1) * steps_to_nid; - jump_south(jps_id(nid), goal_id, jumpnode_id, jumpcost); - if(jumpnode_id == goal_id) - { - jumpnode_id = goal_id; - jumpcost = steps_to_nid * warthog::DBL_ROOT_TWO + jumpcost; - return; - } - } - } - } - - jumpnode_id = jps_id(node_id.id + (mapw + 1) * num_steps); - jumpcost = num_steps * warthog::DBL_ROOT_TWO; - if(label & 32768) { jumpnode_id = jps_id::none(); } -} - -void -offline_jump_point_locator::jump_north( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint16_t label = db_[8 * node_id.id]; - uint16_t num_steps = label & 32767; - - // do not jump over the goal - uint32_t id_delta = num_steps * map_->width(); - uint32_t goal_delta = node_id.id - goal_id.id; - if(id_delta >= goal_delta) - { - uint32_t gx = goal_id.id % map_->width(); - uint32_t nx = node_id.id % map_->width(); - if(nx == gx) - { - jumpnode_id = goal_id; - jumpcost = (goal_delta / map_->width()); - return; - } - } - - // return the jump point at hand - jumpnode_id = jps_id(node_id.id - id_delta); - jumpcost = num_steps; - if(label & 32768) { jumpnode_id = jps_id::none(); } -} - -void -offline_jump_point_locator::jump_south( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint16_t label = db_[8 * node_id.id + 1]; - uint16_t num_steps = label & 32767; - - // do not jump over the goal - uint32_t id_delta = num_steps * map_->width(); - uint32_t goal_delta = goal_id.id - node_id.id; - if(id_delta >= goal_delta) - { - uint32_t gx = goal_id.id % map_->width(); - uint32_t nx = node_id.id % map_->width(); - if(nx == gx) - { - jumpnode_id = goal_id; - jumpcost = goal_delta / map_->width(); - return; - } - } - - // return the jump point at hand - jumpnode_id = jps_id(node_id.id + id_delta); - jumpcost = num_steps; - if(label & 32768) { jumpnode_id = jps_id::none(); } -} - -void -offline_jump_point_locator::jump_east( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint16_t label = db_[8 * node_id.id + 2]; - - // do not jump over the goal - uint32_t id_delta = label & 32767; - uint32_t goal_delta = goal_id.id - node_id.id; - if(id_delta >= goal_delta) - { - jumpnode_id = goal_id; - jumpcost = goal_delta; - return; - } - - // return the jump point at hand - jumpnode_id = jps_id(node_id.id + id_delta); - jumpcost = id_delta; - if(label & 32768) { jumpnode_id = jps_id::none(); } -} - -void -offline_jump_point_locator::jump_west( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, double& jumpcost) -{ - uint16_t label = db_[8 * node_id.id + 3]; - - // do not jump over the goal - uint32_t id_delta = label & 32767; - uint32_t goal_delta = node_id.id - goal_id.id; - if(id_delta >= goal_delta) - { - jumpnode_id = goal_id; - jumpcost = goal_delta; - return; - } - - // return the jump point at hand - jumpnode_id = jps_id(node_id.id - id_delta); - jumpcost = id_delta; - if(label & 32768) { jumpnode_id = jps_id::none(); } -} - -} // namespace jps::jump diff --git a/src/jump/offline_jump_point_locator2.cpp b/src/jump/offline_jump_point_locator2.cpp deleted file mode 100644 index d5e379d..0000000 --- a/src/jump/offline_jump_point_locator2.cpp +++ /dev/null @@ -1,578 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include - -namespace jps::jump -{ - -offline_jump_point_locator2::offline_jump_point_locator2( - warthog::domain::gridmap* map) - : map_(map) -{ - if(map_->padded_mapsize() > ((1 << 23) - 1)) - { - // search nodes are stored as 32bit quantities. - // bit 0 stores the expansion status - // bits 1-23 store the unique node id - // bits 24-31 store the direction from the parent - std::cerr << "map size too big for this implementation of JPS+." - << " aborting." << std::endl; - exit(1); - } - preproc(); -} - -offline_jump_point_locator2::~offline_jump_point_locator2() -{ - delete[] db_; -} - -void -offline_jump_point_locator2::preproc() -{ - if(load(map_->filename())) { return; } - - dbsize_ = 8 * map_->padded_mapsize(); - db_ = new uint16_t[dbsize_]; - for(uint32_t i = 0; i < dbsize_; i++) - db_[i] = 0; - - online_jump_point_locator jpl(map_); - for(uint32_t y = 0; y < map_->header_height(); y++) - { - for(uint32_t x = 0; x < map_->header_width(); x++) - { - jps_id mapid = jps_id{map_->to_padded_id_from_unpadded(x, y)}; - // std::cout << mapid << " "; - for(uint32_t i = 0; i < 8; i++) - { - direction dir = (direction)(1 << i); - // std::cout << dir << ": - //"; - jps_id jumpnode_id; - double jumpcost; - jpl.jump(dir, mapid, jps_id::none(), jumpnode_id, jumpcost); - - // convert from cost to number of steps - if(dir > 8) - { - jumpcost = jumpcost * warthog::DBL_ONE_OVER_ROOT_TWO; - } - uint32_t num_steps = (uint16_t)floor((jumpcost + 0.5)); - // std::cout << (jumpnode_id == INF ? 0 : - // num_steps) << " "; - - // set the leading bit if the jump leads to a dead-end - if(jumpnode_id.is_none()) { db_[mapid.id * 8 + i] |= 32768; } - - // truncate jump cost so we can fit the label into a single - // byte - // if(num_steps > 32767) - //{ - // num_steps = 32767; - // jumpnode_id = 0; - //} - - db_[mapid.id * 8 + i] |= num_steps; - - if(num_steps > 32768) - { - std::cerr << "label overflow; maximum jump distance " - "exceeded. aborting\n"; - exit(1); - } - } - // std::cout << std::endl; - } - } - - save(map_->filename()); -} - -bool -offline_jump_point_locator2::load(const char* filename) -{ - char fname[256]; - strcpy(fname, filename); - strcat(fname, ".jps+"); - FILE* f = fopen(fname, "rb"); - std::cerr << "loading " << fname << "... "; - if(f == NULL) - { - std::cerr << "no dice. oh well. keep going.\n" << std::endl; - return false; - } - - fread(&dbsize_, sizeof(dbsize_), 1, f); - std::cerr << "#labels=" << dbsize_ << std::endl; - - db_ = new uint16_t[dbsize_]; - fread(db_, sizeof(uint16_t), dbsize_, f); - fclose(f); - return true; -} - -void -offline_jump_point_locator2::save(const char* filename) -{ - char fname[256]; - strcpy(fname, filename); - strcat(fname, ".jps+"); - std::cerr << "saving to file " << fname << "; nodes=" << dbsize_ - << " size: " << sizeof(db_[0]) << std::endl; - - FILE* f = fopen(fname, "wb"); - if(f == NULL) - { - std::cerr << "err; cannot write jump-point graph to file " << fname - << ". oh well. try to keep going.\n" - << std::endl; - return; - } - - fwrite(&dbsize_, sizeof(dbsize_), 1, f); - fwrite(db_, sizeof(*db_), dbsize_, f); - fclose(f); - std::cerr << "jump table saved to disk. file=" << fname << std::endl; -} - -void -offline_jump_point_locator2::jump( - direction d, jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs) -{ - switch(d) - { - case NORTH: - jump_north(node_id, goal_id, 0, neighbours, costs); - break; - case SOUTH: - jump_south(node_id, goal_id, 0, neighbours, costs); - break; - case EAST: - jump_east(node_id, goal_id, 0, neighbours, costs); - break; - case WEST: - jump_west(node_id, goal_id, 0, neighbours, costs); - break; - case NORTHEAST: - jump_northeast(node_id, goal_id, neighbours, costs); - break; - case NORTHWEST: - jump_northwest(node_id, goal_id, neighbours, costs); - break; - case SOUTHEAST: - jump_southeast(node_id, goal_id, neighbours, costs); - break; - case SOUTHWEST: - jump_southwest(node_id, goal_id, neighbours, costs); - break; - default: - break; - } -} - -void -offline_jump_point_locator2::jump_northwest( - jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs) - -{ - uint16_t label = 0; - uint16_t num_steps = 0; - uint32_t mapw = map_->width(); - uint32_t diag_step_delta = (mapw + 1); - - // keep jumping until we hit a dead-end. take note of the - // points reachable by a vertical or horizontal jump from - // each intermediate location that we reach diagonally. - jps_id jump_from = node_id; - - // step diagonally to an intermediate location jump_from - label = db_[8 * jump_from.id + 5]; - num_steps += label & 32767; - jump_from = jps_id(node_id.id - num_steps * diag_step_delta); - while(!(label & 32768)) - { - // north of jump_from - uint16_t label_straight1 = db_[8 * jump_from.id]; - if(!(label_straight1 & 32768)) - { - uint32_t jp_cost = (label_straight1 & 32767); - jps_id jp_id = jps_id(jump_from.id - mapw * jp_cost); - neighbours.push_back(jp_id); - costs.push_back(jp_cost + num_steps * warthog::DBL_ROOT_TWO); - } - // west of jump_from - uint16_t label_straight2 - = db_[8 * jump_from.id + 3]; // west of next jp - if(!(label_straight2 & 32768)) - { - uint32_t jp_cost = (label_straight2 & 32767); - jps_id jp_id = jps_id(jump_from.id - jp_cost); - neighbours.push_back(jp_id); - costs.push_back(jp_cost + num_steps * warthog::DBL_ROOT_TWO); - } - label = db_[8 * jump_from.id + 5]; - num_steps += label & 32767; - jump_from = jps_id(node_id.id - num_steps * diag_step_delta); - } - - // goal test (so many div ops! and branches! how ugly!) - if((node_id.id - goal_id.id) - < map_->padded_mapsize()) // heading toward the goal? - { - uint32_t gx, gy, nx, ny; - map_->to_padded_xy(goal_id, gx, gy); - map_->to_padded_xy(node_id, nx, ny); - - uint32_t ydelta = (ny - gy); - uint32_t xdelta = (nx - gx); - if(xdelta < mapw && ydelta < map_->height()) - { - if(ydelta < xdelta && ydelta <= num_steps) - { - jps_id jp_id = jps_id(node_id.id - diag_step_delta * ydelta); - double jp_cost = warthog::DBL_ROOT_TWO * ydelta; - jump_west(jp_id, goal_id, jp_cost, neighbours, costs); - } - else if(xdelta <= num_steps) - { - jps_id jp_id = jps_id(node_id.id - diag_step_delta * xdelta); - double jp_cost = warthog::DBL_ROOT_TWO * xdelta; - jump_north(jp_id, goal_id, jp_cost, neighbours, costs); - } - } - } -} - -void -offline_jump_point_locator2::jump_northeast( - jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs) -{ - uint16_t label = 0; - uint16_t num_steps = 0; - uint32_t mapw = map_->width(); - uint32_t diag_step_delta = (mapw - 1); - - jps_id jump_from = node_id; - // step diagonally to an intermediate location jump_from - label = db_[8 * jump_from.id + 4]; - num_steps += label & 32767; - jump_from = jps_id(node_id.id - num_steps * diag_step_delta); - while(!(label & 32768)) - { - - // north of jump_from - uint16_t label_straight1 = db_[8 * jump_from.id]; - if(!(label_straight1 & 32768)) - { - uint32_t jp_cost = (label_straight1 & 32767); - jps_id jp_id = jps_id(jump_from.id - mapw * jp_cost); - neighbours.push_back(jp_id); - costs.push_back(jp_cost + num_steps * warthog::DBL_ROOT_TWO); - } - // east of jump_from - uint16_t label_straight2 = db_[8 * jump_from.id + 2]; - if(!(label_straight2 & 32768)) - { - uint32_t jp_cost = (label_straight2 & 32767); - jps_id jp_id = jps_id(jump_from.id + jp_cost); - neighbours.push_back(jp_id); - costs.push_back(jp_cost + num_steps * warthog::DBL_ROOT_TWO); - } - label = db_[8 * jump_from.id + 4]; - num_steps += label & 32767; - jump_from = jps_id(node_id.id - num_steps * diag_step_delta); - } - - // goal test (so many div ops! and branches! how ugly!) - if((node_id.id - goal_id.id) - < map_->padded_mapsize()) // heading toward the goal? - { - uint32_t gx, gy, nx, ny; - map_->to_padded_xy(goal_id, gx, gy); - map_->to_padded_xy(node_id, nx, ny); - - uint32_t ydelta = (ny - gy); - uint32_t xdelta = (gx - nx); - if(xdelta < mapw && ydelta < map_->height()) - { - if(ydelta < xdelta && ydelta <= num_steps) - { - jps_id jp_id = jps_id(node_id.id - diag_step_delta * ydelta); - double jp_cost = warthog::DBL_ROOT_TWO * ydelta; - jump_east(jp_id, goal_id, jp_cost, neighbours, costs); - } - else if(xdelta <= num_steps) - { - jps_id jp_id = jps_id(node_id.id - diag_step_delta * xdelta); - double jp_cost = warthog::DBL_ROOT_TWO * xdelta; - jump_north(jp_id, goal_id, jp_cost, neighbours, costs); - } - } - } -} - -void -offline_jump_point_locator2::jump_southwest( - jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs) -{ - uint32_t mapw = map_->width(); - uint32_t diag_step_delta = (mapw - 1); - uint16_t label, num_steps; - label = num_steps = 0; - - jps_id jump_from = node_id; - // step diagonally to an intermediate location jump_from - label = db_[8 * jump_from.id + 7]; - num_steps += label & 32767; - jump_from = jps_id(node_id.id + num_steps * diag_step_delta); - while(!(label & 32768)) - { - // south of jump_from - uint16_t label_straight1 = db_[8 * jump_from.id + 1]; - if(!(label_straight1 & 32768)) - { - uint32_t jp_cost = (label_straight1 & 32767); - jps_id jp_id = jps_id(jump_from.id + mapw * jp_cost); - neighbours.push_back(jp_id); - costs.push_back(jp_cost + num_steps * warthog::DBL_ROOT_TWO); - } - // west of jump_from - uint16_t label_straight2 = db_[8 * jump_from.id + 3]; - if(!(label_straight2 & 32768)) - { - uint32_t jp_cost = (label_straight2 & 32767); - jps_id jp_id = jps_id(jump_from.id - jp_cost); - neighbours.push_back(jp_id); - costs.push_back(jp_cost + num_steps * warthog::DBL_ROOT_TWO); - } - label = db_[8 * jump_from.id + 7]; - num_steps += label & 32767; - jump_from = jps_id(node_id.id + num_steps * diag_step_delta); - } - - // goal test (so many div ops! and branches! how ugly!) - if((goal_id.id - node_id.id) - < map_->padded_mapsize()) // heading toward the goal? - { - uint32_t gx, gy, nx, ny; - map_->to_padded_xy(goal_id, gx, gy); - map_->to_padded_xy(node_id, nx, ny); - - uint32_t ydelta = (gy - ny); - uint32_t xdelta = (nx - gx); - if(xdelta < mapw && ydelta < map_->height()) - { - if(ydelta < xdelta && ydelta <= num_steps) - { - jps_id jp_id = jps_id(node_id.id + diag_step_delta * ydelta); - double jp_cost = warthog::DBL_ROOT_TWO * ydelta; - jump_west(jp_id, goal_id, jp_cost, neighbours, costs); - } - else if(xdelta <= num_steps) - { - jps_id jp_id = jps_id(node_id.id + diag_step_delta * xdelta); - double jp_cost = warthog::DBL_ROOT_TWO * xdelta; - jump_south(jp_id, goal_id, jp_cost, neighbours, costs); - } - } - } -} - -void -offline_jump_point_locator2::jump_southeast( - jps_id node_id, jps_id goal_id, vec_jps_id& neighbours, - vec_jps_cost& costs) - -{ - uint16_t label = 0; - uint16_t num_steps = 0; - uint32_t mapw = map_->width(); - uint32_t diag_step_delta = (mapw + 1); - - jps_id jump_from = node_id; - - // step diagonally to an intermediate location jump_from - label = db_[8 * jump_from.id + 6]; - num_steps += label & 32767; - jump_from = jps_id(node_id.id + num_steps * diag_step_delta); - while(!(label & 32768)) - { - // south of jump_from - uint16_t label_straight1 = db_[8 * jump_from.id + 1]; - if(!(label_straight1 & 32768)) - { - uint32_t jp_cost = (label_straight1 & 32767); - jps_id jp_id = jps_id(jump_from.id + mapw * jp_cost); - neighbours.push_back(jp_id); - costs.push_back(jp_cost + num_steps * warthog::DBL_ROOT_TWO); - } - // east of jump_from - uint16_t label_straight2 = db_[8 * jump_from.id + 2]; - if(!(label_straight2 & 32768)) - { - uint32_t jp_cost = (label_straight2 & 32767); - jps_id jp_id = jps_id(jump_from.id + jp_cost); - neighbours.push_back(jp_id); - costs.push_back(jp_cost + num_steps * warthog::DBL_ROOT_TWO); - } - // step diagonally to an intermediate location jump_from - label = db_[8 * jump_from.id + 6]; - num_steps += label & 32767; - jump_from = jps_id(node_id.id + num_steps * diag_step_delta); - } - - // goal test (so many div ops! and branches! how ugly!) - if((goal_id.id - node_id.id) - < map_->padded_mapsize()) // heading toward the goal? - { - uint32_t gx, gy, nx, ny; - map_->to_padded_xy(goal_id, gx, gy); - map_->to_padded_xy(node_id, nx, ny); - - uint32_t ydelta = (gy - ny); - uint32_t xdelta = (gx - nx); - if(xdelta < mapw && ydelta < map_->height()) - { - if(ydelta < xdelta && ydelta <= num_steps) - { - jps_id jp_id = jps_id(node_id.id + diag_step_delta * ydelta); - double jp_cost = warthog::DBL_ROOT_TWO * ydelta; - jump_east(jp_id, goal_id, jp_cost, neighbours, costs); - } - else if(xdelta <= num_steps) - { - jps_id jp_id = jps_id(node_id.id + diag_step_delta * xdelta); - double jp_cost = warthog::DBL_ROOT_TWO * xdelta; - jump_south(jp_id, goal_id, jp_cost, neighbours, costs); - } - } - } -} - -void -offline_jump_point_locator2::jump_north( - jps_id node_id, jps_id goal_id, double cost_to_node_id, - vec_jps_id& neighbours, vec_jps_cost& costs) -{ - uint16_t label = db_[8 * node_id.id]; - uint16_t num_steps = label & 32767; - - // do not jump over the goal - uint32_t id_delta = num_steps * map_->width(); - uint32_t goal_delta = node_id.id - goal_id.id; - if(id_delta >= goal_delta) - { - uint32_t gx = goal_id.id % map_->width(); - uint32_t nx = node_id.id % map_->width(); - if(nx == gx) - { - neighbours.push_back(goal_id); - costs.push_back((goal_delta / map_->width()) + cost_to_node_id); - return; - } - } - - // return the jump point at hand (if it isn't sterile) - if(!(label & 32768)) - { - jps_id jp_id = jps_id(node_id.id - id_delta); - neighbours.push_back(jp_id); - costs.push_back(num_steps + cost_to_node_id); - } -} - -void -offline_jump_point_locator2::jump_south( - jps_id node_id, jps_id goal_id, double cost_to_node_id, - vec_jps_id& neighbours, vec_jps_cost& costs) -{ - uint16_t label = db_[8 * node_id.id + 1]; - uint16_t num_steps = label & 32767; - - // do not jump over the goal - uint32_t id_delta = num_steps * map_->width(); - uint32_t goal_delta = goal_id.id - node_id.id; - if(id_delta >= goal_delta) - { - uint32_t gx = goal_id.id % map_->width(); - uint32_t nx = node_id.id % map_->width(); - if(nx == gx) - { - neighbours.push_back(goal_id); - costs.push_back((goal_delta / map_->width()) + cost_to_node_id); - return; - } - } - - // return the jump point at hand (if it isn't sterile) - if(!(label & 32768)) - { - jps_id jp_id = jps_id(node_id.id + id_delta); - neighbours.push_back(jp_id); - costs.push_back(num_steps + cost_to_node_id); - } -} - -void -offline_jump_point_locator2::jump_east( - jps_id node_id, jps_id goal_id, double cost_to_node_id, - vec_jps_id& neighbours, vec_jps_cost& costs) -{ - uint16_t label = db_[8 * node_id.id + 2]; - uint32_t num_steps = label & 32767; - - // do not jump over the goal - uint32_t goal_delta = goal_id.id - node_id.id; - if(num_steps >= goal_delta) - { - neighbours.push_back(goal_id); - costs.push_back(goal_delta + cost_to_node_id); - return; - } - - // return the jump point at hand - if(!(label & 32768)) - { - jps_id jp_id = jps_id(node_id.id + num_steps); - neighbours.push_back(jp_id); - costs.push_back(num_steps + cost_to_node_id); - } -} - -void -offline_jump_point_locator2::jump_west( - jps_id node_id, jps_id goal_id, double cost_to_node_id, - vec_jps_id& neighbours, vec_jps_cost& costs) -{ - uint16_t label = db_[8 * node_id.id + 3]; - uint32_t num_steps = label & 32767; - - // do not jump over the goal - uint32_t goal_delta = node_id.id - goal_id.id; - if(num_steps >= goal_delta) - { - neighbours.push_back(goal_id); - costs.push_back(goal_delta + cost_to_node_id); - return; - } - - // return the jump point at hand - if(!(label & 32768)) - { - jps_id jp_id = jps_id(node_id.id - num_steps); - neighbours.push_back(jp_id); - costs.push_back(num_steps + cost_to_node_id); - } -} - -} // namespace jps::jump diff --git a/src/jump/online_jump_point_locator.cpp b/src/jump/online_jump_point_locator.cpp deleted file mode 100644 index 61b810d..0000000 --- a/src/jump/online_jump_point_locator.cpp +++ /dev/null @@ -1,477 +0,0 @@ -#include -#include -#include - -#include -#include - -namespace jps::jump -{ - -online_jump_point_locator::online_jump_point_locator( - warthog::domain::gridmap* map) - : map_(map) //, jumplimit_(UINT32_MAX) -{ - rmap_ = create_rmap(); -} - -online_jump_point_locator::~online_jump_point_locator() -{ - delete rmap_; -} - -// create a copy of the grid map which is rotated by 90 degrees clockwise. -// this version will be used when jumping North or South. -warthog::domain::gridmap* -online_jump_point_locator::create_rmap() -{ - uint32_t maph = map_->header_height(); - uint32_t mapw = map_->header_width(); - uint32_t rmaph = mapw; - uint32_t rmapw = maph; - warthog::domain::gridmap* rmap - = new warthog::domain::gridmap(rmaph, rmapw); - - for(uint32_t x = 0; x < mapw; x++) - { - for(uint32_t y = 0; y < maph; y++) - { - uint32_t label - = map_->get_label(map_->to_padded_id_from_unpadded(x, y)); - uint32_t rx = ((rmapw - 1) - y); - uint32_t ry = x; - rmap->set_label(rmap->to_padded_id_from_unpadded(rx, ry), label); - } - } - return rmap; -} - -// Finds a jump point successor of node (x, y) in Direction d. -// Also given is the location of the goal node (goalx, goaly) for a particular -// search instance. If encountered, the goal node is always returned as a -// jump point successor. -// -// @return: the id of a jump point successor or jps_id::none() if no jp exists. -void -online_jump_point_locator::jump( - direction d, jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost) -{ - switch(d) - { - case NORTH: - jump_north(node_id, goal_id, jumpnode_id, jumpcost); - break; - case SOUTH: - jump_south(node_id, goal_id, jumpnode_id, jumpcost); - break; - case EAST: - jump_east(node_id, goal_id, jumpnode_id, jumpcost); - break; - case WEST: - jump_west(node_id, goal_id, jumpnode_id, jumpcost); - break; - case NORTHEAST: - jump_northeast(node_id, goal_id, jumpnode_id, jumpcost); - break; - case NORTHWEST: - jump_northwest(node_id, goal_id, jumpnode_id, jumpcost); - break; - case SOUTHEAST: - jump_southeast(node_id, goal_id, jumpnode_id, jumpcost); - break; - case SOUTHWEST: - jump_southwest(node_id, goal_id, jumpnode_id, jumpcost); - break; - default: - break; - } -} - -void -online_jump_point_locator::jump_north( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost) -{ - node_id = this->map_id_to_rmap_id(node_id); - goal_id = this->map_id_to_rmap_id(goal_id); - jump_north_(node_id, goal_id, jumpnode_id, jumpcost, rmap_); - jumpnode_id = this->rmap_id_to_map_id(jumpnode_id); -} - -void -online_jump_point_locator::jump_north_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap) -{ - // jumping north in the original map is the same as jumping - // east when we use a version of the map rotated 90 degrees. - jump_east_(node_id, goal_id, jumpnode_id, jumpcost, rmap_); -} - -void -online_jump_point_locator::jump_south( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost) -{ - node_id = this->map_id_to_rmap_id(node_id); - goal_id = this->map_id_to_rmap_id(goal_id); - jump_south_(node_id, goal_id, jumpnode_id, jumpcost, rmap_); - jumpnode_id = this->rmap_id_to_map_id(jumpnode_id); -} - -void -online_jump_point_locator::jump_south_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap) -{ - // jumping north in the original map is the same as jumping - // west when we use a version of the map rotated 90 degrees. - jump_west_(node_id, goal_id, jumpnode_id, jumpcost, rmap_); -} - -void -online_jump_point_locator::jump_east( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost) -{ - jump_east_(node_id, goal_id, jumpnode_id, jumpcost, map_); -} - -void -online_jump_point_locator::jump_east_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap) -{ - jumpnode_id = node_id; - - uint32_t neis[3] = {0, 0, 0}; - bool deadend = false; - - jumpnode_id = node_id; - while(true) - { - // read in tiles from 3 adjacent rows. the curent node - // is in the low byte of the middle row - mymap->get_neighbours_32bit(jumpnode_id, neis); - - // identity forced neighbours and deadend tiles. - // forced neighbours are found in the top or bottom row. they - // can be identified as a non-obstacle tile that follows - // immediately after an obstacle tile. A dead-end tile is - // an obstacle found on the middle row; - uint32_t forced_bits = (~neis[0] << 1) & neis[0]; - forced_bits |= (~neis[2] << 1) & neis[2]; - uint32_t deadend_bits = ~neis[1]; - - // stop if we found any forced or dead-end tiles - int32_t stop_bits = (int32_t)(forced_bits | deadend_bits); - if(stop_bits) - { - int32_t stop_pos = __builtin_ffs(stop_bits) - 1; // returns idx+1 - jumpnode_id.id += (uint32_t)stop_pos; - deadend = deadend_bits & (1 << stop_pos); - break; - } - - // jump to the last position in the cache. we do not jump past the end - // in case the last tile from the row above or below is an obstacle. - // Such a tile, followed by a non-obstacle tile, would yield a forced - // neighbour that we don't want to miss. - jumpnode_id.id += 31; - } - - uint32_t num_steps = jumpnode_id.id - node_id.id; - uint32_t goal_dist = goal_id.id - node_id.id; - if(num_steps > goal_dist) - { - jumpnode_id = goal_id; - jumpcost = goal_dist; - return; - } - - if(deadend) - { - // number of steps to reach the deadend tile is not - // correct here since we just inverted neis[1] and then - // looked for the first set bit. need -1 to fix it. - num_steps -= (1 && num_steps); - jumpnode_id = jps_id::none(); - } - jumpcost = num_steps; -} - -// analogous to ::jump_east -void -online_jump_point_locator::jump_west( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost) -{ - jump_west_(node_id, goal_id, jumpnode_id, jumpcost, map_); -} - -void -online_jump_point_locator::jump_west_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap) -{ - bool deadend = false; - uint32_t neis[3] = {0, 0, 0}; - - jumpnode_id = node_id; - while(true) - { - // cache 32 tiles from three adjacent rows. - // current tile is in the high byte of the middle row - mymap->get_neighbours_upper_32bit(jumpnode_id, neis); - - // identify forced and dead-end nodes - uint32_t forced_bits = (~neis[0] >> 1) & neis[0]; - forced_bits |= (~neis[2] >> 1) & neis[2]; - uint32_t deadend_bits = ~neis[1]; - - // stop if we encounter any forced or deadend nodes - uint32_t stop_bits = (forced_bits | deadend_bits); - if(stop_bits) - { - uint32_t stop_pos = (uint32_t)__builtin_clz(stop_bits); - jumpnode_id.id -= stop_pos; - deadend = deadend_bits & (0x80000000 >> stop_pos); - break; - } - // jump to the end of cache. jumping +32 involves checking - // for forced neis between adjacent sets of contiguous tiles - jumpnode_id.id -= 31; - } - - uint32_t num_steps = node_id.id - jumpnode_id.id; - uint32_t goal_dist = node_id.id - goal_id.id; - if(num_steps > goal_dist) - { - jumpnode_id = goal_id; - jumpcost = goal_dist; - return; - } - - if(deadend) - { - // number of steps to reach the deadend tile is not - // correct here since we just inverted neis[1] and then - // counted leading zeroes. need -1 to fix it. - num_steps -= (1 && num_steps); - jumpnode_id = jps_id::none(); - } - jumpcost = num_steps; -} - -void -online_jump_point_locator::jump_northeast( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost) -{ - uint32_t num_steps = 0; - - // first 3 bits of first 3 bytes represent a 3x3 cell of tiles - // from the grid. next_id at centre. Assume little endian format. - jps_id next_id = node_id; - uint32_t mapw = map_->width(); - - // early return if the first diagonal step is invalid - // (validity of subsequent steps is checked by straight jump functions) - uint32_t neis; - map_->get_neighbours(next_id, (uint8_t*)&neis); - if((neis & 1542) != 1542) - { - jumpnode_id = jps_id::none(); - jumpcost = 0; - return; - } - - // jump a single step at a time (no corner cutting) - jps_id rnext_id = map_id_to_rmap_id(next_id); - jps_id rgoal_id = map_id_to_rmap_id(goal_id); - uint32_t rmapw = rmap_->width(); - while(true) - { - num_steps++; - next_id = jps_id(next_id.id - mapw + 1); - rnext_id = jps_id(rnext_id.id + rmapw + 1); - - // recurse straight before stepping again diagonally; - // (ensures we do not miss any optimal turning points) - jps_id jp_id1, jp_id2; - warthog::cost_t cost1, cost2; - jump_north_(rnext_id, rgoal_id, jp_id1, cost1, rmap_); - if(!jp_id1.is_none()) { break; } - jump_east_(next_id, goal_id, jp_id2, cost2, map_); - if(!jp_id2.is_none()) { break; } - - // couldn't move in either straight dir; node_id is an obstacle - if(!((uint64_t)cost1 && (uint64_t)cost2)) - { - next_id = jps_id::none(); - break; - } - } - jumpnode_id = next_id; - jumpcost = num_steps * warthog::DBL_ROOT_TWO; -} - -void -online_jump_point_locator::jump_northwest( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost) -{ - uint32_t num_steps = 0; - - // first 3 bits of first 3 bytes represent a 3x3 cell of tiles - // from the grid. next_id at centre. Assume little endian format. - jps_id next_id = node_id; - uint32_t mapw = map_->width(); - - // early termination (invalid first step) - uint32_t neis; - map_->get_neighbours(next_id, (uint8_t*)&neis); - if((neis & 771) != 771) - { - jumpnode_id = jps_id::none(); - jumpcost = 0; - return; - } - - // jump a single step at a time (no corner cutting) - jps_id rnext_id = map_id_to_rmap_id(next_id); - jps_id rgoal_id = map_id_to_rmap_id(goal_id); - uint32_t rmapw = rmap_->width(); - while(true) - { - num_steps++; - next_id = jps_id(next_id.id - mapw - 1); - rnext_id = jps_id(rnext_id.id - (rmapw - 1)); - - // recurse straight before stepping again diagonally; - // (ensures we do not miss any optimal turning points) - jps_id jp_id1, jp_id2; - warthog::cost_t cost1, cost2; - jump_north_(rnext_id, rgoal_id, jp_id1, cost1, rmap_); - if(!jp_id1.is_none()) { break; } - jump_west_(next_id, goal_id, jp_id2, cost2, map_); - if(!jp_id2.is_none()) { break; } - - // couldn't move in either straight dir; node_id is an obstacle - if(!((uint64_t)cost1 && (uint64_t)cost2)) - { - next_id = jps_id::none(); - break; - } - } - jumpnode_id = next_id; - jumpcost = num_steps * warthog::DBL_ROOT_TWO; -} - -void -online_jump_point_locator::jump_southeast( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost) -{ - uint32_t num_steps = 0; - - // first 3 bits of first 3 bytes represent a 3x3 cell of tiles - // from the grid. next_id at centre. Assume little endian format. - jps_id next_id = node_id; - uint32_t mapw = map_->width(); - - // early return if the first diagonal step is invalid - // (validity of subsequent steps is checked by straight jump functions) - uint32_t neis; - map_->get_neighbours(next_id, (uint8_t*)&neis); - if((neis & 394752) != 394752) - { - jumpnode_id = jps_id::none(); - jumpcost = 0; - return; - } - - // jump a single step at a time (no corner cutting) - jps_id rnext_id = map_id_to_rmap_id(next_id); - jps_id rgoal_id = map_id_to_rmap_id(goal_id); - uint32_t rmapw = rmap_->width(); - while(true) - { - num_steps++; - next_id = jps_id(next_id.id + mapw + 1); - rnext_id = jps_id(rnext_id.id + rmapw - 1); - - // recurse straight before stepping again diagonally; - // (ensures we do not miss any optimal turning points) - jps_id jp_id1, jp_id2; - warthog::cost_t cost1, cost2; - jump_south_(rnext_id, rgoal_id, jp_id1, cost1, rmap_); - if(!jp_id1.is_none()) { break; } - jump_east_(next_id, goal_id, jp_id2, cost2, map_); - if(!jp_id2.is_none()) { break; } - - // couldn't move in either straight dir; node_id is an obstacle - if(!((uint64_t)cost1 && (uint64_t)cost2)) - { - next_id = jps_id::none(); - break; - } - } - jumpnode_id = next_id; - jumpcost = num_steps * warthog::DBL_ROOT_TWO; -} - -void -online_jump_point_locator::jump_southwest( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost) -{ - uint32_t num_steps = 0; - - // first 3 bits of first 3 bytes represent a 3x3 cell of tiles - // from the grid. next_id at centre. Assume little endian format. - uint32_t neis; - jps_id next_id = node_id; - uint32_t mapw = map_->width(); - - // early termination (first step is invalid) - map_->get_neighbours(next_id, (uint8_t*)&neis); - if((neis & 197376) != 197376) - { - jumpnode_id = jps_id::none(); - jumpcost = 0; - return; - } - - // jump a single step (no corner cutting) - jps_id rnext_id = map_id_to_rmap_id(next_id); - jps_id rgoal_id = map_id_to_rmap_id(goal_id); - uint32_t rmapw = rmap_->width(); - while(true) - { - num_steps++; - next_id = jps_id(next_id.id + mapw - 1); - rnext_id = jps_id(rnext_id.id - (rmapw + 1)); - - // recurse straight before stepping again diagonally; - // (ensures we do not miss any optimal turning points) - jps_id jp_id1, jp_id2; - warthog::cost_t cost1, cost2; - jump_south_(rnext_id, rgoal_id, jp_id1, cost1, rmap_); - if(!jp_id1.is_none()) { break; } - jump_west_(next_id, goal_id, jp_id2, cost2, map_); - if(!jp_id2.is_none()) { break; } - - // couldn't move in either straight dir; node_id is an obstacle - if(!((uint64_t)cost1 && (uint64_t)cost2)) - { - next_id = jps_id::none(); - break; - } - } - jumpnode_id = next_id; - jumpcost = num_steps * warthog::DBL_ROOT_TWO; -} - -} // namespace jps::jump diff --git a/src/jump/online_jump_point_locator2.cpp b/src/jump/online_jump_point_locator2.cpp deleted file mode 100644 index 54c8562..0000000 --- a/src/jump/online_jump_point_locator2.cpp +++ /dev/null @@ -1,788 +0,0 @@ -#include -#include -#include - -#include -#include - -namespace jps::jump -{ - -online_jump_point_locator2::online_jump_point_locator2( - warthog::domain::gridmap* map) - : map_(map) //, jumplimit_(UINT32_MAX) -{ - rmap_ = create_rmap(); - current_node_id_ = current_rnode_id_ = jps_id::none(); - current_goal_id_ = current_rgoal_id_ = jps_id::none(); -} - -online_jump_point_locator2::~online_jump_point_locator2() -{ - delete rmap_; -} - -// create a copy of the grid map which is rotated by 90 degrees clockwise. -// this version will be used when jumping North or South. -warthog::domain::gridmap* -online_jump_point_locator2::create_rmap() -{ - uint32_t maph = map_->header_height(); - uint32_t mapw = map_->header_width(); - uint32_t rmaph = mapw; - uint32_t rmapw = maph; - warthog::domain::gridmap* rmap - = new warthog::domain::gridmap(rmaph, rmapw); - - for(uint32_t x = 0; x < mapw; x++) - { - for(uint32_t y = 0; y < maph; y++) - { - uint32_t label - = map_->get_label(map_->to_padded_id_from_unpadded(x, y)); - uint32_t rx = ((rmapw - 1) - y); - uint32_t ry = x; - rmap->set_label(rmap->to_padded_id_from_unpadded(rx, ry), label); - } - } - return rmap; -} - -// Finds a jump point successor of node (x, y) in Direction d. -// Also given is the location of the goal node (goalx, goaly) for a particular -// search instance. If encountered, the goal node is always returned as a -// jump point successor. -// -// @return: the id of a jump point successor or INF if no jp exists. -void -online_jump_point_locator2::jump( - direction d, jps_id node_id, jps_id goal_id, vec_jps_id& jpoints, - vec_jps_cost& costs) -{ - jump_east_fp = &online_jump_point_locator2::jump_east_; - jump_west_fp = &online_jump_point_locator2::jump_west_; - - // cache node and goal ids so we don't need to convert all the time - if(goal_id != current_goal_id_) - { - current_goal_id_ = goal_id; - current_rgoal_id_ = map_id_to_rmap_id(goal_id); - } - - if(node_id != current_node_id_) - { - current_node_id_ = node_id; - current_rnode_id_ = map_id_to_rmap_id(node_id); - } - - switch(d) - { - case NORTH: - jump_north(jpoints, costs); - break; - case SOUTH: - jump_south(jpoints, costs); - break; - case EAST: - jump_east(jpoints, costs); - break; - case WEST: - jump_west(jpoints, costs); - break; - case NORTHEAST: - jump_northeast(jpoints, costs); - break; - case NORTHWEST: - jump_northwest(jpoints, costs); - break; - case SOUTHEAST: - jump_southeast(jpoints, costs); - break; - case SOUTHWEST: - jump_southwest(jpoints, costs); - break; - default: - break; - } -} - -void -online_jump_point_locator2::jump_north( - vec_jps_id& jpoints, vec_jps_cost& costs) -{ - jps_id rnode_id = current_rnode_id_; - jps_id rgoal_id = current_rgoal_id_; - jps_id jumpnode_id; - warthog::cost_t jumpcost; - - jump_north_(rnode_id, rgoal_id, jumpnode_id, jumpcost, rmap_); - - if(!jumpnode_id.is_none()) - { - jumpnode_id.id = current_node_id_.id - - static_cast(jumpcost) * map_->width(); - jpoints.push_back(jumpnode_id); - costs.push_back(jumpcost); - } -} - -void -online_jump_point_locator2::jump_north_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap) -{ - // jumping north in the original map is the same as jumping - // east when we use a version of the map rotated 90 degrees. - (this->*(jump_east_fp))(node_id, goal_id, jumpnode_id, jumpcost, mymap); -} - -void -online_jump_point_locator2::jump_south( - vec_jps_id& jpoints, vec_jps_cost& costs) -{ - jps_id rnode_id = current_rnode_id_; - jps_id rgoal_id = current_rgoal_id_; - jps_id jumpnode_id; - warthog::cost_t jumpcost; - - jump_south_(rnode_id, rgoal_id, jumpnode_id, jumpcost, rmap_); - - if(!jumpnode_id.is_none()) - { - jumpnode_id = jps_id( - current_node_id_.id - + static_cast(jumpcost) * map_->width()); - jpoints.push_back(jumpnode_id); - costs.push_back(jumpcost); - } -} - -void -online_jump_point_locator2::jump_south_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap) -{ - // jumping north in the original map is the same as jumping - // west when we use a version of the map rotated 90 degrees. - (this->*(jump_west_fp))(node_id, goal_id, jumpnode_id, jumpcost, mymap); -} - -void -online_jump_point_locator2::jump_east(vec_jps_id& jpoints, vec_jps_cost& costs) -{ - jps_id node_id = current_node_id_; - jps_id goal_id = current_goal_id_; - jps_id jumpnode_id; - warthog::cost_t jumpcost; - - (this->*(jump_east_fp))(node_id, goal_id, jumpnode_id, jumpcost, map_); - - if(!jumpnode_id.is_none()) - { - jpoints.push_back(jumpnode_id); - costs.push_back(jumpcost); - } -} - -void -online_jump_point_locator2::jump_east_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap) -{ - uint64_t neis[3] = {0, 0, 0}; - bool deadend = false; - - // read tiles from the grid: - // - along the row of node_id - // - from the row above node_id - // - from the row below node_id - // NB: the jump direction (here, EAST) corresponds to moving from the - // low bit of the tileset and towards the high bit - mymap->get_neighbours_64bit(node_id, neis); - - // extract the bit position of node_id in the tileset - uint32_t bit_offset = node_id.id & 63; - - // look for tiles with forced neighbours in the rows above and below - // A forced neighbour can be identified as a non-obstacle tile that - // follows immediately after an obstacle tile. - // we ignore forced tiles which are at offsets >= bit_offset - // (i.e., all tiles in {WEST of, above, below} the current location) - uint64_t forced_bits = (~neis[0] << 1) & neis[0]; - forced_bits |= (~neis[2] << 1) & neis[2]; - forced_bits &= ~((1LL << bit_offset) | ((1LL << bit_offset) - 1)); - - // look for obstacles tiles in the current row - // we ignore obstacles at offsets > bit_offset - uint64_t deadend_bits = ~neis[1]; - deadend_bits &= ~((1LL << bit_offset) - 1); - - // stop jumping if any forced or deadend locations are found - uint64_t stop_bits = (forced_bits | deadend_bits); - if(stop_bits) - { - // figure out how far we jumped (we count trailing zeroes) - // we then subtract -1 because we want to know how many - // steps from the bit offset to the stop bit - int stop_pos = __builtin_ctzll(stop_bits); - uint32_t num_steps = (stop_pos - bit_offset); - - // don't jump over the target - uint32_t goal_dist = goal_id.id - node_id.id; - if(num_steps > goal_dist) - { - jumpnode_id = goal_id; - jumpcost = goal_dist; - return; - } - - // did we reach a jump point or a dead-end? - deadend = (deadend_bits & (1LL << stop_pos)); - if(deadend) - { - jumpcost = num_steps - (1 && num_steps); - jumpnode_id = jps_id::none(); - return; - } - - jumpnode_id = jps_id(node_id.id + num_steps); - jumpcost = num_steps; - return; - } - - // keep jumping. the procedure below is implemented - // similarly to the above. but now the stride is a - // fixed 64bit and the jumps are word-aligned. - jumpnode_id = jps_id(node_id.id + 64 - bit_offset); - while(true) - { - // we need to forced neighbours might occur across - // 64bit boundaries. to check for these we keep the - // high-byte of the previous set of neighbours - uint64_t high_ra = neis[0] >> 63; - uint64_t high_rb = neis[2] >> 63; - - // read next 64bit set of tile data - mymap->get_neighbours_64bit(jumpnode_id, neis); - - // identify forced neighbours and deadend tiles. - uint64_t forced_bits = ~((neis[0] << 1) | high_ra) & neis[0]; - forced_bits |= ~((neis[2] << 1) | high_rb) & neis[2]; - uint64_t deadend_bits = ~neis[1]; - - // stop if we found any forced or dead-end tiles - uint64_t stop_bits = (forced_bits | deadend_bits); - if(stop_bits) - { - int stop_pos = __builtin_ctzll(stop_bits); - jumpnode_id.id += stop_pos; - deadend = deadend_bits & (1LL << stop_pos); - break; - } - jumpnode_id.id += 64; - } - - // figure out how far we jumped - uint32_t num_steps = jumpnode_id.id - node_id.id; - - // don't jump over the target - uint32_t goal_dist = goal_id.id - node_id.id; - if(num_steps > goal_dist) - { - jumpnode_id = goal_id; - jumpcost = goal_dist; - return; - } - - // did we hit a dead-end? - if(deadend) - { - // in this case we want to return the number of steps to - // the last traversable tile (not to the obstacle) - // need -1 to fix it. - num_steps -= (1 && num_steps); - jumpnode_id = jps_id::none(); - } - - // return the number of steps to reach the jump point or deadend - jumpcost = num_steps; -} - -// analogous to ::jump_east -void -online_jump_point_locator2::jump_west(vec_jps_id& jpoints, vec_jps_cost& costs) -{ - jps_id node_id = current_node_id_; - jps_id goal_id = current_goal_id_; - jps_id jumpnode_id; - warthog::cost_t jumpcost; - - (this->*(jump_west_fp))(node_id, goal_id, jumpnode_id, jumpcost, map_); - - if(!jumpnode_id.is_none()) - { - jpoints.push_back(jumpnode_id); - costs.push_back(jumpcost); - } -} - -void -online_jump_point_locator2::jump_west_( - jps_id node_id, jps_id goal_id, jps_id& jumpnode_id, - warthog::cost_t& jumpcost, warthog::domain::gridmap* mymap) -{ - bool deadend = false; - uint64_t neis[3] = {0, 0, 0}; - - // read sets of 64 tiles from the grid: - // - along the row of node_id - // - from the row above node_id - // - from the row below node_id - // NB: the jump direction (here, WEST) corresponds to moving from the - // high bit of the tileset and towards the low bit - mymap->get_neighbours_64bit(node_id, neis); - - // extract the bit position of node_id in the tileset - uint32_t bit_offset = node_id.id & 63; - - // look for tiles with forced neighbours in the rows above and below - // A forced neighbour can be identified as a non-obstacle tile that - // follows immediately after an obstacle tile. - // NB: we ignore forced tiles which are at offsets <= bit_offset - // (i.e., all tiles in {EAST of, above, below} the current location) - uint64_t forced_bits = (~neis[0] >> 1) & neis[0]; - forced_bits |= (~neis[2] >> 1) & neis[2]; - forced_bits &= ~(UINT64_MAX << bit_offset); - - // look for obstacles tiles in the current row - // NB: we ignore obstacles at offsets < bit_offset - uint64_t deadend_bits = ~neis[1]; - deadend_bits &= (1LL << bit_offset) | ((1LL << bit_offset) - 1); - - // stop jumping if any forced or deadend locations are found - uint64_t stop_bits = (forced_bits | deadend_bits); - if(stop_bits) - { - // figure out how far we jumped (we count leading zeroes) - // we then subtract -1 because we want to know how many - // steps from the bit offset to the stop bit - uint64_t stop_pos = __builtin_clzll(stop_bits); - uint32_t num_steps = (stop_pos - (63 - bit_offset)); - - // don't jump over the target - uint32_t goal_dist = node_id.id - goal_id.id; - if(num_steps > goal_dist) - { - jumpnode_id = goal_id; - jumpcost = goal_dist; - return; - } - - // did we reach a jump point or a dead-end? - deadend = deadend_bits & (0x8000000000000000 >> stop_pos); - if(deadend) - { - // number of steps to reach the deadend tile is not - // correct here since we just inverted neis[1] and then - // counted leading zeroes. need -1 to fix it. - jumpcost = num_steps - (1 && num_steps); - jumpnode_id = jps_id::none(); - return; - } - - jumpnode_id = jps_id(node_id.id - num_steps); - jumpcost = num_steps; - return; - } - - // keep jumping. the procedure below is implemented - // similarly to the above. but now the stride is a - // fixed 64bit and the jumps are word-aligned. - jumpnode_id = jps_id(node_id.id - (bit_offset + 1)); - while(true) - { - // we need to forced neighbours might occur across - // 64bit boundaries. to check for these we keep the - // low-byte of the previous set of neighbours - uint64_t low_ra = neis[0] << 63; - uint64_t low_rb = neis[2] << 63; - - // read next 64 bit set of tile data - mymap->get_neighbours_64bit(jumpnode_id, neis); - - // identify forced and dead-end nodes - uint64_t forced_bits = ~((neis[0] >> 1) | low_ra) & neis[0]; - forced_bits |= ~((neis[2] >> 1) | low_rb) & neis[2]; - uint64_t deadend_bits = ~neis[1]; - - // stop if we encounter any forced or deadend nodes - uint64_t stop_bits = (forced_bits | deadend_bits); - if(stop_bits) - { - uint64_t stop_pos = (uint64_t)__builtin_clzll(stop_bits); - jumpnode_id.id -= stop_pos; - deadend = deadend_bits & (0x8000000000000000 >> stop_pos); - break; - } - jumpnode_id.id -= 64; - } - - // figure out how far we jumped - uint32_t num_steps = node_id.id - jumpnode_id.id; - - // don't jump over the target - uint32_t goal_dist = node_id.id - goal_id.id; - if(num_steps > goal_dist) - { - jumpnode_id = goal_id; - jumpcost = goal_dist; - return; - } - - // did we hit a dead-end? - if(deadend) - { - // in this case we want to return the number of steps to - // the last traversable tile (not to the obstacle) - // need -1 to fix it. - num_steps -= (1 && num_steps); - jumpnode_id = jps_id::none(); - } - - // return the number of steps to reach the jump point or deadend - jumpcost = num_steps; -} - -void -online_jump_point_locator2::jump_northeast( - vec_jps_id& jpoints, vec_jps_cost& costs) -{ - jps_id jumpnode_id, jp1_id, jp2_id; - warthog::cost_t jumpcost, jp1_cost, jp2_cost, cost_to_nodeid; - jumpnode_id = jp1_id = jp2_id = {}; - jumpcost = jp1_cost = jp2_cost = cost_to_nodeid = 0; - - jps_id node_id = current_node_id_; - jps_id goal_id = current_goal_id_; - jps_id rnode_id = current_rnode_id_; - jps_id rgoal_id = current_rgoal_id_; - - // first 3 bits of first 3 bytes represent a 3x3 cell of tiles - // from the grid. node_id at centre. Assume little endian format. - uint32_t neis; - map_->get_neighbours(node_id, (uint8_t*)&neis); - - // early return if the first diagonal step is invalid - // (validity of subsequent steps is checked by straight jump functions) - if((neis & 1542) != 1542) { return; } - - while(!node_id.is_none()) - { - jump_northeast_( - node_id, rnode_id, goal_id, rgoal_id, jumpnode_id, jumpcost, - jp1_id, jp1_cost, jp2_id, jp2_cost); - - if(!jp1_id.is_none()) - { - jp1_id = jps_id( - node_id.id - static_cast(jp1_cost) * map_->width()); - jpoints.push_back(jp1_id); - costs.push_back(cost_to_nodeid + jumpcost + jp1_cost); - if(jp2_cost == 0) { break; } // no corner cutting - } - - if(!jp2_id.is_none()) - { - jpoints.push_back(jp2_id); - costs.push_back(cost_to_nodeid + jumpcost + jp2_cost); - if(jp1_cost == 0) { break; } // no corner cutting - } - node_id = jumpnode_id; - cost_to_nodeid += jumpcost; - } -} - -void -online_jump_point_locator2::jump_northeast_( - jps_id& node_id, jps_id& rnode_id, jps_id goal_id, jps_id rgoal_id, - jps_id& jumpnode_id, warthog::cost_t& jumpcost, jps_id& jp_id1, - warthog::cost_t& cost1, jps_id& jp_id2, warthog::cost_t& cost2) -{ - uint32_t num_steps = 0; - - // jump a single step at a time (no corner cutting) - uint32_t rmapw = rmap_->width(); - uint32_t mapw = map_->width(); - while(true) - { - num_steps++; - node_id = jps_id(node_id.id - mapw + 1); - rnode_id = jps_id(rnode_id.id + rmapw + 1); - - // recurse straight before stepping again diagonally; - // (ensures we do not miss any optimal turning points) - jump_north_(rnode_id, rgoal_id, jp_id1, cost1, rmap_); - (this->*(jump_east_fp))(node_id, goal_id, jp_id2, cost2, map_); - if(!jps_id(jp_id1.id & jp_id2.id).is_none()) { break; } - - // couldn't move in a straight dir; next step is an obstacle - if(!((uint64_t)cost1 && (uint64_t)cost2)) - { - node_id = jp_id1 = jp_id2 = jps_id::none(); - break; - } - } - jumpnode_id = node_id; - jumpcost = num_steps * warthog::DBL_ROOT_TWO; -} - -void -online_jump_point_locator2::jump_northwest( - vec_jps_id& jpoints, vec_jps_cost& costs) -{ - jps_id jumpnode_id, jp1_id, jp2_id; - warthog::cost_t jumpcost, jp1_cost, jp2_cost, cost_to_nodeid; - jumpnode_id = jp1_id = jp2_id = {}; - jumpcost = jp1_cost = jp2_cost = cost_to_nodeid = 0; - - jps_id node_id = current_node_id_; - jps_id goal_id = current_goal_id_; - jps_id rnode_id = current_rnode_id_; - jps_id rgoal_id = current_rgoal_id_; - - // first 3 bits of first 3 bytes represent a 3x3 cell of tiles - // from the grid. node_id at centre. Assume little endian format. - uint32_t neis; - map_->get_neighbours(node_id, (uint8_t*)&neis); - - // early return if the first diagonal step is invalid - // (validity of subsequent steps is checked by straight jump functions) - if((neis & 771) != 771) { return; } - - while(!node_id.is_none()) - { - jump_northwest_( - node_id, rnode_id, goal_id, rgoal_id, jumpnode_id, jumpcost, - jp1_id, jp1_cost, jp2_id, jp2_cost); - - if(!jp1_id.is_none()) - { - jp1_id = jps_id( - node_id.id - static_cast(jp1_cost) * map_->width()); - jpoints.push_back(jp1_id); - costs.push_back(cost_to_nodeid + jumpcost + jp1_cost); - if(jp2_cost == 0) { break; } // no corner cutting - } - - if(!jp2_id.is_none()) - { - jpoints.push_back(jp2_id); - costs.push_back(cost_to_nodeid + jumpcost + jp2_cost); - if(jp1_cost == 0) { break; } // no corner cutting - } - node_id = jumpnode_id; - cost_to_nodeid += jumpcost; - } -} - -void -online_jump_point_locator2::jump_northwest_( - jps_id& node_id, jps_id& rnode_id, jps_id goal_id, jps_id rgoal_id, - jps_id& jumpnode_id, warthog::cost_t& jumpcost, jps_id& jp_id1, - warthog::cost_t& cost1, jps_id& jp_id2, warthog::cost_t& cost2) - -{ - uint32_t num_steps = 0; - - // jump a single step at a time (no corner cutting) - uint32_t rmapw = rmap_->width(); - uint32_t mapw = map_->width(); - while(true) - { - num_steps++; - node_id = jps_id(node_id.id - mapw - 1); - rnode_id = jps_id(rnode_id.id - (rmapw - 1)); - - // recurse straight before stepping again diagonally; - // (ensures we do not miss any optimal turning points) - jump_north_(rnode_id, rgoal_id, jp_id1, cost1, rmap_); - (this->*(jump_west_fp))(node_id, goal_id, jp_id2, cost2, map_); - if(!jps_id(jp_id1.id & jp_id2.id).is_none()) { break; } - - // couldn't move in a straight dir; next step is an obstacle - if(!((uint64_t)cost1 && (uint64_t)cost2)) - { - node_id = jp_id1 = jp_id2 = jps_id::none(); - break; - } - } - jumpnode_id = node_id; - jumpcost = num_steps * warthog::DBL_ROOT_TWO; -} - -void -online_jump_point_locator2::jump_southeast( - vec_jps_id& jpoints, vec_jps_cost& costs) -{ - jps_id jumpnode_id, jp1_id, jp2_id; - warthog::cost_t jumpcost, jp1_cost, jp2_cost, cost_to_nodeid; - jumpnode_id = jp1_id = jp2_id = {}; - jumpcost = jp1_cost = jp2_cost = cost_to_nodeid = 0; - - jps_id node_id = current_node_id_; - jps_id goal_id = current_goal_id_; - jps_id rnode_id = current_rnode_id_; - jps_id rgoal_id = current_rgoal_id_; - - // first 3 bits of first 3 bytes represent a 3x3 cell of tiles - // from the grid. next_id at centre. Assume little endian format. - uint32_t neis; - map_->get_neighbours(node_id, (uint8_t*)&neis); - - // early return if the first diagonal step is invalid - // (validity of subsequent steps is checked by straight jump functions) - if((neis & 394752) != 394752) { return; } - - while(!node_id.is_none()) - { - jump_southeast_( - node_id, rnode_id, goal_id, rgoal_id, jumpnode_id, jumpcost, - jp1_id, jp1_cost, jp2_id, jp2_cost); - - if(!jp1_id.is_none()) - { - jp1_id = jps_id( - node_id.id + static_cast(jp1_cost) * map_->width()); - jpoints.push_back(jp1_id); - costs.push_back(cost_to_nodeid + jumpcost + jp1_cost); - if(jp2_cost == 0) { break; } // no corner cutting - } - - if(!jp2_id.is_none()) - { - jpoints.push_back(jp2_id); - costs.push_back(cost_to_nodeid + jumpcost + jp2_cost); - if(jp1_cost == 0) { break; } // no corner cutting - } - node_id = jumpnode_id; - cost_to_nodeid += jumpcost; - } -} - -void -online_jump_point_locator2::jump_southeast_( - jps_id& node_id, jps_id& rnode_id, jps_id goal_id, jps_id rgoal_id, - jps_id& jumpnode_id, warthog::cost_t& jumpcost, jps_id& jp_id1, - warthog::cost_t& cost1, jps_id& jp_id2, warthog::cost_t& cost2) - -{ - uint32_t num_steps = 0; - - // jump a single step at a time (no corner cutting) - uint32_t rmapw = rmap_->width(); - uint32_t mapw = map_->width(); - while(true) - { - num_steps++; - node_id = jps_id(node_id.id + mapw + 1); - rnode_id = jps_id(rnode_id.id + rmapw - 1); - - // recurse straight before stepping again diagonally; - // (ensures we do not miss any optimal turning points) - jump_south_(rnode_id, rgoal_id, jp_id1, cost1, rmap_); - (this->*(jump_east_fp))(node_id, goal_id, jp_id2, cost2, map_); - if(!jps_id(jp_id1.id & jp_id2.id).is_none()) { break; } - - // couldn't move in a straight dir; next step is an obstacle - if(!((uint64_t)cost1 && (uint64_t)cost2)) - { - node_id = jp_id1 = jp_id2 = jps_id::none(); - break; - } - } - jumpnode_id = node_id; - jumpcost = num_steps * warthog::DBL_ROOT_TWO; -} - -void -online_jump_point_locator2::jump_southwest( - vec_jps_id& jpoints, vec_jps_cost& costs) -{ - jps_id jumpnode_id, jp1_id, jp2_id; - warthog::cost_t jumpcost, jp1_cost, jp2_cost, cost_to_nodeid; - jumpnode_id = jp1_id = jp2_id = {}; - jumpcost = jp1_cost = jp2_cost = cost_to_nodeid = 0; - - jps_id node_id = current_node_id_; - jps_id goal_id = current_goal_id_; - jps_id rnode_id = current_rnode_id_; - jps_id rgoal_id = current_rgoal_id_; - - // first 3 bits of first 3 bytes represent a 3x3 cell of tiles - // from the grid. next_id at centre. Assume little endian format. - uint32_t neis; - map_->get_neighbours(node_id, (uint8_t*)&neis); - - // early termination (first step is invalid) - if((neis & 197376) != 197376) { return; } - - while(!node_id.is_none()) - { - jump_southwest_( - node_id, rnode_id, goal_id, rgoal_id, jumpnode_id, jumpcost, - jp1_id, jp1_cost, jp2_id, jp2_cost); - - if(!jp1_id.is_none()) - { - jp1_id = jps_id( - node_id.id + static_cast(jp1_cost) * map_->width()); - jpoints.push_back(jp1_id); - costs.push_back(cost_to_nodeid + jumpcost + jp1_cost); - if(jp2_cost == 0) { break; } - } - - if(!jp2_id.is_none()) - { - jpoints.push_back(jp2_id); - costs.push_back(cost_to_nodeid + jumpcost + jp2_cost); - if(jp1_cost == 0) { break; } - } - node_id = jumpnode_id; - cost_to_nodeid += jumpcost; - } -} - -void -online_jump_point_locator2::jump_southwest_( - jps_id& node_id, jps_id& rnode_id, jps_id goal_id, jps_id rgoal_id, - jps_id& jumpnode_id, warthog::cost_t& jumpcost, jps_id& jp_id1, - warthog::cost_t& cost1, jps_id& jp_id2, warthog::cost_t& cost2) -{ - // jump a single step (no corner cutting) - uint32_t num_steps = 0; - uint32_t mapw = map_->width(); - uint32_t rmapw = rmap_->width(); - while(true) - { - num_steps++; - node_id = jps_id(node_id.id + mapw - 1); - rnode_id = jps_id(rnode_id.id - (rmapw + 1)); - - // recurse straight before stepping again diagonally; - // (ensures we do not miss any optimal turning points) - jump_south_(rnode_id, rgoal_id, jp_id1, cost1, rmap_); - (this->*(jump_west_fp))(node_id, goal_id, jp_id2, cost2, map_); - if(!jps_id(jp_id1.id & jp_id2.id).is_none()) { break; } - - // couldn't move in a straight dir; next step is an obstacle - if(!((uint64_t)cost1 && (uint64_t)cost2)) - { - node_id = jp_id1 = jp_id2 = jps_id::none(); - break; - } - } - jumpnode_id = node_id; - jumpcost = num_steps * warthog::DBL_ROOT_TWO; -} - -} // namespace jps::jump diff --git a/src/search/CMakeLists.txt b/src/search/CMakeLists.txt index addca89..221278c 100644 --- a/src/search/CMakeLists.txt +++ b/src/search/CMakeLists.txt @@ -2,10 +2,5 @@ cmake_minimum_required(VERSION 3.13) # ( cd src/search && echo *.cpp | sort ) target_sources(warthog_libjps PRIVATE -jps2_expansion_policy.cpp jps.cpp -jps2plus_expansion_policy.cpp -jps4c_expansion_policy.cpp -jps_expansion_policy.cpp -jpsplus_expansion_policy.cpp ) diff --git a/src/search/jps.cpp b/src/search/jps.cpp index 0bf7df2..47d7748 100644 --- a/src/search/jps.cpp +++ b/src/search/jps.cpp @@ -1,4 +1,3 @@ -#include #include #include #include @@ -169,134 +168,4 @@ compute_natural(direction d, uint32_t tiles) return ret; } -#if 0 -warthog::graph::xy_graph* -create_jump_point_graph(warthog::domain::gridmap* gm) -{ - warthog::graph::xy_graph* graph = new warthog::graph::xy_graph(); - warthog::jps::online_jump_point_locator2 jpl(gm); - uint32_t mapwidth = gm->header_width(); - uint32_t mapheight = gm->header_height(); - std::unordered_map id_map; - - // add nodes to graph - for(uint32_t y = 0; y < mapheight; y++) - { - for(uint32_t x = 0; x < mapwidth; x++) - { - uint32_t from_id = gm->to_padded_id(y * mapwidth + x); - if(!gm->get_label(gm->to_padded_id(x, y))) { continue; } - - uint32_t w_id = from_id - 1; - uint32_t e_id = from_id + 1; - uint32_t s_id = from_id + gm->width(); - uint32_t n_id = from_id - gm->width(); - uint32_t nw_id = (from_id - gm->width()) - 1; - uint32_t ne_id = (from_id - gm->width()) + 1; - uint32_t sw_id = (from_id + gm->width()) - 1; - uint32_t se_id = (from_id + gm->width()) + 1; - - // detect all corner turning points (== jump points) - // and add them to the jump point graph - uint32_t tiles; - gm->get_neighbours(from_id, (uint8_t*)&tiles); - if((!gm->get_label(nw_id) && gm->get_label(w_id) - && gm->get_label(n_id)) - || (!gm->get_label(ne_id) && gm->get_label(e_id) - && gm->get_label(n_id)) - || (!gm->get_label(se_id) && gm->get_label(e_id) - && gm->get_label(s_id)) - || (!gm->get_label(sw_id) && gm->get_label(w_id) - && gm->get_label(s_id))) - { - uint32_t graph_id = graph->add_node((int32_t)x, (int32_t)y); - id_map.insert( - std::pair(from_id, graph_id)); - } - } - } - - // add edges to graph - for(uint32_t from_id = 0; from_id < graph->get_num_nodes(); from_id++) - { - int32_t x, y; - graph->get_xy(from_id, x, y); - uint32_t gm_id - = gm->to_padded_id((uint32_t)y * mapwidth + (uint32_t)x); - warthog::graph::node* from = graph->get_node(from_id); - - for(uint32_t i = 0; i < 8; i++) - { - direction d = (direction)(1 << i); - std::vector jpoints; - std::vector jcosts; - jpl.jump(d, gm_id, warthog::INF32, jpoints, jcosts); - for(uint32_t idx = 0; idx < jpoints.size(); idx++) - { - uint32_t jp_id = jpoints[idx] & ((1 << 24) - 1); - // direction d = - // (direction)(jpoints[idx] >> 24); - std::unordered_map::iterator it_to_id; - it_to_id = id_map.find(jp_id); - assert(it_to_id != id_map.end()); - uint32_t to_id = it_to_id->second; - warthog::graph::node* to = graph->get_node(to_id); - from->add_outgoing( - warthog::graph::edge(to_id, (uint32_t)jcosts[idx])); - to->add_outgoing( - warthog::graph::edge(from_id, (uint32_t)jcosts[idx])); - } - } - } - return graph; -} -#endif - -warthog::domain::gridmap* -create_corner_map(warthog::domain::gridmap* gm) -{ - uint32_t mapwidth = gm->header_width(); - uint32_t mapheight = gm->header_height(); - warthog::domain::gridmap* corner_map - = new warthog::domain::gridmap(mapheight, mapwidth); - - uint32_t gmwidth = gm->width(); - - // add nodes to graph - for(uint32_t y = 0; y < mapheight; y++) - { - for(uint32_t x = 0; x < mapwidth; x++) - { - jps_id from_id = jps_id(gm->to_padded_id_from_unpadded(x, y)); - if(!gm->get_label(from_id)) { continue; } - - jps_id w_id = jps_id(from_id.id - 1); - jps_id e_id = jps_id(from_id.id + 1); - jps_id s_id = jps_id(from_id.id + gmwidth); - jps_id n_id = jps_id(from_id.id - gmwidth); - jps_id nw_id = jps_id((from_id.id - gmwidth) - 1); - jps_id ne_id = jps_id((from_id.id - gmwidth) + 1); - jps_id sw_id = jps_id((from_id.id + gmwidth) - 1); - jps_id se_id = jps_id((from_id.id + gmwidth) + 1); - - // detect all corner turning points (== jump points) - // and add them to the jump point graph - uint32_t tiles; - gm->get_neighbours(from_id, (uint8_t*)&tiles); - if((!gm->get_label(nw_id) && gm->get_label(w_id) - && gm->get_label(n_id)) - || (!gm->get_label(ne_id) && gm->get_label(e_id) - && gm->get_label(n_id)) - || (!gm->get_label(se_id) && gm->get_label(e_id) - && gm->get_label(s_id)) - || (!gm->get_label(sw_id) && gm->get_label(w_id) - && gm->get_label(s_id))) - { - corner_map->set_label(from_id, true); - } - } - } - return corner_map; -} - } // namespace jps::search diff --git a/src/search/jps2_expansion_policy.cpp b/src/search/jps2_expansion_policy.cpp deleted file mode 100644 index 35e97de..0000000 --- a/src/search/jps2_expansion_policy.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include - -namespace jps::search -{ - -jps2_expansion_policy::jps2_expansion_policy(warthog::domain::gridmap* map) - : gridmap_expansion_policy_base(map) -{ - jpl_ = new jump::online_jump_point_locator2(map); - jp_ids_.reserve(100); -} - -jps2_expansion_policy::~jps2_expansion_policy() -{ - delete jpl_; -} - -void -jps2_expansion_policy::expand( - warthog::search::search_node* current, - warthog::search::search_problem_instance* problem) -{ - reset(); - jp_ids_.clear(); - jp_costs_.clear(); - - // compute the direction of travel used to reach the current node. - // TODO: store this value with the jump point location so we don't need - // to compute it all the time - jps_id p_id = jps_id(current->get_parent()); - jps_id c_id = jps_id(current->get_id()); - direction dir_c = from_direction(p_id, c_id, map_->width()); - - // get the tiles around the current node c - uint32_t c_tiles; - jps_id current_id = jps_id(current->get_id()); - map_->get_neighbours(current_id, (uint8_t*)&c_tiles); - - // look for jump points in the direction of each natural - // and forced neighbour - uint32_t succ_dirs = compute_successors(dir_c, c_tiles); - jps_id goal_id = jps_id(problem->target_); - - for(uint32_t i = 0; i < 8; i++) - { - direction d = (direction)(1 << i); - if(succ_dirs & d) - { - jpl_->jump(d, current_id, goal_id, jp_ids_, jp_costs_); - } - } - - // uint32_t searchid = problem->get_searchid(); - for(uint32_t i = 0; i < jp_ids_.size(); i++) - { - // bits 0-23 store the id of the jump point - // bits 24-31 store the direction to the parent - jps_id jp_id = jp_ids_.at(i); - warthog::cost_t jp_cost = jp_costs_.at(i); - - warthog::search::search_node* mynode = generate(jp_id); - add_neighbour(mynode, jp_cost); - } -} - -// void -// jps2_expansion_policy::update_parent_direction(warthog::search::search_node* -// n) -//{ -// uint32_t jp_id = jp_ids_.at(this->get_current_successor_index()); -// assert(n->get_id() == (jp_id & warthog::jps::JPS_ID_MASK)); -// direction pdir = -// (direction)*(((uint8_t*)(&jp_id))+3); -// n->set_pdir(pdir); -// } - -warthog::search::search_node* -jps2_expansion_policy::generate_start_node( - warthog::search::search_problem_instance* pi) -{ - jps_id start = jps_id(pi->start_); - uint32_t max_id = map_->width() * map_->height(); - - if(start.id >= max_id) { return nullptr; } - jps_id padded_id = jps_id(start); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -warthog::search::search_node* -jps2_expansion_policy::generate_target_node( - warthog::search::search_problem_instance* pi) -{ - jps_id target = jps_id(pi->target_); - uint32_t max_id = map_->width() * map_->height(); - - if(target.id >= max_id) { return nullptr; } - jps_id padded_id = jps_id(target); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -} // namespace jps::search diff --git a/src/search/jps2plus_expansion_policy.cpp b/src/search/jps2plus_expansion_policy.cpp deleted file mode 100644 index aebffe9..0000000 --- a/src/search/jps2plus_expansion_policy.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include - -namespace jps::search -{ - -jps2plus_expansion_policy::jps2plus_expansion_policy( - warthog::domain::gridmap* map) - : gridmap_expansion_policy_base(map) -{ - jpl_ = new jump::offline_jump_point_locator2(map); - - costs_.reserve(100); - jp_ids_.reserve(100); - reset(); -} - -jps2plus_expansion_policy::~jps2plus_expansion_policy() -{ - delete jpl_; -} - -void -jps2plus_expansion_policy::expand( - warthog::search::search_node* current, - warthog::search::search_problem_instance* problem) -{ - reset(); - costs_.clear(); - jp_ids_.clear(); - - // compute the direction of travel used to reach the current node. - direction dir_c = from_direction( - jps_id(current->get_parent()), jps_id(current->get_id()), - map_->width()); - - // get the tiles around the current node c - uint32_t c_tiles; - jps_id current_id = jps_id(current->get_id()); - map_->get_neighbours(current_id, (uint8_t*)&c_tiles); - - // look for jump points in the direction of each natural - // and forced neighbour - uint32_t succ_dirs = compute_successors(dir_c, c_tiles); - jps_id goal_id = jps_id(problem->target_); - - for(uint32_t i = 0; i < 8; i++) - { - direction d = (direction)(1 << i); - if(succ_dirs & d) - { - jpl_->jump(d, current_id, goal_id, jp_ids_, costs_); - } - } - - // uint32_t searchid = problem->get_searchid(); - for(uint32_t i = 0; i < jp_ids_.size(); i++) - { - // bits 0-23 store the id of the jump point - // bits 24-31 store the direction to the parent - jps_id jp_id = jp_ids_.at(i); - // TODO: FIX jps node id - warthog::search::search_node* mynode = generate(jps_id(jp_id.id)); - add_neighbour(mynode, costs_.at(i)); - } -} - -warthog::search::search_node* -jps2plus_expansion_policy::generate_start_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - jps_id start = jps_id(pi->start_); - - if(start.id >= max_id) { return nullptr; } - jps_id padded_id = jps_id(start); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -warthog::search::search_node* -jps2plus_expansion_policy::generate_target_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - jps_id target = jps_id(pi->target_); - - if(target.id >= max_id) { return nullptr; } - jps_id padded_id = target; - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -} // namespace jps::search diff --git a/src/search/jps4c_expansion_policy.cpp b/src/search/jps4c_expansion_policy.cpp deleted file mode 100644 index 8b99b3e..0000000 --- a/src/search/jps4c_expansion_policy.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include - -namespace jps::search -{ - -jps4c_expansion_policy::jps4c_expansion_policy(warthog::domain::gridmap* map) - : gridmap_expansion_policy_base(map) -{ - jpl_ = new jump::four_connected_jps_locator(map); - reset(); -} - -jps4c_expansion_policy::~jps4c_expansion_policy() -{ - delete jpl_; -} - -void -jps4c_expansion_policy::expand( - warthog::search::search_node* current, - warthog::search::search_problem_instance* problem) -{ - reset(); - - jps_id current_id = jps_id(current->get_id()); - jps_id parent_id = jps_id(current->get_parent()); - jps_id goal_id = jps_id(problem->target_); - - // compute the direction of travel used to reach the current node. - direction dir_c = from_direction_4c(parent_id, current_id, map_->width()); - assert( - dir_c == NONE || dir_c == NORTH || dir_c == SOUTH || dir_c == EAST - || dir_c == WEST); - - // get the tiles around the current node c and determine - // which of the available moves are forced and which are natural - uint32_t c_tiles; - map_->get_neighbours(current_id, (uint8_t*)&c_tiles); - uint32_t succ_dirs = compute_successors_4c(dir_c, c_tiles); - - for(uint32_t i = 0; i < 8; i++) - { - direction d = (direction)(1 << i); - if(succ_dirs & d) - { - double jumpcost; - jps_id succ_id; - jpl_->jump(d, current_id, goal_id, succ_id, jumpcost); - - if(!succ_id.is_none()) - { - warthog::search::search_node* jp_succ - = this->generate(succ_id); - // if(jp_succ->get_searchid() != search_id) { - // jp_succ->reset(search_id); } - add_neighbour(jp_succ, jumpcost); - } - } - } -} - -warthog::search::search_node* -jps4c_expansion_policy::generate_start_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - if(pi->start_.id >= max_id) { return nullptr; } - jps_id padded_id = jps_id(pi->start_); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -warthog::search::search_node* -jps4c_expansion_policy::generate_target_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - if(pi->target_.id >= max_id) { return nullptr; } - jps_id padded_id = jps_id(pi->target_); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -} // namespace jps::search diff --git a/src/search/jps_expansion_policy.cpp b/src/search/jps_expansion_policy.cpp deleted file mode 100644 index 9917ef1..0000000 --- a/src/search/jps_expansion_policy.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include - -namespace jps::search -{ - -jps_expansion_policy::jps_expansion_policy(warthog::domain::gridmap* map) - : gridmap_expansion_policy_base(map) -{ - jpl_ = new jump::online_jump_point_locator(map); - reset(); -} - -jps_expansion_policy::~jps_expansion_policy() -{ - delete jpl_; -} - -void -jps_expansion_policy::expand( - warthog::search::search_node* current, - warthog::search::search_problem_instance* problem) -{ - reset(); - - // compute the direction of travel used to reach the current node. - jps_id current_id = jps_id(current->get_id()); - direction dir_c = from_direction( - jps_id(current->get_parent()), current_id, map_->width()); - - // get the tiles around the current node c - uint32_t c_tiles; - map_->get_neighbours(current_id, (uint8_t*)&c_tiles); - - // look for jump points in the direction of each natural - // and forced neighbour - uint32_t succ_dirs = compute_successors(dir_c, c_tiles); - jps_id goal_id = jps_id(problem->target_); - // uint32_t search_id = problem->get_searchid(); - for(uint32_t i = 0; i < 8; i++) - { - direction d = (direction)(1 << i); - if(succ_dirs & d) - { - warthog::cost_t jumpcost; - jps_id succ_id; - jpl_->jump(d, current_id, goal_id, succ_id, jumpcost); - - if(!succ_id.is_none()) - { - warthog::search::search_node* jp_succ - = this->generate(succ_id); - // if(jp_succ->get_searchid() != search_id) { - // jp_succ->reset(search_id); } - add_neighbour(jp_succ, jumpcost); - } - } - } -} - -warthog::search::search_node* -jps_expansion_policy::generate_start_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - if(static_cast(pi->start_) >= max_id) { return nullptr; } - jps_id padded_id = jps_id(pi->start_); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -warthog::search::search_node* -jps_expansion_policy::generate_target_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - if(static_cast(pi->target_) >= max_id) { return nullptr; } - jps_id padded_id = jps_id(pi->target_); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -} // namespace jps::search diff --git a/src/search/jpsplus_expansion_policy.cpp b/src/search/jpsplus_expansion_policy.cpp deleted file mode 100644 index 58de6db..0000000 --- a/src/search/jpsplus_expansion_policy.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include - -namespace jps::search -{ - -jpsplus_expansion_policy::jpsplus_expansion_policy( - warthog::domain::gridmap* map) - : gridmap_expansion_policy_base(map) -{ - jpl_ = new jump::offline_jump_point_locator(map); -} - -jpsplus_expansion_policy::~jpsplus_expansion_policy() -{ - delete jpl_; -} - -void -jpsplus_expansion_policy::expand( - warthog::search::search_node* current, - warthog::search::search_problem_instance* problem) -{ - reset(); - - // compute the direction of travel used to reach the current node. - direction dir_c = from_direction( - jps_id(current->get_parent()), jps_id(current->get_id()), - map_->width()); - - // get the tiles around the current node c - uint32_t c_tiles; - jps_id current_id = jps_id(current->get_id()); - map_->get_neighbours(current_id, (uint8_t*)&c_tiles); - - // look for jump points in the direction of each natural - // and forced neighbour - uint32_t succ_dirs = compute_successors(dir_c, c_tiles); - jps_id goal_id = jps_id(problem->target_); - for(uint32_t i = 0; i < 8; i++) - { - direction d = (direction)(1 << i); - if(succ_dirs & d) - { - double jumpcost; - jps_id succ_id; - jpl_->jump(d, current_id, goal_id, succ_id, jumpcost); - - if(!succ_id.is_none()) - { - add_neighbour(this->generate(succ_id), jumpcost); - } - } - } -} - -warthog::search::search_node* -jpsplus_expansion_policy::generate_start_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - if((uint32_t)pi->start_ >= max_id) { return nullptr; } - jps_id padded_id = jps_id(pi->start_); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -warthog::search::search_node* -jpsplus_expansion_policy::generate_target_node( - warthog::search::search_problem_instance* pi) -{ - uint32_t max_id = map_->width() * map_->height(); - if((uint32_t)pi->target_ >= max_id) { return nullptr; } - jps_id padded_id = jps_id(pi->target_); - if(map_->get_label(padded_id) == 0) { return nullptr; } - return generate(padded_id); -} - -} // namespace jps::search