diff --git a/README.md b/README.md index b3c2ddc..900a758 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,18 @@ 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 more details on Warthog setup. + @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 @@ -17,8 +29,29 @@ cmake --build build ./build/warthog-jps --alg jps --scen example.scen ``` +The example scenario `example.scen` is not a part of this repo. +Get benchmark from places like MovingAI or pathfinding.ai 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). +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. diff --git a/apps/jps.cpp b/apps/jps.cpp index 65d9808..2d8d29d 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -68,7 +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, jpsP, jpsP32, jps+, jpsP+\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" @@ -246,25 +246,19 @@ main(int argc, char** argv) return run_jps>( scenmgr, mapfile, alg); } - else if(alg == "jpsP") + else if(alg == "jpsP" || alg == "jps2") { using jump_point = jps::jump::jump_point_online; return run_jps>( scenmgr, mapfile, alg); } - else if(alg == "jpsP32") - { - using jump_point = jps::jump::jump_point_online; - return run_jps>( - scenmgr, mapfile, alg); - } else if(alg == "jps+") { using jump_point = jps::jump::jump_point_offline<>; return run_jps>( scenmgr, mapfile, alg); } - else if(alg == "jpsP+") + else if(alg == "jpsP+" || alg == "jps2+") { using jump_point = jps::jump::jump_point_offline<>; return run_jps>( diff --git a/include/jps/domain/rotate_gridmap.h b/include/jps/domain/rotate_gridmap.h index ace9ea5..ae4688b 100644 --- a/include/jps/domain/rotate_gridmap.h +++ b/include/jps/domain/rotate_gridmap.h @@ -1,10 +1,29 @@ #ifndef WARTHOG_DOMAIN_ROTATE_GRIDMAP_H #define WARTHOG_DOMAIN_ROTATE_GRIDMAP_H -// domain::gridmap.h // -// A uniform cost domain::gridmap implementation. The map is stored in -// a compact matrix form. Nodes are represented as single bit quantities. +// 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 @@ -385,7 +404,8 @@ struct gridmap_rotate_ptr : std::array } operator bool() const noexcept { return (*this)[0]; } }; -/// @brief a copy-by-value class pointing to grid/rgrid with conversions +/// @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 { @@ -429,7 +449,8 @@ struct gridmap_rotate_table : std::array } operator bool() const noexcept { return (*this)[0].data(); } }; -/// @brief a copy-by-value class for fast access to grid/rgrid with conversions +/// @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 { @@ -462,6 +483,14 @@ struct gridmap_rotate_table_convs : gridmap_rotate_table, } }; +/// 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: @@ -589,6 +618,12 @@ class rotate_gridmap : public rgridmap_point_conversions 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 diff --git a/include/jps/forward.h b/include/jps/forward.h index 9726f1f..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,7 +18,7 @@ namespace jps { -using namespace warthog::grid; +using namespace ::warthog::grid; using ::warthog::pad_id; // using jps_id = grid_id; struct rmap_id_tag @@ -18,9 +26,6 @@ struct rmap_id_tag using rgrid_id = warthog::identity_base; using warthog::cost_t; -using vec_jps_id = std::vector; -using vec_jps_cost = std::vector; - } // namespace jps #include diff --git a/include/jps/jump/block_online.h b/include/jps/jump/block_online.h new file mode 100644 index 0000000..0bc2c4c --- /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 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 < + // than target is reachable, if equal then trav pos is the target + // if greater, than 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 < + // than target is reachable, if equal then trav pos is the target + // if greater, than 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/jump.h b/include/jps/jump/jump.h index 78fb863..ba01363 100644 --- a/include/jps/jump/jump.h +++ b/include/jps/jump/jump.h @@ -1,6 +1,16 @@ #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 @@ -10,6 +20,7 @@ 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 { @@ -26,12 +37,15 @@ 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 { @@ -46,6 +60,7 @@ get_hori_from_intercardinal(direction_id d) noexcept 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 { @@ -61,11 +76,16 @@ get_vert_from_intercardinal(direction_id d) noexcept (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; - jump_distance hori; - jump_distance vert; + 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 diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index 2cb32e2..70bdee5 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -1,12 +1,20 @@ #ifndef JPS_JUMP_JUMP_POINT_OFFLINE_H #define JPS_JUMP_JUMP_POINT_OFFLINE_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 +// +// 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 Ryan Hechenberger +// @created 2025-11-20 // #include "jump_point_online.h" @@ -18,10 +26,12 @@ namespace jps::jump { -/// @brief Store, set and access offline jump-point results -/// @tparam ChainJump if true: store 1-byte jumps that chain; false: 2-byte -/// full jump -/// @tparam DeadEnd if true: track deadend as negative, false: only jump-points +/// @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 { @@ -318,10 +328,11 @@ class jump_point_offline : public OnlinePoint return res; } - /// @brief shoot ray to target point + /// @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 shoot from loc - /// @param target shoot to target + /// @param loc x/y location (node_id points here) + /// @param target target point to check visiblity to /// @return pair , if both >= 0 /// than target is visible, /// first<0 means intercardinal reaches blocker at -first distance @@ -432,6 +443,7 @@ class jump_point_offline : public OnlinePoint } } + /// @brief set the underlying map, re-computing offline jump table void set_map(const rotate_grid& map) { @@ -439,6 +451,7 @@ class jump_point_offline : public OnlinePoint // 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() { @@ -450,6 +463,8 @@ class jump_point_offline : public OnlinePoint // 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; @@ -462,6 +477,9 @@ class jump_point_offline : public OnlinePoint {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) { @@ -526,6 +544,9 @@ class jump_point_offline : public OnlinePoint // // 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; diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index abcaf21..58f7a91 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -1,14 +1,20 @@ #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 Ryan Hechenberger +// @created 2025-11-20 // +#include "block_online.h" #include "jump.h" #include #include @@ -19,648 +25,6 @@ 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 < - // than target is reachable, if equal then trav pos is the target - // if greater, than 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 < - // than target is reachable, if equal then trav pos is the target - // if greater, than 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()}; - } -}; - class jump_point_online { public: @@ -995,6 +359,6 @@ jump_point_online::jump_target( return {inter_len, cardinal_len}; } -} +} // namespace jps::jump #endif // JPS_JUMP_JUMP_POINT_ONLINE_H diff --git a/include/jps/search/jps.h b/include/jps/search/jps.h index a03c629..2ac7c2d 100644 --- a/include/jps/search/jps.h +++ b/include/jps/search/jps.h @@ -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/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index 2ce9572..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,17 +9,18 @@ // 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_gridmap_expansion_policy.h" +#include "jps_expansion_policy_base.h" #include #include #include @@ -26,19 +28,24 @@ namespace jps::search { -/// @brief -/// @tparam JpsJump +/// @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 intercardinal -/// jump point, block-based jumping is the standard jump used by -/// jump_point_online. jps_2011_expansion_policy gives JPS -/// (B). jps_2011_expansion_policy gives JPS+. +/// 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_gridmap_expansion_policy +class jps_expansion_policy : public jps_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(warthog::domain::gridmap* map) - : jps_gridmap_expansion_policy(map) + : jps_expansion_policy_base(map) { if(map != nullptr) { @@ -65,9 +72,9 @@ class jps_expansion_policy : public jps_gridmap_expansion_policy size_t mem() override { - return jps_gridmap_expansion_policy::mem() + return jps_expansion_policy_base::mem() + (sizeof(jps_expansion_policy) - - sizeof(jps_gridmap_expansion_policy)); + - sizeof(jps_expansion_policy_base)); } jump_point& @@ -85,7 +92,7 @@ class jps_expansion_policy : public jps_gridmap_expansion_policy void set_rmap_(domain::rotate_gridmap& rmap) override { - jps_gridmap_expansion_policy::set_rmap_(rmap); + jps_expansion_policy_base::set_rmap_(rmap); jpl_.set_map(rmap); map_width_ = rmap.map().width(); } 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_gridmap_expansion_policy.h b/include/jps/search/jps_gridmap_expansion_policy.h deleted file mode 100644 index c1b5305..0000000 --- a/include/jps/search/jps_gridmap_expansion_policy.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef JPS_SEARCH_JPS_GRIDMAP_EXPANSION_POLICY_H -#define JPS_SEARCH_JPS_GRIDMAP_EXPANSION_POLICY_H - -// jps_gridmap_expansion_policy.h -// -// This expansion policy is a base policy for the jps algorithms. -// It creates a common class for the rotated gridmap. - -#include "jps.h" -#include -#include - -namespace jps::search -{ - -class jps_gridmap_expansion_policy - : public warthog::search::gridmap_expansion_policy_base -{ -public: - jps_gridmap_expansion_policy(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_gridmap_expansion_policy) - - sizeof(gridmap_expansion_policy_base)) - + map_->mem(); - } - - void - set_map(warthog::domain::gridmap& map) - { - rmap_.create_rmap(map); - gridmap_expansion_policy_base::set_map(map); - set_rmap_(rmap_); - } - 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_GRIDMAP_EXPANSION_POLICY_H diff --git a/include/jps/search/jps_prune_expansion_policy.h b/include/jps/search/jps_prune_expansion_policy.h index 16895bb..24ba147 100644 --- a/include/jps/search/jps_prune_expansion_policy.h +++ b/include/jps/search/jps_prune_expansion_policy.h @@ -1,24 +1,29 @@ #ifndef JPS_SEARCH_JPS_PRUNE_EXPANSION_POLICY_H #define JPS_SEARCH_JPS_PRUNE_EXPANSION_POLICY_H -// jps_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. // -// 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+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 +// @author: dharabor & Ryan Hechenberger // @created: 06/01/2010 +// -#include "jps_gridmap_expansion_policy.h" +#include "jps_expansion_policy_base.h" #include #include #include @@ -26,27 +31,38 @@ namespace jps::search { -/// @brief -/// @tparam JpsJump -/// @tparam InterLimit max length the intercardinal can expand to, =0 for -/// run-time set, -1 to prune all intercardinal points -/// @tparam InterSize size of intercardinal successor array, is stored on the -/// stack. -/// If this successor count is reached withing InterLimit, -/// then end successor unless InterLimit<0 +/// @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 than 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. /// -/// JPS expansion policy that pushes the first cardinal and intercardinal -/// jump point, block-based jumping is the standard jump used by -/// jump_point_online. jps_2011_expansion_policy gives JPS -/// (B). jps_2011_expansion_policy gives JPS+. +/// 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_gridmap_expansion_policy +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_gridmap_expansion_policy(map) + : jps_expansion_policy_base(map) { if(map != nullptr) { @@ -54,7 +70,7 @@ class jps_prune_expansion_policy : public jps_gridmap_expansion_policy map_width_ = rmap_.map().width(); } } - using jps_gridmap_expansion_policy::jps_gridmap_expansion_policy; + using jps_expansion_policy_base::jps_expansion_policy_base; ~jps_prune_expansion_policy() = default; using jump_point = JpsJump; @@ -74,9 +90,9 @@ class jps_prune_expansion_policy : public jps_gridmap_expansion_policy size_t mem() override { - return jps_gridmap_expansion_policy::mem() + return jps_expansion_policy_base::mem() + (sizeof(jps_prune_expansion_policy) - - sizeof(jps_gridmap_expansion_policy)); + - sizeof(jps_expansion_policy_base)); } jump_point& @@ -117,7 +133,7 @@ class jps_prune_expansion_policy : public jps_gridmap_expansion_policy void set_rmap_(domain::rotate_gridmap& rmap) override { - jps_gridmap_expansion_policy::set_rmap_(rmap); + jps_expansion_policy_base::set_rmap_(rmap); jpl_.set_map(rmap); map_width_ = rmap.map().width(); } @@ -214,8 +230,7 @@ jps_prune_expansion_policy::expand( jump::intercardinal_jump_result res[InterSize]; jump::jump_distance inter_total = 0; while(true) - { // InterSize == -1 will loop InterSize results until all are - // found + { auto [result_n, dist] = jpl_.template jump_intercardinal_many( pair_id, res, InterSize, get_jump_limit()); diff --git a/src/search/jps.cpp b/src/search/jps.cpp index b774a56..47d7748 100644 --- a/src/search/jps.cpp +++ b/src/search/jps.cpp @@ -168,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