From 6120bd51d10243257ad836f6426f86f580313c58 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Sun, 22 Jun 2025 16:05:35 +1000 Subject: [PATCH 01/35] refactor progress --- apps/jps.cpp | 2 + include/jps/domain/rotate_gridmap.h | 325 ++++++++++ include/jps/forward.h | 16 +- include/jps/jump/jump.h | 56 ++ include/jps/jump/jump_point_offline.h | 171 ++++++ include/jps/jump/jump_point_online.h | 677 ++++++++------------- include/jps/search/jps_expansion_policy2.h | 1 - 7 files changed, 801 insertions(+), 447 deletions(-) create mode 100644 include/jps/domain/rotate_gridmap.h create mode 100644 include/jps/jump/jump.h create mode 100644 include/jps/jump/jump_point_offline.h diff --git a/apps/jps.cpp b/apps/jps.cpp index 4493c6e..d6a9cb4 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -13,6 +13,8 @@ #include #include #include +#include +#include #include #include #include diff --git a/include/jps/domain/rotate_gridmap.h b/include/jps/domain/rotate_gridmap.h new file mode 100644 index 0000000..333b700 --- /dev/null +++ b/include/jps/domain/rotate_gridmap.h @@ -0,0 +1,325 @@ +#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. +// + +#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 int map_id = 1; +}; +template <> +struct direction_grid_id +{ + using type = grid_id; + static constexpr int map_id = 0; +}; +template <> +struct direction_grid_id +{ + using type = rgrid_id; + static constexpr int map_id = 1; +}; +template <> +struct direction_grid_id +{ + using type = grid_id; + static constexpr int map_id = 0; +}; +template <> +struct direction_grid_id +{ + using type = rgrid_id; + static constexpr int map_id = 1; +}; +template <> +struct direction_grid_id +{ + using type = grid_id; + static constexpr int map_id = 0; +}; +template <> +struct direction_grid_id +{ + using type = rgrid_id; + static constexpr int map_id = 1; +}; +template <> +struct direction_grid_id +{ + using type = grid_id; + static constexpr int map_id = 0; +}; + +} // 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 direction_grid_id_t = typename details::direction_grid_id::type; + +template +inline constexpr int direction_grid_index = details::direction_grid_id::map_id; + +using ::warthog::domain::gridmap; + +struct rgridmap_point_conversions +{ + uint16_t map_unpad_height_m1_ = 0; + uint16_t map_pad_width_ = 0; + uint16_t rmap_pad_width_ = 0; + + 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)}; + } + grid_id + point_to_id(point p) const noexcept + { + return jps_id{ + static_cast(p.y + domain::gridmap::PADDED_ROWS) + * map_pad_width_ + + static_cast(p.x)}; + } + rgrid_id + rpoint_to_rid(point p) const noexcept + { + return rgrid_id{ + static_cast(p.y + domain::gridmap::PADDED_ROWS) + * rmap_pad_width_ + + static_cast(p.x)}; + } + point + id_to_point(grid_id p) const noexcept + { + return { + static_cast(p.id % map_pad_width_), + static_cast(p.id / map_pad_width_ - domain::gridmap::PADDED_ROWS)}; + } + point + rid_to_rpoint(rgrid_id p) const noexcept + { + return { + static_cast(p.id % rmap_pad_width_), + static_cast( + p.id / rmap_pad_width_ - domain::gridmap::PADDED_ROWS)}; + } + 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))); + } + + template + requires std::same_as || std::same_as + direction_grid_id_t to_id_d(GridId id) const noexcept + { + using res_type = direction_grid_id_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 + direction_grid_id_t point_to_id_d(point id) const noexcept + { + using res_type = direction_grid_id_t; + if constexpr (std::same_as) { + return point_to_id(id); + } else { + return rpoint_to_rid(point_to_rpoint(id)); + } + } + + template + direction_grid_id_t rpoint_to_id_d(point id) const noexcept + { + using res_type = direction_grid_id_t; + if constexpr (std::same_as) { + return point_to_id(rpoint_to_point(id)); + } else { + return rpoint_to_rid(id); + } + } +}; + +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]; } +}; +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), rgridmap_point_conversions{static_cast(l_map.header_height()-1), static_cast(l_map.width()), static_cast(l_rmap.width())} + { } + gridmap_rotate_ptr_convs(gridmap_rotate_ptr maps, uint16_t map_unpad_height_m1) noexcept + : gridmap_rotate_ptr(maps), rgridmap_point_conversions{static_cast(maps[0]->header_height()-1), static_cast(maps[0]->width()), static_cast(maps[1]->width())} + { } +}; +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::bittable& map() noexcept { return (*this)[0]; } + const domain::gridmap::bittable& map() const noexcept { return (*this)[0]; } + domain::gridmap::bittable& rmap() noexcept { return (*this)[1]; } + const domain::gridmap::bittable& rmap() const noexcept { return (*this)[1]; } +}; +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), rgridmap_point_conversions(l_map.header_height() - 1) + { } + gridmap_rotate_table_convs(gridmap_rotate_table maps, uint16_t map_unpad_height_m1) noexcept + : gridmap_rotate_table(maps), rgridmap_point_conversions{map_unpad_height_m1, static_cast(maps[0].width()), static_cast(maps[1].width())} + { } +}; + +class rotate_gridmap : public rgridmap_point_conversions +{ +private: + std::unique_ptr rmap_obj; + gridmap_rotate_ptr maps = {}; + uint32_t map_unpad_height_m1_ = 0; + +public: + rotate_gridmap() = default; + rotate_gridmap(domain::gridmap& map, domain::gridmap* rmap = nullptr) + { + if (rmap != nullptr) { + maps[0] = ↦ + maps[1] = rmap; + map_unpad_height_m1_ = map.header_width() - 1; + } else { + create_rmap(map); + } + } + + void create_rmap(domain::gridmap& map) + { + maps[0] = ↦ + + const uint32_t maph = map.header_height(); + const uint32_t mapw = map.header_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_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); + } + } + + // set values + rmap_obj = std::move(tmap); + maps[1] = rmap_obj.get(); + map_unpad_height_m1_ = maph - 1; + } + + 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 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, map_unpad_height_m1_); + } + 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, map_unpad_height_m1_); + } +}; + +} // namespace warthog::grid + +#endif // WARTHOG_DOMAIN_GRIDMAP_H diff --git a/include/jps/forward.h b/include/jps/forward.h index 9107288..9d9a564 100644 --- a/include/jps/forward.h +++ b/include/jps/forward.h @@ -10,25 +10,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 diff --git a/include/jps/jump/jump.h b/include/jps/jump/jump.h new file mode 100644 index 0000000..dbd7f15 --- /dev/null +++ b/include/jps/jump/jump.h @@ -0,0 +1,56 @@ +#ifndef JPS_JUMP_JUMP_H +#define JPS_JUMP_JUMP_H + +#include + +// Common types for jump + +namespace jps::jump +{ + +using jump_distance = int16_t; + +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; +} + +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 + ); +} +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 + ); +} + +struct intercardinal_jump_result +{ + jump_distance inter; + jump_distance hori; + jump_distance vert; +}; + +} // 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..dd4c174 --- /dev/null +++ b/include/jps/jump/jump_point_offline.h @@ -0,0 +1,171 @@ +#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] +// + +#include "jump_point_online.h" + +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 +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, jump_res_width>; + using length = int32_t; + struct alignas(int64_t) cell : std::array + { }; + + std::unique_ptr db; + uint32_t width = 0; +#ifndef NDEBUG + uint32_t d_cells = 0; +#endif + static constexpr uint32_t node_count(uint32_t width, uint32_t height) noexcept + { + return static_cast(width) * static_cast(height); + } + static constexpr size_t mem(uint32_t width, uint32_t height) noexcept + { + return node_count(width, height) * sizeof(jump_res); + } + + static consteval length chain_length() noexcept + { + constexpr length amax = static_cast(std::numeric_limits::max()); + constexpr length amin = static_cast(std::abs(std::numeric_limits::min())); + // 254 127 + return !DeadEnd ? amax - 1 : std::min(amax, amin-1); + } + static consteval jump_res chain_value() noexcept + { + return DeadEnd ? std::numeric_limits::min() : std::numeric_limits::max(); + } + + /// @brief setup db, init to zero + void init(uint32_t width, uint32_t height) + { + db = std::make_unique(node_count(width, height)); + this->width = width; +#ifndef NDEBUG + this->d_cells = node_count(width, height); +#endif + } + /// @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, jps_id loc, length len) noexcept + { + // no negative for deadend + assert(d < 8); + assert(db != nullptr); + assert(loc.id < d_cells); + assert(DeadEnd || len >= 0); + assert(std::abs(len) < std::numeric_limits::max()); + int32_t id = static_cast(loc.id); + const int32_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 + value = std::abs(len) <= chain_length() ? static_cast(len) : chain_value(); + } else { + value = len <= chain_length() ? 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, jps_id loc) noexcept + { + // no negative for deadend + assert(d < 8); + assert(db != nullptr); + assert(loc.id < d_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 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, jps_id loc) noexcept requires(ChainJump) + { + assert(db != nullptr); + assert(loc.id < d_cells); + assert(db[loc.id][d] == chain_value()); + int32_t id = loc.id; + const int32_t id_adj = warthog::grid::dir_id_adj(d, width); + length len = 0; + jump_res j; + do { + // continue from previous jump + id += id_adj; + len += chain_length(); + j = static_cast(db[id][d]); + } while (j == chain_value()); + assert(j != 0); + if constexpr (DeadEnd) { + // check if block and negate + len = j >= 0 ? (len+j) : -(len-j); + } else { + len += j; + } + return len; + } + + /// @brief get jump cell + /// @param loc location + /// @return cell + cell operator[](jps_id loc) const noexcept + { + assert(db != nullptr); + assert(loc.id < d_cells); + return db[loc.id]; + } +}; + +template > +class jump_point_offline +{ + +}; + +} // 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 0f01692..7183ff5 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -9,10 +9,11 @@ // Online Graph Pruning Pathfinding on Grid Maps, AAAI] // -#include #include +#include "jump.h" +#include #include -#include +#include namespace jps::jump { @@ -26,11 +27,13 @@ namespace details /// @param node the starting location /// @param goal the goal location /// @return -template -int32_t +template +jump_distance jump_point_online_hori( - ::warthog::domain::gridmap::bittable map, uint32_t node, uint32_t goal) + ::warthog::domain::gridmap::bittable map, uint32_t node, uint32_t goal[[maybe_unused]] = std::numeric_limits::max()) { + assert(map.data() != nullptr); + assert(Goal != (goal == std::numeric_limits::max())); // read tiles from the grid: // - along the row of node_id // - from the row above node_id @@ -47,7 +50,7 @@ jump_point_online_hori( nei_slider.width8_bits = East ? nei_slider.width8_bits + 8 : 15 - nei_slider.width8_bits; // 15 - width8_bits == 63 - (width8_bits + 6 * 8) - int32_t jump_count = 0; + jump_distance jump_count = 0; // order going east is stored as least significant bit to most significant // bit @@ -88,9 +91,9 @@ jump_point_online_hori( // 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) - - static_cast(nei_slider.width8_bits); - uint32_t goal_jump = East ? goal - node : node - goal; + jump_count += static_cast(stop_pos) + - static_cast(nei_slider.width8_bits); + uint32_t goal_jump[[maybe_unused]] = Goal ? (East ? goal - node : node - goal) : 0; // if blocked: pos + jump_count = first block // otherwise: jump_count = trav cell after turn (jump point // location) @@ -102,10 +105,10 @@ jump_point_online_hori( // must be checked as unsigned for: // 1. goal.is_none(): will not fit int32_t // 2. underflow means goal is in opposite direction, desirable - if(goal_jump <= static_cast(jump_count)) + if(Goal && (goal_jump <= static_cast(jump_count))) { // goal reached - jump_count = static_cast(goal_jump); + jump_count = static_cast(goal_jump); } else if( East ? !(neis[0] & (static_cast(1) << stop_pos)) @@ -122,7 +125,7 @@ jump_point_online_hori( } // failed, goto next 56 bits - jump_count += 63 - nei_slider.width8_bits; + jump_count += static_cast(63 - nei_slider.width8_bits); nei_slider.adj_bytes(East ? 7 : -7); nei_slider.width8_bits = 7; } @@ -130,22 +133,18 @@ jump_point_online_hori( } // namespace details -struct intercardinal_jump_result -{ - jps_id node; - jps_rid rnode; - uint32_t dist; -}; -template + +template + requires InterCardinalId struct IntercardinalWalker { static_assert( - D == NORTHEAST || D == NORTHWEST || D == SOUTHEAST || D == SOUTHWEST, + D == NORTHEAST_ID || D == NORTHWEST_ID || D == SOUTHEAST_ID || D == SOUTHWEST_ID, "Must be intercardinal direction"); union LongJumpRes { - uint32_t dist[2]; /// distance hori/vert of D a jump is valid to - uint64_t joint; + jump_distance dist[2]; /// distance hori/vert of D a jump is valid to + uint32_t joint; }; using map_type = ::warthog::domain::gridmap::bitarray; /// @brief map and rmap (as bit array for small memory size) @@ -154,8 +153,8 @@ struct IntercardinalWalker 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 map and rmap goal locations + // uint32_t goal[2]; /// @brief row scan union { @@ -166,78 +165,82 @@ struct IntercardinalWalker /// @brief convert map width to a map adj_width variable suited to /// intercardinal D2 - template + template + requires InterCardinalId static constexpr int32_t to_map_adj_width(uint32_t width) noexcept { static_assert( - D2 == NORTHEAST || D2 == NORTHWEST || D2 == SOUTHEAST - || D2 == SOUTHWEST, + D2 == NORTHEAST_ID || D2 == NORTHWEST_ID || D2 == SOUTHEAST_ID + || D2 == SOUTHWEST_ID, "Must be intercardinal direction"); assert(width > 0); - if constexpr(D2 == NORTHEAST) + if constexpr(D2 == NORTHEAST_ID) { return -static_cast(width - 1); // - (mapW-1) } - else if constexpr(D2 == SOUTHEAST) + else if constexpr(D2 == SOUTHEAST_ID) { return static_cast(width + 1); // + (mapW+1) } - else if constexpr(D2 == SOUTHWEST) + else if constexpr(D2 == SOUTHWEST_ID) { return static_cast(width - 1); // + (mapW-1) } else - { // NORTHWEST + { // NORTHWEST_ID return -static_cast(width + 1); // - (mapW+1) } } /// @brief convert rmap width to a rmap adj_width variable suited to /// intercardinal D2 - template + template + requires InterCardinalId static constexpr int32_t to_rmap_adj_width(uint32_t width) noexcept { - return to_map_adj_width(width); + return to_map_adj_width(width); } /// @brief convert map adj_width to map width, reciprocal to /// to_map_adj_width - template + template + requires InterCardinalId static constexpr uint32_t from_map_adj_width(int32_t adj_width) noexcept { static_assert( - D2 == NORTHEAST || D2 == NORTHWEST || D2 == SOUTHEAST - || D2 == SOUTHWEST, + D2 == NORTHEAST_ID || D2 == NORTHWEST_ID || D2 == SOUTHEAST_ID + || D2 == SOUTHWEST_ID, "Must be intercardinal direction"); - if constexpr(D2 == NORTHEAST) + if constexpr(D2 == NORTHEAST_ID) { return static_cast(-adj_width + 1); } - else if constexpr(D2 == SOUTHEAST) + else if constexpr(D2 == SOUTHEAST_ID) { return static_cast(adj_width - 1); } - else if constexpr(D2 == SOUTHWEST) + else if constexpr(D2 == SOUTHWEST_ID) { return static_cast(adj_width + 1); } else - { // NORTHWEST + { // NORTHWEST_ID return static_cast(-adj_width - 1); } } /// @brief convert rmap adj_width to rmap width, reciprocal to /// to_rmap_adj_width - template + template + requires InterCardinalId static constexpr uint32_t from_rmap_adj_width(int32_t adj_width) noexcept { - return from_map_adj_width(adj_width); + return from_map_adj_width(adj_width); } - /// @brief set map width` + /// @brief set map width void map_width(uint32_t width) noexcept { @@ -262,64 +265,124 @@ struct IntercardinalWalker return from_rmap_adj_width(adj_width[1]); } - static uint32_t + static jump_distance + jump_east(map_type map, uint32_t width, uint32_t node) + { + jump_distance d = details::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 goal) { - int32_t d = details::jump_point_online_hori( + jump_distance d = details::jump_point_online_hori( ::warthog::domain::gridmap::bittable(map, width, 0), node, goal); // for LongJumpRes, must return 0 for deadend - return d >= 0 ? static_cast(d) : 0; + return d; } - static uint32_t + static jump_distance + jump_west(map_type map, uint32_t width, uint32_t node) + { + jump_distance d = details::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 goal) { - int32_t d = details::jump_point_online_hori( + jump_distance d = details::jump_point_online_hori( ::warthog::domain::gridmap::bittable(map, width, 0), node, goal); // for LongJumpRes, must return 0 for deadend - return d >= 0 ? static_cast(d) : 0; + return d; } - uint32_t + jump_distance jump_hori() { - if constexpr(D == NORTHEAST) + if constexpr(D == NORTHEAST_ID) { - return jump_east(map[0], map_width(), node_at[0], goal[0]); // east + return jump_east(map[0], map_width(), node_at[0]); // east } - else if constexpr(D == SOUTHEAST) + else if constexpr(D == SOUTHEAST_ID) { - return jump_east(map[0], map_width(), node_at[0], goal[0]); // east + return jump_east(map[0], map_width(), node_at[0]); // east } - else if constexpr(D == SOUTHWEST) + else if constexpr(D == SOUTHWEST_ID) { - return jump_west(map[0], map_width(), node_at[0], goal[0]); // west + return jump_west(map[0], map_width(), node_at[0]); // west } - else if constexpr(D == NORTHWEST) + else if constexpr(D == NORTHWEST_ID) { - return jump_west(map[0], map_width(), node_at[0], goal[0]); // west + return jump_west(map[0], map_width(), node_at[0]); // west } } - uint32_t + jump_distance + jump_hori(grid_id goal) + { + if constexpr(D == NORTHEAST_ID) + { + return jump_east(map[0], map_width(), node_at[0], static_cast(goal)); // east + } + else if constexpr(D == SOUTHEAST_ID) + { + return jump_east(map[0], map_width(), node_at[0], static_cast(goal)); // east + } + else if constexpr(D == SOUTHWEST_ID) + { + return jump_west(map[0], map_width(), node_at[0], static_cast(goal)); // west + } + else if constexpr(D == NORTHWEST_ID) + { + return jump_west(map[0], map_width(), node_at[0], static_cast(goal)); // west + } + } + jump_distance jump_vert() { - if constexpr(D == NORTHEAST) + 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 goal) + { + if constexpr(D == NORTHEAST_ID) { return jump_east( - map[1], rmap_width(), node_at[1], goal[1]); // north + map[1], rmap_width(), node_at[1], static_cast(goal)); // north } - else if constexpr(D == SOUTHEAST) + else if constexpr(D == SOUTHEAST_ID) { return jump_west( - map[1], rmap_width(), node_at[1], goal[1]); // south + map[1], rmap_width(), node_at[1], static_cast(goal)); // south } - else if constexpr(D == SOUTHWEST) + else if constexpr(D == SOUTHWEST_ID) { return jump_west( - map[1], rmap_width(), node_at[1], goal[1]); // south + map[1], rmap_width(), node_at[1], static_cast(goal)); // south } - else if constexpr(D == NORTHWEST) + else if constexpr(D == NORTHWEST_ID) { return jump_east( - map[1], rmap_width(), node_at[1], goal[1]); // north + map[1], rmap_width(), node_at[1], static_cast(goal)); // north } } @@ -332,7 +395,7 @@ struct IntercardinalWalker static_cast(node_at[1]) + adj_width[1]); } - /// @brief return get node_at[0]-1..node_at[0]+1 bits. CAUTION: return + /// @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 @@ -355,24 +418,24 @@ struct IntercardinalWalker row_[1] = get_row(); } /// @brief get node id for location node + dist(EAST/WEST of D) - jps_id + grid_id adj_hori(uint32_t node, uint32_t dist) const noexcept { - if constexpr(D == NORTHEAST || D == SOUTHEAST) + if constexpr(D == NORTHEAST_ID || D == SOUTHEAST_ID) { - return jps_id{node + dist}; + return grid_id{node + dist}; } - else { return jps_id{node - dist}; } + else { return grid_id{node - dist}; } } /// @brief get node id for location node + dist(NORTH/SOUTH of D) - jps_id + rgrid_id adj_vert(uint32_t node, uint32_t dist) const noexcept { - if constexpr(D == NORTHEAST || D == SOUTHEAST) + if constexpr(D == NORTHEAST_ID || D == SOUTHEAST_ID) { - return jps_id{node + (adj_width[0] - 1) * dist}; + return rgrid_id{node + (adj_width[0] - 1) * dist}; } - else { return jps_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) @@ -381,7 +444,7 @@ struct IntercardinalWalker { // east | west differernce // north/south does not make a difference - if constexpr(D == NORTHEAST || D == SOUTHEAST) + if constexpr(D == NORTHEAST_ID || D == SOUTHEAST_ID) { // we want from grid // .xx == row[0] = 0bxx. @@ -414,404 +477,150 @@ struct IntercardinalWalker } }; -template class jump_point_online { public: - using gridmap = warthog::domain::gridmap; - using map_type = gridmap::bittable; + using rgridmap = domain::rotate_gridmap; + using bittable = ::warthog::domain::gridmap::bittable; + using rotate_grid = domain::gridmap_rotate_table_convs; + jump_point_online(); - jump_point_online(gridmap* map) + jump_point_online(const rgridmap& map) { - if(map != nullptr) set_map(*map); + set_map(map); } ~jump_point_online() = default; - 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; - } - - 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 rgridmap& map); + // void + // set_goal(jps_id goal_id) noexcept; + // void + // set_goal(point goal_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 jump point, second: id - * of jump point. deadend returns negative steps - * (include 0) to reach before blocker. - */ - std::pair - jump_cardinal(direction d, jps_id node_id, jps_rid rnode_id); + /// @brief returns cardinal jump distance to next jump point + /// @tparam D cardinal direction_id (NORTH_ID,SOUTH_ID,EAST_ID,WEST_ID) + /// @param loc current location on the grid to start the jump from + /// @return >0: jump point n steps away, <=0: blocker -n steps away + template + requires CardinalId + jump_distance + jump_cardinal_next(point loc); + /// @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 loc current location on the grid to start the jump from + /// @return >0: jump point n steps away, <=0: blocker -n steps away + template + requires CardinalId + jump_distance + jump_cardinal_next(domain::direction_grid_id_t node_id); + + /// @brief returns the next jump point intercardinal from loc, + /// where one of hori or vertical jump_cardinal_next has a jump point + /// @tparam D intercardinal direction_id (NORTHEAST_ID,NORTHWEST_ID,SOUTHEAST_ID,SOUTHWEST_ID) + /// @param loc current location on the grid to start the jump from + /// @return >0: jump point n steps away, <=0: blocker -n steps away + template + requires InterCardinalId 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); - - int32_t - jump_north(jps_rid rnode) - { - return jump_east(rmap_, rnode.id, rgoal_.id); - } - int32_t - jump_east(jps_id node) - { - return jump_east(map_, node.id, goal_.id); - } - int32_t - jump_south(jps_rid rnode) - { - return jump_west(rmap_, rnode.id, rgoal_.id); - } - int32_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); - } + jump_intercardinal_next( + point loc); + + /// @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 loc current location on the grid to start the jump from + /// @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 point + /// + /// 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( + point loc, int result_size, intercardinal_jump_result* result, jump_distance max_distance = std::numeric_limits::max()); 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 details::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 details::jump_point_online_hori(map, node); } protected: - static int32_t - jump_east(map_type map, uint32_t node, uint32_t goal); - static int32_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 rgridmap& 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(point loc) { - goal_ = point_to_id(p); - rgoal_ = rpoint_to_rid(point_to_rpoint(p)); + return jump_cardinal_next(map_.point_to_id_d(loc)); } -template -void -jump_point_online::create_rotate_(const gridmap& orig) +template + requires CardinalId +jump_distance +jump_point_online::jump_cardinal_next(domain::direction_grid_id_t node_id) { - 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); - } - } - - // 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) - { - case NORTH: - node.first = jump_north(rnode_id); - node.second.id = node_id.id - - map_.width() - * static_cast(std::abs(node.first)); - break; - case SOUTH: - node.first = jump_south(rnode_id); - node.second.id = node_id.id - + map_.width() - * static_cast(std::abs(node.first)); - break; - case EAST: - node.first = jump_east(node_id); - node.second.id - = node_id.id + static_cast(std::abs(node.first)); - break; - case WEST: - node.first = jump_west(node_id); - node.second.id - = node_id.id - static_cast(std::abs(node.first)); - break; - default: - assert(false); - node = {0, node_id}; + return jump_east(map_[domain::direction_grid_index], 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) - { - 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: + else if constexpr (D == SOUTH_ID || D == WEST_ID) + { + return jump_west(map_[domain::direction_grid_index], node_id); + } else { assert(false); - node = {jps_id::none(), jps_rid::none(), 0}; + return 0; } - return node; -} - -template -int32_t -jump_point_online::jump_east( - map_type map, uint32_t node, uint32_t goal) -{ - return details::jump_point_online_hori(map, node, goal); -} - -template -int32_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]]) +template + requires InterCardinalId +std::pair +jump_point_online::jump_intercardinal_many( + point loc, int result_size, intercardinal_jump_result* result, jump_distance max_distance) { - 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 - ); - /* map: NW N NE @@ -851,8 +660,8 @@ 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_; + walker.map[1] = map_[1]; walker.map_width(map_.width()); walker.rmap_width(rmap_.width()); walker.node_at[0] = static_cast(node); diff --git a/include/jps/search/jps_expansion_policy2.h b/include/jps/search/jps_expansion_policy2.h index 1032824..95398fd 100644 --- a/include/jps/search/jps_expansion_policy2.h +++ b/include/jps/search/jps_expansion_policy2.h @@ -19,7 +19,6 @@ // @created: 06/01/2010 #include "jps.h" -#include #include namespace jps::search From f8bff1425428f9d4e5994fa97bfa25b306fb97cf Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Sun, 22 Jun 2025 16:07:16 +1000 Subject: [PATCH 02/35] update warthog-core submodule commit --- extern/warthog-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/extern/warthog-core b/extern/warthog-core index 0f31e1a..8387db9 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 0f31e1a30148744f43adcc8914794e70f6a42674 +Subproject commit 8387db9c8604a4c66cda3241ef87a75d3164ed16 From 3b5319e0ee1e476377e506b165aff2a69aa1d719 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 27 Jun 2025 12:00:42 +1000 Subject: [PATCH 03/35] update jump_point_online to location and distance --- include/jps/jump/jump_point_online.h | 204 +++++++++------------------ 1 file changed, 70 insertions(+), 134 deletions(-) diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index 7183ff5..00a2486 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -144,7 +144,11 @@ struct IntercardinalWalker union LongJumpRes { jump_distance dist[2]; /// distance hori/vert of D a jump is valid to - uint32_t joint; + + 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) @@ -555,9 +559,9 @@ class jump_point_online /// until either reaching a deadend or some algorithmic specific stopping criteria. template requires InterCardinalId - std::pair + std::pair jump_intercardinal_many( - point loc, int result_size, intercardinal_jump_result* result, jump_distance max_distance = std::numeric_limits::max()); + point loc, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance = std::numeric_limits::max()); size_t mem() @@ -613,14 +617,49 @@ jump_point_online::jump_cardinal_next(domain::direction_grid_id_t node_id) } } + template requires InterCardinalId -std::pair -jump_point_online::jump_intercardinal_many( - point loc, int result_size, intercardinal_jump_result* result, jump_distance max_distance) +intercardinal_jump_result +jump_point_online::jump_intercardinal_next( + point loc) { - assert(!node.is_none() && !rnode.is_none()); + IntercardinalWalker walker; // class to walk + // setup the walker members + // 0 = map, 1 = rmap + walker.map = map_; + walker.map_width(map_[0].width()); + walker.rmap_width(map_[1].width()); + walker.node_at[0] = static_cast(map_.point_to_id(loc)); + walker.node_at[1] = static_cast(map_.rpoint_to_rid(map_.point_to_rpoint(loc))); + // JPS, stop at the first intercardinal turning point + jump_distance walk_count = 0; + walker.first_row(); + while(true) + { + walker.next_row(); // walk_count adjusted at end of loop + if(!walker.valid_row()) // no successors + return {static_cast(-walk_count), 0, 0}; + walk_count += 1; + // + // handle hori/vert long jump + // + auto res = walker.long_jump(); + if(res) + { // at least one jump has a turning point + return {walk_count, res.dist[0], res.dist[1]}; + } + } +} + +template + requires InterCardinalId +std::pair +jump_point_online::jump_intercardinal_many( + point loc, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance) +{ + assert(result_size > 0); /* map: NW N NE @@ -661,138 +700,35 @@ jump_point_online::jump_intercardinal_many( // setup the walker members // 0 = map, 1 = rmap walker.map = map_; - walker.map[1] = 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; - - // 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(); - } + walker.map_width(map_[0].width()); + walker.rmap_width(map_[1].width()); + walker.node_at[0] = static_cast(map_.point_to_id(loc)); + walker.node_at[1] = static_cast(map_.rpoint_to_rid(map_.point_to_rpoint(loc))); // 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]] - { - // 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; - } - // - // 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; - } - 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.first_row(); - // only continue if there is room to store results - while(result_count < result_size) - { - 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; + 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; - } - 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; - } - 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}; } + + return {results_count, walk_count}; } } From 35c6e8c6be51c0ecc52f4e4dfe828c262f949595 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 27 Jun 2025 12:00:54 +1000 Subject: [PATCH 04/35] remove old jps --- include/jps/jump/offline_jump_point_locator.h | 83 ---------- .../jps/jump/offline_jump_point_locator2.h | 90 ---------- include/jps/jump/online_jump_point_locator.h | 130 --------------- include/jps/jump/online_jump_point_locator2.h | 156 ------------------ include/jps/search/jps2_expansion_policy.h | 66 -------- .../jps/search/jps2plus_expansion_policy.h | 59 ------- include/jps/search/jps_expansion_policy.h | 64 ------- include/jps/search/jpsplus_expansion_policy.h | 60 ------- 8 files changed, 708 deletions(-) delete mode 100644 include/jps/jump/offline_jump_point_locator.h delete mode 100644 include/jps/jump/offline_jump_point_locator2.h delete mode 100644 include/jps/jump/online_jump_point_locator.h delete mode 100644 include/jps/jump/online_jump_point_locator2.h delete mode 100644 include/jps/search/jps2_expansion_policy.h delete mode 100644 include/jps/search/jps2plus_expansion_policy.h delete mode 100644 include/jps/search/jps_expansion_policy.h delete mode 100644 include/jps/search/jpsplus_expansion_policy.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/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/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h deleted file mode 100644 index cede0ca..0000000 --- a/include/jps/search/jps_expansion_policy.h +++ /dev/null @@ -1,64 +0,0 @@ -#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 -// 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 -#include -#include -#include -#include - -namespace jps::search -{ - -class jps_expansion_policy - : public warthog::search::gridmap_expansion_policy_base -{ -public: - jps_expansion_policy(warthog::domain::gridmap* map); - virtual ~jps_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::online_jump_point_locator* jpl_; -}; - -} - -#endif // JPS_SEARCH_JPS_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 From 5a18b5dd3af8cde8ac3cc9d5dd8505b6c2f3cfd3 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 27 Jun 2025 14:22:38 +1000 Subject: [PATCH 05/35] rename jps expansion policy --- .../{jps_expansion_policy2.h => jps_2011_expansion_policy.h} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename include/jps/search/{jps_expansion_policy2.h => jps_2011_expansion_policy.h} (100%) diff --git a/include/jps/search/jps_expansion_policy2.h b/include/jps/search/jps_2011_expansion_policy.h similarity index 100% rename from include/jps/search/jps_expansion_policy2.h rename to include/jps/search/jps_2011_expansion_policy.h From a26366477712d181ba9c163922158682ecee5c78 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Wed, 2 Jul 2025 14:00:11 +1000 Subject: [PATCH 06/35] git refactor continue --- extern/warthog-core | 2 +- include/jps/forward.h | 6 +- .../jps/search/jps_2011_expansion_policy.h | 55 +++++++++++++------ 3 files changed, 41 insertions(+), 22 deletions(-) diff --git a/extern/warthog-core b/extern/warthog-core index 8387db9..1a076b8 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 8387db9c8604a4c66cda3241ef87a75d3164ed16 +Subproject commit 1a076b8a480ff89aae5a7dd94e19b0d5e25890bd diff --git a/include/jps/forward.h b/include/jps/forward.h index 9d9a564..10ba493 100644 --- a/include/jps/forward.h +++ b/include/jps/forward.h @@ -12,13 +12,13 @@ namespace jps using namespace warthog::grid; using ::warthog::pad_id; -using jps_id = grid_id; +// using jps_id = grid_id; struct rmap_id_tag { }; -using rgrid_id = warthog::identity_base; +using rgrid_id = warthog::identity_base; using warthog::cost_t; -using vec_jps_id = std::vector; +using vec_jps_id = std::vector; using vec_jps_cost = std::vector; enum class JpsFeature : uint8_t diff --git a/include/jps/search/jps_2011_expansion_policy.h b/include/jps/search/jps_2011_expansion_policy.h index 95398fd..af416b5 100644 --- a/include/jps/search/jps_2011_expansion_policy.h +++ b/include/jps/search/jps_2011_expansion_policy.h @@ -20,37 +20,37 @@ #include "jps.h" #include +#include +#include namespace jps::search { +/// @brief +/// @tparam JpsJump +/// +/// 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+. 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) - { } + jps_expansion_policy2(warthog::domain::gridmap* map) + : gridmap_expansion_policy_base(map), rmap_(map) + { + jpl_.set_map(*map); + } 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* current, + warthog::search::search_problem_instance* pi) override; warthog::search::search_node* generate_start_node(warthog::search::search_problem_instance* pi) override; @@ -78,6 +78,7 @@ class jps_expansion_policy2 } private: + domain::rotate_gridmap rmap_; JpsJump jpl_; }; @@ -90,8 +91,9 @@ jps_expansion_policy2::expand( 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 grid_id current_id = jps_id(current->get_id()); + const point loc = rmap_.id_to_point(current_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()); @@ -105,6 +107,23 @@ jps_expansion_policy2::expand( uint32_t succ_dirs = compute_successors(dir_c, c_tiles); // cardinal directions + ::warthog::util::for_each_integer_sequence< + std::integer_sequence + >([&] { + if(succ_dirs & warthog::grid::to_dir(di)) + { + jump_distance jump_result = jpl_.jump_cardinal_next(loc); + if(jump_result > 0) // jump point + { + // 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); + } + } + }); for(uint32_t i = 0; i < 4; i++) { direction d = (direction)(1 << i); From d399c615b1dbf952ac1770488eccbe98341d1cb6 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Wed, 2 Jul 2025 14:14:01 +1000 Subject: [PATCH 07/35] update cmake files --- cmake/headers.cmake | 17 +- extern/warthog-core | 2 +- src/jump/CMakeLists.txt | 4 - src/jump/offline_jump_point_locator.cpp | 488 -------------- src/jump/offline_jump_point_locator2.cpp | 578 ----------------- src/jump/online_jump_point_locator.cpp | 477 -------------- src/jump/online_jump_point_locator2.cpp | 788 ----------------------- src/search/CMakeLists.txt | 4 - src/search/jps2_expansion_policy.cpp | 103 --- src/search/jps2plus_expansion_policy.cpp | 93 --- src/search/jps_expansion_policy.cpp | 82 --- src/search/jpsplus_expansion_policy.cpp | 78 --- 12 files changed, 9 insertions(+), 2705 deletions(-) delete mode 100644 src/jump/offline_jump_point_locator.cpp delete mode 100644 src/jump/offline_jump_point_locator2.cpp delete mode 100644 src/jump/online_jump_point_locator.cpp delete mode 100644 src/jump/online_jump_point_locator2.cpp delete mode 100644 src/search/jps2_expansion_policy.cpp delete mode 100644 src/search/jps2plus_expansion_policy.cpp delete mode 100644 src/search/jps_expansion_policy.cpp delete mode 100644 src/search/jpsplus_expansion_policy.cpp diff --git a/cmake/headers.cmake b/cmake/headers.cmake index 02f876b..3c5a871 100644 --- a/cmake/headers.cmake +++ b/cmake/headers.cmake @@ -3,16 +3,15 @@ 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/domain/rotate_gridmap.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/jump/jump.h +include/jps/jump/jump_point_offline.h +include/jps/jump/jump_point_online.h -include/jps/search/jps2_expansion_policy.h -include/jps/search/jps.h -include/jps/search/jps2plus_expansion_policy.h +include/jps/search/jps_2011_expansion_policy.h include/jps/search/jps4c_expansion_policy.h -include/jps/search/jps_expansion_policy.h -include/jps/search/jpsplus_expansion_policy.h +include/jps/search/jps.h ) diff --git a/extern/warthog-core b/extern/warthog-core index 1a076b8..1f8b6bf 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 1a076b8a480ff89aae5a7dd94e19b0d5e25890bd +Subproject commit 1f8b6bf8c3238b53af22d54ac1c4f25fc45332ce diff --git a/src/jump/CMakeLists.txt b/src/jump/CMakeLists.txt index 0949bc2..b70dd23 100644 --- a/src/jump/CMakeLists.txt +++ b/src/jump/CMakeLists.txt @@ -3,8 +3,4 @@ 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 ) 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..e077bfb 100644 --- a/src/search/CMakeLists.txt +++ b/src/search/CMakeLists.txt @@ -2,10 +2,6 @@ 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/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/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 From 3f560532e198ce4d5afcd6a4d8d64e5e25e9df1e Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Wed, 2 Jul 2025 14:58:03 +1000 Subject: [PATCH 08/35] rename expansion policy --- .../{jps_2011_expansion_policy.h => jps_expansion_policy.h} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename include/jps/search/{jps_2011_expansion_policy.h => jps_expansion_policy.h} (100%) diff --git a/include/jps/search/jps_2011_expansion_policy.h b/include/jps/search/jps_expansion_policy.h similarity index 100% rename from include/jps/search/jps_2011_expansion_policy.h rename to include/jps/search/jps_expansion_policy.h From b17a5be6f6c07c32fe49a5746ee9b33b8be1cec7 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Wed, 2 Jul 2025 17:33:10 +1000 Subject: [PATCH 09/35] continue --- apps/jps.cpp | 5 - cmake/headers.cmake | 4 +- extern/warthog-core | 2 +- include/jps/jump/four_connected_jps_locator.h | 78 ------ include/jps/jump/jump_point_online.h | 27 +- include/jps/search/jps.h | 56 +++- include/jps/search/jps4c_expansion_policy.h | 58 ----- src/jump/CMakeLists.txt | 5 +- src/jump/four_connected_jps_locator.cpp | 240 ------------------ src/search/CMakeLists.txt | 1 - src/search/jps.cpp | 86 +++---- src/search/jps4c_expansion_policy.cpp | 84 ------ 12 files changed, 110 insertions(+), 536 deletions(-) delete mode 100644 include/jps/jump/four_connected_jps_locator.h delete mode 100644 include/jps/search/jps4c_expansion_policy.h delete mode 100644 src/jump/four_connected_jps_locator.cpp delete mode 100644 src/search/jps4c_expansion_policy.cpp diff --git a/apps/jps.cpp b/apps/jps.cpp index d6a9cb4..97dcf45 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -8,11 +8,6 @@ // #include -#include -#include -#include -#include -#include #include #include #include diff --git a/cmake/headers.cmake b/cmake/headers.cmake index 3c5a871..445eac3 100644 --- a/cmake/headers.cmake +++ b/cmake/headers.cmake @@ -6,12 +6,10 @@ include/jps/forward.h include/jps/domain/rotate_gridmap.h -include/jps/jump/four_connected_jps_locator.h include/jps/jump/jump.h include/jps/jump/jump_point_offline.h include/jps/jump/jump_point_online.h -include/jps/search/jps_2011_expansion_policy.h -include/jps/search/jps4c_expansion_policy.h +include/jps/search/jps_expansion_policy.h include/jps/search/jps.h ) diff --git a/extern/warthog-core b/extern/warthog-core index 1f8b6bf..dbfdaea 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 1f8b6bf8c3238b53af22d54ac1c4f25fc45332ce +Subproject commit dbfdaea7379ba539404d3045b6945bd946a2ef87 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_point_online.h b/include/jps/jump/jump_point_online.h index 00a2486..f50f10e 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -532,17 +532,6 @@ class jump_point_online jump_distance jump_cardinal_next(domain::direction_grid_id_t node_id); - /// @brief returns the next jump point intercardinal from loc, - /// where one of hori or vertical jump_cardinal_next has a jump point - /// @tparam D intercardinal direction_id (NORTHEAST_ID,NORTHWEST_ID,SOUTHEAST_ID,SOUTHWEST_ID) - /// @param loc current location on the grid to start the jump from - /// @return >0: jump point n steps away, <=0: blocker -n steps away - template - requires InterCardinalId - intercardinal_jump_result - jump_intercardinal_next( - point loc); - /// @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) @@ -562,6 +551,12 @@ class jump_point_online std::pair jump_intercardinal_many( point loc, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance = std::numeric_limits::max()); + + template + requires requires (D == 255) || InterCardinalId(D)> + std::pair + jump_target( + point loc, point target); size_t mem() @@ -731,6 +726,16 @@ jump_point_online::jump_intercardinal_many( return {results_count, walk_count}; } + +template + requires requires (D == 255) || InterCardinalId(D)> +std::pair +jump_point_online::jump_target( + point loc, point target) +{ + +} + } #endif // JPS_JUMP_JUMP_POINT_ONLINE_H diff --git a/include/jps/search/jps.h b/include/jps/search/jps.h index e0f7590..e9e1351 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; } @@ -62,6 +62,44 @@ from_direction(jps_id n1_id_p, jps_id n2_id_p, uint32_t map_width_p) return NORTH; } +constexpr inline direction_id +from_direction(point p1, point p2) +{ + union { + struct { + int32_t x; + int32_t y; + } p; + uint64_t xy; + } c; + c.p.x = static_cast(p2.x) - static_cast(p1.x); + c.p.y = static_cast(p2.y) - static_cast(p1.y); + + if (c.p.x == 0) { + return c.p.y >= 0 ? NORTH_ID : SOUTH_ID; + } else if (c.p.y == 0) { + return c.p.x >= 0 ? EAST_ID : WEST_ID; + } else { + // shift>> to mulitple of 4 (0b100) + // (x < 0) = 0b0100 + // (y < 0) = 0b1000 + int shift = ( (static_cast(c.p.x) >> (31-2)) & 0b0100 ) | + ( (static_cast(c.p.y) >> (31-3)) & 0b1000 ); + assert((c.p.x > 0 && c.p.y > 0 && shift == 0) + || (c.p.x < 0 && c.p.y > 0 && shift == 4) + || (c.p.x > 0 && c.p.y < 0 && shift == 8) + || (c.p.x < 0 && c.p.y < 0 && shift == 12)); + return static_cast( + static_cast( + (static_cast(NORTHEAST_ID) << 0) | + (static_cast(NORTHWEST_ID) << 4) | + (static_cast(SOUTHEAST_ID) << 8) | + (static_cast(SOUTHWEST_ID) << 12) + ) >> shift + ); + } +} + // compute the 4-connected canonical last move on the // path from (px, py) to (x, y) // inline direction @@ -81,7 +119,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; } @@ -206,13 +244,13 @@ 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); +// // 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 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/src/jump/CMakeLists.txt b/src/jump/CMakeLists.txt index b70dd23..1556217 100644 --- a/src/jump/CMakeLists.txt +++ b/src/jump/CMakeLists.txt @@ -1,6 +1,5 @@ cmake_minimum_required(VERSION 3.13) # ( cd src/jump && echo *.cpp | sort ) -target_sources(warthog_libjps PRIVATE -four_connected_jps_locator.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/search/CMakeLists.txt b/src/search/CMakeLists.txt index e077bfb..221278c 100644 --- a/src/search/CMakeLists.txt +++ b/src/search/CMakeLists.txt @@ -3,5 +3,4 @@ cmake_minimum_required(VERSION 3.13) # ( cd src/search && echo *.cpp | sort ) target_sources(warthog_libjps PRIVATE jps.cpp -jps4c_expansion_policy.cpp ) diff --git a/src/search/jps.cpp b/src/search/jps.cpp index 0bf7df2..b645d1f 100644 --- a/src/search/jps.cpp +++ b/src/search/jps.cpp @@ -1,4 +1,4 @@ -#include +// #include #include #include #include @@ -252,51 +252,51 @@ create_jump_point_graph(warthog::domain::gridmap* gm) } #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); +// 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(); +// 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; } +// // 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); +// 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; -} +// // 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/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 From 5226875d385ed614683f2e74a52333cdcc041e2a Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 3 Jul 2025 17:39:25 +1000 Subject: [PATCH 10/35] jps online progress nearly complete --- apps/jps.cpp | 41 +-- extern/warthog-core | 2 +- include/jps/domain/rotate_gridmap.h | 28 +- include/jps/jump/jump_point_online.h | 414 +++++++++++++++++----- include/jps/search/jps.h | 38 -- include/jps/search/jps_expansion_policy.h | 149 ++------ 6 files changed, 385 insertions(+), 287 deletions(-) diff --git a/apps/jps.cpp b/apps/jps.cpp index 97dcf45..41b4ee5 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -8,8 +8,6 @@ // #include -#include -#include #include #include #include @@ -18,7 +16,9 @@ #include #include -#include +#include +// #include +#include #include "cfg.h" #include @@ -250,39 +250,8 @@ 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); - } - else if(alg == "jpsV2") - { - using jump_point - = jps::jump::jump_point_online; - return run_jps>( - scenmgr, mapfile, alg); - } - else if(alg == "jpsV2-cardinal") - { - using jump_point = jps::jump::jump_point_online< - jps::JpsFeature::STORE_CARDINAL_JUMP>; - return run_jps>( - scenmgr, mapfile, alg); - } - else if(alg == "jpsV2-prune") - { - using jump_point = jps::jump::jump_point_online< - jps::JpsFeature::PRUNE_INTERCARDINAL>; - return run_jps>( + using jump_point = jps::jump::jump_point_online; + return run_jps>( scenmgr, mapfile, alg); } else diff --git a/extern/warthog-core b/extern/warthog-core index dbfdaea..5d4b16a 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit dbfdaea7379ba539404d3045b6945bd946a2ef87 +Subproject commit 5d4b16aa35ca1536731859f80e0a78429ac28478 diff --git a/include/jps/domain/rotate_gridmap.h b/include/jps/domain/rotate_gridmap.h index 333b700..9357e24 100644 --- a/include/jps/domain/rotate_gridmap.h +++ b/include/jps/domain/rotate_gridmap.h @@ -27,24 +27,28 @@ struct direction_grid_id { using type = rgrid_id; static constexpr int map_id = 1; + static constexpr bool east = true; }; template <> struct direction_grid_id { using type = grid_id; static constexpr int map_id = 0; + static constexpr bool east = true; }; template <> struct direction_grid_id { using type = rgrid_id; static constexpr int map_id = 1; + static constexpr bool east = false; }; template <> struct direction_grid_id { using type = grid_id; static constexpr int map_id = 0; + static constexpr bool east = false; }; template <> struct direction_grid_id @@ -76,10 +80,12 @@ struct direction_grid_id /// @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 direction_grid_id_t = typename details::direction_grid_id::type; +using rgrid_id_t = typename details::direction_grid_id::type; template -inline constexpr int direction_grid_index = details::direction_grid_id::map_id; +constexpr inline int rgrid_index = details::direction_grid_id::map_id; +template +constexpr inline bool rgrid_east = details::direction_grid_id::east; using ::warthog::domain::gridmap; @@ -106,10 +112,10 @@ struct rgridmap_point_conversions grid_id point_to_id(point p) const noexcept { - return jps_id{ - static_cast(p.y + domain::gridmap::PADDED_ROWS) + return grid_id{ + static_cast(p.y + domain::gridmap::PADDED_ROWS) * map_pad_width_ - + static_cast(p.x)}; + + static_cast(p.x)}; } rgrid_id rpoint_to_rid(point p) const noexcept @@ -149,9 +155,9 @@ struct rgridmap_point_conversions template requires std::same_as || std::same_as - direction_grid_id_t to_id_d(GridId id) const noexcept + rgrid_id_t to_id_d(GridId id) const noexcept { - using res_type = direction_grid_id_t; + using res_type = rgrid_id_t; if constexpr (std::same_as) { return id; // is same as output, do nothing } else if constexpr (std::same_as) { @@ -162,9 +168,9 @@ struct rgridmap_point_conversions } template - direction_grid_id_t point_to_id_d(point id) const noexcept + rgrid_id_t point_to_id_d(point id) const noexcept { - using res_type = direction_grid_id_t; + using res_type = rgrid_id_t; if constexpr (std::same_as) { return point_to_id(id); } else { @@ -173,9 +179,9 @@ struct rgridmap_point_conversions } template - direction_grid_id_t rpoint_to_id_d(point id) const noexcept + rgrid_id_t rpoint_to_id_d(point id) const noexcept { - using res_type = direction_grid_id_t; + using res_type = rgrid_id_t; if constexpr (std::same_as) { return point_to_id(rpoint_to_point(id)); } else { diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index f50f10e..d23bb28 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -18,22 +18,21 @@ namespace jps::jump { -namespace details -{ - -/// @brief +/// @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 goal the goal location -/// @return -template +/// @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 goal[[maybe_unused]] = std::numeric_limits::max()) + ::warthog::domain::gridmap::bittable map, uint32_t node, uint32_t target[[maybe_unused]] = std::numeric_limits::max()) { assert(map.data() != nullptr); - assert(Goal != (goal == std::numeric_limits::max())); + assert(Target != (target == std::numeric_limits::max())); // read tiles from the grid: // - along the row of node_id // - from the row above node_id @@ -93,22 +92,22 @@ jump_point_online_hori( // dead end takes president as jump point can't pass a blocker jump_count += static_cast(stop_pos) - static_cast(nei_slider.width8_bits); - uint32_t goal_jump[[maybe_unused]] = Goal ? (East ? goal - node : node - goal) : 0; + 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 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 + // 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. goal.is_none(): will not fit int32_t - // 2. underflow means goal is in opposite direction, desirable - if(Goal && (goal_jump <= static_cast(jump_count))) + // 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))) { - // goal reached - jump_count = static_cast(goal_jump); + // target reached + jump_count = static_cast(target_jump); } else if( East ? !(neis[0] & (static_cast(1) << stop_pos)) @@ -131,7 +130,203 @@ jump_point_online_hori( } } -} // namespace details +/// @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 = 7 - 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 + int32_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; + if constexpr (D == NORTHEAST_ID || D == SOUTHEAST_ID) { + if constexpr (D == NORTHEAST_ID) { + this->adj_width = -static_cast(width) + 1; + } else { + this->adj_width = static_cast(width) + 1; + } + row_mask_ = std::endian::native == std::endian::little + ? 0b0000'0011'0000'0110 + : 0b0000'0110'0000'0011; + } else { // NORTHWEST_ID and SOUTHWEST_ID + if constexpr (D == NORTHWEST_ID) { + this->adj_width = -static_cast(width) - 1; + } else { + this->adj_width = static_cast(width) - 1; + } + row_mask_ = std::endian::native == std::endian::little + ? 0b0000'0110'0000'0011 + : 0b0000'0011'0000'0110; + } + } + + void + next_index() noexcept + { + node_at = static_cast( + static_cast(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( + static_cast(node) + 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 @@ -157,8 +352,8 @@ struct IntercardinalWalker 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 map and rmap target locations + // uint32_t target[2]; /// @brief row scan union { @@ -272,32 +467,32 @@ struct IntercardinalWalker static jump_distance jump_east(map_type map, uint32_t width, uint32_t node) { - jump_distance d = details::jump_point_online_hori( + 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 goal) + jump_east(map_type map, uint32_t width, uint32_t node, uint32_t target) { - jump_distance d = details::jump_point_online_hori( - ::warthog::domain::gridmap::bittable(map, width, 0), node, goal); + 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 = details::jump_point_online_hori( + 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 goal) + jump_west(map_type map, uint32_t width, uint32_t node, uint32_t target) { - jump_distance d = details::jump_point_online_hori( - ::warthog::domain::gridmap::bittable(map, width, 0), node, goal); + 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; } @@ -322,23 +517,23 @@ struct IntercardinalWalker } } jump_distance - jump_hori(grid_id goal) + jump_hori(grid_id target) { if constexpr(D == NORTHEAST_ID) { - return jump_east(map[0], map_width(), node_at[0], static_cast(goal)); // east + 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(goal)); // east + 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(goal)); // west + 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(goal)); // west + return jump_west(map[0], map_width(), node_at[0], static_cast(target)); // west } } jump_distance @@ -366,27 +561,27 @@ struct IntercardinalWalker } } jump_distance - jump_vert(rgrid_id goal) + jump_vert(rgrid_id target) { if constexpr(D == NORTHEAST_ID) { return jump_east( - map[1], rmap_width(), node_at[1], static_cast(goal)); // north + 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(goal)); // south + 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(goal)); // south + 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(goal)); // north + map[1], rmap_width(), node_at[1], static_cast(target)); // north } } @@ -498,9 +693,9 @@ class jump_point_online void set_map(const rgridmap& map); // void - // set_goal(jps_id goal_id) noexcept; + // set_target(jps_id target_id) noexcept; // void - // set_goal(point goal_id) noexcept; + // set_target(point target_id) noexcept; // /// @brief avoid modifying these grids accidentally // bittable @@ -530,7 +725,7 @@ class jump_point_online template requires CardinalId jump_distance - jump_cardinal_next(domain::direction_grid_id_t node_id); + jump_cardinal_next(domain::rgrid_id_t node_id); /// @brief returns all intercardinal jump points up to max_distance (default inf) /// and max of results_size @@ -552,8 +747,13 @@ class jump_point_online jump_intercardinal_many( point loc, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance = std::numeric_limits::max()); - template - requires requires (D == 255) || InterCardinalId(D)> + /// @brief shoot ray to target point + /// @param loc shoot from loc + /// @param target shoot to target + /// @return pair , if both >= 0 than 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( point loc, point target); @@ -568,12 +768,12 @@ class jump_point_online static int32_t jump_east(bittable map, uint32_t node) { - return details::jump_point_online_hori(map, node); + return jump_point_online_hori(map, node); } static int32_t jump_west(bittable map, uint32_t node) { - return details::jump_point_online_hori(map, node); + return jump_point_online_hori(map, node); } protected: @@ -597,57 +797,21 @@ jump_point_online::jump_cardinal_next(point loc) template requires CardinalId jump_distance -jump_point_online::jump_cardinal_next(domain::direction_grid_id_t node_id) +jump_point_online::jump_cardinal_next(domain::rgrid_id_t node_id) { if constexpr (D == NORTH_ID || D == EAST_ID) { - return jump_east(map_[domain::direction_grid_index], node_id); + return jump_east(map_[domain::rgrid_index], node_id); } else if constexpr (D == SOUTH_ID || D == WEST_ID) { - return jump_west(map_[domain::direction_grid_index], node_id); + return jump_west(map_[domain::rgrid_index], node_id); } else { assert(false); return 0; } } - -template - requires InterCardinalId -intercardinal_jump_result -jump_point_online::jump_intercardinal_next( - point loc) -{ - IntercardinalWalker walker; // class to walk - // setup the walker members - // 0 = map, 1 = rmap - walker.map = map_; - walker.map_width(map_[0].width()); - walker.rmap_width(map_[1].width()); - walker.node_at[0] = static_cast(map_.point_to_id(loc)); - walker.node_at[1] = static_cast(map_.rpoint_to_rid(map_.point_to_rpoint(loc))); - - // JPS, stop at the first intercardinal turning point - jump_distance walk_count = 0; - walker.first_row(); - while(true) - { - walker.next_row(); // walk_count adjusted at end of loop - if(!walker.valid_row()) // no successors - return {static_cast(-walk_count), 0, 0}; - walk_count += 1; - // - // handle hori/vert long jump - // - auto res = walker.long_jump(); - if(res) - { // at least one jump has a turning point - return {walk_count, res.dist[0], res.dist[1]}; - } - } -} - template requires InterCardinalId std::pair @@ -727,13 +891,87 @@ jump_point_online::jump_intercardinal_many( } -template - requires requires (D == 255) || InterCardinalId(D)> std::pair jump_point_online::jump_target( 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_[0].width()); + } else { + d = NORTHEAST_ID; + walker.set_map(map_[0], map_[0].width()); + } + } else { // west + if (yd > 0) { // south + d = SOUTHWEST_ID; + walker.set_map(map_[0], map_[0].width()); + } else { + d = NORTHWEST_ID; + walker.set_map(map_[0], map_[0].width()); + } + } + walker.node_at = static_cast(map_.point_to_id(loc)); + inter_len = static_cast(std::min(std::abs(xd), std::abs(yd))); + walker.first_row(); + for (jump_distance i = 0; i < inter_len; ++i) { + walker.next_row(); + if (!walker.valid_row()) { + // diagonal blocked before reaching turn, report + return {static_cast(-i), -1}; + } + } + // 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 + cardinal_len = 0; + } else if (xd > 0) { + // east + cardinal_len = jump_point_online_hori_target>( + map_[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_[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_[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_[domain::rgrid_index], + static_cast(map_.point_to_id_d(loc)), + static_cast(map_.point_to_id_d(target))); + } + return {inter_len, cardinal_len}; } } diff --git a/include/jps/search/jps.h b/include/jps/search/jps.h index e9e1351..a03c629 100644 --- a/include/jps/search/jps.h +++ b/include/jps/search/jps.h @@ -62,44 +62,6 @@ from_direction(grid_id n1_id_p, grid_id n2_id_p, uint32_t map_width_p) return NORTH; } -constexpr inline direction_id -from_direction(point p1, point p2) -{ - union { - struct { - int32_t x; - int32_t y; - } p; - uint64_t xy; - } c; - c.p.x = static_cast(p2.x) - static_cast(p1.x); - c.p.y = static_cast(p2.y) - static_cast(p1.y); - - if (c.p.x == 0) { - return c.p.y >= 0 ? NORTH_ID : SOUTH_ID; - } else if (c.p.y == 0) { - return c.p.x >= 0 ? EAST_ID : WEST_ID; - } else { - // shift>> to mulitple of 4 (0b100) - // (x < 0) = 0b0100 - // (y < 0) = 0b1000 - int shift = ( (static_cast(c.p.x) >> (31-2)) & 0b0100 ) | - ( (static_cast(c.p.y) >> (31-3)) & 0b1000 ); - assert((c.p.x > 0 && c.p.y > 0 && shift == 0) - || (c.p.x < 0 && c.p.y > 0 && shift == 4) - || (c.p.x > 0 && c.p.y < 0 && shift == 8) - || (c.p.x < 0 && c.p.y < 0 && shift == 12)); - return static_cast( - static_cast( - (static_cast(NORTHEAST_ID) << 0) | - (static_cast(NORTHWEST_ID) << 4) | - (static_cast(SOUTHEAST_ID) << 8) | - (static_cast(SOUTHWEST_ID) << 12) - ) >> shift - ); - } -} - // compute the 4-connected canonical last move on the // path from (px, py) to (x, y) // inline direction diff --git a/include/jps/search/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index af416b5..8180ca5 100644 --- a/include/jps/search/jps_expansion_policy.h +++ b/include/jps/search/jps_expansion_policy.h @@ -34,16 +34,16 @@ namespace jps::search /// jps_2011_expansion_policy gives JPS (B). /// jps_2011_expansion_policy gives JPS+. template -class jps_expansion_policy2 +class jps_expansion_policy : public warthog::search::gridmap_expansion_policy_base { public: - jps_expansion_policy2(warthog::domain::gridmap* map) + jps_expansion_policy(warthog::domain::gridmap* map) : gridmap_expansion_policy_base(map), rmap_(map) { jpl_.set_map(*map); } - virtual ~jps_expansion_policy2() = default; + virtual ~jps_expansion_policy() = default; using jump_point = JpsJump; @@ -80,11 +80,13 @@ class jps_expansion_policy2 private: domain::rotate_gridmap rmap_; JpsJump jpl_; + point target_loc_; + grid_id target_id_; }; template void -jps_expansion_policy2::expand( +jps_expansion_policy::expand( warthog::search::search_node* current, warthog::search::search_problem_instance* instance) { @@ -97,6 +99,7 @@ jps_expansion_policy2::expand( // const cost_t current_cost = current->get_g(); const direction dir_c = from_direction( jps_id(current->get_parent()), current_id, 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; @@ -105,6 +108,16 @@ jps_expansion_policy2::expand( // 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(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< @@ -112,147 +125,57 @@ jps_expansion_policy2::expand( >([&] { if(succ_dirs & warthog::grid::to_dir(di)) { - jump_distance jump_result = jpl_.jump_cardinal_next(loc); + auto jump_result = jpl_.template jump_cardinal_next(loc); if(jump_result > 0) // jump point { // 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); + = this->generate(pad_id(static_cast(current_id.id + warthog::grid::dir_id_adj(di) * jump_result))); + add_neighbour(jp_succ, jump_result * warthog::DBL_ONE); } } }); - for(uint32_t i = 0; i < 4; i++) - { - direction d = (direction)(1 << i); - if(succ_dirs & d) + // intercardinal directions + ::warthog::util::for_each_integer_sequence< + std::integer_sequence + >([&] { + if(succ_dirs & warthog::grid::to_dir(di)) { - auto jump_result = jpl_.jump_cardinal(d, current_id, current_rid); + jump::intercardinal_jump_result res; + auto jump_result = jpl_.template jump_intercardinal_many(loc, &res, 1); if(jump_result.first > 0) // jump point { // 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 - } + = this->generate(pad_id(static_cast(current_id.id + warthog::grid::dir_id_adj(di) * jump_result.first))); + add_neighbour(jp_succ, jump_result * warthog::DBL_ROOT_TWO); } } - } + }); } template warthog::search::search_node* -jps_expansion_policy2::generate_start_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_); + pad_id padded_id = pad_id(pi->start_); if(map_->get_label(padded_id) == 0) { return nullptr; } - jpl_.set_goal(jps_id(pi->target_)); + target_id_ = grid_id(pi->target_); + target_loc_ = rmap_.map().to_unpadded_xy(target_id_); return generate(padded_id); } template warthog::search::search_node* -jps_expansion_policy2::generate_target_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_); + pad_id padded_id = pad_id(pi->target_); if(map_->get_label(padded_id) == 0) { return nullptr; } return generate(padded_id); } From 7817a98768842f2253f4bc66ba5d1ae9831a936f Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 4 Jul 2025 16:55:24 +1000 Subject: [PATCH 11/35] warthog jps beta --- extern/warthog-core | 2 +- include/jps/domain/rotate_gridmap.h | 58 ++++++++++++---- include/jps/jump/jump_point_online.h | 82 +++++------------------ include/jps/search/jps_expansion_policy.h | 57 ++++++++++++---- 4 files changed, 106 insertions(+), 93 deletions(-) diff --git a/extern/warthog-core b/extern/warthog-core index 5d4b16a..796cafc 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 5d4b16aa35ca1536731859f80e0a78429ac28478 +Subproject commit 796cafc517eded6ac2c6e9ebcc0614e297ecc0a9 diff --git a/include/jps/domain/rotate_gridmap.h b/include/jps/domain/rotate_gridmap.h index 9357e24..125f9ff 100644 --- a/include/jps/domain/rotate_gridmap.h +++ b/include/jps/domain/rotate_gridmap.h @@ -95,6 +95,13 @@ struct rgridmap_point_conversions uint16_t map_pad_width_ = 0; uint16_t rmap_pad_width_ = 0; + void conv_assign(const gridmap& map, const gridmap& rmap) noexcept + { + map_unpad_height_m1_ = static_cast(map.header_height() - 1); + map_pad_width_ = static_cast(map.width()); + rmap_pad_width_ = static_cast(rmap.width()); + } + point point_to_rpoint(point p) const noexcept { @@ -168,24 +175,24 @@ struct rgridmap_point_conversions } template - rgrid_id_t point_to_id_d(point id) const noexcept + rgrid_id_t point_to_id_d(point loc) const noexcept { using res_type = rgrid_id_t; if constexpr (std::same_as) { - return point_to_id(id); + return point_to_id(loc); } else { - return rpoint_to_rid(point_to_rpoint(id)); + return rpoint_to_rid(point_to_rpoint(loc)); } } template - rgrid_id_t rpoint_to_id_d(point id) const noexcept + rgrid_id_t rpoint_to_id_d(point loc) const noexcept { using res_type = rgrid_id_t; if constexpr (std::same_as) { - return point_to_id(rpoint_to_point(id)); + return point_to_id(rpoint_to_point(loc)); } else { - return rpoint_to_rid(id); + return rpoint_to_rid(loc); } } }; @@ -201,6 +208,7 @@ struct gridmap_rotate_ptr : std::array 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]; } }; struct gridmap_rotate_ptr_convs : gridmap_rotate_ptr, rgridmap_point_conversions { @@ -208,9 +216,13 @@ struct gridmap_rotate_ptr_convs : gridmap_rotate_ptr, rgridmap_point_conversions gridmap_rotate_ptr_convs(domain::gridmap& l_map, domain::gridmap& l_rmap) noexcept : gridmap_rotate_ptr(l_map, l_rmap), rgridmap_point_conversions{static_cast(l_map.header_height()-1), static_cast(l_map.width()), static_cast(l_rmap.width())} { } - gridmap_rotate_ptr_convs(gridmap_rotate_ptr maps, uint16_t map_unpad_height_m1) noexcept - : gridmap_rotate_ptr(maps), rgridmap_point_conversions{static_cast(maps[0]->header_height()-1), static_cast(maps[0]->width()), static_cast(maps[1]->width())} - { } + gridmap_rotate_ptr_convs(gridmap_rotate_ptr maps) noexcept + : gridmap_rotate_ptr(maps), rgridmap_point_conversions{} + { + if (*this) { + conv_assign(map(), rmap()); + } + } }; struct gridmap_rotate_table : std::array { @@ -226,6 +238,7 @@ struct gridmap_rotate_table : std::array const domain::gridmap::bittable& map() const noexcept { return (*this)[0]; } domain::gridmap::bittable& rmap() noexcept { return (*this)[1]; } const domain::gridmap::bittable& rmap() const noexcept { return (*this)[1]; } + operator bool() const noexcept { return (*this)[0].data(); } }; struct gridmap_rotate_table_convs : gridmap_rotate_table, rgridmap_point_conversions { @@ -243,7 +256,6 @@ class rotate_gridmap : public rgridmap_point_conversions private: std::unique_ptr rmap_obj; gridmap_rotate_ptr maps = {}; - uint32_t map_unpad_height_m1_ = 0; public: rotate_gridmap() = default; @@ -252,12 +264,28 @@ class rotate_gridmap : public rgridmap_point_conversions if (rmap != nullptr) { maps[0] = ↦ maps[1] = rmap; - map_unpad_height_m1_ = map.header_width() - 1; + 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] = ↦ @@ -280,7 +308,7 @@ class rotate_gridmap : public rgridmap_point_conversions // set values rmap_obj = std::move(tmap); maps[1] = rmap_obj.get(); - map_unpad_height_m1_ = maph - 1; + conv_assign(*maps[0], *maps[1]); } domain::gridmap& map() noexcept @@ -304,6 +332,10 @@ class rotate_gridmap : public rgridmap_point_conversions return *maps[1]; } + operator bool() const noexcept + { + return maps[0] != nullptr; + } operator gridmap_rotate_ptr() const noexcept { assert(maps[0] != nullptr && maps[1] != nullptr); @@ -312,7 +344,7 @@ class rotate_gridmap : public rgridmap_point_conversions operator gridmap_rotate_ptr_convs() const noexcept { assert(maps[0] != nullptr && maps[1] != nullptr); - return gridmap_rotate_ptr_convs(maps, map_unpad_height_m1_); + return gridmap_rotate_ptr_convs(maps); } operator gridmap_rotate_table() const noexcept { diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index d23bb28..631ff97 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -347,11 +347,11 @@ struct IntercardinalWalker }; using map_type = ::warthog::domain::gridmap::bitarray; /// @brief map and rmap (as bit array for small memory size) - map_type map[2]; + std::array map; /// @brief location of current node on map and rmap - uint32_t node_at[2]; + std::array node_at; /// @brief map and rmap value to adjust node_at for each row - int32_t adj_width[2]; + std::array adj_width; // /// @brief map and rmap target locations // uint32_t target[2]; /// @brief row scan @@ -364,79 +364,32 @@ struct IntercardinalWalker /// @brief convert map width to a map adj_width variable suited to /// intercardinal D2 - template - requires InterCardinalId static constexpr int32_t to_map_adj_width(uint32_t width) noexcept { - static_assert( - D2 == NORTHEAST_ID || D2 == NORTHWEST_ID || D2 == SOUTHEAST_ID - || D2 == SOUTHWEST_ID, - "Must be intercardinal direction"); - assert(width > 0); - if constexpr(D2 == NORTHEAST_ID) - { - return -static_cast(width - 1); // - (mapW-1) - } - else if constexpr(D2 == SOUTHEAST_ID) - { - return static_cast(width + 1); // + (mapW+1) - } - else if constexpr(D2 == SOUTHWEST_ID) - { - return static_cast(width - 1); // + (mapW-1) - } - else - { // NORTHWEST_ID - return -static_cast(width + 1); // - (mapW+1) - } + return dir_id_adj(D, width); } /// @brief convert rmap width to a rmap adj_width variable suited to /// intercardinal D2 - template - requires InterCardinalId static constexpr int32_t to_rmap_adj_width(uint32_t width) noexcept { - return to_map_adj_width(width); + return dir_id_adj(dir_id_cw(D), width); } /// @brief convert map adj_width to map width, reciprocal to /// to_map_adj_width - template - requires InterCardinalId static constexpr uint32_t from_map_adj_width(int32_t adj_width) noexcept { - static_assert( - D2 == NORTHEAST_ID || D2 == NORTHWEST_ID || D2 == SOUTHEAST_ID - || D2 == SOUTHWEST_ID, - "Must be intercardinal direction"); - if constexpr(D2 == NORTHEAST_ID) - { - return static_cast(-adj_width + 1); - } - else if constexpr(D2 == SOUTHEAST_ID) - { - return static_cast(adj_width - 1); - } - else if constexpr(D2 == SOUTHWEST_ID) - { - return static_cast(adj_width + 1); - } - else - { // NORTHWEST_ID - return static_cast(-adj_width - 1); - } + return dir_id_adj_inv_intercardinal(D, adj_width); } /// @brief convert rmap adj_width to rmap width, reciprocal to /// to_rmap_adj_width - template - requires InterCardinalId static constexpr uint32_t from_rmap_adj_width(int32_t adj_width) noexcept { - return from_map_adj_width(adj_width); + return dir_id_adj_inv_intercardinal(dir_id_cw(D), adj_width); } /// @brief set map width @@ -679,19 +632,18 @@ struct IntercardinalWalker class jump_point_online { public: - using rgridmap = domain::rotate_gridmap; using bittable = ::warthog::domain::gridmap::bittable; using rotate_grid = domain::gridmap_rotate_table_convs; - jump_point_online(); - jump_point_online(const rgridmap& map) + jump_point_online() = default; + jump_point_online(const rotate_grid& map) { set_map(map); } ~jump_point_online() = default; void - set_map(const rgridmap& map); + set_map(const rotate_grid& map); // void // set_target(jps_id target_id) noexcept; // void @@ -768,12 +720,12 @@ class jump_point_online static int32_t jump_east(bittable map, uint32_t node) { - return jump_point_online_hori(map, node); + return jump_point_online_hori(map, node); } static int32_t jump_west(bittable map, uint32_t node) { - return jump_point_online_hori(map, node); + return jump_point_online_hori(map, node); } protected: @@ -781,7 +733,7 @@ class jump_point_online }; void -jump_point_online::set_map(const rgridmap& orig) +jump_point_online::set_map(const rotate_grid& orig) { map_ = orig; } @@ -801,11 +753,11 @@ jump_point_online::jump_cardinal_next(domain::rgrid_id_t node_id) { if constexpr (D == NORTH_ID || D == EAST_ID) { - return jump_east(map_[domain::rgrid_index], node_id); + return jump_east(map_[domain::rgrid_index], static_cast(node_id)); } else if constexpr (D == SOUTH_ID || D == WEST_ID) { - return jump_west(map_[domain::rgrid_index], node_id); + return jump_west(map_[domain::rgrid_index], static_cast(node_id)); } else { assert(false); return 0; @@ -858,7 +810,7 @@ jump_point_online::jump_intercardinal_many( IntercardinalWalker walker; // class to walk // setup the walker members // 0 = map, 1 = rmap - walker.map = map_; + walker.map = {map_[0], map_[1]}; walker.map_width(map_[0].width()); walker.rmap_width(map_[1].width()); walker.node_at[0] = static_cast(map_.point_to_id(loc)); @@ -934,7 +886,7 @@ jump_point_online::jump_target( 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; + xd -= dia_unit.x; yd -= dia_unit.y; } else { // no diagonal inter_len = 0; diff --git a/include/jps/search/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index 8180ca5..5dc61e8 100644 --- a/include/jps/search/jps_expansion_policy.h +++ b/include/jps/search/jps_expansion_policy.h @@ -39,9 +39,11 @@ class jps_expansion_policy { public: jps_expansion_policy(warthog::domain::gridmap* map) - : gridmap_expansion_policy_base(map), rmap_(map) + : gridmap_expansion_policy_base(map) { - jpl_.set_map(*map); + if (map != nullptr) { + set_map(*map); + } } virtual ~jps_expansion_policy() = default; @@ -77,11 +79,29 @@ class jps_expansion_policy return jpl_; } + void set_map(warthog::domain::gridmap& map) + { + rmap_.create_rmap(map); + jpl_.set_map(rmap_); + map_width_ = rmap_.map().width(); + // std::ofstream map1("map1.txt"); + // std::ofstream map2("map2.txt"); + // rmap_.map().print(map1); + // rmap_.rmap().print(map2); + } + void set_map(domain::gridmap_rotate_ptr rmap) + { + rmap_.link(rmap); + jpl_.set_map(rmap_); + map_width_ = rmap_.map().width(); + } + private: domain::rotate_gridmap rmap_; JpsJump jpl_; - point target_loc_; - grid_id target_id_; + point target_loc_ = {}; + grid_id target_id_ = {}; + uint32_t map_width_ = 0; }; template @@ -93,12 +113,14 @@ jps_expansion_policy::expand( reset(); // compute the direction of travel used to reach the current node. - const grid_id current_id = jps_id(current->get_id()); + const grid_id current_id = grid_id(current->get_id()); const point loc = rmap_.id_to_point(current_id); + assert(rmap_.map().get_label(current_id) && rmap_.map().get_label(rmap_.point_to_id_d(loc))); // loc must be trav on map + assert(rmap_.rmap().get_label(pad_id(rmap_.point_to_id_d(loc).id))); // loc must be trav on rmap // 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()); + 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 @@ -122,15 +144,17 @@ jps_expansion_policy::expand( // cardinal directions ::warthog::util::for_each_integer_sequence< std::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(loc); if(jump_result > 0) // jump point { // successful jump - warthog::search::search_node* jp_succ - = this->generate(pad_id(static_cast(current_id.id + warthog::grid::dir_id_adj(di) * jump_result))); + 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); } } @@ -138,7 +162,8 @@ jps_expansion_policy::expand( // intercardinal directions ::warthog::util::for_each_integer_sequence< std::integer_sequence - >([&] { + >([&](auto iv) { + constexpr direction_id di = decltype(iv)::value; if(succ_dirs & warthog::grid::to_dir(di)) { jump::intercardinal_jump_result res; @@ -146,9 +171,10 @@ jps_expansion_policy::expand( if(jump_result.first > 0) // jump point { // successful jump - warthog::search::search_node* jp_succ - = this->generate(pad_id(static_cast(current_id.id + warthog::grid::dir_id_adj(di) * jump_result.first))); - add_neighbour(jp_succ, jump_result * warthog::DBL_ROOT_TWO); + 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); } } }); @@ -164,7 +190,10 @@ jps_expansion_policy::generate_start_node( pad_id padded_id = pad_id(pi->start_); if(map_->get_label(padded_id) == 0) { return nullptr; } target_id_ = grid_id(pi->target_); - target_loc_ = rmap_.map().to_unpadded_xy(target_id_); + uint32_t x, y; + rmap_.map().to_unpadded_xy(target_id_, x, y); + target_loc_.x = static_cast(x); + target_loc_.y = static_cast(y); return generate(padded_id); } From e77e0f9c227bff87a37596ca4471b01a2452eca6 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 11 Jul 2025 09:32:02 +1000 Subject: [PATCH 12/35] jps prune method start --- include/jps/search/jps_expansion_policy.h | 6 +- .../jps/search/jps_prune_expansion_policy.h | 218 ++++++++++++++++++ 2 files changed, 221 insertions(+), 3 deletions(-) create mode 100644 include/jps/search/jps_prune_expansion_policy.h diff --git a/include/jps/search/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index 5dc61e8..33ceb50 100644 --- a/include/jps/search/jps_expansion_policy.h +++ b/include/jps/search/jps_expansion_policy.h @@ -1,5 +1,5 @@ -#ifndef JPS_SEARCH_JPS_EXPANSION_POLICY2_H -#define JPS_SEARCH_JPS_EXPANSION_POLICY2_H +#ifndef JPS_SEARCH_JPS_EXPANSION_POLICY_H +#define JPS_SEARCH_JPS_EXPANSION_POLICY_H // jps_expansion_policy.h // @@ -211,4 +211,4 @@ jps_expansion_policy::generate_target_node( } -#endif // JPS_SEARCH_JPS_EXPANSION_POLICY2_H +#endif // JPS_SEARCH_JPS_EXPANSION_POLICY_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..99e3fbf --- /dev/null +++ b/include/jps/search/jps_prune_expansion_policy.h @@ -0,0 +1,218 @@ +#ifndef JPS_SEARCH_JPS_PRUNE_EXPANSION_POLICY_H +#define JPS_SEARCH_JPS_PRUNE_EXPANSION_POLICY_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 +#include + +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 +/// +/// 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+. +template +class jps_prune_expansion_policy + : public warthog::search::gridmap_expansion_policy_base +{ + static_assert(InterSize >= 1, "InterSize must be at least 2."); +public: + jps_expansion_policy(warthog::domain::gridmap* map) + : gridmap_expansion_policy_base(map) + { + if (map != nullptr) { + set_map(*map); + } + } + virtual ~jps_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 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_; + } + + void set_map(warthog::domain::gridmap& map) + { + rmap_.create_rmap(map); + jpl_.set_map(rmap_); + map_width_ = rmap_.map().width(); + // std::ofstream map1("map1.txt"); + // std::ofstream map2("map2.txt"); + // rmap_.map().print(map1); + // rmap_.rmap().print(map2); + } + void set_map(domain::gridmap_rotate_ptr rmap) + { + rmap_.link(rmap); + jpl_.set_map(rmap_); + map_width_ = rmap_.map().width(); + } + +private: + domain::rotate_gridmap rmap_; + JpsJump jpl_; + point target_loc_ = {}; + grid_id target_id_ = {}; + uint32_t map_width_ = 0; +}; + +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()); + const point loc = rmap_.id_to_point(current_id); + assert(rmap_.map().get_label(current_id) && rmap_.map().get_label(rmap_.point_to_id_d(loc))); // loc must be trav on map + assert(rmap_.rmap().get_label(pad_id(rmap_.point_to_id_d(loc).id))); // loc must be trav on rmap + // 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(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< + std::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(loc); + 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< + std::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(loc, &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_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_unpadded_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); +} + +} + +#endif // JPS_SEARCH_JPS_PRUNE_EXPANSION_POLICY_H From 5277a2df9a4760b4e48a9108a7e9e723a4399ad2 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Tue, 15 Jul 2025 09:11:11 +1000 Subject: [PATCH 13/35] progress jps prune --- extern/warthog-core | 2 +- include/jps/forward.h | 3 + include/jps/jump/jump_point_online.h | 2 +- .../jps/search/jps_prune_expansion_policy.h | 66 ++++++++++++++++--- 4 files changed, 61 insertions(+), 12 deletions(-) diff --git a/extern/warthog-core b/extern/warthog-core index 796cafc..ece1605 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 796cafc517eded6ac2c6e9ebcc0614e297ecc0a9 +Subproject commit ece1605d177c2dedb1eb04f43ed176be96bc1d47 diff --git a/include/jps/forward.h b/include/jps/forward.h index 10ba493..36685f7 100644 --- a/include/jps/forward.h +++ b/include/jps/forward.h @@ -37,4 +37,7 @@ operator|(JpsFeature a, JpsFeature b) noexcept } // namespace jps +#include +#include + #endif // JPS_SEARCH_FORWARD_H diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index 631ff97..f3e6a65 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -686,7 +686,7 @@ class jump_point_online /// @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 point + /// @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) diff --git a/include/jps/search/jps_prune_expansion_policy.h b/include/jps/search/jps_prune_expansion_policy.h index 99e3fbf..09c8cc7 100644 --- a/include/jps/search/jps_prune_expansion_policy.h +++ b/include/jps/search/jps_prune_expansion_policy.h @@ -28,7 +28,7 @@ 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 InterLimit max length the intercardinal can expand to, =c 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 /// @@ -100,12 +100,28 @@ class jps_prune_expansion_policy map_width_ = rmap_.map().width(); } + 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); + } + private: domain::rotate_gridmap rmap_; JpsJump jpl_; point target_loc_ = {}; grid_id target_id_ = {}; uint32_t map_width_ = 0; + jump::jump_distance jump_limit_ = std::numeric_limits::max(); }; template @@ -170,15 +186,45 @@ jps_prune_expansion_policy::expand( 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(loc, &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); + 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) { // InterSize == -1 will loop InterSize results until all are found + auto [result_n, dist] = jpl_.template jump_intercardinal_many(loc, 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); + int32_t node = static_cast(current_id.id) + node_adj_ic * (inter_total + res_i.inter); + const auto cost = warthog::DBL_ROOT_TWO * (inter_total + res_i.inter); + assert(rmap_.map().get(node)); // successor must be traversable + if (res_i.hori > 0) { + // horizontal + const int32_t node_j = node + node_adj_hori * res_i.hori; + const auto cost_j = cost + warthog::DBL_ONE * res_i.hori; + assert(rmap_.map().get(node_j)); // successor must be traversable + warthog::search::search_node* jp_succ = this->generate(node_j); + add_neighbour(node_j, cost_j); + } + if (res_i.vert > 0) { + // horizontal + const int32_t node_j = node + node_adj_vert * res_i.vert; + const auto cost_j = cost + warthog::DBL_ONE * res_i.vert; + assert(rmap_.map().get(node_j)); // successor must be traversable + warthog::search::search_node* jp_succ = this->generate(node_j); + add_neighbour(node_j, 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); + } } } }); From 219d9948776b0454f61369f86d24df129584bf5e Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Tue, 15 Jul 2025 15:44:14 +1000 Subject: [PATCH 14/35] jpsP pass tests --- apps/jps.cpp | 18 ++++++++-- extern/warthog-core | 2 +- .../jps/search/jps_prune_expansion_policy.h | 36 +++++++++++-------- 3 files changed, 39 insertions(+), 17 deletions(-) diff --git a/apps/jps.cpp b/apps/jps.cpp index 41b4ee5..1895102 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -19,6 +19,7 @@ #include // #include #include +#include #include "cfg.h" #include @@ -67,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, jpsP32\n"; // << "" // << "The following are valid parameters for GENERATING instances:\n" // << "\t --gen [map file (required)]\n" @@ -254,6 +254,20 @@ main(int argc, char** argv) return run_jps>( scenmgr, mapfile, alg); } + else if(alg == "jpsP") + { + using jump_point = jps::jump::jump_point_online; + return run_jps>( + scenmgr, mapfile, alg); + 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 { std::cerr << "err; invalid search algorithm: " << alg << "\n"; diff --git a/extern/warthog-core b/extern/warthog-core index ece1605..712878d 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit ece1605d177c2dedb1eb04f43ed176be96bc1d47 +Subproject commit 712878d39236601e656e4c615b4bf8c8af2ba9b7 diff --git a/include/jps/search/jps_prune_expansion_policy.h b/include/jps/search/jps_prune_expansion_policy.h index 09c8cc7..61af878 100644 --- a/include/jps/search/jps_prune_expansion_policy.h +++ b/include/jps/search/jps_prune_expansion_policy.h @@ -28,7 +28,7 @@ namespace jps::search /// @brief /// @tparam JpsJump -/// @tparam InterLimit max length the intercardinal can expand to, =c for run-time set, -1 to prune all intercardinal points +/// @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 /// @@ -42,14 +42,14 @@ class jps_prune_expansion_policy { static_assert(InterSize >= 1, "InterSize must be at least 2."); public: - jps_expansion_policy(warthog::domain::gridmap* map) + jps_prune_expansion_policy(warthog::domain::gridmap* map) : gridmap_expansion_policy_base(map) { if (map != nullptr) { set_map(*map); } } - virtual ~jps_expansion_policy() = default; + virtual ~jps_prune_expansion_policy() = default; using jump_point = JpsJump; @@ -134,7 +134,7 @@ jps_prune_expansion_policy::expand( // 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); + point loc = rmap_.id_to_point(current_id); assert(rmap_.map().get_label(current_id) && rmap_.map().get_label(rmap_.point_to_id_d(loc))); // loc must be trav on map assert(rmap_.rmap().get_label(pad_id(rmap_.point_to_id_d(loc).id))); // loc must be trav on rmap // const jps_rid current_rid = jpl_.id_to_rid(current_id); @@ -198,24 +198,24 @@ jps_prune_expansion_policy::expand( // successful jump const jump::intercardinal_jump_result res_i = res[result_i]; assert(res_i.inter > 0); - int32_t node = static_cast(current_id.id) + node_adj_ic * (inter_total + res_i.inter); + 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(node)); // successor must be traversable + assert(rmap_.map().get(pad_id{node})); // successor must be traversable if (res_i.hori > 0) { // horizontal - const int32_t node_j = node + node_adj_hori * res_i.hori; + 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(node_j)); // successor must be traversable - warthog::search::search_node* jp_succ = this->generate(node_j); - add_neighbour(node_j, cost_j); + 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 int32_t node_j = node + node_adj_vert * res_i.vert; + 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(node_j)); // successor must be traversable - warthog::search::search_node* jp_succ = this->generate(node_j); - add_neighbour(node_j, cost_j); + 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 @@ -224,6 +224,14 @@ jps_prune_expansion_policy::expand( // repeat until all jump points are discovered inter_total += dist; loc = loc + dist * dir_unit_point(di); + } 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; } } } From 1a5a9d25130b92c671f79313b9642503fc5106d0 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 17 Jul 2025 11:07:49 +1000 Subject: [PATCH 15/35] update warthog-core commit --- extern/warthog-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/extern/warthog-core b/extern/warthog-core index 712878d..50c5d04 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 712878d39236601e656e4c615b4bf8c8af2ba9b7 +Subproject commit 50c5d041e4d5359d6902f795ce8b2c0fcf3bac86 From bd2276605801993c26b4b7755150afde828ef7b8 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 17 Jul 2025 13:41:11 +1000 Subject: [PATCH 16/35] merge from gppc, refactor jps gridmap in expansion policy --- include/jps/search/jps_expansion_policy.h | 38 ++++-------- .../jps/search/jps_gridmap_expansion_policy.h | 59 +++++++++++++++++++ .../jps/search/jps_prune_expansion_policy.h | 45 +++++--------- 3 files changed, 85 insertions(+), 57 deletions(-) create mode 100644 include/jps/search/jps_gridmap_expansion_policy.h diff --git a/include/jps/search/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index 33ceb50..88c042f 100644 --- a/include/jps/search/jps_expansion_policy.h +++ b/include/jps/search/jps_expansion_policy.h @@ -18,7 +18,7 @@ // @author: dharabor // @created: 06/01/2010 -#include "jps.h" +#include "jps_gridmap_expansion_policy.h" #include #include #include @@ -35,16 +35,10 @@ namespace jps::search /// jps_2011_expansion_policy gives JPS+. template class jps_expansion_policy - : public warthog::search::gridmap_expansion_policy_base + : public jps_gridmap_expansion_policy { public: - jps_expansion_policy(warthog::domain::gridmap* map) - : gridmap_expansion_policy_base(map) - { - if (map != nullptr) { - set_map(*map); - } - } + using jps_gridmap_expansion_policy::jps_gridmap_expansion_policy; virtual ~jps_expansion_policy() = default; using jump_point = JpsJump; @@ -64,8 +58,8 @@ class jps_expansion_policy size_t mem() override { - return expansion_policy::mem() + sizeof(*this) + map_->mem() - + jpl_.mem(); + return jps_gridmap_expansion_policy::mem() + + (sizeof(jps_expansion_policy) - sizeof(jps_gridmap_expansion_policy)); } jump_point& @@ -79,25 +73,15 @@ class jps_expansion_policy return jpl_; } - void set_map(warthog::domain::gridmap& map) +protected: + void set_rmap_(domain::rotate_gridmap& rmap) override { - rmap_.create_rmap(map); - jpl_.set_map(rmap_); - map_width_ = rmap_.map().width(); - // std::ofstream map1("map1.txt"); - // std::ofstream map2("map2.txt"); - // rmap_.map().print(map1); - // rmap_.rmap().print(map2); - } - void set_map(domain::gridmap_rotate_ptr rmap) - { - rmap_.link(rmap); - jpl_.set_map(rmap_); - map_width_ = rmap_.map().width(); + jps_gridmap_expansion_policy::set_rmap_(rmap); + jpl_.set_map(rmap); + map_width_ = rmap.map().width(); } private: - domain::rotate_gridmap rmap_; JpsJump jpl_; point target_loc_ = {}; grid_id target_id_ = {}; @@ -209,6 +193,6 @@ jps_expansion_policy::generate_target_node( return generate(padded_id); } -} +} // namespace jps::search #endif // JPS_SEARCH_JPS_EXPANSION_POLICY_H diff --git a/include/jps/search/jps_gridmap_expansion_policy.h b/include/jps/search/jps_gridmap_expansion_policy.h new file mode 100644 index 0000000..a968c83 --- /dev/null +++ b/include/jps/search/jps_gridmap_expansion_policy.h @@ -0,0 +1,59 @@ +#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(nullptr) + { + if (map != nullptr) { + set_map(*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 61af878..ef093b3 100644 --- a/include/jps/search/jps_prune_expansion_policy.h +++ b/include/jps/search/jps_prune_expansion_policy.h @@ -18,7 +18,7 @@ // @author: dharabor // @created: 06/01/2010 -#include "jps.h" +#include "jps_gridmap_expansion_policy.h" #include #include #include @@ -38,17 +38,12 @@ namespace jps::search /// jps_2011_expansion_policy gives JPS+. template class jps_prune_expansion_policy - : public warthog::search::gridmap_expansion_policy_base + : public jps_gridmap_expansion_policy { static_assert(InterSize >= 1, "InterSize must be at least 2."); public: - jps_prune_expansion_policy(warthog::domain::gridmap* map) - : gridmap_expansion_policy_base(map) - { - if (map != nullptr) { - set_map(*map); - } - } + + using jps_gridmap_expansion_policy::jps_gridmap_expansion_policy; virtual ~jps_prune_expansion_policy() = default; using jump_point = JpsJump; @@ -68,8 +63,8 @@ class jps_prune_expansion_policy size_t mem() override { - return expansion_policy::mem() + sizeof(*this) + map_->mem() - + jpl_.mem(); + return jps_gridmap_expansion_policy::mem() + + (sizeof(jps_prune_expansion_policy) - sizeof(jps_gridmap_expansion_policy)); } jump_point& @@ -83,23 +78,6 @@ class jps_prune_expansion_policy return jpl_; } - void set_map(warthog::domain::gridmap& map) - { - rmap_.create_rmap(map); - jpl_.set_map(rmap_); - map_width_ = rmap_.map().width(); - // std::ofstream map1("map1.txt"); - // std::ofstream map2("map2.txt"); - // rmap_.map().print(map1); - // rmap_.rmap().print(map2); - } - void set_map(domain::gridmap_rotate_ptr rmap) - { - rmap_.link(rmap); - jpl_.set_map(rmap_); - map_width_ = rmap_.map().width(); - } - bool set_jump_limit(jump::jump_distance limit = std::numeric_limits::max()) noexcept requires(InterLimit == 0) { if (limit < 1) @@ -115,8 +93,15 @@ class jps_prune_expansion_policy return InterLimit < 0 ? std::numeric_limits::max() : static_cast(InterLimit); } +protected: + void set_rmap_(domain::rotate_gridmap& rmap) override + { + jps_gridmap_expansion_policy::set_rmap_(rmap); + jpl_.set_map(rmap); + map_width_ = rmap.map().width(); + } + private: - domain::rotate_gridmap rmap_; JpsJump jpl_; point target_loc_ = {}; grid_id target_id_ = {}; @@ -267,6 +252,6 @@ jps_prune_expansion_policy::generate_target_node return generate(padded_id); } -} +} // namespace jps::search #endif // JPS_SEARCH_JPS_PRUNE_EXPANSION_POLICY_H From 297b14b06769b0709dff737d4f51da9cfcd34ad3 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 17 Jul 2025 13:41:18 +1000 Subject: [PATCH 17/35] update warthog-core version --- extern/warthog-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/extern/warthog-core b/extern/warthog-core index 50c5d04..d64a80e 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 50c5d041e4d5359d6902f795ce8b2c0fcf3bac86 +Subproject commit d64a80e2283ead2d84fe5044cfeeb3358099bf26 From 77946d376ae3dc5345dc6ef7d945adfebe92f961 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 17 Jul 2025 14:00:25 +1000 Subject: [PATCH 18/35] jps expansion policy constructor bugfix --- include/jps/search/jps_expansion_policy.h | 11 +++++++++-- include/jps/search/jps_gridmap_expansion_policy.h | 4 ++-- include/jps/search/jps_prune_expansion_policy.h | 11 +++++++++-- 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/include/jps/search/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index 88c042f..e27bb4b 100644 --- a/include/jps/search/jps_expansion_policy.h +++ b/include/jps/search/jps_expansion_policy.h @@ -38,8 +38,15 @@ class jps_expansion_policy : public jps_gridmap_expansion_policy { public: - using jps_gridmap_expansion_policy::jps_gridmap_expansion_policy; - virtual ~jps_expansion_policy() = default; + jps_expansion_policy(warthog::domain::gridmap* map) + : jps_gridmap_expansion_policy(map) + { + if (map != nullptr) { + jpl_.set_map(rmap_); + map_width_ = rmap_.map().width(); + } + } + ~jps_expansion_policy() = default; using jump_point = JpsJump; diff --git a/include/jps/search/jps_gridmap_expansion_policy.h b/include/jps/search/jps_gridmap_expansion_policy.h index a968c83..0f44808 100644 --- a/include/jps/search/jps_gridmap_expansion_policy.h +++ b/include/jps/search/jps_gridmap_expansion_policy.h @@ -18,10 +18,10 @@ class jps_gridmap_expansion_policy { public: jps_gridmap_expansion_policy(warthog::domain::gridmap* map) - : gridmap_expansion_policy_base(nullptr) + : gridmap_expansion_policy_base(map) { if (map != nullptr) { - set_map(*map); + rmap_.create_rmap(*map); } } diff --git a/include/jps/search/jps_prune_expansion_policy.h b/include/jps/search/jps_prune_expansion_policy.h index ef093b3..c8a814d 100644 --- a/include/jps/search/jps_prune_expansion_policy.h +++ b/include/jps/search/jps_prune_expansion_policy.h @@ -42,9 +42,16 @@ class jps_prune_expansion_policy { static_assert(InterSize >= 1, "InterSize must be at least 2."); public: - + jps_prune_expansion_policy(warthog::domain::gridmap* map) + : jps_gridmap_expansion_policy(map) + { + if (map != nullptr) { + jpl_.set_map(rmap_); + map_width_ = rmap_.map().width(); + } + } using jps_gridmap_expansion_policy::jps_gridmap_expansion_policy; - virtual ~jps_prune_expansion_policy() = default; + ~jps_prune_expansion_policy() = default; using jump_point = JpsJump; From a36d11a856547542539602ec5a2b257481a47711 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 17 Jul 2025 19:17:48 +1000 Subject: [PATCH 19/35] offline progress --- include/jps/jump/jump_point_offline.h | 29 ++++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index dd4c174..73bd705 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -17,7 +17,7 @@ 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 -template +template struct jump_point_table { using direction_id = warthog::grid::direction_id; @@ -160,10 +160,33 @@ struct jump_point_table } }; -template > -class jump_point_offline +template , typename OnlinePoint = jump_point_online> +class jump_point_offline : public OnlinePoint { +public: + using jump_point_online::jump_point_online; + template + requires CardinalId + jump_distance + jump_cardinal_next(point loc) + { + if constexpr (domain::rgrid_index == 0) { + return jump_cardinal_next(this->map_.point_to_id(loc)); + } else { + return jump_cardinal_next(this->map_.rpoint_to_rid(this->map_.point_to_rpoint(loc))); + } + } + template + requires CardinalId + jump_distance + jump_cardinal_next(domain::rgrid_id_t node_id) + { + + } + +protected: + JumpTable jump_table_; }; } // namespace jps::jump From 24dc630861d1ec191e0adba1c9f87f77204adab8 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Wed, 23 Jul 2025 13:15:45 +1000 Subject: [PATCH 20/35] jps update --- extern/warthog-core | 2 +- include/jps/jump/jump_point_offline.h | 92 ++++++++++++++++++++++++--- include/jps/jump/jump_point_online.h | 39 ++++-------- 3 files changed, 97 insertions(+), 36 deletions(-) diff --git a/extern/warthog-core b/extern/warthog-core index d64a80e..0d69ab3 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit d64a80e2283ead2d84fe5044cfeeb3358099bf26 +Subproject commit 0d69ab3a1e7a329b4a5a86afa94e4348b6a112a1 diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index 73bd705..d850185 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -67,7 +67,7 @@ struct jump_point_table /// @param d direction of line /// @param loc location /// @param len length to cover, exclusive end - void set_line(direction_id d, jps_id loc, length len) noexcept + void set_line(direction_id d, grid_id loc, length len) noexcept { // no negative for deadend assert(d < 8); @@ -101,7 +101,7 @@ struct jump_point_table /// @param d direction /// @param loc location /// @return jump from loc in d - length get_jump(direction_id d, jps_id loc) noexcept + length get_jump(direction_id d, grid_id loc) noexcept { // no negative for deadend assert(d < 8); @@ -124,7 +124,7 @@ struct jump_point_table /// @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, jps_id loc) noexcept requires(ChainJump) + length chain_jump(direction_id d, grid_id loc) noexcept requires(ChainJump) { assert(db != nullptr); assert(loc.id < d_cells); @@ -152,7 +152,7 @@ struct jump_point_table /// @brief get jump cell /// @param loc location /// @return cell - cell operator[](jps_id loc) const noexcept + cell operator[](grid_id loc) const noexcept { assert(db != nullptr); assert(loc.id < d_cells); @@ -171,18 +171,92 @@ class jump_point_offline : public OnlinePoint jump_distance jump_cardinal_next(point loc) { - if constexpr (domain::rgrid_index == 0) { - return jump_cardinal_next(this->map_.point_to_id(loc)); - } else { - return jump_cardinal_next(this->map_.rpoint_to_rid(this->map_.point_to_rpoint(loc))); - } + return static_cast(jump_table_.get_jump(D, this->map_.point_to_id(loc))); } template requires CardinalId jump_distance jump_cardinal_next(domain::rgrid_id_t node_id) { + if constexpr (std::same_as, grid_id>) { + return static_cast(jump_table_.get_jump(D, node_id)); + } else { + return static_cast(jump_table_.get_jump(D, this->map_.rid_to_id(node_id))); + } + } + + template + requires InterCardinalId + std::pair + jump_intercardinal_many( + point loc, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance = std::numeric_limits::max()) + { + constexpr direction_id Dhori = get_intercardinal_hori(D); + constexpr direction_id Dvert = get_intercardinal_vert(D); + grid_id node_id = this->map_.point_to_id(loc); + const auto node_adj = static_cast(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_id)); + if (dist <= 0) { + res.second = static_cast(-res.second + dist); + break; + } + res.first += 1; + res.second += dist; + node_id.id += dist * node_adj; + // found point + intercardinal_jump_result resi; + resi.inter = res.second; + resi.hori = static_cast(jump_table_.get_jump(Dhori, node_id)); + resi.vert = static_cast(jump_table_.get_jump(Dvert, node_id)); + *(result++) = resi; + if (res.second > max_distance) + break; + } + return res; + } + void + set_map(const rotate_grid& map) + { + OnlinePoint::set_map(map); + + // compute offline jump-point table + } + void compute_jump_table() + { + uint32_t width = this->map_.width(); + uint32_t height = this->map_.height(); + jump_table_.init(width, height); + + // handle cardinal scans + struct CardinalScan + { + direction_id d; + uint32_t rows; + uint32_t cols; + uint32_t start; + uint32_t row_adj; + }; + const scans[4] = {{NORTH_ID, width, height, this->map_.point_to_id(point(0,0)), dir_id_adj_hori(EAST_ID)}, + {SOUTH_ID, width, height, this->map_.point_to_id(point(0,height-1)), dir_id_adj_hori(EAST_ID)}, + {EAST_ID, height, width, this->map_.point_to_id(point(0,0)), dir_id_adj_vert(SOUTH_ID, width)}, + {WEST_ID, height, width, this->map_.point_to_id(point(width-1,0)), dir_id_adj_vert(SOUTH_ID, width)} + }; + + for (auto s : scans) { + uint32_t col_adj = dir_id_adj(s.d, width); + uint32_t node = s.start; + for (uint32_t i = 0; i < s.rows; ++i) { + uint32_t row_node = node; + for (uint32_t j = 0; j < s.cols; ++j) { + if (this->map_.get(row_node)) { + jump_distance d = OnlinePoint::jump_cardinal_next< + } + } + } + } } protected: diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index f3e6a65..0b0ea77 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -213,7 +213,7 @@ struct BasicIntercardinalWalker /// @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 - int32_t adj_width; + uint32_t adj_width; /// @brief row scan union { @@ -228,21 +228,12 @@ struct BasicIntercardinalWalker 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) { - if constexpr (D == NORTHEAST_ID) { - this->adj_width = -static_cast(width) + 1; - } else { - this->adj_width = static_cast(width) + 1; - } row_mask_ = std::endian::native == std::endian::little ? 0b0000'0011'0000'0110 : 0b0000'0110'0000'0011; } else { // NORTHWEST_ID and SOUTHWEST_ID - if constexpr (D == NORTHWEST_ID) { - this->adj_width = -static_cast(width) - 1; - } else { - this->adj_width = static_cast(width) - 1; - } row_mask_ = std::endian::native == std::endian::little ? 0b0000'0110'0000'0011 : 0b0000'0011'0000'0110; @@ -252,8 +243,7 @@ struct BasicIntercardinalWalker void next_index() noexcept { - node_at = static_cast( - static_cast(node_at) + adj_width); + node_at += adj_width; } /// @brief call for first row, then call next_row void @@ -282,8 +272,7 @@ struct BasicIntercardinalWalker grid_id adj_id(uint32_t node, int32_t dist) const noexcept { - return grid_id(static_cast( - static_cast(node) + dist * adj_width)); + return grid_id{static_cast(node + static_cast(dist) * adj_width)}; } @@ -351,7 +340,7 @@ struct IntercardinalWalker /// @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; + std::array adj_width; // /// @brief map and rmap target locations // uint32_t target[2]; /// @brief row scan @@ -364,32 +353,32 @@ struct IntercardinalWalker /// @brief convert map width to a map adj_width variable suited to /// intercardinal D2 - static constexpr int32_t + 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 int32_t + static constexpr uint32_t to_rmap_adj_width(uint32_t width) noexcept { - return dir_id_adj(dir_id_cw(D), width); + 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(int32_t adj_width) noexcept + 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(int32_t adj_width) noexcept + from_rmap_adj_width(uint32_t adj_width) noexcept { - return dir_id_adj_inv_intercardinal(dir_id_cw(D), adj_width); + return dir_id_adj_inv_intercardinal(dir_id_cw90(D), adj_width); } /// @brief set map width @@ -541,10 +530,8 @@ struct IntercardinalWalker 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]); + 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 From 3d4ebbbd7e4b733d1490bd3b7398da44fb3234aa Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 24 Jul 2025 00:39:49 +1000 Subject: [PATCH 21/35] state --- include/jps/jump/jump_point_offline.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index d850185..265f435 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -245,14 +245,23 @@ class jump_point_offline : public OnlinePoint {WEST_ID, height, width, this->map_.point_to_id(point(width-1,0)), dir_id_adj_vert(SOUTH_ID, width)} }; + using jump_cardinal_type = jump_distance(OnlinePoint*, uint32_t); for (auto s : scans) { uint32_t col_adj = dir_id_adj(s.d, width); uint32_t node = s.start; + jump_cardinal_type* jfn = warthog::util::choose_integer_sequence + >(s.d, [](auto iv) { + // constexpr direction_id di = decltype(iv)::value; + return [](OnlinePoint* this_, uint32_t node_id) { + return this->template jump_cardinal_next({node_id}); + }; + }); for (uint32_t i = 0; i < s.rows; ++i) { uint32_t row_node = node; for (uint32_t j = 0; j < s.cols; ++j) { if (this->map_.get(row_node)) { - jump_distance d = OnlinePoint::jump_cardinal_next< + jump_distance d = std::invoke(jfn, this, node); } } } From 6762ef8b1138f171a22b60ab6a7e11bced126c24 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 25 Jul 2025 17:22:44 +1000 Subject: [PATCH 22/35] offline jps + restructured rotated grid --- apps/jps.cpp | 8 +- extern/warthog-core | 2 +- include/jps/domain/rotate_gridmap.h | 76 +++++--- include/jps/jump/jump_point_offline.h | 219 +++++++++++++++++----- include/jps/jump/jump_point_online.h | 1 + include/jps/search/jps_expansion_policy.h | 4 +- 6 files changed, 235 insertions(+), 75 deletions(-) diff --git a/apps/jps.cpp b/apps/jps.cpp index 1895102..d3bbcb1 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -17,7 +17,7 @@ #include #include -// #include +#include #include #include @@ -268,6 +268,12 @@ main(int argc, char** argv) 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 { std::cerr << "err; invalid search algorithm: " << alg << "\n"; diff --git a/extern/warthog-core b/extern/warthog-core index 0d69ab3..570dd51 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 0d69ab3a1e7a329b4a5a86afa94e4348b6a112a1 +Subproject commit 570dd51d4a512d1052d3ec060b6393951fc2c642 diff --git a/include/jps/domain/rotate_gridmap.h b/include/jps/domain/rotate_gridmap.h index 125f9ff..014bfc0 100644 --- a/include/jps/domain/rotate_gridmap.h +++ b/include/jps/domain/rotate_gridmap.h @@ -86,27 +86,35 @@ template constexpr inline int rgrid_index = details::direction_grid_id::map_id; template constexpr inline bool rgrid_east = details::direction_grid_id::east; +template +constexpr inline int rgrid_hori = rgrid_index == 0; using ::warthog::domain::gridmap; struct rgridmap_point_conversions { - uint16_t map_unpad_height_m1_ = 0; - uint16_t map_pad_width_ = 0; - uint16_t rmap_pad_width_ = 0; + static constexpr uint16_t M1P = gridmap::PADDED_ROWS - 1; // height adjustment to rotate + uint16_t map_height_m1p_ = 0; + uint16_t map_width_ = 0; + uint16_t rmap_width_ = 0; void conv_assign(const gridmap& map, const gridmap& rmap) noexcept { - map_unpad_height_m1_ = static_cast(map.header_height() - 1); - map_pad_width_ = static_cast(map.width()); - rmap_pad_width_ = static_cast(rmap.width()); + map_height_m1p_ = static_cast(map.height() + M1P); + map_width_ = static_cast(map.width()); + rmap_width_ = static_cast(rmap.width()); } + /// @return returns the padded width of map + uint16_t width() const noexcept { return map_width_; } + /// @return returns the padded height of map + uint16_t height() const noexcept { return map_height_m1p_ - M1P; } + point point_to_rpoint(point p) const noexcept { return { - static_cast(map_unpad_height_m1_ - p.y), + static_cast(map_height_m1p_ - p.y), static_cast(p.x)}; } point @@ -114,38 +122,38 @@ struct rgridmap_point_conversions { return { static_cast(p.y), - static_cast(map_unpad_height_m1_ - p.x)}; + static_cast(map_height_m1p_ - (p.x))}; } grid_id point_to_id(point p) const noexcept { return grid_id{ - static_cast(p.y + domain::gridmap::PADDED_ROWS) - * map_pad_width_ + static_cast(p.y) + * map_width_ + static_cast(p.x)}; } rgrid_id rpoint_to_rid(point p) const noexcept { return rgrid_id{ - static_cast(p.y + domain::gridmap::PADDED_ROWS) - * rmap_pad_width_ + static_cast(p.y) + * rmap_width_ + static_cast(p.x)}; } point id_to_point(grid_id p) const noexcept { return { - static_cast(p.id % map_pad_width_), - static_cast(p.id / map_pad_width_ - domain::gridmap::PADDED_ROWS)}; + static_cast(p.id % map_width_), + static_cast(p.id / map_width_)}; } point rid_to_rpoint(rgrid_id p) const noexcept { return { - static_cast(p.id % rmap_pad_width_), + static_cast(p.id % rmap_width_), static_cast( - p.id / rmap_pad_width_ - domain::gridmap::PADDED_ROWS)}; + p.id / rmap_width_)}; } rgrid_id id_to_rid(grid_id mapid) const noexcept @@ -214,7 +222,7 @@ 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), rgridmap_point_conversions{static_cast(l_map.header_height()-1), static_cast(l_map.width()), static_cast(l_rmap.width())} + : gridmap_rotate_ptr(l_map, l_rmap), rgridmap_point_conversions{static_cast(l_map.height()+M1P), static_cast(l_map.width()), static_cast(l_rmap.width())} { } gridmap_rotate_ptr_convs(gridmap_rotate_ptr maps) noexcept : gridmap_rotate_ptr(maps), rgridmap_point_conversions{} @@ -244,10 +252,12 @@ struct gridmap_rotate_table_convs : gridmap_rotate_table, rgridmap_point_convers { 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), rgridmap_point_conversions(l_map.header_height() - 1) - { } - gridmap_rotate_table_convs(gridmap_rotate_table maps, uint16_t map_unpad_height_m1) noexcept - : gridmap_rotate_table(maps), rgridmap_point_conversions{map_unpad_height_m1, static_cast(maps[0].width()), static_cast(maps[1].width())} + : gridmap_rotate_table(l_map, l_rmap), rgridmap_point_conversions{} + { + 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} { } }; @@ -290,18 +300,19 @@ class rotate_gridmap : public rgridmap_point_conversions { maps[0] = ↦ - const uint32_t maph = map.header_height(); - const uint32_t mapw = map.header_width(); + const uint32_t maph = map.height(); + const uint32_t maphmp1 = maph + M1P; + 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_unpadded(x, y)); - uint32_t rx = (maph - 1) - y; + bool label = map.get_label(map.to_padded_id_from_padded(x, y)); + uint32_t rx = maphmp1 - y; uint32_t ry = x; - tmap->set_label(tmap->to_padded_id_from_unpadded(rx, ry), label); + tmap->set_label(tmap->to_padded_id_from_padded(rx, ry), label); } } @@ -309,6 +320,17 @@ class rotate_gridmap : public rgridmap_point_conversions 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 @@ -354,7 +376,7 @@ class rotate_gridmap : public rgridmap_point_conversions operator gridmap_rotate_table_convs() const noexcept { assert(maps[0] != nullptr && maps[1] != nullptr); - return gridmap_rotate_table_convs(*this, map_unpad_height_m1_); + return gridmap_rotate_table_convs(*this, static_cast(*this)); } }; diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index 265f435..5fb7116 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -10,6 +10,10 @@ // #include "jump_point_online.h" +#include +#include +#include +#include namespace jps::jump { @@ -29,16 +33,14 @@ struct jump_point_table std::unique_ptr db; uint32_t width = 0; -#ifndef NDEBUG - uint32_t d_cells = 0; -#endif + uint32_t cells = 0; static constexpr uint32_t node_count(uint32_t width, uint32_t height) noexcept { return static_cast(width) * static_cast(height); } - static constexpr size_t mem(uint32_t width, uint32_t height) noexcept + constexpr size_t mem() noexcept { - return node_count(width, height) * sizeof(jump_res); + return cells * sizeof(jump_res); } static consteval length chain_length() noexcept @@ -56,11 +58,9 @@ struct jump_point_table /// @brief setup db, init to zero void init(uint32_t width, uint32_t height) { - db = std::make_unique(node_count(width, height)); + this->cells = node_count(width, height); this->width = width; -#ifndef NDEBUG - this->d_cells = node_count(width, height); -#endif + 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 @@ -72,7 +72,7 @@ struct jump_point_table // no negative for deadend assert(d < 8); assert(db != nullptr); - assert(loc.id < d_cells); + assert(loc.id < cells); assert(DeadEnd || len >= 0); assert(std::abs(len) < std::numeric_limits::max()); int32_t id = static_cast(loc.id); @@ -93,6 +93,7 @@ struct jump_point_table } } db[id][d] = value; + if constexpr (!ChainJump) break; id += id_adj; len += len_adj; } @@ -106,7 +107,7 @@ struct jump_point_table // no negative for deadend assert(d < 8); assert(db != nullptr); - assert(loc.id < d_cells); + assert(loc.id < cells); if constexpr (!ChainJump) { return static_cast(db[loc.id][d]); } else { @@ -118,6 +119,24 @@ struct jump_point_table } } } + /// @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 @@ -127,7 +146,7 @@ struct jump_point_table length chain_jump(direction_id d, grid_id loc) noexcept requires(ChainJump) { assert(db != nullptr); - assert(loc.id < d_cells); + assert(loc.id < cells); assert(db[loc.id][d] == chain_value()); int32_t id = loc.id; const int32_t id_adj = warthog::grid::dir_id_adj(d, width); @@ -155,15 +174,44 @@ struct jump_point_table cell operator[](grid_id loc) const noexcept { assert(db != nullptr); - assert(loc.id < d_cells); + 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 OnlinePoint = jump_point_online> class jump_point_offline : public OnlinePoint { public: + using typename OnlinePoint::bittable; + using typename OnlinePoint::rotate_grid; using jump_point_online::jump_point_online; template @@ -191,10 +239,10 @@ class jump_point_offline : public OnlinePoint jump_intercardinal_many( point loc, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance = std::numeric_limits::max()) { - constexpr direction_id Dhori = get_intercardinal_hori(D); - constexpr direction_id Dvert = get_intercardinal_vert(D); + constexpr direction_id Dhori = dir_intercardinal_hori(D); + constexpr direction_id Dvert = dir_intercardinal_vert(D); grid_id node_id = this->map_.point_to_id(loc); - const auto node_adj = static_cast(dir_id_adj(D, this->map_.width_)); + 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_id)); @@ -221,47 +269,130 @@ class jump_point_offline : public OnlinePoint set_map(const rotate_grid& map) { OnlinePoint::set_map(map); - // compute offline jump-point table + compute_jump_table(); } void compute_jump_table() { - uint32_t width = this->map_.width(); - uint32_t height = this->map_.height(); + const uint32_t width = this->map_.width(); + const uint32_t height = this->map_.height(); + const uint32_t rwidth = this->map_.rmap().width(); jump_table_.init(width, height); + auto&& point_in_range = [=](point p) noexcept { return p.x < width && p.y < height; }; // handle cardinal scans + struct CardinalScan { direction_id d; - uint32_t rows; - uint32_t cols; - uint32_t start; - uint32_t row_adj; - }; - const scans[4] = {{NORTH_ID, width, height, this->map_.point_to_id(point(0,0)), dir_id_adj_hori(EAST_ID)}, - {SOUTH_ID, width, height, this->map_.point_to_id(point(0,height-1)), dir_id_adj_hori(EAST_ID)}, - {EAST_ID, height, width, this->map_.point_to_id(point(0,0)), dir_id_adj_vert(SOUTH_ID, width)}, - {WEST_ID, height, width, this->map_.point_to_id(point(width-1,0)), dir_id_adj_vert(SOUTH_ID, width)} + 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)} + }}; using jump_cardinal_type = jump_distance(OnlinePoint*, uint32_t); - for (auto s : scans) { - uint32_t col_adj = dir_id_adj(s.d, width); - uint32_t node = s.start; - jump_cardinal_type* jfn = warthog::util::choose_integer_sequence - >(s.d, [](auto iv) { - // constexpr direction_id di = decltype(iv)::value; - return [](OnlinePoint* this_, uint32_t node_id) { - return this->template jump_cardinal_next({node_id}); - }; - }); - for (uint32_t i = 0; i < s.rows; ++i) { - uint32_t row_node = node; - for (uint32_t j = 0; j < s.cols; ++j) { - if (this->map_.get(row_node)) { - jump_distance d = std::invoke(jfn, this, node); + warthog::util::for_each_integer_sequence< std::integer_sequence >( + [&](auto iv) { + constexpr direction_id di = decltype(iv)::value; + const auto map = this->map_[domain::rgrid_index]; + 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 (point node = s.start; point_in_range(node); node = node + adj) { + point current_node = node; + while (point_in_range(current_node)) { + auto current_id = this->map_.template point_to_id_d(current_node); + if (map.get(static_cast(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, this->map_.point_to_id(current_node), 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 + // + 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_hori(s.d); + const spoint adj = dir_unit_point(s.d); + for (int axis = 0; axis < 2; ++axis) { + // 0 = follow hori, then follow vert + point start = s.start; + 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))) { + // block cell + // this->jump_table_.get(this->map_.point_to_id(loc))[s.d] = 0; + loc = loc + adj; + continue; + } + // calc distance + point currentloc = loc; + jump_distance dist = 0; + while (true) { + point nextloc = currentloc + adj; + if (point_in_range(nextloc) && this->map_.map().get(this->map_.point_to_id(nextloc))) { + // traversable tile + dist += 1; + auto cell = jump_table_[this->map_.point_to_id(nextloc)]; + if (cell[dh] > 0 || cell[dv] > 0) { + // end point here + jump_table_.set_line(s.d, this->map_.point_to_id(currentloc), 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(currentloc), -dist); + } + loc = nextloc + adj; // skip nextloc as this is a blocker + break; // done with this line + } + currentloc = nextloc; + } } } } diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index 0b0ea77..d05f245 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -802,6 +802,7 @@ jump_point_online::jump_intercardinal_many( walker.rmap_width(map_[1].width()); walker.node_at[0] = static_cast(map_.point_to_id(loc)); walker.node_at[1] = static_cast(map_.rpoint_to_rid(map_.point_to_rpoint(loc))); + assert(map_.map().get(grid_id(walker.node_at[0])) && map_.rmap().get(grid_id(walker.node_at[1]))); // JPS, stop at the first intercardinal turning point uint16_t results_count = 0; diff --git a/include/jps/search/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index e27bb4b..e8f3fda 100644 --- a/include/jps/search/jps_expansion_policy.h +++ b/include/jps/search/jps_expansion_policy.h @@ -106,8 +106,8 @@ jps_expansion_policy::expand( // 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); - assert(rmap_.map().get_label(current_id) && rmap_.map().get_label(rmap_.point_to_id_d(loc))); // loc must be trav on map - assert(rmap_.rmap().get_label(pad_id(rmap_.point_to_id_d(loc).id))); // loc must be trav on rmap + assert(rmap_.map().get_label(current_id) && rmap_.map().get_label(rmap_.point_to_id(loc))); // loc must be trav on map + assert(rmap_.rmap().get_label(static_cast(rmap_.rpoint_to_rid(rmap_.point_to_rpoint(loc))))); // loc must be trav on rmap // 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( From 013f9fc20e6c601053eb7ce23fd6370e37d23c34 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 31 Jul 2025 13:38:42 +1000 Subject: [PATCH 23/35] fixed online jps --- include/jps/domain/rotate_gridmap.h | 121 +++++++++++++++--- include/jps/forward.h | 14 -- include/jps/jump/jump_point_offline.h | 31 ++--- include/jps/jump/jump_point_online.h | 38 ++---- include/jps/search/jps_expansion_policy.h | 9 +- .../jps/search/jps_prune_expansion_policy.h | 13 +- 6 files changed, 135 insertions(+), 91 deletions(-) diff --git a/include/jps/domain/rotate_gridmap.h b/include/jps/domain/rotate_gridmap.h index 014bfc0..a0df822 100644 --- a/include/jps/domain/rotate_gridmap.h +++ b/include/jps/domain/rotate_gridmap.h @@ -26,53 +26,72 @@ template <> struct direction_grid_id { using type = rgrid_id; - static constexpr int map_id = 1; + static constexpr size_t map_id = 1; static constexpr bool east = true; }; template <> struct direction_grid_id { using type = grid_id; - static constexpr int map_id = 0; + static constexpr size_t map_id = 0; static constexpr bool east = true; }; template <> struct direction_grid_id { using type = rgrid_id; - static constexpr int map_id = 1; + static constexpr size_t map_id = 1; static constexpr bool east = false; }; template <> struct direction_grid_id { using type = grid_id; - static constexpr int map_id = 0; + static constexpr size_t map_id = 0; static constexpr bool east = false; }; template <> struct direction_grid_id { using type = rgrid_id; - static constexpr int map_id = 1; + static constexpr size_t map_id = 1; + static constexpr bool east = true; }; template <> struct direction_grid_id { using type = grid_id; - static constexpr int map_id = 0; + static constexpr size_t map_id = 0; + static constexpr bool east = true; }; template <> struct direction_grid_id { using type = rgrid_id; - static constexpr int map_id = 1; + static constexpr size_t map_id = 1; + static constexpr bool east = false; }; template <> struct direction_grid_id { using type = grid_id; - static constexpr int map_id = 0; + static constexpr size_t map_id = 0; + static constexpr bool east = false; +}; + +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 @@ -81,26 +100,86 @@ struct direction_grid_id /// @tparam D value in direction or direction_id template using rgrid_id_t = typename details::direction_grid_id::type; +template +using grid_identity = details::grid_identity>; template -constexpr inline int rgrid_index = details::direction_grid_id::map_id; +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 int rgrid_hori = rgrid_index == 0; +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 or id.r depending on template parameter +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; + } +} +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; + } +} +template +constexpr T get(const grid_pair_id& id) noexcept +{ + return get::index>(id); +} +template +constexpr T& get(grid_pair_id& id) noexcept +{ + return get::index>(id); +} +template +constexpr rgrid_id_t get_d(const grid_pair_id& id) noexcept +{ + return get>(id); +} +template +constexpr rgrid_id_t& get_d(grid_pair_id& id) noexcept +{ + return get>(id); +} +template +constexpr rgrid_id_t get_d(const grid_pair_id& id) noexcept +{ + return get>(id); +} +template +constexpr rgrid_id_t& get_d(grid_pair_id& id) noexcept +{ + return get>(id); +} + struct rgridmap_point_conversions { - static constexpr uint16_t M1P = gridmap::PADDED_ROWS - 1; // height adjustment to rotate + static constexpr uint16_t XADJ = uint16_t(-1u); // height adjustment to rotate + static constexpr uint16_t YADJ = gridmap::PADDED_ROWS; // height adjustment to rotate uint16_t map_height_m1p_ = 0; uint16_t map_width_ = 0; uint16_t rmap_width_ = 0; void conv_assign(const gridmap& map, const gridmap& rmap) noexcept { - map_height_m1p_ = static_cast(map.height() + M1P); + map_height_m1p_ = static_cast(map.height() + XADJ); map_width_ = static_cast(map.width()); rmap_width_ = static_cast(rmap.width()); } @@ -108,21 +187,21 @@ struct rgridmap_point_conversions /// @return returns the padded width of map uint16_t width() const noexcept { return map_width_; } /// @return returns the padded height of map - uint16_t height() const noexcept { return map_height_m1p_ - M1P; } + uint16_t height() const noexcept { return map_height_m1p_ - XADJ; } point point_to_rpoint(point p) const noexcept { return { static_cast(map_height_m1p_ - p.y), - static_cast(p.x)}; + static_cast(p.x + YADJ)}; } point rpoint_to_point(point p) const noexcept { return { - static_cast(p.y), - static_cast(map_height_m1p_ - (p.x))}; + static_cast(p.y - YADJ), + static_cast(map_height_m1p_ - p.x)}; } grid_id point_to_id(point p) const noexcept @@ -167,6 +246,10 @@ struct rgridmap_point_conversions 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 @@ -222,7 +305,7 @@ 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), rgridmap_point_conversions{static_cast(l_map.height()+M1P), static_cast(l_map.width()), static_cast(l_rmap.width())} + : gridmap_rotate_ptr(l_map, l_rmap), rgridmap_point_conversions{static_cast(l_map.height()+XADJ), static_cast(l_map.width()), static_cast(l_rmap.width())} { } gridmap_rotate_ptr_convs(gridmap_rotate_ptr maps) noexcept : gridmap_rotate_ptr(maps), rgridmap_point_conversions{} @@ -301,7 +384,7 @@ class rotate_gridmap : public rgridmap_point_conversions maps[0] = ↦ const uint32_t maph = map.height(); - const uint32_t maphmp1 = maph + M1P; + 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); @@ -311,7 +394,7 @@ class rotate_gridmap : public rgridmap_point_conversions { bool label = map.get_label(map.to_padded_id_from_padded(x, y)); uint32_t rx = maphmp1 - y; - uint32_t ry = x; + uint32_t ry = static_cast(x + YADJ); tmap->set_label(tmap->to_padded_id_from_padded(rx, ry), label); } } diff --git a/include/jps/forward.h b/include/jps/forward.h index 36685f7..9726f1f 100644 --- a/include/jps/forward.h +++ b/include/jps/forward.h @@ -21,20 +21,6 @@ using warthog::cost_t; using vec_jps_id = std::vector; using vec_jps_cost = std::vector; -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 diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index 5fb7116..337b883 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -217,47 +217,36 @@ class jump_point_offline : public OnlinePoint template requires CardinalId jump_distance - jump_cardinal_next(point loc) + jump_cardinal_next(domain::grid_pair_id node_id) { - return static_cast(jump_table_.get_jump(D, this->map_.point_to_id(loc))); - } - template - requires CardinalId - jump_distance - jump_cardinal_next(domain::rgrid_id_t node_id) - { - if constexpr (std::same_as, grid_id>) { - return static_cast(jump_table_.get_jump(D, node_id)); - } else { - return static_cast(jump_table_.get_jump(D, this->map_.rid_to_id(node_id))); - } + return static_cast(jump_table_.get_jump(D, get(node_id))); } template requires InterCardinalId std::pair jump_intercardinal_many( - point loc, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance = std::numeric_limits::max()) + 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_id = this->map_.point_to_id(loc); + 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_id)); + 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.id += dist * node_adj; + 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_id)); - resi.vert = static_cast(jump_table_.get_jump(Dvert, node_id)); + 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; @@ -307,8 +296,8 @@ class jump_point_offline : public OnlinePoint for (point node = s.start; point_in_range(node); node = node + adj) { point current_node = node; while (point_in_range(current_node)) { - auto current_id = this->map_.template point_to_id_d(current_node); - if (map.get(static_cast(current_id))) { + auto current_id = this->map_.point_to_pair_id(current_node); + if (map.get(get(current_id))) { jump_distance d = OnlinePoint::template jump_cardinal_next(current_id); if (d == 0)[[unlikely]] { // immediently blocked diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index d05f245..fe3dcc0 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -649,14 +649,6 @@ class jump_point_online // return rmap_; // } - /// @brief returns cardinal jump distance to next jump point - /// @tparam D cardinal direction_id (NORTH_ID,SOUTH_ID,EAST_ID,WEST_ID) - /// @param loc current location on the grid to start the jump from - /// @return >0: jump point n steps away, <=0: blocker -n steps away - template - requires CardinalId - jump_distance - jump_cardinal_next(point loc); /// @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 loc current location on the grid to start the jump from @@ -664,7 +656,7 @@ class jump_point_online template requires CardinalId jump_distance - jump_cardinal_next(domain::rgrid_id_t node_id); + 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 @@ -684,7 +676,7 @@ class jump_point_online requires InterCardinalId std::pair jump_intercardinal_many( - point loc, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance = std::numeric_limits::max()); + 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 loc shoot from loc @@ -695,7 +687,7 @@ class jump_point_online /// second<0 mean target is blocked in general std::pair jump_target( - point loc, point target); + domain::grid_pair_id node_id, point loc, point target); size_t mem() @@ -728,23 +720,15 @@ jump_point_online::set_map(const rotate_grid& orig) template requires CardinalId jump_distance -jump_point_online::jump_cardinal_next(point loc) -{ - return jump_cardinal_next(map_.point_to_id_d(loc)); -} - -template - requires CardinalId -jump_distance -jump_point_online::jump_cardinal_next(domain::rgrid_id_t node_id) +jump_point_online::jump_cardinal_next(domain::grid_pair_id node_id) { if constexpr (D == NORTH_ID || D == EAST_ID) { - return jump_east(map_[domain::rgrid_index], static_cast(node_id)); + return jump_east(map_[domain::rgrid_index], static_cast(get_d(node_id))); } else if constexpr (D == SOUTH_ID || D == WEST_ID) { - return jump_west(map_[domain::rgrid_index], static_cast(node_id)); + return jump_west(map_[domain::rgrid_index], static_cast(get_d(node_id))); } else { assert(false); return 0; @@ -755,7 +739,7 @@ template requires InterCardinalId std::pair jump_point_online::jump_intercardinal_many( - point loc, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance) + domain::grid_pair_id node_id, intercardinal_jump_result* result, uint16_t result_size, jump_distance max_distance) { assert(result_size > 0); /* @@ -800,8 +784,8 @@ jump_point_online::jump_intercardinal_many( walker.map = {map_[0], map_[1]}; walker.map_width(map_[0].width()); walker.rmap_width(map_[1].width()); - walker.node_at[0] = static_cast(map_.point_to_id(loc)); - walker.node_at[1] = static_cast(map_.rpoint_to_rid(map_.point_to_rpoint(loc))); + 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]))); // JPS, stop at the first intercardinal turning point @@ -833,7 +817,7 @@ jump_point_online::jump_intercardinal_many( std::pair jump_point_online::jump_target( - point loc, point 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); @@ -860,7 +844,7 @@ jump_point_online::jump_target( walker.set_map(map_[0], map_[0].width()); } } - walker.node_at = static_cast(map_.point_to_id(loc)); + walker.node_at = static_cast(get(node_id)); inter_len = static_cast(std::min(std::abs(xd), std::abs(yd))); walker.first_row(); for (jump_distance i = 0; i < inter_len; ++i) { diff --git a/include/jps/search/jps_expansion_policy.h b/include/jps/search/jps_expansion_policy.h index e8f3fda..ddf99af 100644 --- a/include/jps/search/jps_expansion_policy.h +++ b/include/jps/search/jps_expansion_policy.h @@ -106,6 +106,7 @@ jps_expansion_policy::expand( // 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(current_id) && rmap_.map().get_label(rmap_.point_to_id(loc))); // loc must be trav on map assert(rmap_.rmap().get_label(static_cast(rmap_.rpoint_to_rid(rmap_.point_to_rpoint(loc))))); // loc must be trav on rmap // const jps_rid current_rid = jpl_.id_to_rid(current_id); @@ -123,7 +124,7 @@ jps_expansion_policy::expand( 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(loc, target_loc_); target_dist.second >= 0) { + 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_); @@ -139,7 +140,7 @@ jps_expansion_policy::expand( constexpr direction_id di = decltype(iv)::value; if(succ_dirs & warthog::grid::to_dir(di)) { - auto jump_result = jpl_.template jump_cardinal_next(loc); + auto jump_result = jpl_.template jump_cardinal_next(pair_id); if(jump_result > 0) // jump point { // successful jump @@ -158,7 +159,7 @@ jps_expansion_policy::expand( if(succ_dirs & warthog::grid::to_dir(di)) { jump::intercardinal_jump_result res; - auto jump_result = jpl_.template jump_intercardinal_many(loc, &res, 1); + auto jump_result = jpl_.template jump_intercardinal_many(pair_id, &res, 1); if(jump_result.first > 0) // jump point { // successful jump @@ -182,7 +183,7 @@ jps_expansion_policy::generate_start_node( if(map_->get_label(padded_id) == 0) { return nullptr; } target_id_ = grid_id(pi->target_); uint32_t x, y; - rmap_.map().to_unpadded_xy(target_id_, 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); diff --git a/include/jps/search/jps_prune_expansion_policy.h b/include/jps/search/jps_prune_expansion_policy.h index c8a814d..8df089a 100644 --- a/include/jps/search/jps_prune_expansion_policy.h +++ b/include/jps/search/jps_prune_expansion_policy.h @@ -127,8 +127,8 @@ jps_prune_expansion_policy::expand( // 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); - assert(rmap_.map().get_label(current_id) && rmap_.map().get_label(rmap_.point_to_id_d(loc))); // loc must be trav on map - assert(rmap_.rmap().get_label(pad_id(rmap_.point_to_id_d(loc).id))); // loc must be trav on rmap + 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_.map().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( @@ -144,7 +144,7 @@ jps_prune_expansion_policy::expand( 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(loc, target_loc_); target_dist.second >= 0) { + 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_); @@ -160,7 +160,7 @@ jps_prune_expansion_policy::expand( constexpr direction_id di = decltype(iv)::value; if(succ_dirs & warthog::grid::to_dir(di)) { - auto jump_result = jpl_.template jump_cardinal_next(loc); + auto jump_result = jpl_.template jump_cardinal_next(pair_id); if(jump_result > 0) // jump point { // successful jump @@ -184,7 +184,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(loc, res, InterSize, get_jump_limit()); + 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 @@ -216,6 +216,7 @@ jps_prune_expansion_policy::expand( // 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); @@ -241,7 +242,7 @@ jps_prune_expansion_policy::generate_start_node( if(map_->get_label(padded_id) == 0) { return nullptr; } target_id_ = grid_id(pi->target_); uint32_t x, y; - rmap_.map().to_unpadded_xy(target_id_, 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); From 6aa191179616b5f01adc4005f81df5fd5b867270 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 31 Jul 2025 16:17:58 +1000 Subject: [PATCH 24/35] offline bugfixes --- extern/warthog-core | 2 +- include/jps/jump/jump_point_offline.h | 34 ++++++++++++++------------- include/jps/jump/jump_point_online.h | 2 ++ 3 files changed, 21 insertions(+), 17 deletions(-) diff --git a/extern/warthog-core b/extern/warthog-core index 570dd51..910e4ba 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 570dd51d4a512d1052d3ec060b6393951fc2c642 +Subproject commit 910e4ba7ea047baeff9f2081c04e27b99aa5f43b diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index 337b883..9fcbbfa 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -76,7 +76,7 @@ struct jump_point_table assert(DeadEnd || len >= 0); assert(std::abs(len) < std::numeric_limits::max()); int32_t id = static_cast(loc.id); - const int32_t id_adj = warthog::grid::dir_id_adj(d, width); + 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; @@ -93,7 +93,6 @@ struct jump_point_table } } db[id][d] = value; - if constexpr (!ChainJump) break; id += id_adj; len += len_adj; } @@ -265,7 +264,6 @@ class jump_point_offline : public OnlinePoint { const uint32_t width = this->map_.width(); const uint32_t height = this->map_.height(); - const uint32_t rwidth = this->map_.rmap().width(); jump_table_.init(width, height); auto&& point_in_range = [=](point p) noexcept { return p.x < width && p.y < height; }; @@ -288,16 +286,19 @@ class jump_point_offline : public OnlinePoint warthog::util::for_each_integer_sequence< std::integer_sequence >( [&](auto iv) { constexpr direction_id di = decltype(iv)::value; - const auto map = this->map_[domain::rgrid_index]; + 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 (point node = s.start; point_in_range(node); node = node + adj) { + // 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(get(current_id))) { + if (map.get(grid_id(get(current_id)))) { jump_distance d = OnlinePoint::template jump_cardinal_next(current_id); if (d == 0)[[unlikely]] { // immediently blocked @@ -306,7 +307,7 @@ class jump_point_offline : public OnlinePoint // assert(!this->map_.map().get(grid_id{node.id + col_adj})); } else { // store result - jump_table_.set_line(di, this->map_.point_to_id(current_node), d); + 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})); @@ -336,11 +337,13 @@ class jump_point_offline : public OnlinePoint for (auto s : Iscans) { const direction_id dh = dir_intercardinal_hori(s.d); - const direction_id dv = dir_intercardinal_hori(s.d); + const direction_id dv = dir_intercardinal_vert(s.d); const spoint adj = dir_unit_point(s.d); for (int axis = 0; axis < 2; ++axis) { - // 0 = follow hori, then follow vert + // 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); @@ -351,9 +354,8 @@ class jump_point_offline : public OnlinePoint while (true) { if (!point_in_range(loc)) break; // out of bounds, end - if (this->map_.map().get(this->map_.point_to_id(loc))) { - // block cell - // this->jump_table_.get(this->map_.point_to_id(loc))[s.d] = 0; + if (!this->map_.map().get(this->map_.point_to_id(loc))) { + // blocker loc = loc + adj; continue; } @@ -365,17 +367,17 @@ class jump_point_offline : public OnlinePoint if (point_in_range(nextloc) && this->map_.map().get(this->map_.point_to_id(nextloc))) { // traversable tile dist += 1; - auto cell = jump_table_[this->map_.point_to_id(nextloc)]; + const auto& cell = jump_table_[this->map_.point_to_id(nextloc)]; if (cell[dh] > 0 || cell[dv] > 0) { - // end point here - jump_table_.set_line(s.d, this->map_.point_to_id(currentloc), dist); + // 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(currentloc), -dist); + jump_table_.set_line(s.d, this->map_.point_to_id(loc), -dist); } loc = nextloc + adj; // skip nextloc as this is a blocker break; // done with this line diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index fe3dcc0..1bbcb88 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -32,6 +32,8 @@ 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 74d0e8cdfda9453d431e22cc29ca3c28fad471cc Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Wed, 6 Aug 2025 19:10:02 +1000 Subject: [PATCH 25/35] bugfix --- extern/warthog-core | 2 +- include/jps/jump/jump_point_offline.h | 14 +++++++++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/extern/warthog-core b/extern/warthog-core index 910e4ba..d2fe357 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 910e4ba7ea047baeff9f2081c04e27b99aa5f43b +Subproject commit d2fe3570c44621768e897b908e72d364eae484a8 diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index 9fcbbfa..86feb3f 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -339,6 +339,9 @@ class jump_point_offline : public OnlinePoint 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.map = this->map_.map(); + walker.adj_width = dir_id_adj(s.d, this->map_.map().width()); for (int axis = 0; axis < 2; ++axis) { // 0 = follow hori edge, then follow vert edge point start = s.start; @@ -362,9 +365,18 @@ class jump_point_offline : public OnlinePoint // 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; - if (point_in_range(nextloc) && this->map_.map().get(this->map_.point_to_id(nextloc))) { + 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)]; From ef23968fd303b2d4f76f988671162722a0a3199c Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 7 Aug 2025 16:26:50 +1000 Subject: [PATCH 26/35] BasicIntercardinalWalker now correctly sets map --- include/jps/jump/jump_point_offline.h | 3 +-- include/jps/jump/jump_point_online.h | 11 +++++++++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index 86feb3f..abd8cb7 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -340,8 +340,7 @@ class jump_point_offline : public OnlinePoint const direction_id dv = dir_intercardinal_vert(s.d); const spoint adj = dir_unit_point(s.d); BasicIntercardinalWalker walker; - walker.map = this->map_.map(); - walker.adj_width = dir_id_adj(s.d, this->map_.map().width()); + walker.set_map(s.d, this->map_.map(), this->map_.map().width()); for (int axis = 0; axis < 2; ++axis) { // 0 = follow hori edge, then follow vert edge point start = s.start; diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index 1bbcb88..78fe8fd 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -14,6 +14,7 @@ #include #include #include +#include namespace jps::jump { @@ -242,6 +243,16 @@ struct BasicIntercardinalWalker } } + void set_map(direction_id d, map_type map, uint32_t width) noexcept + { + assert(is_intercardinal_id(d)); + warthog::util::choose_integer_sequence< + std::integer_sequence + >(d, [&,map,width](auto dv) { + set_map(map, width); + }); + } + void next_index() noexcept { From b156dde638a08050d91162b3091ba8d75e442de9 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 7 Aug 2025 16:27:49 +1000 Subject: [PATCH 27/35] update warthog core to new version --- extern/warthog-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/extern/warthog-core b/extern/warthog-core index d2fe357..c02dc41 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit d2fe3570c44621768e897b908e72d364eae484a8 +Subproject commit c02dc4111db9c232476b6246b680aa4669e07b88 From 87e6e01844f6de8f5052674bfaea84ef13065c97 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Thu, 7 Aug 2025 16:29:34 +1000 Subject: [PATCH 28/35] jps trace branch beta --- apps/jps.cpp | 18 ++++++++++++++++-- extern/warthog-core | 2 +- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/apps/jps.cpp b/apps/jps.cpp index d3bbcb1..053d8f0 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -152,7 +153,7 @@ run_experiments( return 0; } -template +template int run_jps( warthog::util::scenario_manager& scenmgr, std::string mapname, @@ -163,7 +164,14 @@ run_jps( warthog::heuristic::octile_heuristic heuristic(map.width(), map.height()); warthog::util::pqueue_min open; - warthog::search::unidirectional_search jps(&heuristic, &expander, &open); + using listener = std::conditional_t, std::tuple<>>; + warthog::search::unidirectional_search jps(&heuristic, &expander, &open, listener{}); + if constexpr (Trace) { + std::cerr << "Trace\n"; + auto& T = std::get<::warthog::io::grid_trace>(jps.get_listeners()); + T.set_grid(&map); + T.stream_open("jps.trace.yaml"); + } int ret = run_experiments( jps, alg_name, scenmgr, verbose, checkopt, std::cout); @@ -274,6 +282,12 @@ main(int argc, char** argv) return run_jps>( scenmgr, mapfile, alg); } + else if (alg == "jps-trace") + { + using jump_point = jps::jump::jump_point_offline<>; + return run_jps, true>( + scenmgr, mapfile, alg); + } else { std::cerr << "err; invalid search algorithm: " << alg << "\n"; diff --git a/extern/warthog-core b/extern/warthog-core index c02dc41..0700e60 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit c02dc4111db9c232476b6246b680aa4669e07b88 +Subproject commit 0700e60563270b9516a6ce020903ed3bb7989b3f From b2860cdab48e0e52661fcde2282416061a29a7f7 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 8 Aug 2025 11:05:40 +1000 Subject: [PATCH 29/35] bugfix in compute table --- include/jps/jump/jump_point_offline.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/jps/jump/jump_point_offline.h b/include/jps/jump/jump_point_offline.h index abd8cb7..e97c247 100644 --- a/include/jps/jump/jump_point_offline.h +++ b/include/jps/jump/jump_point_offline.h @@ -390,7 +390,7 @@ class jump_point_offline : public OnlinePoint if (dist != 0) { jump_table_.set_line(s.d, this->map_.point_to_id(loc), -dist); } - loc = nextloc + adj; // skip nextloc as this is a blocker + loc = nextloc; break; // done with this line } currentloc = nextloc; From 44afdd4f54e3b7a8887d9dc0a207b53365d72d14 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Fri, 5 Dec 2025 17:29:43 +1100 Subject: [PATCH 30/35] update version number (#24) --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dea0e84..d0be506 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.13) project(WarthogJPS - VERSION 0.5.0 + VERSION 0.5.1 LANGUAGES CXX C) set_property(GLOBAL PROPERTY WARTHOG_warthog-jps ON) From 1477daf59c464ca3bc4cbad677948d78aaab3cec Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Wed, 25 Feb 2026 13:51:22 +1100 Subject: [PATCH 31/35] remove unused expansion policy --- .../jps/search/jps_gridmap_expansion_policy.h | 59 ------------------- 1 file changed, 59 deletions(-) delete mode 100644 include/jps/search/jps_gridmap_expansion_policy.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 0f44808..0000000 --- a/include/jps/search/jps_gridmap_expansion_policy.h +++ /dev/null @@ -1,59 +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 From c489be32b5c16e5a7779ff8a3a46249fda95bd65 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Tue, 10 Mar 2026 13:15:07 +1100 Subject: [PATCH 32/35] update jps.cpp to match warthog.cpp and apply grid trace --- apps/jps.cpp | 126 ++++++++++++++++++++++++++++++++++---------- extern/warthog-core | 2 +- 2 files changed, 100 insertions(+), 28 deletions(-) diff --git a/apps/jps.cpp b/apps/jps.cpp index b8aa921..341f5f3 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -15,7 +15,9 @@ #include #include #include +#ifdef WARTHOG_POSTHOC #include +#endif #include #include @@ -24,6 +26,7 @@ #include "cfg.h" #include +#include #include #include @@ -33,6 +36,7 @@ #include #include #include +#include // #include "time_constraints.h" @@ -44,6 +48,16 @@ int checkopt = 0; int verbose = 0; // display program help on startup int print_help = 0; +// run only this query, or -1 for all +int filter_id = -1; +#ifdef WARTHOG_POSTHOC +// write trace to file, empty string to disable +std::string trace_file; +using listener_grid = ::warthog::io::grid_trace; +using listener_type = std::tuple; +#else +using listener_type = std::tuple<>; +#endif void help(std::ostream& out) @@ -66,6 +80,11 @@ help(std::ostream& out) "values in the scen file)\n" << "\t--verbose (optional; prints debugging info when compiled " "with debug symbols)\n" + << "\t--filter [id] (optional; run only query [id])\n" +#ifdef WARTHOG_POSTHOC + << "\t--trace [.trace.yaml file] (optional; write posthoc trace for " + "first query to [file])\n" +#endif << "Invoking the program this way solves all instances in [scen " "file] with algorithm [alg]\n" << "Currently recognised values for [alg]:\n" @@ -88,7 +107,15 @@ check_optimality( if(fabs(delta - epsilon) > epsilon) { - std::cerr << std::setprecision(15); + 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 << "optimality check failed!" << std::endl; std::cerr << std::endl; std::cerr << "optimal path length: " << exp->distance() @@ -97,11 +124,17 @@ check_optimality( std::cerr << "precision: " << precision << " epsilon: " << epsilon << std::endl; std::cerr << "delta: " << delta << std::endl; - exit(1); + return false; } return true; } +#ifdef WARTHOG_POSTHOC +#define WARTHOG_POSTHOC_DO(f) f +#else +#define WARTHOG_POSTHOC_DO(f) +#endif + template int run_experiments( @@ -109,12 +142,34 @@ run_experiments( warthog::util::scenario_manager& scenmgr, bool verbose, bool checkopt, std::ostream& out) { + WARTHOG_GINFO_FMT("start search with algorithm {}", alg_name); + warthog::search::search_parameters par; + warthog::search::solution sol; auto* expander = algo.get_expander(); if(expander == nullptr) return 1; + out << "id\talg\texpanded\tgenerated\treopen\tsurplus\theapops" << "\tnanos\tplen\tpcost\tscost\tmap\n"; - for(unsigned int i = 0; i < scenmgr.num_experiments(); i++) + for(uint32_t i = filter_id >= 0 ? static_cast(filter_id) : 0, + ie = filter_id >= 0 + ? i + 1 + : static_cast(scenmgr.num_experiments()); + i < ie; i++) { +#ifdef WARTHOG_POSTHOC + std::optional trace_stream; // open and pass to trace if used + if constexpr(std::same_as< + listener_type, + std::remove_cvref_t>) + { + if(i == filter_id && !trace_file.empty()) + { + listener_grid& l = std::get(algo.get_listeners()); + trace_stream.emplace(trace_file); + l.open(*trace_stream); + } + } +#endif warthog::util::experiment* exp = scenmgr.get_experiment(i); warthog::pack_id startid @@ -122,10 +177,21 @@ run_experiments( warthog::pack_id goalid = expander->get_pack(exp->goalx(), exp->goaly()); warthog::search::problem_instance pi(startid, goalid, verbose); - warthog::search::search_parameters par; - warthog::search::solution sol; + sol.reset(); algo.get_path(&pi, &par, &sol); + +#ifdef WARTHOG_POSTHOC + if constexpr(std::same_as< + listener_type, + std::remove_cvref_t>) + { + if (trace_stream.has_value()) { + // close + std::get(algo.get_listeners()).close(); + } + } +#endif out << i << "\t" << alg_name << "\t" << sol.met_.nodes_expanded_ << "\t" << sol.met_.nodes_generated_ << "\t" @@ -138,14 +204,20 @@ run_experiments( if(checkopt) { - if(!check_optimality(sol, exp)) return 4; + if(!check_optimality(sol, exp)) + { + WARTHOG_GCRIT("search error: failed suboptimal 4"); + return 4; + } } } + WARTHOG_GINFO_FMT( + "search complete; total memory: {}", algo.mem() + scenmgr.mem()); return 0; } -template +template int run_jps( warthog::util::scenario_manager& scenmgr, std::string mapname, @@ -156,24 +228,11 @@ run_jps( warthog::heuristic::octile_heuristic heuristic(map.width(), map.height()); warthog::util::pqueue_min open; - using listener = std::conditional_t, std::tuple<>>; - warthog::search::unidirectional_search jps(&heuristic, &expander, &open, listener{}); - if constexpr (Trace) { - std::cerr << "Trace\n"; - auto& T = std::get<::warthog::io::grid_trace>(jps.get_listeners()); - T.set_grid(&map); - T.stream_open("jps.trace.yaml"); - } + warthog::search::unidirectional_search jps(&heuristic, &expander, &open, listener_type( WARTHOG_POSTHOC_DO(&map) )); int ret = run_experiments( jps, alg_name, scenmgr, verbose, checkopt, std::cout); - if(ret != 0) - { - std::cerr << "run_experiments error code " << ret << std::endl; - return ret; - } - std::cerr << "done. total memory: " << jps.mem() + scenmgr.mem() << "\n"; - return 0; + return ret; } } // namespace @@ -183,13 +242,17 @@ main(int argc, char** argv) { // parse arguments warthog::util::param valid_args[] - = {{"alg", required_argument, 0, 1}, + = {{"alg", required_argument, 0, 0}, {"scen", required_argument, 0, 0}, - {"map", required_argument, 0, 1}, + {"map", required_argument, 0, 0}, // {"gen", required_argument, 0, 3}, {"help", no_argument, &print_help, 1}, {"checkopt", no_argument, &checkopt, 1}, {"verbose", no_argument, &verbose, 1}, + {"filter", required_argument, &filter_id, 1}, +#ifdef WARTHOG_POSTHOC + {"trace", required_argument, 0, 0}, +#endif {"costs", required_argument, 0, 1}, {0, 0, 0, 0}}; @@ -199,7 +262,7 @@ main(int argc, char** argv) if(argc == 1 || print_help) { help(std::cout); - exit(0); + return 0; } std::string sfile = cfg.get_param_value("scen"); @@ -208,6 +271,14 @@ main(int argc, char** argv) std::string mapfile = cfg.get_param_value("map"); std::string costfile = cfg.get_param_value("costs"); + if(filter_id == 1) + { + filter_id = std::stoi(cfg.get_param_value("filter")); + } +#ifdef WARTHOG_POSTHOC + trace_file = cfg.get_param_value("trace"); +#endif + // if(gen != "") // { // warthog::util::scenario_manager sm; @@ -221,7 +292,7 @@ main(int argc, char** argv) if(alg == "" || sfile == "") { help(std::cout); - exit(0); + return 0; } // load up the instances @@ -231,12 +302,13 @@ main(int argc, char** argv) if(scenmgr.num_experiments() == 0) { std::cerr << "err; scenario file does not contain any instances\n"; - exit(0); + return 1; } // the map filename can be given or (default) taken from the scenario file if(mapfile == "") { + // first, try to load the map from the scenario file mapfile = warthog::util::find_map_filename(scenmgr, sfile); if(mapfile.empty()) { diff --git a/extern/warthog-core b/extern/warthog-core index 885ac31..6f2a957 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 885ac3199a006cb403ffdce26d2f1db910912526 +Subproject commit 6f2a957a4f909396752c099b9c5db2ad0acf1e69 From f204cdc7d879d729c90d64f4cd177da26ae5bf15 Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Tue, 10 Mar 2026 15:03:46 +1100 Subject: [PATCH 33/35] updated how trace renders successors for JPS --- apps/jps.cpp | 4 +- cmake/headers.cmake | 2 + include/jps/io/octile_grid_trace.h | 107 +++++++++++++++++++++++++++++ 3 files changed, 111 insertions(+), 2 deletions(-) create mode 100644 include/jps/io/octile_grid_trace.h diff --git a/apps/jps.cpp b/apps/jps.cpp index 341f5f3..e43851d 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -16,7 +16,7 @@ #include #include #ifdef WARTHOG_POSTHOC -#include +#include #endif #include @@ -53,7 +53,7 @@ int filter_id = -1; #ifdef WARTHOG_POSTHOC // write trace to file, empty string to disable std::string trace_file; -using listener_grid = ::warthog::io::grid_trace; +using listener_grid = ::warthog::io::octile_grid_trace; using listener_type = std::tuple; #else using listener_type = std::tuple<>; diff --git a/cmake/headers.cmake b/cmake/headers.cmake index 50b5884..203e310 100644 --- a/cmake/headers.cmake +++ b/cmake/headers.cmake @@ -6,6 +6,8 @@ include/jps/forward.h include/jps/domain/rotate_gridmap.h +include/jps/io/octile_grid_trace.h + include/jps/jump/block_online.h include/jps/jump/jump.h include/jps/jump/jump_point_offline.h diff --git a/include/jps/io/octile_grid_trace.h b/include/jps/io/octile_grid_trace.h new file mode 100644 index 0000000..05ae913 --- /dev/null +++ b/include/jps/io/octile_grid_trace.h @@ -0,0 +1,107 @@ +#ifndef JPS_IO_OCTILE_GRID_TRACE_H +#define JPS_IO_OCTILE_GRID_TRACE_H + +// io/octile_grid_trace.h +// +// Adds support for Octile grid trace, draws successor lines intercardinal then cardinal. +// +// @author: Ryan Hechenberger +// @created: 2025-08-07 +// + +#include + +namespace warthog::io +{ + +/// @brief class that produces a posthoc trace for the gridmap domain, grid +/// must be set. +class octile_grid_trace : public grid_trace +{ +public: + using node = search::search_node; + + using grid_trace::grid_trace; + + void + print_posthoc_header() override; + +protected: + domain::gridmap* grid_; +}; + +inline void +octile_grid_trace::print_posthoc_header() +{ + if(*this) + { + stream() << R"posthoc(version: 1.4.0 +views: + cell: + - $: rect + width: 1 + height: 1 + x: ${{$.x}} + y: ${{$.y}} + fill: ${{$.fill}} + clear: ${{$.clear}} + succesor: + - $: cell + x: ${{$.x}} + y: ${{$.y}} + fill: ${{$.fill}} + clear: ${{$.clear}} + - $: drawindirect + $if: ${{ !!parent }} + x: ${{$.x}} + y: ${{$.y}} + dx: ${{$.x-parent.x}} + dy: ${{$.y-parent.y}} + fill: ${{$.fill}} + drawindirect: + - $: path + points: [ { x: "${{$.x + 0.5}}", y: "${{$.y + 0.5}}" }, + { x: "${{ parent.x + 0.5 + ( Math.abs($.dx) < Math.abs($.dy) ? $.dx : Math.sign($.dx) * Math.abs($.dy) ) }}", + y: "${{ parent.y + 0.5 + ( Math.abs($.dy) < Math.abs($.dx) ? $.dy : Math.sign($.dy) * Math.abs($.dx) ) }}" }, + { x: "${{parent.x + 0.5}}", y: "${{parent.y + 0.5}}" } + ] + fill: ${{$.fill}} + line-width: 0.25 + clear: ${{$.clear}} + main: + - $: cell + $if: ${{ $.type == 'source' }} + fill: green + clear: false + - $: cell + $if: ${{ $.type == 'destination' }} + fill: red + clear: false + - $: cell + $if: ${{ $.type == 'expand' }} + fill: cyan + clear: false + - $: cell + $if: ${{ $.type == 'expand' }} + fill: blue + clear: close + - $: cell + $if: ${{ $.type == 'generate' }} + fill: purple + clear: false + - $: succesor + $if: ${{ $.type == 'generate' }} + fill: orange + clear: close +pivot: + x: ${{ $.x + 0.5 }} + y: ${{ $.y + 0.5 }} + scale: 1 +events: +)posthoc"; + } +} + +} // namespace warthog::io + +#endif // WARTHOG_IO_GRID_TRACE_H From 1c17877c63c338555dedc5bc62152b153c11ea1c Mon Sep 17 00:00:00 2001 From: Ryan Hechenberger Date: Tue, 17 Mar 2026 14:40:31 +1100 Subject: [PATCH 34/35] update warthog-core version --- extern/warthog-core | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/extern/warthog-core b/extern/warthog-core index 6f2a957..ba9b5f6 160000 --- a/extern/warthog-core +++ b/extern/warthog-core @@ -1 +1 @@ -Subproject commit 6f2a957a4f909396752c099b9c5db2ad0acf1e69 +Subproject commit ba9b5f66281ca6a767360787037ae80a7226e38e From 145637aa025c564be8be9793a6df838a8b98a563 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 17 Mar 2026 03:42:15 +0000 Subject: [PATCH 35/35] auto clang-format action --- apps/jps.cpp | 24 ++++++++++++++---------- include/jps/io/octile_grid_trace.h | 3 ++- include/jps/jump/jump_point_online.h | 2 -- 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/apps/jps.cpp b/apps/jps.cpp index e43851d..5448d47 100644 --- a/apps/jps.cpp +++ b/apps/jps.cpp @@ -33,10 +33,10 @@ #include #include #include +#include #include #include #include -#include // #include "time_constraints.h" @@ -157,14 +157,16 @@ run_experiments( i < ie; i++) { #ifdef WARTHOG_POSTHOC - std::optional trace_stream; // open and pass to trace if used + std::optional + trace_stream; // open and pass to trace if used if constexpr(std::same_as< - listener_type, - std::remove_cvref_t>) + listener_type, + std::remove_cvref_t>) { if(i == filter_id && !trace_file.empty()) { - listener_grid& l = std::get(algo.get_listeners()); + listener_grid& l + = std::get(algo.get_listeners()); trace_stream.emplace(trace_file); l.open(*trace_stream); } @@ -180,13 +182,14 @@ run_experiments( sol.reset(); algo.get_path(&pi, &par, &sol); - + #ifdef WARTHOG_POSTHOC if constexpr(std::same_as< - listener_type, - std::remove_cvref_t>) + listener_type, + std::remove_cvref_t>) { - if (trace_stream.has_value()) { + if(trace_stream.has_value()) + { // close std::get(algo.get_listeners()).close(); } @@ -228,7 +231,8 @@ run_jps( warthog::heuristic::octile_heuristic heuristic(map.width(), map.height()); warthog::util::pqueue_min open; - warthog::search::unidirectional_search jps(&heuristic, &expander, &open, listener_type( WARTHOG_POSTHOC_DO(&map) )); + warthog::search::unidirectional_search jps( + &heuristic, &expander, &open, listener_type(WARTHOG_POSTHOC_DO(&map))); int ret = run_experiments( jps, alg_name, scenmgr, verbose, checkopt, std::cout); diff --git a/include/jps/io/octile_grid_trace.h b/include/jps/io/octile_grid_trace.h index 05ae913..b7d638c 100644 --- a/include/jps/io/octile_grid_trace.h +++ b/include/jps/io/octile_grid_trace.h @@ -3,7 +3,8 @@ // io/octile_grid_trace.h // -// Adds support for Octile grid trace, draws successor lines intercardinal then cardinal. +// Adds support for Octile grid trace, draws successor lines intercardinal then +// cardinal. // // @author: Ryan Hechenberger // @created: 2025-08-07 diff --git a/include/jps/jump/jump_point_online.h b/include/jps/jump/jump_point_online.h index c6cfeec..80c089a 100644 --- a/include/jps/jump/jump_point_online.h +++ b/include/jps/jump/jump_point_online.h @@ -19,8 +19,6 @@ #include #include #include -#include "jump.h" -#include #include #include