diff --git a/libopenage/curve/container/array.h b/libopenage/curve/container/array.h index e3d69af553..21f6e9227b 100644 --- a/libopenage/curve/container/array.h +++ b/libopenage/curve/container/array.h @@ -2,6 +2,7 @@ #pragma once +#include #include #include "curve/container/iterator.h" @@ -109,6 +110,23 @@ class Array : event::EventEntity { */ std::pair next_frame(const time::time_t &t, const index_t index) const; + + /** + * Insert a range of elements into the Array. + * + * @param t Time of insertion. + * @param begin_it iterator pointing to the first element in the container you wish to insert. + * @param end_it iterator pointing to one after the last element in the container you wish to insert. + * @param i Index of the array at which insertion will begin. + * + * @return Time-value pair of the first keyframe with time > t. + */ + void set_insert_range(const time::time_t &t, auto begin_it, auto end_it, index_t i = 0) { + ENSURE(std::distance(begin_it, end_it) <= Size - i, + "trying to insert more values than there are postions: max allowed = " << Size - i); + std::for_each(begin_it, end_it, [&](const T &val) { this->set_insert(t, i++, val); }); + } + /** * Insert a new keyframe value at time t. * @@ -118,7 +136,7 @@ class Array : event::EventEntity { * @param index Index of the array element. * @param value Keyframe value. */ - void set_insert(const time::time_t &t, const index_t index, T value); + void set_insert(const time::time_t &t, const index_t index, const T &value); /** * Insert a new keyframe value at time t. Erase all other keyframes with elem->time > t. @@ -127,7 +145,7 @@ class Array : event::EventEntity { * @param index Index of the array element. * @param value Keyframe value. */ - void set_last(const time::time_t &t, const index_t index, T value); + void set_last(const time::time_t &t, const index_t index, const T &value); /** * Replace all keyframes at elem->time == t with a new keyframe value. @@ -136,7 +154,7 @@ class Array : event::EventEntity { * @param index Index of the array element. * @param value Keyframe value. */ - void set_replace(const time::time_t &t, const index_t index, T value); + void set_replace(const time::time_t &t, const index_t index, const T &value); /** * Copy keyframes from another container to this container. @@ -324,7 +342,7 @@ consteval size_t Array::size() const { template void Array::set_insert(const time::time_t &t, const index_t index, - T value) { + const T &value) { // find elem_ptr in container to get the last keyframe with time <= t auto hint = this->last_elements[index]; auto e = this->containers.at(index).insert_after(Keyframe{t, value}, hint); @@ -338,7 +356,7 @@ void Array::set_insert(const time::time_t &t, template void Array::set_last(const time::time_t &t, const index_t index, - T value) { + const T &value) { // find elem_ptr in container to get the last keyframe with time <= t auto hint = this->last_elements[index]; auto e = this->containers.at(index).last(t, hint); @@ -363,7 +381,7 @@ void Array::set_last(const time::time_t &t, template void Array::set_replace(const time::time_t &t, const index_t index, - T value) { + const T &value) { // find elem_ptr in container to get the last keyframe with time <= t auto hint = this->last_elements[index]; auto e = this->containers.at(index).insert_overwrite(Keyframe{t, value}, hint); diff --git a/libopenage/curve/tests/container.cpp b/libopenage/curve/tests/container.cpp index 16da2476e9..1b62f62fb6 100644 --- a/libopenage/curve/tests/container.cpp +++ b/libopenage/curve/tests/container.cpp @@ -244,9 +244,7 @@ void test_queue() { } void test_array() { - auto loop = std::make_shared(); - - Array a(loop, 0); + Array a(nullptr, 2); a.set_insert(1, 0, 0); a.set_insert(1, 1, 1); a.set_insert(1, 2, 2); @@ -273,7 +271,7 @@ void test_array() { TESTEQUALS(res.at(2), 0); TESTEQUALS(res.at(3), 0); - Array other(loop, 0); + Array other(nullptr, 2); other.set_last(0, 0, 999); other.set_last(0, 1, 999); other.set_last(0, 2, 999); @@ -344,6 +342,21 @@ void test_array() { TESTEQUALS(*it, 6); ++it; TESTEQUALS(*it, 7); + + // Test set_insert-range + std::vector vec = {100, 200, 300}; + + a.set_insert_range(5, vec.begin(), vec.end(), 1); + // a = [[0:0, 1:4, 2:25, 3:35],[0:0, 1:5, 6:100],[0:0, 1:6, 6:200],[0:0, 1:7, 5:40, 6:300]] + + it = a.begin(6); + TESTEQUALS(*it, 35); + ++it; + TESTEQUALS(*it, 100); + ++it; + TESTEQUALS(*it, 200); + ++it; + TESTEQUALS(*it, 300); } diff --git a/libopenage/gamestate/map.cpp b/libopenage/gamestate/map.cpp index 88578fabdb..dd9383f831 100644 --- a/libopenage/gamestate/map.cpp +++ b/libopenage/gamestate/map.cpp @@ -18,7 +18,7 @@ namespace openage::gamestate { Map::Map(const std::shared_ptr &state, const std::shared_ptr &terrain) : terrain{terrain}, - pathfinder{std::make_shared()}, + pathfinder{std::make_shared>()}, grid_lookup{} { // Create a grid for each path type // TODO: This is non-deterministic because of the unordered set. Is this a problem? @@ -26,10 +26,9 @@ Map::Map(const std::shared_ptr &state, std::unordered_set path_types = nyan_db->get_obj_children_all("engine.util.path_type.PathType"); size_t grid_idx = 0; auto chunk_size = this->terrain->get_chunk(0)->get_size(); - auto side_length = std::max(chunk_size[0], chunk_size[1]); auto grid_size = this->terrain->get_chunks_size(); for (const auto &path_type : path_types) { - auto grid = std::make_shared(grid_idx, grid_size, side_length); + auto grid = std::make_shared>(grid_idx, grid_size); this->pathfinder->add_grid(grid); this->grid_lookup.emplace(path_type, grid_idx); @@ -70,7 +69,7 @@ const std::shared_ptr &Map::get_terrain() const { return this->terrain; } -const std::shared_ptr &Map::get_pathfinder() const { +const std::shared_ptr> &Map::get_pathfinder() const { return this->pathfinder; } diff --git a/libopenage/gamestate/map.h b/libopenage/gamestate/map.h index 2817f74457..5dd3e83d0a 100644 --- a/libopenage/gamestate/map.h +++ b/libopenage/gamestate/map.h @@ -7,12 +7,16 @@ #include +#include "pathfinding/definitions.h" #include "pathfinding/types.h" + #include "util/vector.h" namespace openage { namespace path { + +template class Pathfinder; } // namespace path @@ -54,7 +58,7 @@ class Map { * * @return Pathfinder. */ - const std::shared_ptr &get_pathfinder() const; + const std::shared_ptr> &get_pathfinder() const; /** * Get the grid ID associated with a nyan path grid object. @@ -74,7 +78,7 @@ class Map { /** * Pathfinder. */ - std::shared_ptr pathfinder; + std::shared_ptr> pathfinder; /** * Lookup table for mapping path grid objects in nyan to grid indices. diff --git a/libopenage/gamestate/system/move.cpp b/libopenage/gamestate/system/move.cpp index 5a44baf113..97ca1d7ec3 100644 --- a/libopenage/gamestate/system/move.cpp +++ b/libopenage/gamestate/system/move.cpp @@ -26,6 +26,7 @@ #include "gamestate/game_entity.h" #include "gamestate/game_state.h" #include "gamestate/map.h" +#include "pathfinding/definitions.h" #include "pathfinding/path.h" #include "pathfinding/pathfinder.h" #include "util/fixed_point.h" @@ -33,8 +34,7 @@ namespace openage::gamestate::system { - -std::vector find_path(const std::shared_ptr &pathfinder, +std::vector find_path(const std::shared_ptr> &pathfinder, path::grid_id_t grid_id, const coord::phys3 &start, const coord::phys3 &end, diff --git a/libopenage/pathfinding/CMakeLists.txt b/libopenage/pathfinding/CMakeLists.txt index 33c2844ace..81fdc89e6a 100644 --- a/libopenage/pathfinding/CMakeLists.txt +++ b/libopenage/pathfinding/CMakeLists.txt @@ -9,6 +9,7 @@ add_sources(libopenage path.cpp pathfinder.cpp portal.cpp + portal_node.cpp sector.cpp tests.cpp types.cpp diff --git a/libopenage/pathfinding/cost_field.cpp b/libopenage/pathfinding/cost_field.cpp index 12889663eb..88642b9334 100644 --- a/libopenage/pathfinding/cost_field.cpp +++ b/libopenage/pathfinding/cost_field.cpp @@ -11,57 +11,4 @@ namespace openage::path { -CostField::CostField(size_t size) : - size{size}, - valid_until{time::TIME_MIN}, - cells(this->size * this->size, COST_MIN) { - log::log(DBG << "Created cost field with size " << this->size << "x" << this->size); -} - -size_t CostField::get_size() const { - return this->size; -} - -cost_t CostField::get_cost(const coord::tile_delta &pos) const { - return this->cells.at(pos.ne + pos.se * this->size); -} - -cost_t CostField::get_cost(size_t x, size_t y) const { - return this->cells.at(x + y * this->size); -} - -cost_t CostField::get_cost(size_t idx) const { - return this->cells.at(idx); -} - -void CostField::set_cost(const coord::tile_delta &pos, cost_t cost, const time::time_t &valid_until) { - this->set_cost(pos.ne + pos.se * this->size, cost, valid_until); -} - -void CostField::set_cost(size_t x, size_t y, cost_t cost, const time::time_t &valid_until) { - this->set_cost(x + y * this->size, cost, valid_until); -} - -const std::vector &CostField::get_costs() const { - return this->cells; -} - -void CostField::set_costs(std::vector &&cells, const time::time_t &valid_until) { - ENSURE(cells.size() == this->cells.size(), - "cells vector has wrong size: " << cells.size() - << "; expected: " - << this->cells.size()); - - this->cells = std::move(cells); - this->valid_until = valid_until; -} - -bool CostField::is_dirty(const time::time_t &time) const { - return time >= this->valid_until; -} - -void CostField::clear_dirty() { - this->valid_until = time::TIME_MAX; -} - } // namespace openage::path diff --git a/libopenage/pathfinding/cost_field.h b/libopenage/pathfinding/cost_field.h index c03b494a84..f1b29f0969 100644 --- a/libopenage/pathfinding/cost_field.h +++ b/libopenage/pathfinding/cost_field.h @@ -4,10 +4,19 @@ #include #include +#include +#include "curve/container/array.h" #include "pathfinding/types.h" #include "time/time.h" +#include "cost_field.h" + +#include "error/error.h" +#include "log/log.h" + +#include "coord/tile.h" +#include "pathfinding/definitions.h" namespace openage { namespace coord { @@ -19,21 +28,21 @@ namespace path { /** * Cost field in the flow-field pathfinding algorithm. */ + +template class CostField { public: /** - * Create a square cost field with a specified size. - * - * @param size Side length of the field. + * Create a square cost field. */ - CostField(size_t size); + CostField(const std::shared_ptr &loop = nullptr, size_t id = 0); /** * Get the size of the cost field. * * @return Size of the cost field. */ - size_t get_size() const; + constexpr size_t get_size() const; /** * Get the cost at a specified position. @@ -89,6 +98,7 @@ class CostField { inline void set_cost(size_t idx, cost_t cost, const time::time_t &valid_until) { this->cells[idx] = cost; this->valid_until = valid_until; + this->cell_cost_history.set_insert(valid_until, idx, this->cells[idx]); } /** @@ -96,7 +106,7 @@ class CostField { * * @return Cost field values. */ - const std::vector &get_costs() const; + const std::array &get_costs() const; /** * Set the cost field values. @@ -104,7 +114,28 @@ class CostField { * @param cells Cost field values. * @param valid_until Time at which the cost value expires. */ - void set_costs(std::vector &&cells, const time::time_t &changed); + void set_costs(std::array &&cells, const time::time_t &changed); + + /** + * Stamp a cost field cell at a given time. + * + * @param idx Index of the cell. + * @param cost Cost to set. + * @param stamped_at Time at which the cost cell is to be stamped. + * + * @return True if the cell was successfully stamped, false if the cell was already stamped. + */ + bool stamp(size_t idx, cost_t cost, const time::time_t &stamped_at); + + /** + * Unstamp a cost field cell at a given time. + * + * @param idx Index of the cell. + * @param unstamped_at Time at which the cost cell is to be unstamped. + * + * @return True if the cell was successfully unstamped, false if the cell was already not stamped. + */ + bool unstamp(size_t idx, const time::time_t &unstamped_at); /** * Check if the cost field is dirty at the specified time. @@ -120,12 +151,10 @@ class CostField { */ void clear_dirty(); -private: - /** - * Side length of the field. - */ - size_t size; + const curve::Array &get_cost_history() const; + +private: /** * Time the cost field expires. */ @@ -134,7 +163,115 @@ class CostField { /** * Cost field values. */ - std::vector cells; + std::array cells; + + /** + * Cost stamp vector. + */ + std::array, N * N> cost_stamps; + + + /** + * Array curve recording cell cost history, + */ + curve::Array cell_cost_history; +}; + +template +CostField::CostField(const std::shared_ptr &loop, size_t id) : + valid_until{time::TIME_MIN}, + cell_cost_history(loop, id) { + cells.fill(COST_MIN); + log::log(DBG << "Created cost field with size " << N << "x" << N); +} + +template +constexpr size_t CostField::get_size() const { + return N; +} + +template +cost_t CostField::get_cost(const coord::tile_delta &pos) const { + return this->cells.at(pos.ne + pos.se * N); +} + +template +cost_t CostField::get_cost(size_t x, size_t y) const { + return this->cells.at(x + y * N); +} + +template +cost_t CostField::get_cost(size_t idx) const { + return this->cells.at(idx); +} + +template +void CostField::set_cost(const coord::tile_delta &pos, cost_t cost, const time::time_t &valid_until) { + this->set_cost(pos.ne + pos.se * N, cost, valid_until); +} + +template +void CostField::set_cost(size_t x, size_t y, cost_t cost, const time::time_t &valid_until) { + this->set_cost(x + y * N, cost, valid_until); +} + +template +const std::array &CostField::get_costs() const { + return this->cells; +} + +template +void CostField::set_costs(std::array &&cells, const time::time_t &valid_until) { + ENSURE(cells.size() == this->cells.size(), + "cells vector has wrong size: " << cells.size() + << "; expected: " + << this->cells.size()); + + this->cells = std::move(cells); + this->valid_until = valid_until; + this->cell_cost_history.set_insert_range(valid_until, this->cells.begin(), this->cells.end()); +} + +template +bool CostField::stamp(size_t idx, cost_t cost, const time::time_t &stamped_at) { + if (this->cost_stamps[idx].has_value()) { + return false; + } + return false; + + cost_t original_cost = this->get_cost(idx); + this->cost_stamps[idx]->original_cost = original_cost; + this->cost_stamps[idx]->stamp_time = stamped_at; + + this->set_cost(idx, cost, stamped_at); + return true; +} + +template +bool CostField::unstamp(size_t idx, const time::time_t &unstamped_at) { + if (!this->cost_stamps[idx].has_value() or unstamped_at < this->cost_stamps[idx]->stamp_time) { + return false; + } + cost_t original_cost = cost_stamps[idx]->original_cost; + + this->set_cost(idx, original_cost, unstamped_at); + this->cost_stamps[idx].reset(); + return true; +} + +template +bool CostField::is_dirty(const time::time_t &time) const { + return time >= this->valid_until; +} + +template +void CostField::clear_dirty() { + this->valid_until = time::TIME_MAX; +} + +template +const curve::Array &CostField::get_cost_history() const { + return this->cell_cost_history; }; } // namespace path diff --git a/libopenage/pathfinding/definitions.h b/libopenage/pathfinding/definitions.h index 2ace3c79ab..ef7d1d464e 100644 --- a/libopenage/pathfinding/definitions.h +++ b/libopenage/pathfinding/definitions.h @@ -32,6 +32,12 @@ constexpr cost_t COST_MAX = 254; constexpr cost_t COST_IMPASSABLE = 255; +/** + * size of sectors. + */ +constexpr size_t SECTOR_SIZE = 100; + + /** * Start value for goal cells. */ diff --git a/libopenage/pathfinding/demo/definitions.h b/libopenage/pathfinding/demo/definitions.h new file mode 100644 index 0000000000..2e90a5534e --- /dev/null +++ b/libopenage/pathfinding/demo/definitions.h @@ -0,0 +1,10 @@ +// Copyright 2025-2025 the openage authors. See copying.md for legal info. + +#pragma once + +namespace openage::path::tests { + + //Side length of the field + constexpr size_t SECTOR_SIZE = 10; + +} \ No newline at end of file diff --git a/libopenage/pathfinding/demo/demo_0.cpp b/libopenage/pathfinding/demo/demo_0.cpp index 31f66bb51f..83840979fa 100644 --- a/libopenage/pathfinding/demo/demo_0.cpp +++ b/libopenage/pathfinding/demo/demo_0.cpp @@ -27,12 +27,8 @@ namespace openage::path::tests { void path_demo_0(const util::Path &path) { - // Side length of the field - // Creates a 10x10 grid - auto field_length = 10; - // Cost field with some obstacles - auto cost_field = std::make_shared(field_length); + auto cost_field = std::make_shared>(); const time::time_t time = time::TIME_ZERO; cost_field->set_cost(coord::tile_delta{0, 0}, COST_IMPASSABLE, time); @@ -47,7 +43,7 @@ void path_demo_0(const util::Path &path) { log::log(INFO << "Created cost field"); // Create an integration field from the cost field - auto integration_field = std::make_shared(field_length); + auto integration_field = std::make_shared>(); log::log(INFO << "Created integration field"); // Set cell (7, 7) to be the initial target cell @@ -56,7 +52,7 @@ void path_demo_0(const util::Path &path) { log::log(INFO << "Calculated integration field for target cell (7, 7)"); // Create a flow field from the integration field - auto flow_field = std::make_shared(field_length); + auto flow_field = std::make_shared>(); log::log(INFO << "Created flow field"); flow_field->build(integration_field); @@ -89,7 +85,7 @@ void path_demo_0(const util::Path &path) { auto grid_x = tile_pos.first; auto grid_y = tile_pos.second; - if (grid_x >= 0 and grid_x < field_length and grid_y >= 0 and grid_y < field_length) { + if (grid_x >= 0 and grid_x < SECTOR_SIZE and grid_y >= 0 and grid_y < SECTOR_SIZE) { log::log(INFO << "Selected new target cell (" << grid_x << ", " << grid_y << ")"); // Recalculate the integration field and the flow field @@ -203,7 +199,7 @@ void RenderManager0::run() { this->window->close(); } -void RenderManager0::show_cost_field(const std::shared_ptr &field) { +void RenderManager0::show_cost_field(const std::shared_ptr> &field) { Eigen::Matrix4f model = Eigen::Matrix4f::Identity(); auto unifs = this->cost_shader->new_uniform_input( "model", @@ -223,7 +219,7 @@ void RenderManager0::show_cost_field(const std::shared_ptr &fie this->field_pass->set_renderables({renderable}); } -void RenderManager0::show_integration_field(const std::shared_ptr &field) { +void RenderManager0::show_integration_field(const std::shared_ptr> &field) { Eigen::Matrix4f model = Eigen::Matrix4f::Identity(); auto unifs = this->integration_shader->new_uniform_input( "model", @@ -243,8 +239,8 @@ void RenderManager0::show_integration_field(const std::shared_ptrfield_pass->set_renderables({renderable}); } -void RenderManager0::show_flow_field(const std::shared_ptr &flow_field, - const std::shared_ptr &int_field) { +void RenderManager0::show_flow_field(const std::shared_ptr> &flow_field, + const std::shared_ptr> &int_field) { Eigen::Matrix4f model = Eigen::Matrix4f::Identity(); auto unifs = this->flow_shader->new_uniform_input( "model", @@ -264,7 +260,7 @@ void RenderManager0::show_flow_field(const std::shared_ptr &flo this->field_pass->set_renderables({renderable}); } -void RenderManager0::show_vectors(const std::shared_ptr &field) { +void RenderManager0::show_vectors(const std::shared_ptr> &field) { static const std::unordered_map offset_dir{ {flow_dir_t::NORTH, {-0.25f, 0.0f, 0.0f}}, {flow_dir_t::NORTH_EAST, {-0.25f, 0.0f, -0.25f}}, @@ -551,7 +547,7 @@ void RenderManager0::init_passes() { renderer->get_display_target()); } -renderer::resources::MeshData RenderManager0::get_cost_field_mesh(const std::shared_ptr &field, +renderer::resources::MeshData RenderManager0::get_cost_field_mesh(const std::shared_ptr> &field, size_t resolution) { // increase by 1 in every dimension because to get the vertex length // of each dimension @@ -637,7 +633,7 @@ renderer::resources::MeshData RenderManager0::get_cost_field_mesh(const std::sha return {std::move(vert_data), std::move(idx_data), info}; } -renderer::resources::MeshData RenderManager0::get_integration_field_mesh(const std::shared_ptr &field, +renderer::resources::MeshData RenderManager0::get_integration_field_mesh(const std::shared_ptr> &field, size_t resolution) { // increase by 1 in every dimension because to get the vertex length // of each dimension @@ -723,8 +719,8 @@ renderer::resources::MeshData RenderManager0::get_integration_field_mesh(const s return {std::move(vert_data), std::move(idx_data), info}; } -renderer::resources::MeshData RenderManager0::get_flow_field_mesh(const std::shared_ptr &flow_field, - const std::shared_ptr &int_field, +renderer::resources::MeshData RenderManager0::get_flow_field_mesh(const std::shared_ptr> &flow_field, + const std::shared_ptr> &int_field, size_t resolution) { // increase by 1 in every dimension because to get the vertex length // of each dimension diff --git a/libopenage/pathfinding/demo/demo_0.h b/libopenage/pathfinding/demo/demo_0.h index adf5651971..0dfefa5635 100644 --- a/libopenage/pathfinding/demo/demo_0.h +++ b/libopenage/pathfinding/demo/demo_0.h @@ -4,6 +4,10 @@ #include +#include "definitions.h" +#include "pathfinding/definitions.h" + + #include "util/path.h" @@ -28,12 +32,20 @@ class MeshData; } // namespace renderer namespace path { + +template class CostField; + +template class IntegrationField; + +template class FlowField; + namespace tests { + /** * Show the functionality of the different flowfield types: * - Cost field @@ -80,14 +92,14 @@ class RenderManager0 { * * @param field Cost field. */ - void show_cost_field(const std::shared_ptr &field); + void show_cost_field(const std::shared_ptr> &field); /** * Draw an integration field to the screen. * * @param field Integration field. */ - void show_integration_field(const std::shared_ptr &field); + void show_integration_field(const std::shared_ptr> &field); /** * Draw a flow field to the screen. @@ -95,15 +107,15 @@ class RenderManager0 { * @param flow_field Flow field. * @param int_field Integration field. */ - void show_flow_field(const std::shared_ptr &flow_field, - const std::shared_ptr &int_field); + void show_flow_field(const std::shared_ptr> &flow_field, + const std::shared_ptr> &int_field); /** * Draw the steering vectors of a flow field to the screen. * * @param field Flow field. */ - void show_vectors(const std::shared_ptr &field); + void show_vectors(const std::shared_ptr> &field); /** * Hide drawn steering vectors. @@ -144,7 +156,7 @@ class RenderManager0 { * * @return Mesh data for the cost field. */ - static renderer::resources::MeshData get_cost_field_mesh(const std::shared_ptr &field, + static renderer::resources::MeshData get_cost_field_mesh(const std::shared_ptr> &field, size_t resolution = 2); /** @@ -156,7 +168,7 @@ class RenderManager0 { * * @return Mesh data for the integration field. */ - static renderer::resources::MeshData get_integration_field_mesh(const std::shared_ptr &field, + static renderer::resources::MeshData get_integration_field_mesh(const std::shared_ptr> &field, size_t resolution = 2); /** @@ -165,8 +177,8 @@ class RenderManager0 { * @param flow_field Flow field to visualize. * @param int_field Integration field. */ - static renderer::resources::MeshData get_flow_field_mesh(const std::shared_ptr &flow_field, - const std::shared_ptr &int_field, + static renderer::resources::MeshData get_flow_field_mesh(const std::shared_ptr> &flow_field, + const std::shared_ptr> &int_field, size_t resolution = 2); /** diff --git a/libopenage/pathfinding/demo/demo_1.cpp b/libopenage/pathfinding/demo/demo_1.cpp index 61df3e8995..6b94e2d3a6 100644 --- a/libopenage/pathfinding/demo/demo_1.cpp +++ b/libopenage/pathfinding/demo/demo_1.cpp @@ -25,14 +25,14 @@ namespace openage::path::tests { void path_demo_1(const util::Path &path) { - auto grid = std::make_shared(0, util::Vector2s{4, 3}, 10); + auto grid = std::make_shared>(0, util::Vector2s{4, 3}); // Initialize the cost field for each sector. for (auto §or : grid->get_sectors()) { auto cost_field = sector->get_cost_field(); // Read the data from the preconfigured table - std::vector sector_cost = SECTORS_COST.at(sector->get_id()); + std::array sector_cost = SECTORS_COST.at(sector->get_id()); // Set the cost field for the sector cost_field->set_costs(std::move(sector_cost), time::TIME_MAX); @@ -92,7 +92,7 @@ void path_demo_1(const util::Path &path) { } // Create a pathfinder for searching paths on the grid - auto pathfinder = std::make_shared(); + auto pathfinder = std::make_shared>(); pathfinder->add_grid(grid); // Add a timer to measure the pathfinding speed @@ -134,8 +134,8 @@ void path_demo_1(const util::Path &path) { window->add_mouse_button_callback([&](const QMouseEvent &ev) { if (ev.type() == QEvent::MouseButtonRelease) { // From the mouse position, calculate the position/cell on the grid - auto cell_count_x = grid->get_size()[0] * grid->get_sector_size(); - auto cell_count_y = grid->get_size()[1] * grid->get_sector_size(); + auto cell_count_x = grid->get_size()[0] * SECTOR_SIZE; + auto cell_count_y = grid->get_size()[1] * SECTOR_SIZE; auto window_size = window->get_size(); double cell_size_x = static_cast(window_size[0]) / cell_count_x; @@ -200,7 +200,7 @@ void path_demo_1(const util::Path &path) { RenderManager1::RenderManager1(const std::shared_ptr &app, const std::shared_ptr &window, const util::Path &path, - const std::shared_ptr &grid) : + const std::shared_ptr> &grid) : path{path}, grid{grid}, app{app}, @@ -301,8 +301,8 @@ void RenderManager1::init_passes() { // Create object for the grid auto model = Eigen::Affine3f::Identity(); model.prescale(Eigen::Vector3f{ - 2.0f / (this->grid->get_size()[0] * this->grid->get_sector_size()), - 2.0f / (this->grid->get_size()[1] * this->grid->get_sector_size()), + 2.0f / (this->grid->get_size()[0] * SECTOR_SIZE), + 2.0f / (this->grid->get_size()[1] * SECTOR_SIZE), 1.0f}); model.pretranslate(Eigen::Vector3f{-1.0f, -1.0f, 0.0f}); auto grid_unifs = grid_shader->new_uniform_input( @@ -409,12 +409,12 @@ void RenderManager1::init_shaders() { } -renderer::resources::MeshData RenderManager1::get_grid_mesh(const std::shared_ptr &grid) { +renderer::resources::MeshData RenderManager1::get_grid_mesh(const std::shared_ptr> &grid) { // increase by 1 in every dimension because to get the vertex length // of each dimension util::Vector2s size{ - grid->get_size()[0] * grid->get_sector_size() + 1, - grid->get_size()[1] * grid->get_sector_size() + 1}; + grid->get_size()[0] * SECTOR_SIZE + 1, + grid->get_size()[1] * SECTOR_SIZE + 1}; // add vertices for the cells of the grid std::vector verts{}; @@ -464,10 +464,10 @@ renderer::resources::MeshData RenderManager1::get_grid_mesh(const std::shared_pt return {std::move(vert_data), std::move(idx_data), info}; } -void RenderManager1::create_impassible_tiles(const std::shared_ptr &grid) { +void RenderManager1::create_impassible_tiles(const std::shared_ptr> &grid) { auto width = grid->get_size()[0]; auto height = grid->get_size()[1]; - auto sector_size = grid->get_sector_size(); + auto sector_size = SECTOR_SIZE; float tile_offset_width = 2.0f / (width * sector_size); float tile_offset_height = 2.0f / (height * sector_size); @@ -526,10 +526,10 @@ void RenderManager1::create_impassible_tiles(const std::shared_ptr & } } -void RenderManager1::create_portal_tiles(const std::shared_ptr &grid) { +void RenderManager1::create_portal_tiles(const std::shared_ptr> &grid) { auto width = grid->get_size()[0]; auto height = grid->get_size()[1]; - auto sector_size = grid->get_sector_size(); + auto sector_size = SECTOR_SIZE; float tile_offset_width = 2.0f / (width * sector_size); float tile_offset_height = 2.0f / (height * sector_size); @@ -606,7 +606,7 @@ void RenderManager1::create_portal_tiles(const std::shared_ptr &grid void RenderManager1::create_waypoint_tiles(const Path &path) { auto width = grid->get_size()[0]; auto height = grid->get_size()[1]; - auto sector_size = grid->get_sector_size(); + auto sector_size = SECTOR_SIZE; float tile_offset_width = 2.0f / (width * sector_size); float tile_offset_height = 2.0f / (height * sector_size); diff --git a/libopenage/pathfinding/demo/demo_1.h b/libopenage/pathfinding/demo/demo_1.h index b2d3037eb8..cde6f2e4b5 100644 --- a/libopenage/pathfinding/demo/demo_1.h +++ b/libopenage/pathfinding/demo/demo_1.h @@ -2,6 +2,7 @@ #pragma once +#include "definitions.h" #include "pathfinding/definitions.h" #include "pathfinding/path.h" #include "renderer/resources/mesh_data.h" @@ -22,11 +23,12 @@ class GuiApplicationWithLogger; } // namespace renderer namespace path { + +template class Grid; namespace tests { - /** * Show the functionality of the high-level pathfinder: * - Grids @@ -55,7 +57,7 @@ class RenderManager1 { RenderManager1(const std::shared_ptr &app, const std::shared_ptr &window, const util::Path &path, - const std::shared_ptr &grid); + const std::shared_ptr> &grid); ~RenderManager1() = default; /** @@ -92,21 +94,21 @@ class RenderManager1 { * * @return Mesh data for the grid. */ - static renderer::resources::MeshData get_grid_mesh(const std::shared_ptr &grid); + static renderer::resources::MeshData get_grid_mesh(const std::shared_ptr> &grid); /** * Create renderables for the impassible tiles in the grid. * * @param grid Pathing grid. */ - void create_impassible_tiles(const std::shared_ptr &grid); + void create_impassible_tiles(const std::shared_ptr> &grid); /** * Create renderables for the portal tiles in the grid. * * @param grid Pathing grid. */ - void create_portal_tiles(const std::shared_ptr &grid); + void create_portal_tiles(const std::shared_ptr> &grid); /** * Path to the project rootdir. @@ -116,7 +118,7 @@ class RenderManager1 { /** * Pathing grid of the demo. */ - std::shared_ptr grid; + std::shared_ptr> grid; /* Renderer objects */ @@ -182,7 +184,7 @@ class RenderManager1 { // Cost for the sectors in the grid // taken from Figure 23.1 in "Crowd Pathfinding and Steering Using Flow Field Tiles" -const std::vector> SECTORS_COST = { +const std::vector> SECTORS_COST = { { // clang-format off 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, diff --git a/libopenage/pathfinding/demo/tests.h b/libopenage/pathfinding/demo/tests.h index 2f411f6bf1..6200d334d2 100644 --- a/libopenage/pathfinding/demo/tests.h +++ b/libopenage/pathfinding/demo/tests.h @@ -5,7 +5,6 @@ #include "../../util/compiler.h" // pxd: from libopenage.util.path cimport Path - namespace openage { namespace util { class Path; diff --git a/libopenage/pathfinding/field_cache.cpp b/libopenage/pathfinding/field_cache.cpp index 86f985e3ba..b587068e1e 100644 --- a/libopenage/pathfinding/field_cache.cpp +++ b/libopenage/pathfinding/field_cache.cpp @@ -4,29 +4,4 @@ namespace openage::path { -void FieldCache::add(const cache_key_t cache_key, - const field_cache_t cache_entry) { - this->cache[cache_key] = cache_entry; -} - -bool FieldCache::evict(const cache_key_t cache_key) { - return this->cache.erase(cache_key) != 0; -} - -bool FieldCache::is_cached(const cache_key_t cache_key) const { - return this->cache.contains(cache_key); -} - -std::shared_ptr FieldCache::get_integration_field(const cache_key_t cache_key) const { - return this->cache.at(cache_key).first; -} - -std::shared_ptr FieldCache::get_flow_field(const cache_key_t cache_key) const { - return this->cache.at(cache_key).second; -} - -field_cache_t FieldCache::get(const cache_key_t cache_key) const { - return this->cache.at(cache_key); -} - } // namespace openage::path diff --git a/libopenage/pathfinding/field_cache.h b/libopenage/pathfinding/field_cache.h index b5c39b44de..482267a4b4 100644 --- a/libopenage/pathfinding/field_cache.h +++ b/libopenage/pathfinding/field_cache.h @@ -15,12 +15,17 @@ struct tile_delta; } // namespace coord namespace path { + +template class IntegrationField; + +template class FlowField; /** * Cache to store already calculated flow and integration fields for the pathfinding algorithm. */ +template class FieldCache { public: FieldCache() = default; @@ -33,7 +38,7 @@ class FieldCache { * @param cache_entry Field entry (integration field, flow field). */ void add(const cache_key_t cache_key, - const field_cache_t cache_entry); + const field_cache_t cache_entry); /** * Evict field entry from the cache. @@ -60,7 +65,7 @@ class FieldCache { * * @return Integration field. */ - std::shared_ptr get_integration_field(const cache_key_t cache_key) const; + std::shared_ptr> get_integration_field(const cache_key_t cache_key) const; /** * Get a cached flow field. @@ -69,7 +74,7 @@ class FieldCache { * * @return Flow field. */ - std::shared_ptr get_flow_field(const cache_key_t cache_key) const; + std::shared_ptr> get_flow_field(const cache_key_t cache_key) const; /** * Get a cached field entry. @@ -80,7 +85,7 @@ class FieldCache { * * @return Field entry (integration field, flow field). */ - field_cache_t get(const cache_key_t cache_key) const; + field_cache_t get(const cache_key_t cache_key) const; private: /** @@ -101,10 +106,40 @@ class FieldCache { * when the field is reused. */ std::unordered_map, pair_hash> cache; }; +template +void FieldCache::add(const cache_key_t cache_key, + const field_cache_t cache_entry) { + this->cache[cache_key] = cache_entry; +} + +template +bool FieldCache::evict(const cache_key_t cache_key) { + return this->cache.erase(cache_key) != 0; +} +template +bool FieldCache::is_cached(const cache_key_t cache_key) const { + return this->cache.contains(cache_key); +} + +template +std::shared_ptr> FieldCache::get_integration_field(const cache_key_t cache_key) const { + return this->cache.at(cache_key).first; +} + +template +std::shared_ptr> FieldCache::get_flow_field(const cache_key_t cache_key) const { + return this->cache.at(cache_key).second; +} + +template +field_cache_t FieldCache::get(const cache_key_t cache_key) const { + return this->cache.at(cache_key); +} + } // namespace path } // namespace openage diff --git a/libopenage/pathfinding/flow_field.cpp b/libopenage/pathfinding/flow_field.cpp index e582345b48..c56595f85e 100644 --- a/libopenage/pathfinding/flow_field.cpp +++ b/libopenage/pathfinding/flow_field.cpp @@ -2,282 +2,6 @@ #include "flow_field.h" -#include - -#include "error/error.h" -#include "log/log.h" - -#include "coord/tile.h" -#include "pathfinding/integration_field.h" -#include "pathfinding/portal.h" - - namespace openage::path { -FlowField::FlowField(size_t size) : - size{size}, - cells(this->size * this->size, FLOW_INIT) { - log::log(DBG << "Created flow field with size " << this->size << "x" << this->size); -} - -FlowField::FlowField(const std::shared_ptr &integration_field) : - size{integration_field->get_size()}, - cells(this->size * this->size, FLOW_INIT) { - this->build(integration_field); -} - -size_t FlowField::get_size() const { - return this->size; -} - -flow_t FlowField::get_cell(const coord::tile_delta &pos) const { - return this->cells.at(pos.ne + pos.se * this->size); -} - -flow_t FlowField::get_cell(size_t x, size_t y) const { - return this->cells.at(x + y * this->size); -} - -flow_t FlowField::get_cell(size_t idx) const { - return this->cells.at(idx); -} - -flow_dir_t FlowField::get_dir(const coord::tile_delta &pos) const { - return static_cast(this->get_cell(pos) & FLOW_DIR_MASK); -} - -flow_dir_t FlowField::get_dir(size_t x, size_t y) const { - return static_cast(this->get_cell(x, y) & FLOW_DIR_MASK); -} - -flow_dir_t FlowField::get_dir(size_t idx) const { - return static_cast(this->get_cell(idx) & FLOW_DIR_MASK); -} - -void FlowField::build(const std::shared_ptr &integration_field) { - ENSURE(integration_field->get_size() == this->get_size(), - "integration field size " - << integration_field->get_size() << "x" << integration_field->get_size() - << " does not match flow field size " - << this->get_size() << "x" << this->get_size()); - - auto &integrate_cells = integration_field->get_cells(); - auto &flow_cells = this->cells; - - for (size_t y = 0; y < this->size; ++y) { - for (size_t x = 0; x < this->size; ++x) { - size_t idx = y * this->size + x; - - const auto &integrate_cell = integrate_cells[idx]; - auto &flow_cell = flow_cells[idx]; - - if (integrate_cell.cost == INTEGRATED_COST_UNREACHABLE) { - // Cell cannot be used as path - continue; - } - - flow_t transfer_flags = integrate_cell.flags & FLOW_FLAGS_MASK; - flow_cell |= transfer_flags; - - if (flow_cell & FLOW_TARGET_MASK) { - // target cells are pathable - flow_cell |= FLOW_PATHABLE_MASK; - - // they also have a preset flow direction so we can skip here - continue; - } - - // Store which of the non-diagonal directions are unreachable. - // north == 0x01, east == 0x02, south == 0x04, west == 0x08 - uint8_t directions_unreachable = 0x00; - - // Find the neighbor with the smallest cost. - flow_dir_t direction = static_cast(flow_cell & FLOW_DIR_MASK); - auto smallest_cost = INTEGRATED_COST_UNREACHABLE; - - // Cardinal directions - if (y > 0) { - auto cost = integrate_cells[idx - this->size].cost; - if (cost == INTEGRATED_COST_UNREACHABLE) { - directions_unreachable |= 0x01; - } - else if (cost < smallest_cost) { - smallest_cost = cost; - direction = flow_dir_t::NORTH; - } - } - if (x < this->size - 1) { - auto cost = integrate_cells[idx + 1].cost; - if (cost == INTEGRATED_COST_UNREACHABLE) { - directions_unreachable |= 0x02; - } - else if (cost < smallest_cost) { - smallest_cost = cost; - direction = flow_dir_t::EAST; - } - } - if (y < this->size - 1) { - auto cost = integrate_cells[idx + this->size].cost; - if (cost == INTEGRATED_COST_UNREACHABLE) { - directions_unreachable |= 0x04; - } - else if (cost < smallest_cost) { - smallest_cost = cost; - direction = flow_dir_t::SOUTH; - } - } - if (x > 0) { - auto cost = integrate_cells[idx - 1].cost; - if (cost == INTEGRATED_COST_UNREACHABLE) { - directions_unreachable |= 0x08; - } - else if (cost < smallest_cost) { - smallest_cost = cost; - direction = flow_dir_t::WEST; - } - } - - // Diagonal directions - if (x < this->size - 1 and y > 0 - and not(directions_unreachable & 0x01 and directions_unreachable & 0x02)) { - auto cost = integrate_cells[idx - this->size + 1].cost; - if (cost < smallest_cost) { - smallest_cost = cost; - direction = flow_dir_t::NORTH_EAST; - } - } - if (x < this->size - 1 and y < this->size - 1 - and not(directions_unreachable & 0x02 and directions_unreachable & 0x04)) { - auto cost = integrate_cells[idx + this->size + 1].cost; - if (cost < smallest_cost) { - smallest_cost = cost; - direction = flow_dir_t::SOUTH_EAST; - } - } - if (x > 0 and y < this->size - 1 - and not(directions_unreachable & 0x04 and directions_unreachable & 0x08)) { - auto cost = integrate_cells[idx + this->size - 1].cost; - if (cost < smallest_cost) { - smallest_cost = cost; - direction = flow_dir_t::SOUTH_WEST; - } - } - if (x > 0 and y > 0 - and not(directions_unreachable & 0x01 and directions_unreachable & 0x08)) { - auto cost = integrate_cells[idx - this->size - 1].cost; - if (cost < smallest_cost) { - smallest_cost = cost; - direction = flow_dir_t::NORTH_WEST; - } - } - - // Set the flow field cell to pathable. - flow_cell |= FLOW_PATHABLE_MASK; - - // Set the flow field cell to the direction of the smallest cost. - flow_cell |= static_cast(direction); - } - } -} - -void FlowField::build(const std::shared_ptr &integration_field, - const std::shared_ptr & /* other */, - sector_id_t other_sector_id, - const std::shared_ptr &portal) { - ENSURE(integration_field->get_size() == this->get_size(), - "integration field size " - << integration_field->get_size() << "x" << integration_field->get_size() - << " does not match flow field size " - << this->get_size() << "x" << this->get_size()); - - auto &flow_cells = this->cells; - auto direction = portal->get_direction(); - - // portal entry and exit cell coordinates - auto entry_start = portal->get_entry_start(other_sector_id); - auto exit_start = portal->get_exit_start(other_sector_id); - auto exit_end = portal->get_exit_end(other_sector_id); - - // TODO: Compare integration values from other side of portal - // auto &integrate_cells = integration_field->get_cells(); - - // set the direction for the flow field cells that are part of the portal - if (direction == PortalDirection::NORTH_SOUTH) { - bool other_is_north = entry_start.se > exit_start.se; - if (other_is_north) { - auto y = exit_start.se; - for (auto x = exit_start.ne; x <= exit_end.ne; ++x) { - auto idx = y * this->size + x; - flow_cells[idx] = flow_cells[idx] | FLOW_PATHABLE_MASK; - flow_cells[idx] = flow_cells[idx] | static_cast(flow_dir_t::NORTH); - } - } - else { - auto y = exit_start.se; - for (auto x = exit_start.ne; x <= exit_end.ne; ++x) { - auto idx = y * this->size + x; - flow_cells[idx] = flow_cells[idx] | FLOW_PATHABLE_MASK; - flow_cells[idx] = flow_cells[idx] | static_cast(flow_dir_t::SOUTH); - } - } - } - else if (direction == PortalDirection::EAST_WEST) { - bool other_is_east = entry_start.ne < exit_start.ne; - if (other_is_east) { - auto x = exit_start.ne; - for (auto y = exit_start.se; y <= exit_end.se; ++y) { - auto idx = y * this->size + x; - flow_cells[idx] = flow_cells[idx] | FLOW_PATHABLE_MASK; - flow_cells[idx] = flow_cells[idx] | static_cast(flow_dir_t::EAST); - } - } - else { - auto x = exit_start.ne; - for (auto y = exit_start.se; y <= exit_end.se; ++y) { - auto idx = y * this->size + x; - flow_cells[idx] = flow_cells[idx] | FLOW_PATHABLE_MASK; - flow_cells[idx] = flow_cells[idx] | static_cast(flow_dir_t::WEST); - } - } - } - else { - throw Error(ERR << "Invalid portal direction: " << static_cast(direction)); - } - - this->build(integration_field); -} - -const std::vector &FlowField::get_cells() const { - return this->cells; -} - -void FlowField::reset() { - std::fill(this->cells.begin(), this->cells.end(), FLOW_INIT); - - log::log(DBG << "Flow field has been reset"); -} - -void FlowField::reset_dynamic_flags() { - flow_t mask = 0xFF & ~(FLOW_LOS_MASK); - for (flow_t &cell : this->cells) { - cell = cell & mask; - } - - log::log(DBG << "Flow field dynamic flags have been reset"); -} - -void FlowField::transfer_dynamic_flags(const std::shared_ptr &integration_field) { - auto &integrate_cells = integration_field->get_cells(); - auto &flow_cells = this->cells; - - for (size_t idx = 0; idx < integrate_cells.size(); ++idx) { - if (integrate_cells[idx].flags & INTEGRATE_LOS_MASK) { - // Cell is in line of sight - flow_cells[idx] |= FLOW_LOS_MASK; - } - } - - log::log(DBG << "Flow field dynamic flags have been transferred"); -} - } // namespace openage::path diff --git a/libopenage/pathfinding/flow_field.h b/libopenage/pathfinding/flow_field.h index 5dc91fed96..2082114b82 100644 --- a/libopenage/pathfinding/flow_field.h +++ b/libopenage/pathfinding/flow_field.h @@ -2,12 +2,20 @@ #pragma once +#include #include #include #include #include +#include "error/error.h" +#include "log/log.h" + +#include "coord/tile.h" + #include "pathfinding/definitions.h" +#include "pathfinding/integration_field.h" +#include "pathfinding/portal.h" #include "pathfinding/types.h" @@ -17,31 +25,33 @@ struct tile_delta; } // namespace coord namespace path { + +template class IntegrationField; + class Portal; +template class FlowField { public: /** * Create a square flow field with a specified size. - * - * @param size Side length of the field. */ - FlowField(size_t size); + FlowField(); /** * Create a flow field from an existing integration field. * * @param integration_field Integration field. */ - FlowField(const std::shared_ptr &integration_field); + FlowField(const std::shared_ptr> &integration_field); /** * Get the size of the flow field. * * @return Size of the flow field. */ - size_t get_size() const; + constexpr size_t get_size() const; /** * Get the flow field value at a specified position. @@ -104,7 +114,7 @@ class FlowField { * * @param integration_field Integration field. */ - void build(const std::shared_ptr &integration_field); + void build(const std::shared_ptr> &integration_field); /** * Build the flow field for a portal. @@ -114,8 +124,8 @@ class FlowField { * @param other_sector_id Sector ID of the other field. * @param portal Portal connecting the two fields. */ - void build(const std::shared_ptr &integration_field, - const std::shared_ptr &other, + void build(const std::shared_ptr> &integration_field, + const std::shared_ptr> &other, sector_id_t other_sector_id, const std::shared_ptr &portal); @@ -124,7 +134,7 @@ class FlowField { * * @return Flow field values. */ - const std::vector &get_cells() const; + const std::array &get_cells() const; /** * Reset the flow field values for rebuilding the field. @@ -155,19 +165,294 @@ class FlowField { * * @param integration_field Integration field. */ - void transfer_dynamic_flags(const std::shared_ptr &integration_field); + void transfer_dynamic_flags(const std::shared_ptr> &integration_field); private: - /** - * Side length of the field. - */ - size_t size; - /** * Flow field cells. */ - std::vector cells; + std::array cells; }; + +template +FlowField::FlowField() { + cells.fill(FLOW_INIT); + log::log(DBG << "Created flow field with size " << N << "x" << N); +} + +template +FlowField::FlowField(const std::shared_ptr> &integration_field) { + cells.fill(N * N, FLOW_INIT); + this->build(integration_field); +} + +template +constexpr size_t FlowField::get_size() const { + return N; +} + +template +flow_t FlowField::get_cell(const coord::tile_delta &pos) const { + return this->cells.at(pos.ne + pos.se * N); +} + +template +flow_t FlowField::get_cell(size_t x, size_t y) const { + return this->cells.at(x + y * N); +} + +template +flow_t FlowField::get_cell(size_t idx) const { + return this->cells.at(idx); +} + +template +flow_dir_t FlowField::get_dir(const coord::tile_delta &pos) const { + return static_cast(this->get_cell(pos) & FLOW_DIR_MASK); +} + +template +flow_dir_t FlowField::get_dir(size_t x, size_t y) const { + return static_cast(this->get_cell(x, y) & FLOW_DIR_MASK); +} + +template +flow_dir_t FlowField::get_dir(size_t idx) const { + return static_cast(this->get_cell(idx) & FLOW_DIR_MASK); +} + +template +void FlowField::build(const std::shared_ptr> &integration_field) { + ENSURE(integration_field->get_size() == this->get_size(), + "integration field size " + << integration_field->get_size() << "x" << integration_field->get_size() + << " does not match flow field size " + << this->get_size() << "x" << this->get_size()); + + auto &integrate_cells = integration_field->get_cells(); + auto &flow_cells = this->cells; + + for (size_t y = 0; y < N; ++y) { + for (size_t x = 0; x < N; ++x) { + size_t idx = y * N + x; + + const auto &integrate_cell = integrate_cells[idx]; + auto &flow_cell = flow_cells[idx]; + + if (integrate_cell.cost == INTEGRATED_COST_UNREACHABLE) { + // Cell cannot be used as path + continue; + } + + flow_t transfer_flags = integrate_cell.flags & FLOW_FLAGS_MASK; + flow_cell |= transfer_flags; + + if (flow_cell & FLOW_TARGET_MASK) { + // target cells are pathable + flow_cell |= FLOW_PATHABLE_MASK; + + // they also have a preset flow direction so we can skip here + continue; + } + + // Store which of the non-diagonal directions are unreachable. + // north == 0x01, east == 0x02, south == 0x04, west == 0x08 + uint8_t directions_unreachable = 0x00; + + // Find the neighbor with the smallest cost. + flow_dir_t direction = static_cast(flow_cell & FLOW_DIR_MASK); + auto smallest_cost = INTEGRATED_COST_UNREACHABLE; + + // Cardinal directions + if (y > 0) { + auto cost = integrate_cells[idx - N].cost; + if (cost == INTEGRATED_COST_UNREACHABLE) { + directions_unreachable |= 0x01; + } + else if (cost < smallest_cost) { + smallest_cost = cost; + direction = flow_dir_t::NORTH; + } + } + if (x < N - 1) { + auto cost = integrate_cells[idx + 1].cost; + if (cost == INTEGRATED_COST_UNREACHABLE) { + directions_unreachable |= 0x02; + } + else if (cost < smallest_cost) { + smallest_cost = cost; + direction = flow_dir_t::EAST; + } + } + if (y < N - 1) { + auto cost = integrate_cells[idx + N].cost; + if (cost == INTEGRATED_COST_UNREACHABLE) { + directions_unreachable |= 0x04; + } + else if (cost < smallest_cost) { + smallest_cost = cost; + direction = flow_dir_t::SOUTH; + } + } + if (x > 0) { + auto cost = integrate_cells[idx - 1].cost; + if (cost == INTEGRATED_COST_UNREACHABLE) { + directions_unreachable |= 0x08; + } + else if (cost < smallest_cost) { + smallest_cost = cost; + direction = flow_dir_t::WEST; + } + } + + // Diagonal directions + if (x < N - 1 and y > 0 + and not(directions_unreachable & 0x01 and directions_unreachable & 0x02)) { + auto cost = integrate_cells[idx - N + 1].cost; + if (cost < smallest_cost) { + smallest_cost = cost; + direction = flow_dir_t::NORTH_EAST; + } + } + if (x < N - 1 and y < N - 1 + and not(directions_unreachable & 0x02 and directions_unreachable & 0x04)) { + auto cost = integrate_cells[idx + N + 1].cost; + if (cost < smallest_cost) { + smallest_cost = cost; + direction = flow_dir_t::SOUTH_EAST; + } + } + if (x > 0 and y < N - 1 + and not(directions_unreachable & 0x04 and directions_unreachable & 0x08)) { + auto cost = integrate_cells[idx + N - 1].cost; + if (cost < smallest_cost) { + smallest_cost = cost; + direction = flow_dir_t::SOUTH_WEST; + } + } + if (x > 0 and y > 0 + and not(directions_unreachable & 0x01 and directions_unreachable & 0x08)) { + auto cost = integrate_cells[idx - N - 1].cost; + if (cost < smallest_cost) { + smallest_cost = cost; + direction = flow_dir_t::NORTH_WEST; + } + } + + // Set the flow field cell to pathable. + flow_cell |= FLOW_PATHABLE_MASK; + + // Set the flow field cell to the direction of the smallest cost. + flow_cell |= static_cast(direction); + } + } +} + +template +void FlowField::build(const std::shared_ptr> &integration_field, + const std::shared_ptr> & /* other */, + sector_id_t other_sector_id, + const std::shared_ptr &portal) { + ENSURE(integration_field->get_size() == this->get_size(), + "integration field size " + << integration_field->get_size() << "x" << integration_field->get_size() + << " does not match flow field size " + << this->get_size() << "x" << this->get_size()); + + auto &flow_cells = this->cells; + auto direction = portal->get_direction(); + + // portal entry and exit cell coordinates + auto entry_start = portal->get_entry_start(other_sector_id); + auto exit_start = portal->get_exit_start(other_sector_id); + auto exit_end = portal->get_exit_end(other_sector_id); + + // TODO: Compare integration values from other side of portal + // auto &integrate_cells = integration_field->get_cells(); + + // set the direction for the flow field cells that are part of the portal + if (direction == PortalDirection::NORTH_SOUTH) { + bool other_is_north = entry_start.se > exit_start.se; + if (other_is_north) { + auto y = exit_start.se; + for (auto x = exit_start.ne; x <= exit_end.ne; ++x) { + auto idx = y * N + x; + flow_cells[idx] = flow_cells[idx] | FLOW_PATHABLE_MASK; + flow_cells[idx] = flow_cells[idx] | static_cast(flow_dir_t::NORTH); + } + } + else { + auto y = exit_start.se; + for (auto x = exit_start.ne; x <= exit_end.ne; ++x) { + auto idx = y * N + x; + flow_cells[idx] = flow_cells[idx] | FLOW_PATHABLE_MASK; + flow_cells[idx] = flow_cells[idx] | static_cast(flow_dir_t::SOUTH); + } + } + } + else if (direction == PortalDirection::EAST_WEST) { + bool other_is_east = entry_start.ne < exit_start.ne; + if (other_is_east) { + auto x = exit_start.ne; + for (auto y = exit_start.se; y <= exit_end.se; ++y) { + auto idx = y * N + x; + flow_cells[idx] = flow_cells[idx] | FLOW_PATHABLE_MASK; + flow_cells[idx] = flow_cells[idx] | static_cast(flow_dir_t::EAST); + } + } + else { + auto x = exit_start.ne; + for (auto y = exit_start.se; y <= exit_end.se; ++y) { + auto idx = y * N + x; + flow_cells[idx] = flow_cells[idx] | FLOW_PATHABLE_MASK; + flow_cells[idx] = flow_cells[idx] | static_cast(flow_dir_t::WEST); + } + } + } + else { + throw Error(ERR << "Invalid portal direction: " << static_cast(direction)); + } + + this->build(integration_field); +} + +template +const std::array &FlowField::get_cells() const { + return this->cells; +} + +template +void FlowField::reset() { + std::fill(this->cells.begin(), this->cells.end(), FLOW_INIT); + + log::log(DBG << "Flow field has been reset"); +} + +template +void FlowField::reset_dynamic_flags() { + flow_t mask = 0xFF & ~(FLOW_LOS_MASK); + for (flow_t &cell : this->cells) { + cell = cell & mask; + } + + log::log(DBG << "Flow field dynamic flags have been reset"); +} + +template +void FlowField::transfer_dynamic_flags(const std::shared_ptr> &integration_field) { + auto &integrate_cells = integration_field->get_cells(); + auto &flow_cells = this->cells; + + for (size_t idx = 0; idx < integrate_cells.size(); ++idx) { + if (integrate_cells[idx].flags & INTEGRATE_LOS_MASK) { + // Cell is in line of sight + flow_cells[idx] |= FLOW_LOS_MASK; + } + } + + log::log(DBG << "Flow field dynamic flags have been transferred"); +} + } // namespace path } // namespace openage diff --git a/libopenage/pathfinding/grid.cpp b/libopenage/pathfinding/grid.cpp index 45427b009d..eccd91ddc2 100644 --- a/libopenage/pathfinding/grid.cpp +++ b/libopenage/pathfinding/grid.cpp @@ -11,114 +11,4 @@ namespace openage::path { -Grid::Grid(grid_id_t id, - const util::Vector2s &size, - size_t sector_size) : - id{id}, - size{size}, - sector_size{sector_size} { - for (size_t y = 0; y < size[1]; y++) { - for (size_t x = 0; x < size[0]; x++) { - this->sectors.push_back( - std::make_shared(x + y * this->size[0], - coord::chunk(x, y), - sector_size)); - } - } -} - -Grid::Grid(grid_id_t id, - const util::Vector2s &size, - std::vector> &§ors) : - id{id}, - size{size}, - sectors{std::move(sectors)} { - ENSURE(this->sectors.size() == size[0] * size[1], - "Grid has size " << size[0] << "x" << size[1] << " (" << size[0] * size[1] << " sectors), " - << "but only " << this->sectors.size() << " sectors were provided"); - - this->sector_size = sectors.at(0)->get_cost_field()->get_size(); -} - -grid_id_t Grid::get_id() const { - return this->id; -} - -const util::Vector2s &Grid::get_size() const { - return this->size; -} - -size_t Grid::get_sector_size() const { - return this->sector_size; -} - -const std::shared_ptr &Grid::get_sector(size_t x, size_t y) { - return this->sectors.at(x + y * this->size[0]); -} - -const std::shared_ptr &Grid::get_sector(sector_id_t id) const { - return this->sectors.at(id); -} - -const std::vector> &Grid::get_sectors() const { - return this->sectors; -} - -void Grid::init_portals() { - // Create portals between neighboring sectors. - portal_id_t portal_id = 0; - for (size_t y = 0; y < this->size[1]; y++) { - for (size_t x = 0; x < this->size[0]; x++) { - auto sector = this->get_sector(x, y); - - if (x < this->size[0] - 1) { - auto neighbor = this->get_sector(x + 1, y); - auto portals = sector->find_portals(neighbor, PortalDirection::EAST_WEST, portal_id); - for (auto &portal : portals) { - sector->add_portal(portal); - neighbor->add_portal(portal); - } - portal_id += portals.size(); - } - if (y < this->size[1] - 1) { - auto neighbor = this->get_sector(x, y + 1); - auto portals = sector->find_portals(neighbor, PortalDirection::NORTH_SOUTH, portal_id); - for (auto &portal : portals) { - sector->add_portal(portal); - neighbor->add_portal(portal); - } - portal_id += portals.size(); - } - } - } - - // Connect mutually reachable exits of sectors. - for (auto §or : this->sectors) { - sector->connect_exits(); - } -} - -const nodemap_t &Grid::get_portal_map() { - return portal_nodes; -} - -void Grid::init_portal_nodes() { - // create portal_nodes - for (auto §or : this->sectors) { - for (auto &portal : sector->get_portals()) { - if (!this->portal_nodes.contains(portal->get_id())) { - auto portal_node = std::make_shared(portal); - portal_node->node_sector_0 = sector->get_id(); - portal_node->node_sector_1 = portal_node->portal->get_exit_sector(sector->get_id()); - this->portal_nodes[portal->get_id()] = portal_node; - } - } - } - - // init portal_node exits - for (auto &[id, node] : this->portal_nodes) { - node->init_exits(this->portal_nodes); - } -} - } // namespace openage::path diff --git a/libopenage/pathfinding/grid.h b/libopenage/pathfinding/grid.h index 314d107a2e..9c6a00d5fc 100644 --- a/libopenage/pathfinding/grid.h +++ b/libopenage/pathfinding/grid.h @@ -7,17 +7,21 @@ #include #include -#include "pathfinding/pathfinder.h" +#include "pathfinding/portal_node.h" #include "pathfinding/types.h" + #include "util/vector.h" namespace openage::path { + +template class Sector; /** * Grid for flow field pathfinding. */ +template class Grid { public: /** @@ -25,11 +29,9 @@ class Grid { * * @param id ID of the grid. * @param size Size of the grid in sectors (width x height). - * @param sector_size Side length of each sector. */ Grid(grid_id_t id, - const util::Vector2s &size, - size_t sector_size); + const util::Vector2s &size); /** * Create a grid of width x height sectors from a list of existing sectors. @@ -40,7 +42,7 @@ class Grid { */ Grid(grid_id_t id, const util::Vector2s &size, - std::vector> &§ors); + std::vector>> &§ors); /** * Get the ID of the grid. @@ -56,12 +58,6 @@ class Grid { */ const util::Vector2s &get_size() const; - /** - * Get the side length of the sectors on the grid (in number of cells). - * - * @return Sector side length (in number of cells). - */ - size_t get_sector_size() const; /** * Get the sector at a specified position. @@ -71,7 +67,7 @@ class Grid { * * @return Sector at the specified position. */ - const std::shared_ptr &get_sector(size_t x, size_t y); + const std::shared_ptr> &get_sector(size_t x, size_t y); /** * Get the sector with a specified ID @@ -80,14 +76,14 @@ class Grid { * * @return Sector with the specified ID. */ - const std::shared_ptr &get_sector(sector_id_t id) const; + const std::shared_ptr> &get_sector(sector_id_t id) const; /** * Get the sectors of the grid. * * @return Sectors of the grid. */ - const std::vector> &get_sectors() const; + const std::vector>> &get_sectors() const; /** * Initialize the portals of the sectors on the grid. @@ -99,7 +95,7 @@ class Grid { /** * returns map of portal ids to portal nodes */ - const nodemap_t &get_portal_map(); + const PortalNode::nodemap_t &get_portal_map(); /** * Initialize the portal nodes of the grid with neigbouring nodes and distance costs. @@ -117,23 +113,131 @@ class Grid { */ util::Vector2s size; - /** - * Side length of the grid sectors. - */ - size_t sector_size; /** * Sectors of the grid. */ - std::vector> sectors; + std::vector>> sectors; /** * maps portal_ids to portal nodes, which store their neigbouring nodes and associated distance costs * for pathfinding */ - nodemap_t portal_nodes; + PortalNode::nodemap_t portal_nodes; }; +template +Grid::Grid(grid_id_t id, + const util::Vector2s &size) : + id{id}, + size{size} { + for (size_t y = 0; y < size[1]; y++) { + for (size_t x = 0; x < size[0]; x++) { + this->sectors.push_back( + std::make_shared>(x + y * this->size[0], + coord::chunk(x, y))); + } + } +} + +template +Grid::Grid(grid_id_t id, + const util::Vector2s &size, + std::vector>> &§ors) : + id{id}, + size{size}, + sectors{std::move(sectors)} { + ENSURE(this->sectors.size() == size[0] * size[1], + "Grid has size " << size[0] << "x" << size[1] << " (" << size[0] * size[1] << " sectors), " + << "but only " << this->sectors.size() << " sectors were provided"); +} + +template +grid_id_t Grid::get_id() const { + return this->id; +} + +template +const util::Vector2s &Grid::get_size() const { + return this->size; +} + +template +const std::shared_ptr> &Grid::get_sector(size_t x, size_t y) { + return this->sectors.at(x + y * this->size[0]); +} + +template +const std::shared_ptr> &Grid::get_sector(sector_id_t id) const { + return this->sectors.at(id); +} + +template +const std::vector>> &Grid::get_sectors() const { + return this->sectors; +} + +template +void Grid::init_portals() { + // Create portals between neighboring sectors. + portal_id_t portal_id = 0; + for (size_t y = 0; y < this->size[1]; y++) { + for (size_t x = 0; x < this->size[0]; x++) { + auto sector = this->get_sector(x, y); + + if (x < this->size[0] - 1) { + auto neighbor = this->get_sector(x + 1, y); + auto portals = sector->find_portals(neighbor, PortalDirection::EAST_WEST, portal_id); + for (auto &portal : portals) { + sector->add_portal(portal); + neighbor->add_portal(portal); + } + portal_id += portals.size(); + } + if (y < this->size[1] - 1) { + auto neighbor = this->get_sector(x, y + 1); + auto portals = sector->find_portals(neighbor, PortalDirection::NORTH_SOUTH, portal_id); + for (auto &portal : portals) { + sector->add_portal(portal); + neighbor->add_portal(portal); + } + portal_id += portals.size(); + } + } + } + + // Connect mutually reachable exits of sectors. + for (auto §or : this->sectors) { + sector->connect_exits(); + } +} + +template +const PortalNode::nodemap_t &Grid::get_portal_map() { + return portal_nodes; +} + + +template +void Grid::init_portal_nodes() { + // create portal_nodes + for (auto §or : this->sectors) { + for (auto &portal : sector->get_portals()) { + if (!this->portal_nodes.contains(portal->get_id())) { + auto portal_node = std::make_shared(portal); + portal_node->node_sector_0 = sector->get_id(); + portal_node->node_sector_1 = portal_node->portal->get_exit_sector(sector->get_id()); + this->portal_nodes[portal->get_id()] = portal_node; + } + } + } + + // init portal_node exits + for (auto &[id, node] : this->portal_nodes) { + node->init_exits(this->portal_nodes); + } +} + } // namespace openage::path diff --git a/libopenage/pathfinding/integration_field.cpp b/libopenage/pathfinding/integration_field.cpp index 26134506b8..400cc5b89f 100644 --- a/libopenage/pathfinding/integration_field.cpp +++ b/libopenage/pathfinding/integration_field.cpp @@ -2,718 +2,6 @@ #include "integration_field.h" -#include - -#include "error/error.h" -#include "log/log.h" - -#include "coord/tile.h" -#include "pathfinding/cost_field.h" -#include "pathfinding/definitions.h" -#include "pathfinding/portal.h" - - namespace openage::path { -IntegrationField::IntegrationField(size_t size) : - size{size}, - cells(this->size * this->size, INTEGRATE_INIT) { - log::log(DBG << "Created integration field with size " << this->size << "x" << this->size); -} - -size_t IntegrationField::get_size() const { - return this->size; -} - -const integrated_t &IntegrationField::get_cell(const coord::tile_delta &pos) const { - return this->cells.at(pos.ne + pos.se * this->size); -} - -const integrated_t &IntegrationField::get_cell(size_t x, size_t y) const { - return this->cells.at(x + y * this->size); -} - -const integrated_t &IntegrationField::get_cell(size_t idx) const { - return this->cells.at(idx); -} - -std::vector IntegrationField::integrate_los(const std::shared_ptr &cost_field, - const coord::tile_delta &target) { - ENSURE(cost_field->get_size() == this->get_size(), - "cost field size " - << cost_field->get_size() << "x" << cost_field->get_size() - << " does not match integration field size " - << this->get_size() << "x" << this->get_size()); - - ENSURE(target.ne >= 0 - and target.se >= 0 - and target.ne < static_cast(this->size) - and target.se < static_cast(this->size), - "target cell (" << target.ne << ", " << target.se << ") " - << "is out of bounds for integration field of size " - << this->size << "x" << this->size); - - std::vector start_cells; - integrated_cost_t start_cost = INTEGRATED_COST_START; - - // Target cell index - auto target_idx = target.ne + target.se * this->size; - - this->cells[target_idx].cost = start_cost; - this->cells[target_idx].flags |= INTEGRATE_TARGET_MASK; - - if (cost_field->get_cost(target_idx) > COST_MIN) { - // Do a preliminary LOS integration wave for targets that have cost > min cost - // This avoids the bresenham's line algorithm calculations - // (which wouldn't return accurate results for blocker == target) - // and makes sure that sorrounding cells that are min cost are considered - // in line-of-sight. - - this->cells[target_idx].flags |= INTEGRATE_FOUND_MASK; - this->cells[target_idx].flags |= INTEGRATE_LOS_MASK; - - // Add neighbors to current wave - if (target.se > 0) { - start_cells.push_back(target_idx - this->size); - } - if (target.ne > 0) { - start_cells.push_back(target_idx - 1); - } - if (target.se < static_cast(this->size - 1)) { - start_cells.push_back(target_idx + this->size); - } - if (target.ne < static_cast(this->size - 1)) { - start_cells.push_back(target_idx + 1); - } - - // Increment wave cost as we technically handled the first wave in this block - start_cost += 1; - } - else { - // Move outwards from the target cell, updating the integration field - start_cells.push_back(target_idx); - } - - return this->integrate_los(cost_field, target, start_cost, std::move(start_cells)); -} - -std::vector IntegrationField::integrate_los(const std::shared_ptr &cost_field, - const std::shared_ptr &other, - sector_id_t other_sector_id, - const std::shared_ptr &portal, - const coord::tile_delta &target) { - ENSURE(cost_field->get_size() == this->get_size(), - "cost field size " - << cost_field->get_size() << "x" << cost_field->get_size() - << " does not match integration field size " - << this->get_size() << "x" << this->get_size()); - - std::vector wavefront_blocked_portal; - - std::vector start_cells; - - auto exit_start = portal->get_exit_start(other_sector_id); - auto exit_end = portal->get_exit_end(other_sector_id); - auto entry_start = portal->get_entry_start(other_sector_id); - - auto x_diff = exit_start.ne - entry_start.ne; - auto y_diff = exit_start.se - entry_start.se; - - auto &cost_cells = cost_field->get_costs(); - auto &other_cells = other->get_cells(); - - // transfer masks for flags from the other side of the portal - // only LOS and wavefront blocked flags are relevant - integrated_flags_t transfer_mask = INTEGRATE_LOS_MASK | INTEGRATE_WAVEFRONT_BLOCKED_MASK; - - // every portal cell is a target cell - for (auto y = exit_start.se; y <= exit_end.se; ++y) { - for (auto x = exit_start.ne; x <= exit_end.ne; ++x) { - // cell index on the exit side of the portal - auto target_idx = x + y * this->size; - - // cell index on the entry side of the portal - auto entry_idx = x - x_diff + (y - y_diff) * this->size; - - // Set the cost of all target cells to the start value - this->cells[target_idx].cost = INTEGRATED_COST_START; - this->cells[target_idx].flags = other_cells[entry_idx].flags & transfer_mask; - - this->cells[target_idx].flags |= INTEGRATE_TARGET_MASK; - - if (not(this->cells[target_idx].flags & transfer_mask)) { - // If neither LOS nor wavefront blocked flags are set for the portal entry, - // the portal exit cell doesn't affect the LOS and we can skip further checks - continue; - } - - // Get the cost of the current cell - auto cell_cost = cost_cells[target_idx]; - - if (cell_cost > COST_MIN or this->cells[target_idx].flags & INTEGRATE_WAVEFRONT_BLOCKED_MASK) { - // cell blocks line of sight - - // set the blocked flag for the cell if it wasn't set already - this->cells[target_idx].flags |= INTEGRATE_WAVEFRONT_BLOCKED_MASK; - wavefront_blocked_portal.push_back(target_idx); - - // set the found flag for the cell, so that the start costs - // are not changed in the main LOS integration - this->cells[target_idx].flags |= INTEGRATE_FOUND_MASK; - - // check each neighbor for a corner - auto corners = this->get_los_corners(cost_field, target, coord::tile_delta(x, y)); - for (auto &corner : corners) { - // draw a line from the corner to the edge of the field - // to get the cells blocked by the corner - auto blocked_cells = this->bresenhams_line(target, corner.first, corner.second); - for (auto blocked_idx : blocked_cells) { - if (cost_cells[blocked_idx] > COST_MIN) { - // stop if blocked_idx is not min cost - // because this idx may create a new corner - break; - } - // set the blocked flag for the cell - this->cells[blocked_idx].flags |= INTEGRATE_WAVEFRONT_BLOCKED_MASK; - - // clear los flag if it was set - this->cells[blocked_idx].flags &= ~INTEGRATE_LOS_MASK; - - wavefront_blocked_portal.push_back(blocked_idx); - } - } - continue; - } - - // the cell has the LOS flag and is added to the start cells - start_cells.push_back(target_idx); - } - } - - if (start_cells.empty()) { - // Main LOS integration will not enter its loop - // so we can take a shortcut and just return the - // wavefront blocked cells we already found - return wavefront_blocked_portal; - } - - // Call main LOS integration function - auto wavefront_blocked_main = this->integrate_los(cost_field, - target, - INTEGRATED_COST_START, - std::move(start_cells)); - wavefront_blocked_main.insert(wavefront_blocked_main.end(), - wavefront_blocked_portal.begin(), - wavefront_blocked_portal.end()); - return wavefront_blocked_main; -} - -std::vector IntegrationField::integrate_los(const std::shared_ptr &cost_field, - const coord::tile_delta &target, - integrated_cost_t start_cost, - std::vector &&start_wave) { - ENSURE(cost_field->get_size() == this->get_size(), - "cost field size " - << cost_field->get_size() << "x" << cost_field->get_size() - << " does not match integration field size " - << this->get_size() << "x" << this->get_size()); - - // Store the wavefront_blocked cells - std::vector wavefront_blocked; - - // Cells that still have to be visited by the current wave - std::vector current_wave = std::move(start_wave); - - // Cells that have to be visited in the next wave - std::vector next_wave; - - // Preallocate ~30% of the field size for the wavefront - // This reduces the number of reallocations on push_back operations - // TODO: Find "optimal" value for reserve - current_wave.reserve(this->size * 3); - next_wave.reserve(this->size * 3); - - // Cost of the current wave - integrated_cost_t wave_cost = start_cost; - - // Get the cost field values - auto &cost_cells = cost_field->get_costs(); - auto &integrate_cells = this->cells; - - do { - for (size_t i = 0; i < current_wave.size(); ++i) { - // inner loop: handle a wave - auto idx = current_wave[i]; - auto &cell = integrate_cells[idx]; - - if (cell.flags & INTEGRATE_FOUND_MASK) { - // Skip cells that are already in the line of sight - continue; - } - else if (cell.flags & INTEGRATE_WAVEFRONT_BLOCKED_MASK) { - // Stop at cells that are blocked by a LOS corner - cell.cost = wave_cost - 1 + cost_cells[idx]; - cell.flags |= INTEGRATE_FOUND_MASK; - continue; - } - - // Add the current cell to the found cells - cell.flags |= INTEGRATE_FOUND_MASK; - - // Get the x and y coordinates of the current cell - auto x = idx % this->size; - auto y = idx / this->size; - - // Get the cost of the current cell - auto cell_cost = cost_cells[idx]; - - if (cell_cost > COST_MIN) { - // cell blocks line of sight - // and we have to check for corners - if (cell_cost != COST_IMPASSABLE) { - // Add the current cell to the blocked wavefront if it's not a wall - wavefront_blocked.push_back(idx); - cell.cost = wave_cost - 1 + cell_cost; - cell.flags |= INTEGRATE_WAVEFRONT_BLOCKED_MASK; - } - - // check each neighbor for a corner - auto corners = this->get_los_corners(cost_field, target, coord::tile_delta(x, y)); - for (auto &corner : corners) { - // draw a line from the corner to the edge of the field - // to get the cells blocked by the corner - auto blocked_cells = this->bresenhams_line(target, corner.first, corner.second); - for (auto blocked_idx : blocked_cells) { - if (cost_cells[blocked_idx] > COST_MIN) { - // stop if blocked_idx is impassable - break; - } - // set the blocked flag for the cell - integrate_cells[blocked_idx].flags |= INTEGRATE_WAVEFRONT_BLOCKED_MASK; - - // clear los flag if it was set - integrate_cells[blocked_idx].flags &= ~INTEGRATE_LOS_MASK; - - wavefront_blocked.push_back(blocked_idx); - } - } - continue; - } - - // The cell is in the line of sight at min cost - // Set the LOS flag and cost - cell.cost = wave_cost; - cell.flags |= INTEGRATE_LOS_MASK; - - // Search the neighbors of the current cell - if (y > 0) { - auto neighbor_idx = idx - this->size; - next_wave.push_back(neighbor_idx); - } - if (x > 0) { - auto neighbor_idx = idx - 1; - next_wave.push_back(neighbor_idx); - } - if (y < this->size - 1) { - auto neighbor_idx = idx + this->size; - next_wave.push_back(neighbor_idx); - } - if (x < this->size - 1) { - auto neighbor_idx = idx + 1; - next_wave.push_back(neighbor_idx); - } - } - - // increment the cost and advance the wavefront outwards - wave_cost += 1; - current_wave.swap(next_wave); - next_wave.clear(); - } - while (not current_wave.empty()); - - return wavefront_blocked; -} - -void IntegrationField::integrate_cost(const std::shared_ptr &cost_field, - const coord::tile_delta &target) { - ENSURE(cost_field->get_size() == this->get_size(), - "cost field size " - << cost_field->get_size() << "x" << cost_field->get_size() - << " does not match integration field size " - << this->get_size() << "x" << this->get_size()); - - // Target cell index - auto target_idx = target.ne + target.se * this->size; - - // Move outwards from the target cell, updating the integration field - this->cells[target_idx].cost = INTEGRATED_COST_START; - this->cells[target_idx].flags |= INTEGRATE_TARGET_MASK; - this->integrate_cost(cost_field, {target_idx}); -} - -void IntegrationField::integrate_cost(const std::shared_ptr &cost_field, - sector_id_t other_sector_id, - const std::shared_ptr &portal) { - ENSURE(cost_field->get_size() == this->get_size(), - "cost field size " - << cost_field->get_size() << "x" << cost_field->get_size() - << " does not match integration field size " - << this->get_size() << "x" << this->get_size()); - - // Integrate the cost of the cells on the exit side (this) of the portal - std::vector start_cells; - auto exit_start = portal->get_exit_start(other_sector_id); - auto exit_end = portal->get_exit_end(other_sector_id); - for (auto y = exit_start.se; y <= exit_end.se; ++y) { - for (auto x = exit_start.ne; x <= exit_end.ne; ++x) { - // every portal cell is a target cell - auto target_idx = x + y * this->size; - - // Set the cost of all target cells to the start value - this->cells[target_idx].cost = INTEGRATED_COST_START; - this->cells[target_idx].flags |= INTEGRATE_TARGET_MASK; - start_cells.push_back(target_idx); - - // TODO: Transfer flags and cost from the other integration field - } - } - - // Integrate the rest of the cost field - this->integrate_cost(cost_field, std::move(start_cells)); -} - -void IntegrationField::integrate_cost(const std::shared_ptr &cost_field, - std::vector &&start_cells) { - // Cells that still have to be visited by the current wave - std::vector current_wave = std::move(start_cells); - - // Cells that have to be visited in the next wave - std::vector next_wave; - - // Preallocate ~30% of the field size for the wavefront - // This reduces the number of reallocations on push_back operations - // TODO: Find "optimal" value for reserve - current_wave.reserve(this->size * 3); - next_wave.reserve(this->size * 3); - - // Get the cost field values - auto &cost_cells = cost_field->get_costs(); - - // Move outwards from the wavefront, updating the integration field - while (not current_wave.empty()) { - for (size_t i = 0; i < current_wave.size(); ++i) { - auto idx = current_wave[i]; - - // Get the x and y coordinates of the current cell - auto x = idx % this->size; - auto y = idx / this->size; - - auto integrated_current = this->cells[idx].cost; - - // Get the neighbors of the current cell - if (y > 0) { - auto neighbor_idx = idx - this->size; - this->update_neighbor(neighbor_idx, - cost_cells[neighbor_idx], - integrated_current, - next_wave); - } - if (x > 0) { - auto neighbor_idx = idx - 1; - this->update_neighbor(neighbor_idx, - cost_cells[neighbor_idx], - integrated_current, - next_wave); - } - if (y < this->size - 1) { - auto neighbor_idx = idx + this->size; - this->update_neighbor(neighbor_idx, - cost_cells[neighbor_idx], - integrated_current, - next_wave); - } - if (x < this->size - 1) { - auto neighbor_idx = idx + 1; - this->update_neighbor(neighbor_idx, - cost_cells[neighbor_idx], - integrated_current, - next_wave); - } - } - - current_wave.swap(next_wave); - next_wave.clear(); - } -} - -const std::vector &IntegrationField::get_cells() const { - return this->cells; -} - -void IntegrationField::reset() { - std::fill(this->cells.begin(), this->cells.end(), INTEGRATE_INIT); - - log::log(DBG << "Integration field has been reset"); -} - -void IntegrationField::reset_dynamic_flags() { - integrated_flags_t mask = 0xFF & ~(INTEGRATE_LOS_MASK | INTEGRATE_WAVEFRONT_BLOCKED_MASK | INTEGRATE_FOUND_MASK); - for (integrated_t &cell : this->cells) { - cell.flags = cell.flags & mask; - } - - log::log(DBG << "Integration field dynamic flags have been reset"); -} - -void IntegrationField::update_neighbor(size_t idx, - cost_t cell_cost, - integrated_cost_t integrated_cost, - std::vector &wave) { - ENSURE(cell_cost > COST_INIT, "cost field cell value must be non-zero"); - - // Check if the cell is impassable - // then we don't need to update the integration field - if (cell_cost == COST_IMPASSABLE) { - return; - } - - auto cost = integrated_cost + cell_cost; - if (cost < this->cells[idx].cost) { - // If the new integration value is smaller than the current one, - // update the cell and add it to the open list - this->cells[idx].cost = cost; - - wave.push_back(idx); - } -} - -std::vector> IntegrationField::get_los_corners(const std::shared_ptr &cost_field, - const coord::tile_delta &target, - const coord::tile_delta &blocker) { - std::vector> corners; - - // Get the cost of the blocking cell's neighbors - - // Set all costs to IMPASSABLE at the beginning - auto top_cost = COST_IMPASSABLE; - auto left_cost = COST_IMPASSABLE; - auto bottom_cost = COST_IMPASSABLE; - auto right_cost = COST_IMPASSABLE; - - std::pair top_left{blocker.ne, blocker.se}; - std::pair top_right{blocker.ne + 1, blocker.se}; - std::pair bottom_left{blocker.ne, blocker.se + 1}; - std::pair bottom_right{blocker.ne + 1, blocker.se + 1}; - - // Get neighbor costs (if they exist) - if (blocker.se > 0) { - top_cost = cost_field->get_cost(blocker.ne, blocker.se - 1); - } - if (blocker.ne > 0) { - left_cost = cost_field->get_cost(blocker.ne - 1, blocker.se); - } - if (static_cast(blocker.se) < this->size - 1) { - bottom_cost = cost_field->get_cost(blocker.ne, blocker.se + 1); - } - if (static_cast(blocker.ne) < this->size - 1) { - right_cost = cost_field->get_cost(blocker.ne + 1, blocker.se); - } - - // Check which corners are blocking LOS - // TODO: Currently super complicated and could likely be optimized - if (blocker.ne == target.ne) { - // blocking cell is parallel to target on y-axis - if (blocker.se < target.se) { - if (left_cost == COST_MIN) { - // top - corners.push_back(bottom_left); - } - if (right_cost == COST_MIN) { - // top - corners.push_back(bottom_right); - } - } - else { - if (left_cost == COST_MIN) { - // bottom - corners.push_back(top_left); - } - if (right_cost == COST_MIN) { - // bottom - corners.push_back(top_right); - } - } - } - else if (blocker.se == target.se) { - // blocking cell is parallel to target on x-axis - if (blocker.ne < target.ne) { - if (top_cost == COST_MIN) { - // right - corners.push_back(top_right); - } - if (bottom_cost == COST_MIN) { - // right - corners.push_back(bottom_right); - } - } - else { - if (top_cost == COST_MIN) { - // left - corners.push_back(top_left); - } - if (bottom_cost == COST_MIN) { - // left - corners.push_back(bottom_left); - } - } - } - else { - // blocking cell is diagonal to target on - if (blocker.ne < target.ne) { - if (blocker.se < target.se) { - // top and right - if (top_cost == COST_MIN and right_cost == COST_MIN) { - // right - corners.push_back(top_right); - } - if (left_cost == COST_MIN and bottom_cost == COST_MIN) { - // bottom - corners.push_back(bottom_left); - } - } - else { - // bottom and right - if (bottom_cost == COST_MIN and right_cost == COST_MIN) { - // right - corners.push_back(bottom_right); - } - if (left_cost == COST_MIN and top_cost == COST_MIN) { - // top - corners.push_back(top_left); - } - } - } - else { - if (blocker.se < target.se) { - // top and left - if (top_cost == COST_MIN and left_cost == COST_MIN) { - // left - corners.push_back(top_left); - } - if (right_cost == COST_MIN and bottom_cost == COST_MIN) { - // bottom - corners.push_back(bottom_right); - } - } - else { - // bottom and left - if (bottom_cost == COST_MIN and left_cost == COST_MIN) { - // left - corners.push_back(bottom_left); - } - if (right_cost == COST_MIN and top_cost == COST_MIN) { - // top - corners.push_back(top_right); - } - } - } - } - - return corners; -} - -std::vector IntegrationField::bresenhams_line(const coord::tile_delta &target, - int corner_x, - int corner_y) { - std::vector cells; - - // cell coordinates - // these have to be offset depending on the line direction - auto cell_x = corner_x; - auto cell_y = corner_y; - - // field edge boundary - int boundary = this->size; - - // target coordinates - // offset by 0.5 to get the center of the cell - double tx = target.ne + 0.5; - double ty = target.se + 0.5; - - // slope of the line - double dx = std::abs(tx - corner_x); - double dy = std::abs(ty - corner_y); - auto m = dy / dx; - - // error margin for the line - // if the error is greater than 1.0, we have to move in the y direction - auto error = m; - - // Check which direction the line is going - if (corner_x < tx) { - if (corner_y < ty) { // left and up - cell_y -= 1; - cell_x -= 1; - while (cell_x >= 0 and cell_y >= 0) { - cells.push_back(cell_x + cell_y * this->size); - if (error > 1.0) { - cell_y -= 1; - error -= 1.0; - } - else { - cell_x -= 1; - error += m; - } - } - } - - else { // left and down - cell_x -= 1; - while (cell_x >= 0 and cell_y < boundary) { - cells.push_back(cell_x + cell_y * this->size); - if (error > 1.0) { - cell_y += 1; - error -= 1.0; - } - else { - cell_x -= 1; - error += m; - } - } - } - } - else { - if (corner_y < ty) { // right and up - cell_y -= 1; - while (cell_x < boundary and cell_y >= 0) { - cells.push_back(cell_x + cell_y * this->size); - if (error > 1.0) { - cell_y -= 1; - error -= 1.0; - } - else { - cell_x += 1; - error += m; - } - } - } - else { // right and down - while (cell_x < boundary and cell_y < boundary) { - cells.push_back(cell_x + cell_y * this->size); - if (error > 1.0) { - cell_y += 1; - error -= 1.0; - } - else { - cell_x += 1; - error += m; - } - } - } - } - - return cells; -} - - } // namespace openage::path diff --git a/libopenage/pathfinding/integration_field.h b/libopenage/pathfinding/integration_field.h index c9149cc82f..8db54c23a9 100644 --- a/libopenage/pathfinding/integration_field.h +++ b/libopenage/pathfinding/integration_field.h @@ -2,14 +2,24 @@ #pragma once +#include +#include #include #include #include #include #include -#include "pathfinding/types.h" +#include "coord/tile.h" + +#include "error/error.h" +#include "log/log.h" + +#include "pathfinding/cost_field.h" +#include "pathfinding/definitions.h" +#include "pathfinding/portal.h" +#include "pathfinding/types.h" namespace openage { namespace coord { @@ -17,20 +27,22 @@ struct tile_delta; } // namespace coord namespace path { + +template class CostField; + class Portal; /** * Integration field in the flow-field pathfinding algorithm. */ +template class IntegrationField { public: /** - * Create a square integration field with a specified size. - * - * @param size Side length of the field. + * Create a square integration field. */ - IntegrationField(size_t size); + IntegrationField(); /** * Get the size of the integration field. @@ -77,7 +89,7 @@ class IntegrationField { * * @return Cells flagged as "wavefront blocked". */ - std::vector integrate_los(const std::shared_ptr &cost_field, + std::vector integrate_los(const std::shared_ptr> &cost_field, const coord::tile_delta &target); /** @@ -95,8 +107,8 @@ class IntegrationField { * * @return Cells flagged as "wavefront blocked". */ - std::vector integrate_los(const std::shared_ptr &cost_field, - const std::shared_ptr &other, + std::vector integrate_los(const std::shared_ptr> &cost_field, + const std::shared_ptr> &other, sector_id_t other_sector_id, const std::shared_ptr &portal, const coord::tile_delta &target); @@ -115,7 +127,7 @@ class IntegrationField { * * @return Cells flagged as "wavefront blocked". */ - std::vector integrate_los(const std::shared_ptr &cost_field, + std::vector integrate_los(const std::shared_ptr> &cost_field, const coord::tile_delta &target, integrated_cost_t start_cost, std::vector &&start_wave); @@ -126,7 +138,7 @@ class IntegrationField { * @param cost_field Cost field to integrate. * @param target Coordinates of the target cell. */ - void integrate_cost(const std::shared_ptr &cost_field, + void integrate_cost(const std::shared_ptr> &cost_field, const coord::tile_delta &target); /** @@ -137,7 +149,7 @@ class IntegrationField { * @param other_sector_id Sector ID of the other integration field. * @param portal Portal connecting the two fields. */ - void integrate_cost(const std::shared_ptr &cost_field, + void integrate_cost(const std::shared_ptr> &cost_field, sector_id_t other_sector_id, const std::shared_ptr &portal); @@ -147,7 +159,7 @@ class IntegrationField { * @param cost_field Cost field to integrate. * @param start_cells Cells flagged as "wavefront blocked" from a LOS pass. */ - void integrate_cost(const std::shared_ptr &cost_field, + void integrate_cost(const std::shared_ptr> &cost_field, std::vector &&start_cells); /** @@ -155,7 +167,7 @@ class IntegrationField { * * @return Integration field values. */ - const std::vector &get_cells() const; + const std::array &get_cells() const; /** * Reset the integration field for a new integration. @@ -199,7 +211,7 @@ class IntegrationField { * * @return Field coordinates of the LOS corners. */ - std::vector> get_los_corners(const std::shared_ptr &cost_field, + std::vector> get_los_corners(const std::shared_ptr> &cost_field, const coord::tile_delta &target, const coord::tile_delta &blocker); @@ -221,16 +233,725 @@ class IntegrationField { int corner_x, int corner_y); - /** - * Side length of the field. - */ - size_t size; - /** * Integration field values. */ - std::vector cells; + std::array cells; }; +template +IntegrationField::IntegrationField() { + cells.fill(INTEGRATE_INIT); + log::log(DBG << "Created integration field with size " << N << "x" << N); +} + +template +size_t IntegrationField::get_size() const { + return N; +} + +template +const integrated_t &IntegrationField::get_cell(const coord::tile_delta &pos) const { + return this->cells.at(pos.ne + pos.se * N); +} + +template +const integrated_t &IntegrationField::get_cell(size_t x, size_t y) const { + return this->cells.at(x + y * N); +} + +template +const integrated_t &IntegrationField::get_cell(size_t idx) const { + return this->cells.at(idx); +} + +template +std::vector IntegrationField::integrate_los(const std::shared_ptr> &cost_field, + const coord::tile_delta &target) { + ENSURE(cost_field->get_size() == this->get_size(), + "cost field size " + << cost_field->get_size() << "x" << cost_field->get_size() + << " does not match integration field size " + << this->get_size() << "x" << this->get_size()); + + ENSURE(target.ne >= 0 + and target.se >= 0 + and target.ne < static_cast(N) + and target.se < static_cast(N), + "target cell (" << target.ne << ", " << target.se << ") " + << "is out of bounds for integration field of size " + << N << "x" << N); + + std::vector start_cells; + integrated_cost_t start_cost = INTEGRATED_COST_START; + + // Target cell index + auto target_idx = target.ne + target.se * N; + + this->cells[target_idx].cost = start_cost; + this->cells[target_idx].flags |= INTEGRATE_TARGET_MASK; + + if (cost_field->get_cost(target_idx) > COST_MIN) { + // Do a preliminary LOS integration wave for targets that have cost > min cost + // This avoids the bresenham's line algorithm calculations + // (which wouldn't return accurate results for blocker == target) + // and makes sure that sorrounding cells that are min cost are considered + // in line-of-sight. + + this->cells[target_idx].flags |= INTEGRATE_FOUND_MASK; + this->cells[target_idx].flags |= INTEGRATE_LOS_MASK; + + // Add neighbors to current wave + if (target.se > 0) { + start_cells.push_back(target_idx - N); + } + if (target.ne > 0) { + start_cells.push_back(target_idx - 1); + } + if (target.se < static_cast(N - 1)) { + start_cells.push_back(target_idx + N); + } + if (target.ne < static_cast(N - 1)) { + start_cells.push_back(target_idx + 1); + } + + // Increment wave cost as we technically handled the first wave in this block + start_cost += 1; + } + else { + // Move outwards from the target cell, updating the integration field + start_cells.push_back(target_idx); + } + + return this->integrate_los(cost_field, target, start_cost, std::move(start_cells)); +} + +template +std::vector IntegrationField::integrate_los(const std::shared_ptr> &cost_field, + const std::shared_ptr> &other, + sector_id_t other_sector_id, + const std::shared_ptr &portal, + const coord::tile_delta &target) { + ENSURE(cost_field->get_size() == this->get_size(), + "cost field size " + << cost_field->get_size() << "x" << cost_field->get_size() + << " does not match integration field size " + << this->get_size() << "x" << this->get_size()); + + std::vector wavefront_blocked_portal; + + std::vector start_cells; + + auto exit_start = portal->get_exit_start(other_sector_id); + auto exit_end = portal->get_exit_end(other_sector_id); + auto entry_start = portal->get_entry_start(other_sector_id); + + auto x_diff = exit_start.ne - entry_start.ne; + auto y_diff = exit_start.se - entry_start.se; + + auto &cost_cells = cost_field->get_costs(); + auto &other_cells = other->get_cells(); + + // transfer masks for flags from the other side of the portal + // only LOS and wavefront blocked flags are relevant + integrated_flags_t transfer_mask = INTEGRATE_LOS_MASK | INTEGRATE_WAVEFRONT_BLOCKED_MASK; + + // every portal cell is a target cell + for (auto y = exit_start.se; y <= exit_end.se; ++y) { + for (auto x = exit_start.ne; x <= exit_end.ne; ++x) { + // cell index on the exit side of the portal + auto target_idx = x + y * N; + + // cell index on the entry side of the portal + auto entry_idx = x - x_diff + (y - y_diff) * N; + + // Set the cost of all target cells to the start value + this->cells[target_idx].cost = INTEGRATED_COST_START; + this->cells[target_idx].flags = other_cells[entry_idx].flags & transfer_mask; + + this->cells[target_idx].flags |= INTEGRATE_TARGET_MASK; + + if (not(this->cells[target_idx].flags & transfer_mask)) { + // If neither LOS nor wavefront blocked flags are set for the portal entry, + // the portal exit cell doesn't affect the LOS and we can skip further checks + continue; + } + + // Get the cost of the current cell + auto cell_cost = cost_cells[target_idx]; + + if (cell_cost > COST_MIN or this->cells[target_idx].flags & INTEGRATE_WAVEFRONT_BLOCKED_MASK) { + // cell blocks line of sight + + // set the blocked flag for the cell if it wasn't set already + this->cells[target_idx].flags |= INTEGRATE_WAVEFRONT_BLOCKED_MASK; + wavefront_blocked_portal.push_back(target_idx); + + // set the found flag for the cell, so that the start costs + // are not changed in the main LOS integration + this->cells[target_idx].flags |= INTEGRATE_FOUND_MASK; + + // check each neighbor for a corner + auto corners = this->get_los_corners(cost_field, target, coord::tile_delta(x, y)); + for (auto &corner : corners) { + // draw a line from the corner to the edge of the field + // to get the cells blocked by the corner + auto blocked_cells = this->bresenhams_line(target, corner.first, corner.second); + for (auto blocked_idx : blocked_cells) { + if (cost_cells[blocked_idx] > COST_MIN) { + // stop if blocked_idx is not min cost + // because this idx may create a new corner + break; + } + // set the blocked flag for the cell + this->cells[blocked_idx].flags |= INTEGRATE_WAVEFRONT_BLOCKED_MASK; + + // clear los flag if it was set + this->cells[blocked_idx].flags &= ~INTEGRATE_LOS_MASK; + + wavefront_blocked_portal.push_back(blocked_idx); + } + } + continue; + } + + // the cell has the LOS flag and is added to the start cells + start_cells.push_back(target_idx); + } + } + + if (start_cells.empty()) { + // Main LOS integration will not enter its loop + // so we can take a shortcut and just return the + // wavefront blocked cells we already found + return wavefront_blocked_portal; + } + + // Call main LOS integration function + auto wavefront_blocked_main = this->integrate_los(cost_field, + target, + INTEGRATED_COST_START, + std::move(start_cells)); + wavefront_blocked_main.insert(wavefront_blocked_main.end(), + wavefront_blocked_portal.begin(), + wavefront_blocked_portal.end()); + return wavefront_blocked_main; +} + +template +std::vector IntegrationField::integrate_los(const std::shared_ptr> &cost_field, + const coord::tile_delta &target, + integrated_cost_t start_cost, + std::vector &&start_wave) { + ENSURE(cost_field->get_size() == this->get_size(), + "cost field size " + << cost_field->get_size() << "x" << cost_field->get_size() + << " does not match integration field size " + << this->get_size() << "x" << this->get_size()); + + // Store the wavefront_blocked cells + std::vector wavefront_blocked; + + // Cells that still have to be visited by the current wave + std::vector current_wave = std::move(start_wave); + + // Cells that have to be visited in the next wave + std::vector next_wave; + + // Preallocate ~30% of the field size for the wavefront + // This reduces the number of reallocations on push_back operations + // TODO: Find "optimal" value for reserve + current_wave.reserve(N * 3); + next_wave.reserve(N * 3); + + // Cost of the current wave + integrated_cost_t wave_cost = start_cost; + + // Get the cost field values + auto &cost_cells = cost_field->get_costs(); + auto &integrate_cells = this->cells; + + do { + for (size_t i = 0; i < current_wave.size(); ++i) { + // inner loop: handle a wave + auto idx = current_wave[i]; + auto &cell = integrate_cells[idx]; + + if (cell.flags & INTEGRATE_FOUND_MASK) { + // Skip cells that are already in the line of sight + continue; + } + else if (cell.flags & INTEGRATE_WAVEFRONT_BLOCKED_MASK) { + // Stop at cells that are blocked by a LOS corner + cell.cost = wave_cost - 1 + cost_cells[idx]; + cell.flags |= INTEGRATE_FOUND_MASK; + continue; + } + + // Add the current cell to the found cells + cell.flags |= INTEGRATE_FOUND_MASK; + + // Get the x and y coordinates of the current cell + auto x = idx % N; + auto y = idx / N; + + // Get the cost of the current cell + auto cell_cost = cost_cells[idx]; + + if (cell_cost > COST_MIN) { + // cell blocks line of sight + // and we have to check for corners + if (cell_cost != COST_IMPASSABLE) { + // Add the current cell to the blocked wavefront if it's not a wall + wavefront_blocked.push_back(idx); + cell.cost = wave_cost - 1 + cell_cost; + cell.flags |= INTEGRATE_WAVEFRONT_BLOCKED_MASK; + } + + // check each neighbor for a corner + auto corners = this->get_los_corners(cost_field, target, coord::tile_delta(x, y)); + for (auto &corner : corners) { + // draw a line from the corner to the edge of the field + // to get the cells blocked by the corner + auto blocked_cells = this->bresenhams_line(target, corner.first, corner.second); + for (auto blocked_idx : blocked_cells) { + if (cost_cells[blocked_idx] > COST_MIN) { + // stop if blocked_idx is impassable + break; + } + // set the blocked flag for the cell + integrate_cells[blocked_idx].flags |= INTEGRATE_WAVEFRONT_BLOCKED_MASK; + + // clear los flag if it was set + integrate_cells[blocked_idx].flags &= ~INTEGRATE_LOS_MASK; + + wavefront_blocked.push_back(blocked_idx); + } + } + continue; + } + + // The cell is in the line of sight at min cost + // Set the LOS flag and cost + cell.cost = wave_cost; + cell.flags |= INTEGRATE_LOS_MASK; + + // Search the neighbors of the current cell + if (y > 0) { + auto neighbor_idx = idx - N; + next_wave.push_back(neighbor_idx); + } + if (x > 0) { + auto neighbor_idx = idx - 1; + next_wave.push_back(neighbor_idx); + } + if (y < N - 1) { + auto neighbor_idx = idx + N; + next_wave.push_back(neighbor_idx); + } + if (x < N - 1) { + auto neighbor_idx = idx + 1; + next_wave.push_back(neighbor_idx); + } + } + + // increment the cost and advance the wavefront outwards + wave_cost += 1; + current_wave.swap(next_wave); + next_wave.clear(); + } + while (not current_wave.empty()); + + return wavefront_blocked; +} +template +void IntegrationField::integrate_cost(const std::shared_ptr> &cost_field, + const coord::tile_delta &target) { + ENSURE(cost_field->get_size() == this->get_size(), + "cost field size " + << cost_field->get_size() << "x" << cost_field->get_size() + << " does not match integration field size " + << this->get_size() << "x" << this->get_size()); + + // Target cell index + auto target_idx = target.ne + target.se * N; + + // Move outwards from the target cell, updating the integration field + this->cells[target_idx].cost = INTEGRATED_COST_START; + this->cells[target_idx].flags |= INTEGRATE_TARGET_MASK; + this->integrate_cost(cost_field, {target_idx}); +} + +template +void IntegrationField::integrate_cost(const std::shared_ptr> &cost_field, + sector_id_t other_sector_id, + const std::shared_ptr &portal) { + ENSURE(cost_field->get_size() == this->get_size(), + "cost field size " + << cost_field->get_size() << "x" << cost_field->get_size() + << " does not match integration field size " + << this->get_size() << "x" << this->get_size()); + + // Integrate the cost of the cells on the exit side (this) of the portal + std::vector start_cells; + auto exit_start = portal->get_exit_start(other_sector_id); + auto exit_end = portal->get_exit_end(other_sector_id); + for (auto y = exit_start.se; y <= exit_end.se; ++y) { + for (auto x = exit_start.ne; x <= exit_end.ne; ++x) { + // every portal cell is a target cell + auto target_idx = x + y * N; + + // Set the cost of all target cells to the start value + this->cells[target_idx].cost = INTEGRATED_COST_START; + this->cells[target_idx].flags |= INTEGRATE_TARGET_MASK; + start_cells.push_back(target_idx); + + // TODO: Transfer flags and cost from the other integration field + } + } + + // Integrate the rest of the cost field + this->integrate_cost(cost_field, std::move(start_cells)); +} + +template +void IntegrationField::integrate_cost(const std::shared_ptr> &cost_field, + std::vector &&start_cells) { + // Cells that still have to be visited by the current wave + std::vector current_wave = std::move(start_cells); + + // Cells that have to be visited in the next wave + std::vector next_wave; + + // Preallocate ~30% of the field size for the wavefront + // This reduces the number of reallocations on push_back operations + // TODO: Find "optimal" value for reserve + current_wave.reserve(N * 3); + next_wave.reserve(N * 3); + + // Get the cost field values + auto &cost_cells = cost_field->get_costs(); + + // Move outwards from the wavefront, updating the integration field + while (not current_wave.empty()) { + for (size_t i = 0; i < current_wave.size(); ++i) { + auto idx = current_wave[i]; + + // Get the x and y coordinates of the current cell + auto x = idx % N; + auto y = idx / N; + + auto integrated_current = this->cells[idx].cost; + + // Get the neighbors of the current cell + if (y > 0) { + auto neighbor_idx = idx - N; + this->update_neighbor(neighbor_idx, + cost_cells[neighbor_idx], + integrated_current, + next_wave); + } + if (x > 0) { + auto neighbor_idx = idx - 1; + this->update_neighbor(neighbor_idx, + cost_cells[neighbor_idx], + integrated_current, + next_wave); + } + if (y < N - 1) { + auto neighbor_idx = idx + N; + this->update_neighbor(neighbor_idx, + cost_cells[neighbor_idx], + integrated_current, + next_wave); + } + if (x < N - 1) { + auto neighbor_idx = idx + 1; + this->update_neighbor(neighbor_idx, + cost_cells[neighbor_idx], + integrated_current, + next_wave); + } + } + + current_wave.swap(next_wave); + next_wave.clear(); + } +} +template +const std::array &IntegrationField::get_cells() const { + return this->cells; +} + +template +void IntegrationField::reset() { + std::fill(this->cells.begin(), this->cells.end(), INTEGRATE_INIT); + + log::log(DBG << "Integration field has been reset"); +} + +template +void IntegrationField::reset_dynamic_flags() { + integrated_flags_t mask = 0xFF & ~(INTEGRATE_LOS_MASK | INTEGRATE_WAVEFRONT_BLOCKED_MASK | INTEGRATE_FOUND_MASK); + for (integrated_t &cell : this->cells) { + cell.flags = cell.flags & mask; + } + + log::log(DBG << "Integration field dynamic flags have been reset"); +} + +template +void IntegrationField::update_neighbor(size_t idx, + cost_t cell_cost, + integrated_cost_t integrated_cost, + std::vector &wave) { + ENSURE(cell_cost > COST_INIT, "cost field cell value must be non-zero"); + + // Check if the cell is impassable + // then we don't need to update the integration field + if (cell_cost == COST_IMPASSABLE) { + return; + } + + auto cost = integrated_cost + cell_cost; + if (cost < this->cells[idx].cost) { + // If the new integration value is smaller than the current one, + // update the cell and add it to the open list + this->cells[idx].cost = cost; + + wave.push_back(idx); + } +} + +template +std::vector> IntegrationField::get_los_corners(const std::shared_ptr> &cost_field, + const coord::tile_delta &target, + const coord::tile_delta &blocker) { + std::vector> corners; + + // Get the cost of the blocking cell's neighbors + + // Set all costs to IMPASSABLE at the beginning + auto top_cost = COST_IMPASSABLE; + auto left_cost = COST_IMPASSABLE; + auto bottom_cost = COST_IMPASSABLE; + auto right_cost = COST_IMPASSABLE; + + std::pair top_left{blocker.ne, blocker.se}; + std::pair top_right{blocker.ne + 1, blocker.se}; + std::pair bottom_left{blocker.ne, blocker.se + 1}; + std::pair bottom_right{blocker.ne + 1, blocker.se + 1}; + + // Get neighbor costs (if they exist) + if (blocker.se > 0) { + top_cost = cost_field->get_cost(blocker.ne, blocker.se - 1); + } + if (blocker.ne > 0) { + left_cost = cost_field->get_cost(blocker.ne - 1, blocker.se); + } + if (static_cast(blocker.se) < N - 1) { + bottom_cost = cost_field->get_cost(blocker.ne, blocker.se + 1); + } + if (static_cast(blocker.ne) < N - 1) { + right_cost = cost_field->get_cost(blocker.ne + 1, blocker.se); + } + + // Check which corners are blocking LOS + // TODO: Currently super complicated and could likely be optimized + if (blocker.ne == target.ne) { + // blocking cell is parallel to target on y-axis + if (blocker.se < target.se) { + if (left_cost == COST_MIN) { + // top + corners.push_back(bottom_left); + } + if (right_cost == COST_MIN) { + // top + corners.push_back(bottom_right); + } + } + else { + if (left_cost == COST_MIN) { + // bottom + corners.push_back(top_left); + } + if (right_cost == COST_MIN) { + // bottom + corners.push_back(top_right); + } + } + } + else if (blocker.se == target.se) { + // blocking cell is parallel to target on x-axis + if (blocker.ne < target.ne) { + if (top_cost == COST_MIN) { + // right + corners.push_back(top_right); + } + if (bottom_cost == COST_MIN) { + // right + corners.push_back(bottom_right); + } + } + else { + if (top_cost == COST_MIN) { + // left + corners.push_back(top_left); + } + if (bottom_cost == COST_MIN) { + // left + corners.push_back(bottom_left); + } + } + } + else { + // blocking cell is diagonal to target on + if (blocker.ne < target.ne) { + if (blocker.se < target.se) { + // top and right + if (top_cost == COST_MIN and right_cost == COST_MIN) { + // right + corners.push_back(top_right); + } + if (left_cost == COST_MIN and bottom_cost == COST_MIN) { + // bottom + corners.push_back(bottom_left); + } + } + else { + // bottom and right + if (bottom_cost == COST_MIN and right_cost == COST_MIN) { + // right + corners.push_back(bottom_right); + } + if (left_cost == COST_MIN and top_cost == COST_MIN) { + // top + corners.push_back(top_left); + } + } + } + else { + if (blocker.se < target.se) { + // top and left + if (top_cost == COST_MIN and left_cost == COST_MIN) { + // left + corners.push_back(top_left); + } + if (right_cost == COST_MIN and bottom_cost == COST_MIN) { + // bottom + corners.push_back(bottom_right); + } + } + else { + // bottom and left + if (bottom_cost == COST_MIN and left_cost == COST_MIN) { + // left + corners.push_back(bottom_left); + } + if (right_cost == COST_MIN and top_cost == COST_MIN) { + // top + corners.push_back(top_right); + } + } + } + } + + return corners; +} + +template +std::vector IntegrationField::bresenhams_line(const coord::tile_delta &target, + int corner_x, + int corner_y) { + std::vector cells; + + // cell coordinates + // these have to be offset depending on the line direction + auto cell_x = corner_x; + auto cell_y = corner_y; + + // field edge boundary + int boundary = N; + + // target coordinates + // offset by 0.5 to get the center of the cell + double tx = target.ne + 0.5; + double ty = target.se + 0.5; + + // slope of the line + double dx = std::abs(tx - corner_x); + double dy = std::abs(ty - corner_y); + auto m = dy / dx; + + // error margin for the line + // if the error is greater than 1.0, we have to move in the y direction + auto error = m; + + // Check which direction the line is going + if (corner_x < tx) { + if (corner_y < ty) { // left and up + cell_y -= 1; + cell_x -= 1; + while (cell_x >= 0 and cell_y >= 0) { + cells.push_back(cell_x + cell_y * N); + if (error > 1.0) { + cell_y -= 1; + error -= 1.0; + } + else { + cell_x -= 1; + error += m; + } + } + } + + else { // left and down + cell_x -= 1; + while (cell_x >= 0 and cell_y < boundary) { + cells.push_back(cell_x + cell_y * N); + if (error > 1.0) { + cell_y += 1; + error -= 1.0; + } + else { + cell_x -= 1; + error += m; + } + } + } + } + else { + if (corner_y < ty) { // right and up + cell_y -= 1; + while (cell_x < boundary and cell_y >= 0) { + cells.push_back(cell_x + cell_y * N); + if (error > 1.0) { + cell_y -= 1; + error -= 1.0; + } + else { + cell_x += 1; + error += m; + } + } + } + else { // right and down + while (cell_x < boundary and cell_y < boundary) { + cells.push_back(cell_x + cell_y * N); + if (error > 1.0) { + cell_y += 1; + error -= 1.0; + } + else { + cell_x += 1; + error += m; + } + } + } + } + + return cells; +} + } // namespace path } // namespace openage diff --git a/libopenage/pathfinding/integrator.cpp b/libopenage/pathfinding/integrator.cpp index f4dcf324b1..e9847ceb9c 100644 --- a/libopenage/pathfinding/integrator.cpp +++ b/libopenage/pathfinding/integrator.cpp @@ -2,220 +2,6 @@ #include "integrator.h" -#include "log/log.h" - -#include "pathfinding/cost_field.h" -#include "pathfinding/field_cache.h" -#include "pathfinding/flow_field.h" -#include "pathfinding/integration_field.h" -#include "pathfinding/portal.h" -#include "time/time.h" - - namespace openage::path { -Integrator::Integrator() : - field_cache{std::make_unique()} { -} - -std::shared_ptr Integrator::integrate(const std::shared_ptr &cost_field, - const coord::tile_delta &target, - bool with_los) { - auto integration_field = std::make_shared(cost_field->get_size()); - - log::log(DBG << "Integrating cost field for target coord " << target); - if (with_los) { - log::log(SPAM << "Performing LOS pass"); - auto wavefront_blocked = integration_field->integrate_los(cost_field, target); - integration_field->integrate_cost(cost_field, std::move(wavefront_blocked)); - } - else { - log::log(SPAM << "Skipping LOS pass"); - integration_field->integrate_cost(cost_field, target); - } - - return integration_field; -} - -std::shared_ptr Integrator::integrate(const std::shared_ptr &cost_field, - const std::shared_ptr &other, - sector_id_t other_sector_id, - const std::shared_ptr &portal, - const coord::tile_delta &target, - const time::time_t &time, - bool with_los) { - auto cache_key = std::make_pair(portal->get_id(), other_sector_id); - if (cost_field->is_dirty(time)) { - log::log(DBG << "Evicting cached integration and flow fields for portal " << portal->get_id() - << " from sector " << other_sector_id); - this->field_cache->evict(cache_key); - } - else if (this->field_cache->is_cached(cache_key)) { - log::log(DBG << "Using cached integration field for portal " << portal->get_id() - << " from sector " << other_sector_id); - - // retrieve cached integration field - auto cached_integration_field = this->field_cache->get_integration_field(cache_key); - - if (with_los) { - log::log(SPAM << "Performing LOS pass on cached field"); - - // Make a copy of the cached field to avoid modifying the cached field - auto integration_field = std::make_shared(*cached_integration_field); - - // Only integrate LOS; leave the rest of the field as is - integration_field->integrate_los(cost_field, other, other_sector_id, portal, target); - - return integration_field; - } - return cached_integration_field; - } - - log::log(DBG << "Integrating cost field for portal " << portal->get_id() - << " from sector " << other_sector_id); - - // Create a new integration field - auto integration_field = std::make_shared(cost_field->get_size()); - - // LOS pass - std::vector wavefront_blocked; - if (with_los) { - log::log(SPAM << "Performing LOS pass"); - wavefront_blocked = integration_field->integrate_los(cost_field, other, other_sector_id, portal, target); - } - - // Cost integration - if (wavefront_blocked.empty()) { - // No LOS pass or no blocked cells - // use the portal as the target - integration_field->integrate_cost(cost_field, other_sector_id, portal); - } - else { - // LOS pass was performed and some cells were blocked - // use the blocked cells as the start wave - integration_field->integrate_cost(cost_field, std::move(wavefront_blocked)); - } - - return integration_field; -} - -std::shared_ptr Integrator::build(const std::shared_ptr &integration_field) { - auto flow_field = std::make_shared(integration_field->get_size()); - - log::log(DBG << "Building flow field from integration field"); - flow_field->build(integration_field); - - return flow_field; -} - -std::shared_ptr Integrator::build(const std::shared_ptr &integration_field, - const std::shared_ptr &other, - sector_id_t other_sector_id, - const std::shared_ptr &portal, - bool with_los) { - auto cache_key = std::make_pair(portal->get_id(), other_sector_id); - if (this->field_cache->is_cached(cache_key)) { - log::log(DBG << "Using cached flow field for portal " << portal->get_id() - << " from sector " << other_sector_id); - - // retrieve cached flow field - auto cached_flow_field = this->field_cache->get_flow_field(cache_key); - - if (with_los) { - log::log(SPAM << "Transferring LOS flags to cached flow field"); - - // Make a copy of the cached flow field - auto flow_field = std::make_shared(*cached_flow_field); - - // Transfer the LOS flags to the flow field - flow_field->transfer_dynamic_flags(integration_field); - - return flow_field; - } - - return cached_flow_field; - } - - log::log(DBG << "Building flow field for portal " << portal->get_id() - << " from sector " << other_sector_id); - - auto flow_field = std::make_shared(integration_field->get_size()); - flow_field->build(integration_field, other, other_sector_id, portal); - - return flow_field; -} - -Integrator::get_return_t Integrator::get(const std::shared_ptr &cost_field, - const coord::tile_delta &target) { - auto integration_field = this->integrate(cost_field, target); - auto flow_field = this->build(integration_field); - - return std::make_pair(integration_field, flow_field); -} - -Integrator::get_return_t Integrator::get(const std::shared_ptr &cost_field, - const std::shared_ptr &other, - sector_id_t other_sector_id, - const std::shared_ptr &portal, - const coord::tile_delta &target, - const time::time_t &time, - bool with_los) { - auto cache_key = std::make_pair(portal->get_id(), other_sector_id); - if (cost_field->is_dirty(time)) { - log::log(DBG << "Evicting cached integration and flow fields for portal " << portal->get_id() - << " from sector " << other_sector_id); - this->field_cache->evict(cache_key); - } - else if (this->field_cache->is_cached(cache_key)) { - log::log(DBG << "Using cached integration and flow fields for portal " << portal->get_id() - << " from sector " << other_sector_id); - - // retrieve cached fields - auto cached_fields = this->field_cache->get(cache_key); - auto cached_integration_field = cached_fields.first; - auto cached_flow_field = cached_fields.second; - - if (with_los) { - log::log(SPAM << "Performing LOS pass on cached field"); - - // Make a copy of the cached integration field - auto integration_field = std::make_shared(*cached_integration_field); - - // Only integrate LOS; leave the rest of the field as is - integration_field->integrate_los(cost_field, other, other_sector_id, portal, target); - - log::log(SPAM << "Transferring LOS flags to cached flow field"); - - // Make a copy of the cached flow field - auto flow_field = std::make_shared(*cached_flow_field); - - // Transfer the LOS flags to the flow field - flow_field->transfer_dynamic_flags(integration_field); - - return std::make_pair(integration_field, flow_field); - } - - return std::make_pair(cached_integration_field, cached_flow_field); - } - - auto integration_field = this->integrate(cost_field, other, other_sector_id, portal, target, time, with_los); - auto flow_field = this->build(integration_field, other, other_sector_id, portal); - - log::log(DBG << "Caching integration and flow fields for portal ID: " << portal->get_id() - << ", sector ID: " << other_sector_id); - - // Copy the fields to the cache. - std::shared_ptr cached_integration_field = std::make_shared(*integration_field); - cached_integration_field->reset_dynamic_flags(); - - std::shared_ptr cached_flow_field = std::make_shared(*flow_field); - cached_flow_field->reset_dynamic_flags(); - - field_cache_t field_cache = field_cache_t(cached_integration_field, cached_flow_field); - - this->field_cache->add(cache_key, field_cache); - - return std::make_pair(integration_field, flow_field); -} - } // namespace openage::path diff --git a/libopenage/pathfinding/integrator.h b/libopenage/pathfinding/integrator.h index 490d5eaacb..95200ade78 100644 --- a/libopenage/pathfinding/integrator.h +++ b/libopenage/pathfinding/integrator.h @@ -4,10 +4,17 @@ #include -#include "pathfinding/types.h" +#include "pathfinding/cost_field.h" #include "pathfinding/field_cache.h" -#include "util/hash.h" +#include "pathfinding/flow_field.h" +#include "pathfinding/integration_field.h" +#include "pathfinding/portal.h" +#include "pathfinding/types.h" + #include "time/time.h" +#include "util/hash.h" + +#include "log/log.h" namespace openage { @@ -16,14 +23,22 @@ struct tile_delta; } // namespace coord namespace path { + +template class CostField; + +template class FlowField; + +template class IntegrationField; + class Portal; /** * Integrator for the flow field pathfinding algorithm. */ +template class Integrator { public: /** @@ -45,9 +60,9 @@ class Integrator { * * @return Integration field. */ - std::shared_ptr integrate(const std::shared_ptr &cost_field, - const coord::tile_delta &target, - bool with_los = true); + std::shared_ptr> integrate(const std::shared_ptr> &cost_field, + const coord::tile_delta &target, + bool with_los = true); /** * Integrate the cost field from a portal. @@ -65,13 +80,13 @@ class Integrator { * * @return Integration field. */ - std::shared_ptr integrate(const std::shared_ptr &cost_field, - const std::shared_ptr &other, - sector_id_t other_sector_id, - const std::shared_ptr &portal, - const coord::tile_delta &target, - const time::time_t &time, - bool with_los = true); + std::shared_ptr> integrate(const std::shared_ptr> &cost_field, + const std::shared_ptr> &other, + sector_id_t other_sector_id, + const std::shared_ptr &portal, + const coord::tile_delta &target, + const time::time_t &time, + bool with_los = true); /** * Build the flow field from an integration field. @@ -80,7 +95,7 @@ class Integrator { * * @return Flow field. */ - std::shared_ptr build(const std::shared_ptr &integration_field); + std::shared_ptr> build(const std::shared_ptr> &integration_field); /** * Build the flow field from a portal. @@ -93,13 +108,13 @@ class Integrator { * * @return Flow field. */ - std::shared_ptr build(const std::shared_ptr &integration_field, - const std::shared_ptr &other, - sector_id_t other_sector_id, - const std::shared_ptr &portal, - bool with_los = true); + std::shared_ptr> build(const std::shared_ptr> &integration_field, + const std::shared_ptr> &other, + sector_id_t other_sector_id, + const std::shared_ptr &portal, + bool with_los = true); - using get_return_t = std::pair, std::shared_ptr>; + using get_return_t = std::pair>, std::shared_ptr>>; /** * Get the integration field and flow field for a target. @@ -109,7 +124,7 @@ class Integrator { * * @return Integration field and flow field. */ - get_return_t get(const std::shared_ptr &cost_field, + get_return_t get(const std::shared_ptr> &cost_field, const coord::tile_delta &target); /** @@ -125,20 +140,231 @@ class Integrator { * * @return Integration field and flow field. */ - get_return_t get(const std::shared_ptr &cost_field, - const std::shared_ptr &other, + get_return_t get(const std::shared_ptr> &cost_field, + const std::shared_ptr> &other, sector_id_t other_sector_id, const std::shared_ptr &portal, const coord::tile_delta &target, - const time::time_t &time, + const time::time_t &time, bool with_los = true); private: /** * Cache for already computed fields. */ - std::unique_ptr field_cache; + std::unique_ptr> field_cache; }; +template +Integrator::Integrator() : + field_cache{std::make_unique>()} { +} + +template +std::shared_ptr> Integrator::integrate(const std::shared_ptr> &cost_field, + const coord::tile_delta &target, + bool with_los) { + auto integration_field = std::make_shared>(); + + log::log(DBG << "Integrating cost field for target coord " << target); + if (with_los) { + log::log(SPAM << "Performing LOS pass"); + auto wavefront_blocked = integration_field->integrate_los(cost_field, target); + integration_field->integrate_cost(cost_field, std::move(wavefront_blocked)); + } + else { + log::log(SPAM << "Skipping LOS pass"); + integration_field->integrate_cost(cost_field, target); + } + + return integration_field; +} + +template +std::shared_ptr> Integrator::integrate(const std::shared_ptr> &cost_field, + const std::shared_ptr> &other, + sector_id_t other_sector_id, + const std::shared_ptr &portal, + const coord::tile_delta &target, + const time::time_t &time, + bool with_los) { + auto cache_key = std::make_pair(portal->get_id(), other_sector_id); + if (cost_field->is_dirty(time)) { + log::log(DBG << "Evicting cached integration and flow fields for portal " << portal->get_id() + << " from sector " << other_sector_id); + this->field_cache->evict(cache_key); + } + else if (this->field_cache->is_cached(cache_key)) { + log::log(DBG << "Using cached integration field for portal " << portal->get_id() + << " from sector " << other_sector_id); + + // retrieve cached integration field + auto cached_integration_field = this->field_cache->get_integration_field(cache_key); + + if (with_los) { + log::log(SPAM << "Performing LOS pass on cached field"); + + // Make a copy of the cached field to avoid modifying the cached field + auto integration_field = std::make_shared>(*cached_integration_field); + + // Only integrate LOS; leave the rest of the field as is + integration_field->integrate_los(cost_field, other, other_sector_id, portal, target); + + return integration_field; + } + return cached_integration_field; + } + + log::log(DBG << "Integrating cost field for portal " << portal->get_id() + << " from sector " << other_sector_id); + + // Create a new integration field + auto integration_field = std::make_shared>(); + + // LOS pass + std::vector wavefront_blocked; + if (with_los) { + log::log(SPAM << "Performing LOS pass"); + wavefront_blocked = integration_field->integrate_los(cost_field, other, other_sector_id, portal, target); + } + + // Cost integration + if (wavefront_blocked.empty()) { + // No LOS pass or no blocked cells + // use the portal as the target + integration_field->integrate_cost(cost_field, other_sector_id, portal); + } + else { + // LOS pass was performed and some cells were blocked + // use the blocked cells as the start wave + integration_field->integrate_cost(cost_field, std::move(wavefront_blocked)); + } + + return integration_field; +} + +template +std::shared_ptr> Integrator::build(const std::shared_ptr> &integration_field) { + auto flow_field = std::make_shared>(); + + log::log(DBG << "Building flow field from integration field"); + flow_field->build(integration_field); + + return flow_field; +} + +template +std::shared_ptr> Integrator::build(const std::shared_ptr> &integration_field, + const std::shared_ptr> &other, + sector_id_t other_sector_id, + const std::shared_ptr &portal, + bool with_los) { + auto cache_key = std::make_pair(portal->get_id(), other_sector_id); + if (this->field_cache->is_cached(cache_key)) { + log::log(DBG << "Using cached flow field for portal " << portal->get_id() + << " from sector " << other_sector_id); + + // retrieve cached flow field + auto cached_flow_field = this->field_cache->get_flow_field(cache_key); + + if (with_los) { + log::log(SPAM << "Transferring LOS flags to cached flow field"); + + // Make a copy of the cached flow field + auto flow_field = std::make_shared>(*cached_flow_field); + + // Transfer the LOS flags to the flow field + flow_field->transfer_dynamic_flags(integration_field); + + return flow_field; + } + + return cached_flow_field; + } + + log::log(DBG << "Building flow field for portal " << portal->get_id() + << " from sector " << other_sector_id); + + auto flow_field = std::make_shared>(); + flow_field->build(integration_field, other, other_sector_id, portal); + + return flow_field; +} + +template +Integrator::get_return_t Integrator::get(const std::shared_ptr> &cost_field, + const coord::tile_delta &target) { + auto integration_field = this->integrate(cost_field, target); + auto flow_field = this->build(integration_field); + + return std::make_pair(integration_field, flow_field); +} + +template +Integrator::get_return_t Integrator::get(const std::shared_ptr> &cost_field, + const std::shared_ptr> &other, + sector_id_t other_sector_id, + const std::shared_ptr &portal, + const coord::tile_delta &target, + const time::time_t &time, + bool with_los) { + auto cache_key = std::make_pair(portal->get_id(), other_sector_id); + if (cost_field->is_dirty(time)) { + log::log(DBG << "Evicting cached integration and flow fields for portal " << portal->get_id() + << " from sector " << other_sector_id); + this->field_cache->evict(cache_key); + } + else if (this->field_cache->is_cached(cache_key)) { + log::log(DBG << "Using cached integration and flow fields for portal " << portal->get_id() + << " from sector " << other_sector_id); + + // retrieve cached fields + auto cached_fields = this->field_cache->get(cache_key); + auto cached_integration_field = cached_fields.first; + auto cached_flow_field = cached_fields.second; + + if (with_los) { + log::log(SPAM << "Performing LOS pass on cached field"); + + // Make a copy of the cached integration field + auto integration_field = std::make_shared>(); + + // Only integrate LOS; leave the rest of the field as is + integration_field->integrate_los(cost_field, other, other_sector_id, portal, target); + + log::log(SPAM << "Transferring LOS flags to cached flow field"); + + // Make a copy of the cached flow field + auto flow_field = std::make_shared>(*cached_flow_field); + + // Transfer the LOS flags to the flow field + flow_field->transfer_dynamic_flags(integration_field); + + return std::make_pair(integration_field, flow_field); + } + + return std::make_pair(cached_integration_field, cached_flow_field); + } + + auto integration_field = this->integrate(cost_field, other, other_sector_id, portal, target, time, with_los); + auto flow_field = this->build(integration_field, other, other_sector_id, portal); + + log::log(DBG << "Caching integration and flow fields for portal ID: " << portal->get_id() + << ", sector ID: " << other_sector_id); + + // Copy the fields to the cache. + std::shared_ptr> cached_integration_field = std::make_shared>(); + cached_integration_field->reset_dynamic_flags(); + + std::shared_ptr> cached_flow_field = std::make_shared>(*flow_field); + cached_flow_field->reset_dynamic_flags(); + + field_cache_t field_cache = field_cache_t(cached_integration_field, cached_flow_field); + + this->field_cache->add(cache_key, field_cache); + + return std::make_pair(integration_field, flow_field); +} + } // namespace path } // namespace openage diff --git a/libopenage/pathfinding/pathfinder.cpp b/libopenage/pathfinding/pathfinder.cpp index 5a9c785f0c..b555f9cf39 100644 --- a/libopenage/pathfinding/pathfinder.cpp +++ b/libopenage/pathfinding/pathfinder.cpp @@ -1,568 +1,8 @@ -// Copyright 2024-2024 the openage authors. See copying.md for legal info. +// Copyright 2024-2024 the openage authors. See copying.md for legal info.s #include "pathfinder.h" -#include "coord/chunk.h" -#include "coord/phys.h" -#include "error/error.h" -#include "pathfinding/cost_field.h" -#include "pathfinding/flow_field.h" -#include "pathfinding/grid.h" -#include "pathfinding/integration_field.h" -#include "pathfinding/integrator.h" -#include "pathfinding/portal.h" -#include "pathfinding/sector.h" - - namespace openage::path { -Pathfinder::Pathfinder() : - grids{}, - integrator{std::make_shared()} { -} - -const Path Pathfinder::get_path(const PathRequest &request) { - auto grid = this->grids.at(request.grid_id); - auto sector_size = grid->get_sector_size(); - - // Check if the target is within the grid - auto grid_size = grid->get_size(); - auto grid_width = grid_size[0] * sector_size; - auto grid_height = grid_size[1] * sector_size; - if (request.target.ne < 0 - or request.target.se < 0 - or request.target.ne >= static_cast(grid_width) - or request.target.se >= static_cast(grid_height)) { - log::log(DBG << "Path not found (start = " - << request.start << "; target = " - << request.target << "): " - << "Target is out of bounds."); - return Path{request.grid_id, PathResult::OUT_OF_BOUNDS, {}}; - } - - auto start_sector_x = request.start.ne / sector_size; - auto start_sector_y = request.start.se / sector_size; - auto start_sector = grid->get_sector(start_sector_x, start_sector_y); - - auto target_sector_x = request.target.ne / sector_size; - auto target_sector_y = request.target.se / sector_size; - auto target_sector = grid->get_sector(target_sector_x, target_sector_y); - - auto target = request.target - target_sector->get_position().to_tile(sector_size); - if (target_sector->get_cost_field()->get_cost(target) == COST_IMPASSABLE) { - // TODO: This may be okay if the target is a building or unit - log::log(DBG << "Path not found (start = " - << request.start << "; target = " - << request.target << "): " - << "Target is impassable."); - return Path{request.grid_id, PathResult::NOT_FOUND, {}}; - } - - // Integrate the target field - coord::tile_delta target_delta = request.target - target_sector->get_position().to_tile(sector_size); - auto target_integration_field = this->integrator->integrate(target_sector->get_cost_field(), - target_delta); - - if (target_sector == start_sector) { - auto start = request.start - start_sector->get_position().to_tile(sector_size); - - if (target_integration_field->get_cell(start.ne, start.se).cost != INTEGRATED_COST_UNREACHABLE) { - // Exit early if the start and target are in the same sector - // and are reachable from within the same sector - auto flow_field = this->integrator->build(target_integration_field); - auto flow_field_waypoints = this->get_waypoints({std::make_pair(target_sector->get_id(), flow_field)}, request); - - std::vector waypoints{}; - if (flow_field_waypoints.at(0) != request.start) { - waypoints.push_back(request.start); - } - waypoints.insert(waypoints.end(), flow_field_waypoints.begin(), flow_field_waypoints.end()); - - log::log(DBG << "Path found (start = " - << request.start << "; target = " - << request.target << "): " - << "Path is within the same sector."); - return Path{request.grid_id, PathResult::FOUND, waypoints}; - } - } - - // Check which portals are reachable from the target field - std::unordered_set target_portal_ids; - for (auto &portal : target_sector->get_portals()) { - auto center_cell = portal->get_entry_center(target_sector->get_id()); - - if (target_integration_field->get_cell(center_cell).cost != INTEGRATED_COST_UNREACHABLE) { - target_portal_ids.insert(portal->get_id()); - } - } - - // Check which portals are reachable from the start field - coord::tile_delta start = request.start - start_sector->get_position().to_tile(sector_size); - auto start_integration_field = this->integrator->integrate(start_sector->get_cost_field(), - start, - false); - - std::unordered_set start_portal_ids; - for (auto &portal : start_sector->get_portals()) { - auto center_cell = portal->get_entry_center(start_sector->get_id()); - - if (start_integration_field->get_cell(center_cell).cost != INTEGRATED_COST_UNREACHABLE) { - start_portal_ids.insert(portal->get_id()); - } - } - - if (target_portal_ids.empty() or start_portal_ids.empty()) { - // Exit early if no portals are reachable from the start or target - log::log(DBG << "Path not found (start = " - << request.start << "; target = " - << request.target << "): " - << "No portals are reachable from the start or target."); - return Path{request.grid_id, PathResult::NOT_FOUND, {}}; - } - - // High-level pathfinding - // Find the portals to use to get from the start to the target - auto portal_result = this->portal_a_star(request, target_portal_ids, start_portal_ids); - auto portal_status = portal_result.first; - auto portal_path = portal_result.second; - - // Low-level pathfinding - // Find the path within the sectors - - // Build flow field for the target sector - auto prev_integration_field = target_integration_field; - auto prev_flow_field = this->integrator->build(prev_integration_field); - auto prev_sector_id = target_sector->get_id(); - - Integrator::get_return_t sector_fields{prev_integration_field, prev_flow_field}; - - std::vector>> flow_fields; - flow_fields.reserve(portal_path.size() + 1); - flow_fields.push_back(std::make_pair(target_sector->get_id(), sector_fields.second)); - - int los_depth = 1; - - for (auto &portal : portal_path) { - auto prev_sector = grid->get_sector(prev_sector_id); - auto next_sector_id = portal->get_exit_sector(prev_sector_id); - auto next_sector = grid->get_sector(next_sector_id); - - target_delta = request.target - next_sector->get_position().to_tile(sector_size); - bool with_los = los_depth > 0; - - sector_fields = this->integrator->get(next_sector->get_cost_field(), - prev_integration_field, - prev_sector_id, - portal, - target_delta, - request.time, - with_los); - flow_fields.push_back(std::make_pair(next_sector_id, sector_fields.second)); - - prev_integration_field = sector_fields.first; - prev_sector_id = next_sector_id; - los_depth -= 1; - } - - // reverse the flow fields so they are ordered from start to target - std::reverse(flow_fields.begin(), flow_fields.end()); - - // traverse the flow fields to get the waypoints - auto flow_field_waypoints = this->get_waypoints(flow_fields, request); - std::vector waypoints{}; - if (flow_field_waypoints.at(0) != request.start) { - waypoints.push_back(request.start); - } - waypoints.insert(waypoints.end(), flow_field_waypoints.begin(), flow_field_waypoints.end()); - - if (portal_status == PathResult::NOT_FOUND) { - log::log(DBG << "Path not found (start = " - << request.start << "; target = " - << request.target << ")"); - } - else { - log::log(DBG << "Path found (start = " - << request.start << "; target = " - << request.target << ")"); - } - - return Path{request.grid_id, portal_status, waypoints}; -} - -const std::shared_ptr &Pathfinder::get_grid(grid_id_t id) const { - return this->grids.at(id); -} - -void Pathfinder::add_grid(const std::shared_ptr &grid) { - this->grids[grid->get_id()] = grid; -} - -const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &request, - const std::unordered_set &target_portal_ids, - const std::unordered_set &start_portal_ids) const { - std::vector> result; - - auto grid = this->grids.at(request.grid_id); - auto &portal_map = grid->get_portal_map(); - auto sector_size = grid->get_sector_size(); - - auto start_sector_x = request.start.ne / sector_size; - auto start_sector_y = request.start.se / sector_size; - auto start_sector = grid->get_sector(start_sector_x, start_sector_y); - - // path node storage, always provides cheapest next node. - heap_t node_candidates; - - std::unordered_set visited_portals; - - // TODO: Compute cost to travel from one portal to another when creating portals - // const int distance_cost = 1; - - // create start nodes - for (auto &portal : start_sector->get_portals()) { - if (not start_portal_ids.contains(portal->get_id())) { - // only consider portals that are reachable from the start cell - continue; - } - - auto &portal_node = portal_map.at(portal->get_id()); - portal_node->entry_sector = start_sector->get_id(); - - auto sector_pos = grid->get_sector(portal->get_exit_sector(start_sector->get_id()))->get_position().to_tile(sector_size); - auto portal_pos = portal->get_exit_center(start_sector->get_id()); - auto portal_abs_pos = sector_pos + portal_pos; - auto heuristic_cost = Pathfinder::heuristic_cost(portal_abs_pos, request.target); - portal_node->current_cost = Pathfinder::heuristic_cost(portal_abs_pos, request.start); - portal_node->heuristic_cost = heuristic_cost; - portal_node->future_cost = portal_node->current_cost + heuristic_cost; - - portal_node->heap_node = node_candidates.push(portal_node); - portal_node->prev_portal = nullptr; - portal_node->was_best = false; - visited_portals.insert(portal->get_id()); - } - - // track the closest we can get to the end position - // used when no path is found - auto closest_node = node_candidates.top(); - - // while there are candidates to visit - while (not node_candidates.empty()) { - auto current_node = node_candidates.pop(); - - current_node->was_best = true; - - // check if the current node is a portal in the target sector that can - // be reached from the target cell - auto exit_portal_id = current_node->portal->get_id(); - if (target_portal_ids.contains(exit_portal_id)) { - auto backtrace = current_node->generate_backtrace(); - for (auto &node : backtrace) { - result.push_back(node->portal); - } - log::log(DBG << "Portal path found with " << result.size() << " portal traversals."); - return std::make_pair(PathResult::FOUND, result); - } - - // check if the current node is the closest to the target - if (current_node->heuristic_cost < closest_node->heuristic_cost) { - closest_node = current_node; - } - - // get the exits of the current node - ENSURE(current_node->entry_sector != std::nullopt, "Entry sector not set for portal node."); - const auto &exits = current_node->get_exits(current_node->entry_sector.value()); - - // evaluate all neighbors of the current candidate for further progress - for (auto &[exit, distance_cost] : exits) { - exit->entry_sector = current_node->portal->get_exit_sector(current_node->entry_sector.value()); - bool not_visited = !visited_portals.contains(exit->portal->get_id()); - - if (not_visited) { - exit->was_best = false; - } - else if (exit->was_best) { - continue; - } - - - auto tentative_cost = current_node->current_cost + distance_cost; - - if (not_visited or tentative_cost < exit->current_cost) { - if (not_visited) { - // Get heuristic cost (from exit node to target cell) - auto exit_sector = grid->get_sector(exit->portal->get_exit_sector(exit->entry_sector.value())); - auto exit_sector_pos = exit_sector->get_position().to_tile(sector_size); - auto exit_portal_pos = exit->portal->get_exit_center(exit->entry_sector.value()); - exit->heuristic_cost = Pathfinder::heuristic_cost( - exit_sector_pos + exit_portal_pos, - request.target); - } - - // update the cost knowledge - exit->current_cost = tentative_cost; - exit->future_cost = exit->current_cost + exit->heuristic_cost; - exit->prev_portal = current_node; - - if (not_visited) { - exit->heap_node = node_candidates.push(exit); - visited_portals.insert(exit->portal->get_id()); - } - else { - node_candidates.decrease(exit->heap_node); - } - } - } - } - - // no path found, return the closest node - auto backtrace = closest_node->generate_backtrace(); - for (auto &node : backtrace) { - result.push_back(node->portal); - } - - log::log(DBG << "Portal path not found."); - log::log(DBG << "Closest portal: " << closest_node->portal->get_id()); - return std::make_pair(PathResult::NOT_FOUND, result); -} - -const std::vector Pathfinder::get_waypoints(const std::vector>> &flow_fields, - const PathRequest &request) const { - ENSURE(flow_fields.size() > 0, "At least 1 flow field is required for finding waypoints."); - - std::vector waypoints; - - auto grid = this->get_grid(request.grid_id); - auto sector_size = grid->get_sector_size(); - coord::tile_t start_x = request.start.ne % sector_size; - coord::tile_t start_y = request.start.se % sector_size; - - bool los_reached = false; - - coord::tile_t current_x = start_x; - coord::tile_t current_y = start_y; - flow_dir_t current_direction = flow_fields.at(0).second->get_dir(current_x, current_y); - for (size_t i = 0; i < flow_fields.size(); ++i) { - auto §or = grid->get_sector(flow_fields[i].first); - auto sector_pos = sector->get_position().to_tile(sector_size); - auto &flow_field = flow_fields[i].second; - - // navigate the flow field vectors until we reach its edge (or the target) - flow_t cell; - do { - cell = flow_field->get_cell(current_x, current_y); - - if (cell & FLOW_LOS_MASK) { - // check if we reached an LOS cell - auto cell_pos = sector_pos + coord::tile_delta(current_x, current_y); - waypoints.push_back(cell_pos); - los_reached = true; - break; - } - - // check if we need to change direction - auto cell_direction = static_cast(cell & FLOW_DIR_MASK); - if (cell_direction != current_direction) { - // add the current cell as a waypoint - auto cell_pos = sector_pos + coord::tile_delta(current_x, current_y); - waypoints.push_back(cell_pos); - current_direction = cell_direction; - } - - // move to the next cell - switch (current_direction) { - case flow_dir_t::NORTH: - current_y -= 1; - break; - case flow_dir_t::NORTH_EAST: - current_x += 1; - current_y -= 1; - break; - case flow_dir_t::EAST: - current_x += 1; - break; - case flow_dir_t::SOUTH_EAST: - current_x += 1; - current_y += 1; - break; - case flow_dir_t::SOUTH: - current_y += 1; - break; - case flow_dir_t::SOUTH_WEST: - current_x -= 1; - current_y += 1; - break; - case flow_dir_t::WEST: - current_x -= 1; - break; - case flow_dir_t::NORTH_WEST: - current_x -= 1; - current_y -= 1; - break; - default: - throw Error{ERR << "Invalid flow direction: " << static_cast(current_direction)}; - } - } - while (not(cell & FLOW_TARGET_MASK)); - - if (los_reached or i == flow_fields.size() - 1) { - // exit the loop if we found an LOS cell or reached - // the target cell in the last flow field - break; - } - - // reset the current position for the next flow field - switch (current_direction) { - case flow_dir_t::NORTH: - current_y = sector_size - 1; - break; - case flow_dir_t::NORTH_EAST: - current_x = current_x + 1; - current_y = sector_size - 1; - break; - case flow_dir_t::EAST: - current_x = 0; - break; - case flow_dir_t::SOUTH_EAST: - current_x = 0; - current_y = current_y + 1; - break; - case flow_dir_t::SOUTH: - current_y = 0; - break; - case flow_dir_t::SOUTH_WEST: - current_x = current_x - 1; - current_y = 0; - break; - case flow_dir_t::WEST: - current_x = sector_size - 1; - break; - case flow_dir_t::NORTH_WEST: - current_x = sector_size - 1; - current_y = current_y - 1; - break; - default: - throw Error{ERR << "Invalid flow direction: " << static_cast(current_direction)}; - } - } - - // add the target position as the last waypoint - waypoints.push_back(request.target); - - return waypoints; -} - -int Pathfinder::heuristic_cost(const coord::tile &portal_pos, - const coord::tile &target_pos) { - auto portal_phys_pos = portal_pos.to_phys2(); - auto target_phys_pos = target_pos.to_phys2(); - auto delta = target_phys_pos - portal_phys_pos; - - return delta.length(); -} - -int Pathfinder::distance_cost(const coord::tile_delta &portal1_pos, - const coord::tile_delta &portal2_pos) { - auto delta = portal2_pos.to_phys2() - portal1_pos.to_phys2(); - - return delta.length(); -} - - -PortalNode::PortalNode(const std::shared_ptr &portal) : - portal{portal}, - entry_sector{std::nullopt}, - future_cost{std::numeric_limits::max()}, - current_cost{std::numeric_limits::max()}, - heuristic_cost{std::numeric_limits::max()}, - was_best{false}, - prev_portal{nullptr}, - heap_node{nullptr} {} - -PortalNode::PortalNode(const std::shared_ptr &portal, - sector_id_t entry_sector, - const node_t &prev_portal) : - portal{portal}, - entry_sector{entry_sector}, - future_cost{std::numeric_limits::max()}, - current_cost{std::numeric_limits::max()}, - heuristic_cost{std::numeric_limits::max()}, - was_best{false}, - prev_portal{prev_portal}, - heap_node{nullptr} {} - -PortalNode::PortalNode(const std::shared_ptr &portal, - sector_id_t entry_sector, - const node_t &prev_portal, - int past_cost, - int heuristic_cost) : - portal{portal}, - entry_sector{entry_sector}, - future_cost{past_cost + heuristic_cost}, - current_cost{past_cost}, - heuristic_cost{heuristic_cost}, - was_best{false}, - prev_portal{prev_portal}, - heap_node{nullptr} { } - -bool PortalNode::operator<(const PortalNode &other) const { - return this->future_cost < other.future_cost; -} - -bool PortalNode::operator==(const PortalNode &other) const { - return this->portal->get_id() == other.portal->get_id(); -} - -std::vector PortalNode::generate_backtrace() { - std::vector waypoints; - - node_t current = this->shared_from_this(); - do { - waypoints.push_back(current); - current = current->prev_portal; - } - while (current != nullptr); - - return waypoints; -} - -void PortalNode::init_exits(const nodemap_t &node_map) { - auto exits = this->portal->get_exits(this->node_sector_0); - for (auto &exit : exits) { - int distance_cost = Pathfinder::distance_cost( - this->portal->get_exit_center(this->node_sector_0), - exit->get_entry_center(this->node_sector_1)); - - auto exit_node = node_map.at(exit->get_id()); - this->exits_1[exit_node] = distance_cost; - } - - exits = this->portal->get_exits(this->node_sector_1); - for (auto &exit : exits) { - int distance_cost = Pathfinder::distance_cost( - this->portal->get_exit_center(this->node_sector_1), - exit->get_entry_center(this->node_sector_0)); - - auto exit_node = node_map.at(exit->get_id()); - this->exits_0[exit_node] = distance_cost; - } -} - -const PortalNode::exits_t &PortalNode::get_exits(sector_id_t entry_sector) { - ENSURE(entry_sector == this->node_sector_0 || entry_sector == this->node_sector_1, "Invalid entry sector"); - - if (this->node_sector_0 == entry_sector) { - return exits_1; - } - else { - return exits_0; - } -} - - -bool compare_node_cost::operator()(const node_t &lhs, const node_t &rhs) const { - return *lhs < *rhs; -} - -} // namespace openage::path +// namespace openage::path diff --git a/libopenage/pathfinding/pathfinder.h b/libopenage/pathfinding/pathfinder.h index 8d569d78d8..0a026fee21 100644 --- a/libopenage/pathfinding/pathfinder.h +++ b/libopenage/pathfinding/pathfinder.h @@ -7,16 +7,40 @@ #include #include +#include "coord/chunk.h" +#include "coord/phys.h" #include "coord/tile.h" + #include "datastructure/pairing_heap.h" + +#include "error/error.h" + +#include "log/log.h" + +#include "pathfinding/cost_field.h" +#include "pathfinding/definitions.h" +#include "pathfinding/flow_field.h" +#include "pathfinding/grid.h" +#include "pathfinding/integration_field.h" +#include "pathfinding/integrator.h" #include "pathfinding/path.h" +#include "pathfinding/portal.h" +#include "pathfinding/sector.h" #include "pathfinding/types.h" namespace openage::path { + + +template class Grid; + +template class Integrator; + class Portal; + +template class FlowField; /** @@ -30,6 +54,8 @@ class FlowField; * Afterwards, flow fields are calculated from the target sector to the start * sector, which are then used to guide the actual unit movement. */ + +template class Pathfinder { public: /** @@ -45,14 +71,14 @@ class Pathfinder { * * @return Pathfinding grid. */ - const std::shared_ptr &get_grid(grid_id_t id) const; + const std::shared_ptr> &get_grid(grid_id_t id) const; /** * Add a grid to the pathfinder. * * @param grid Grid to add. */ - void add_grid(const std::shared_ptr &grid); + void add_grid(const std::shared_ptr> &grid); /** * Get the path for a pathfinding request. @@ -98,7 +124,7 @@ class Pathfinder { * * @return Waypoint coordinates to traverse in order to reach the target. */ - const std::vector get_waypoints(const std::vector>> &flow_fields, + const std::vector get_waypoints(const std::vector>>> &flow_fields, const PathRequest &request) const; /** @@ -118,155 +144,469 @@ class Pathfinder { * * Each grid can have separate pathing. */ - std::unordered_map> grids; + std::unordered_map>> grids; /** * Integrator for flow field calculations. */ - std::shared_ptr integrator; + std::shared_ptr> integrator; }; - class PortalNode; using node_t = std::shared_ptr; -/** - * Cost comparison for node_t on the pairing heap. - * - * Extracts the nodes from the shared_ptr and compares them. We have - * to use a custom comparison function because otherwise the shared_ptr - * would be compared instead of the actual node. - */ -struct compare_node_cost { - bool operator()(const node_t &lhs, const node_t &rhs) const; -}; -using heap_t = datastructure::PairingHeap; +using heap_t = datastructure::PairingHeap; using nodemap_t = std::unordered_map; -/** - * One navigation waypoint in a path. - */ -class PortalNode : public std::enable_shared_from_this { -public: - PortalNode(const std::shared_ptr &portal); - PortalNode(const std::shared_ptr &portal, - sector_id_t entry_sector, - const node_t &prev_portal); - PortalNode(const std::shared_ptr &portal, - sector_id_t entry_sector, - const node_t &prev_portal, - int past_cost, - int heuristic_cost); - - /** - * Orders nodes according to their future cost value. - */ - bool operator<(const PortalNode &other) const; - - /** - * Compare the node to another one. - * They are the same if their portal is. - */ - bool operator==(const PortalNode &other) const; - - /** - * Calculates the actual movement cose to another node. - */ - int cost_to(const PortalNode &other) const; - - /** - * Create a backtrace path beginning at this node. - */ - std::vector generate_backtrace(); - - /** - * init PortalNode::exits. - */ - void init_exits(const nodemap_t &node_map); - - - /** - * maps node_t of a neigbhour portal to the distance cost to travel between the portals - */ - using exits_t = std::map; - - - /** - * Get the exit portals reachable via the portal when entering from a specified sector. - * - * @param entry_sector Sector from which the portal is entered. - * - * @return Exit portals nodes reachable from the portal. - */ - const exits_t &get_exits(sector_id_t entry_sector); - - /** - * The portal this node is associated to. - */ - std::shared_ptr portal; - - /** - * Sector where the portal is entered. - */ - std::optional entry_sector; - - /** - * Future cost estimation value for this node. - */ - int future_cost; - - /** - * Evaluated past cost value for the node. - * This stores the actual cost from start to this node. - */ - int current_cost; - - /** - * Heuristic cost cache. - * Calculated once, is the heuristic distance from this node - * to the goal. - */ - int heuristic_cost; - - /** - * Does this node already have an alternative path? - * If the node was once selected as the best next hop, - * this is set to true. - */ - bool was_best = false; - - /** - * Node where this one was reached by least cost. - */ - node_t prev_portal; - - /** - * Priority queue node that contains this path node. - */ - heap_t::element_t heap_node; - - /** - * First sector connected by the portal. - */ - sector_id_t node_sector_0; - - /** - * Second sector connected by the portal. - */ - sector_id_t node_sector_1; - - /** - * Exits in sector 0 reachable from the portal. - */ - exits_t exits_0; - - /** - * Exits in sector 1 reachable from the portal. - */ - exits_t exits_1; -}; +template +Pathfinder::Pathfinder() : + grids{}, + integrator{std::make_shared>()} {} + +template +const Path Pathfinder::get_path(const PathRequest &request) { + auto grid = this->grids.at(request.grid_id); + + // Check if the target is within the grid + auto grid_size = grid->get_size(); + auto grid_width = grid_size[0] * N; + auto grid_height = grid_size[1] * N; + if (request.target.ne < 0 + or request.target.se < 0 + or request.target.ne >= static_cast(grid_width) + or request.target.se >= static_cast(grid_height)) { + log::log(DBG << "Path not found (start = " + << request.start << "; target = " + << request.target << "): " + << "Target is out of bounds."); + return Path{request.grid_id, PathResult::OUT_OF_BOUNDS, {}}; + } + + auto start_sector_x = request.start.ne / N; + auto start_sector_y = request.start.se / N; + auto start_sector = grid->get_sector(start_sector_x, start_sector_y); + + auto target_sector_x = request.target.ne / N; + auto target_sector_y = request.target.se / N; + auto target_sector = grid->get_sector(target_sector_x, target_sector_y); + + auto target = request.target - target_sector->get_position().to_tile(N); + if (target_sector->get_cost_field()->get_cost(target) == COST_IMPASSABLE) { + // TODO: This may be okay if the target is a building or unit + log::log(DBG << "Path not found (start = " + << request.start << "; target = " + << request.target << "): " + << "Target is impassable."); + return Path{request.grid_id, PathResult::NOT_FOUND, {}}; + } + + // Integrate the target field + coord::tile_delta target_delta = request.target - target_sector->get_position().to_tile(N); + auto target_integration_field = this->integrator->integrate(target_sector->get_cost_field(), + target_delta); + + if (target_sector == start_sector) { + auto start = request.start - start_sector->get_position().to_tile(N); + + if (target_integration_field->get_cell(start.ne, start.se).cost != INTEGRATED_COST_UNREACHABLE) { + // Exit early if the start and target are in the same sector + // and are reachable from within the same sector + auto flow_field = this->integrator->build(target_integration_field); + auto flow_field_waypoints = this->get_waypoints({std::make_pair(target_sector->get_id(), flow_field)}, request); + + std::vector waypoints{}; + if (flow_field_waypoints.at(0) != request.start) { + waypoints.push_back(request.start); + } + waypoints.insert(waypoints.end(), flow_field_waypoints.begin(), flow_field_waypoints.end()); + + log::log(DBG << "Path found (start = " + << request.start << "; target = " + << request.target << "): " + << "Path is within the same sector."); + return Path{request.grid_id, PathResult::FOUND, waypoints}; + } + } + + // Check which portals are reachable from the target field + std::unordered_set target_portal_ids; + for (auto &portal : target_sector->get_portals()) { + auto center_cell = portal->get_entry_center(target_sector->get_id()); + + if (target_integration_field->get_cell(center_cell).cost != INTEGRATED_COST_UNREACHABLE) { + target_portal_ids.insert(portal->get_id()); + } + } + + // Check which portals are reachable from the start field + coord::tile_delta start = request.start - start_sector->get_position().to_tile(N); + auto start_integration_field = this->integrator->integrate(start_sector->get_cost_field(), + start, + false); + + std::unordered_set start_portal_ids; + for (auto &portal : start_sector->get_portals()) { + auto center_cell = portal->get_entry_center(start_sector->get_id()); + + if (start_integration_field->get_cell(center_cell).cost != INTEGRATED_COST_UNREACHABLE) { + start_portal_ids.insert(portal->get_id()); + } + } + + if (target_portal_ids.empty() or start_portal_ids.empty()) { + // Exit early if no portals are reachable from the start or target + log::log(DBG << "Path not found (start = " + << request.start << "; target = " + << request.target << "): " + << "No portals are reachable from the start or target."); + return Path{request.grid_id, PathResult::NOT_FOUND, {}}; + } + + // High-level pathfinding + // Find the portals to use to get from the start to the target + auto portal_result = this->portal_a_star(request, target_portal_ids, start_portal_ids); + auto portal_status = portal_result.first; + auto portal_path = portal_result.second; + + // Low-level pathfinding + // Find the path within the sectors + + // Build flow field for the target sector + auto prev_integration_field = target_integration_field; + auto prev_flow_field = this->integrator->build(prev_integration_field); + auto prev_sector_id = target_sector->get_id(); + + typename Integrator::get_return_t sector_fields{prev_integration_field, prev_flow_field}; + + std::vector>>> flow_fields; + flow_fields.reserve(portal_path.size() + 1); + flow_fields.push_back(std::make_pair(target_sector->get_id(), sector_fields.second)); + + int los_depth = 1; + + for (auto &portal : portal_path) { + auto prev_sector = grid->get_sector(prev_sector_id); + auto next_sector_id = portal->get_exit_sector(prev_sector_id); + auto next_sector = grid->get_sector(next_sector_id); + + target_delta = request.target - next_sector->get_position().to_tile(N); + bool with_los = los_depth > 0; + + sector_fields = this->integrator->get(next_sector->get_cost_field(), + prev_integration_field, + prev_sector_id, + portal, + target_delta, + request.time, + with_los); + flow_fields.push_back(std::make_pair(next_sector_id, sector_fields.second)); + + prev_integration_field = sector_fields.first; + prev_sector_id = next_sector_id; + los_depth -= 1; + } + + // reverse the flow fields so they are ordered from start to target + std::reverse(flow_fields.begin(), flow_fields.end()); + + // traverse the flow fields to get the waypoints + auto flow_field_waypoints = this->get_waypoints(flow_fields, request); + std::vector waypoints{}; + if (flow_field_waypoints.at(0) != request.start) { + waypoints.push_back(request.start); + } + waypoints.insert(waypoints.end(), flow_field_waypoints.begin(), flow_field_waypoints.end()); + + if (portal_status == PathResult::NOT_FOUND) { + log::log(DBG << "Path not found (start = " + << request.start << "; target = " + << request.target << ")"); + } + else { + log::log(DBG << "Path found (start = " + << request.start << "; target = " + << request.target << ")"); + } + + return Path{request.grid_id, portal_status, waypoints}; +} + +template +const Pathfinder::portal_star_t Pathfinder::portal_a_star(const PathRequest &request, + const std::unordered_set &target_portal_ids, + const std::unordered_set &start_portal_ids) const { + std::vector> result; + + auto grid = this->grids.at(request.grid_id); + auto &portal_map = grid->get_portal_map(); + + auto start_sector_x = request.start.ne / N; + auto start_sector_y = request.start.se / N; + auto start_sector = grid->get_sector(start_sector_x, start_sector_y); + + // path node storage, always provides cheapest next node. + heap_t node_candidates; + + std::unordered_set visited_portals; + + // TODO: Compute cost to travel from one portal to another when creating portals + // const int distance_cost = 1; + + // create start nodes + for (auto &portal : start_sector->get_portals()) { + if (not start_portal_ids.contains(portal->get_id())) { + // only consider portals that are reachable from the start cell + continue; + } + + auto &portal_node = portal_map.at(portal->get_id()); + portal_node->entry_sector = start_sector->get_id(); + + auto sector_pos = grid->get_sector(portal->get_exit_sector(start_sector->get_id()))->get_position().to_tile(N); + auto portal_pos = portal->get_exit_center(start_sector->get_id()); + auto portal_abs_pos = sector_pos + portal_pos; + auto heuristic_cost = Pathfinder::heuristic_cost(portal_abs_pos, request.target); + portal_node->current_cost = Pathfinder::heuristic_cost(portal_abs_pos, request.start); + portal_node->heuristic_cost = heuristic_cost; + portal_node->future_cost = portal_node->current_cost + heuristic_cost; + + portal_node->heap_node = node_candidates.push(portal_node); + portal_node->prev_portal = nullptr; + portal_node->was_best = false; + visited_portals.insert(portal->get_id()); + } + + // track the closest we can get to the end position + // used when no path is found + auto closest_node = node_candidates.top(); + + // while there are candidates to visit + while (not node_candidates.empty()) { + auto current_node = node_candidates.pop(); + + current_node->was_best = true; + + // check if the current node is a portal in the target sector that can + // be reached from the target cell + auto exit_portal_id = current_node->portal->get_id(); + if (target_portal_ids.contains(exit_portal_id)) { + auto backtrace = current_node->generate_backtrace(); + for (auto &node : backtrace) { + result.push_back(node->portal); + } + log::log(DBG << "Portal path found with " << result.size() << " portal traversals."); + return std::make_pair(PathResult::FOUND, result); + } + + // check if the current node is the closest to the target + if (current_node->heuristic_cost < closest_node->heuristic_cost) { + closest_node = current_node; + } + + // get the exits of the current node + ENSURE(current_node->entry_sector != std::nullopt, "Entry sector not set for portal node."); + const auto &exits = current_node->get_exits(current_node->entry_sector.value()); + + // evaluate all neighbors of the current candidate for further progress + for (auto &[exit, distance_cost] : exits) { + exit->entry_sector = current_node->portal->get_exit_sector(current_node->entry_sector.value()); + bool not_visited = !visited_portals.contains(exit->portal->get_id()); + + if (not_visited) { + exit->was_best = false; + } + else if (exit->was_best) { + continue; + } + + + auto tentative_cost = current_node->current_cost + distance_cost; + + if (not_visited or tentative_cost < exit->current_cost) { + if (not_visited) { + // Get heuristic cost (from exit node to target cell) + auto exit_sector = grid->get_sector(exit->portal->get_exit_sector(exit->entry_sector.value())); + auto exit_sector_pos = exit_sector->get_position().to_tile(N); + auto exit_portal_pos = exit->portal->get_exit_center(exit->entry_sector.value()); + exit->heuristic_cost = Pathfinder::heuristic_cost( + exit_sector_pos + exit_portal_pos, + request.target); + } + + // update the cost knowledge + exit->current_cost = tentative_cost; + exit->future_cost = exit->current_cost + exit->heuristic_cost; + exit->prev_portal = current_node; + + if (not_visited) { + exit->heap_node = node_candidates.push(exit); + visited_portals.insert(exit->portal->get_id()); + } + else { + node_candidates.decrease(exit->heap_node); + } + } + } + } + + // no path found, return the closest node + auto backtrace = closest_node->generate_backtrace(); + for (auto &node : backtrace) { + result.push_back(node->portal); + } + + log::log(DBG << "Portal path not found."); + log::log(DBG << "Closest portal: " << closest_node->portal->get_id()); + return std::make_pair(PathResult::NOT_FOUND, result); +} + +template +const std::vector Pathfinder::get_waypoints(const std::vector>>> &flow_fields, + const PathRequest &request) const { + ENSURE(flow_fields.size() > 0, "At least 1 flow field is required for finding waypoints."); + + std::vector waypoints; + + auto grid = this->get_grid(request.grid_id); + coord::tile_t start_x = request.start.ne % N; + coord::tile_t start_y = request.start.se % N; + + bool los_reached = false; + + coord::tile_t current_x = start_x; + coord::tile_t current_y = start_y; + flow_dir_t current_direction = flow_fields.at(0).second->get_dir(current_x, current_y); + for (size_t i = 0; i < flow_fields.size(); ++i) { + auto §or = grid->get_sector(flow_fields[i].first); + auto sector_pos = sector->get_position().to_tile(N); + auto &flow_field = flow_fields[i].second; + + // navigate the flow field vectors until we reach its edge (or the target) + flow_t cell; + do { + cell = flow_field->get_cell(current_x, current_y); + + if (cell & FLOW_LOS_MASK) { + // check if we reached an LOS cell + auto cell_pos = sector_pos + coord::tile_delta(current_x, current_y); + waypoints.push_back(cell_pos); + los_reached = true; + break; + } + + // check if we need to change direction + auto cell_direction = static_cast(cell & FLOW_DIR_MASK); + if (cell_direction != current_direction) { + // add the current cell as a waypoint + auto cell_pos = sector_pos + coord::tile_delta(current_x, current_y); + waypoints.push_back(cell_pos); + current_direction = cell_direction; + } + + // move to the next cell + switch (current_direction) { + case flow_dir_t::NORTH: + current_y -= 1; + break; + case flow_dir_t::NORTH_EAST: + current_x += 1; + current_y -= 1; + break; + case flow_dir_t::EAST: + current_x += 1; + break; + case flow_dir_t::SOUTH_EAST: + current_x += 1; + current_y += 1; + break; + case flow_dir_t::SOUTH: + current_y += 1; + break; + case flow_dir_t::SOUTH_WEST: + current_x -= 1; + current_y += 1; + break; + case flow_dir_t::WEST: + current_x -= 1; + break; + case flow_dir_t::NORTH_WEST: + current_x -= 1; + current_y -= 1; + break; + default: + throw Error{ERR << "Invalid flow direction: " << static_cast(current_direction)}; + } + } + while (not(cell & FLOW_TARGET_MASK)); + + if (los_reached or i == flow_fields.size() - 1) { + // exit the loop if we found an LOS cell or reached + // the target cell in the last flow field + break; + } + + // reset the current position for the next flow field + switch (current_direction) { + case flow_dir_t::NORTH: + current_y = N - 1; + break; + case flow_dir_t::NORTH_EAST: + current_x = current_x + 1; + current_y = N - 1; + break; + case flow_dir_t::EAST: + current_x = 0; + break; + case flow_dir_t::SOUTH_EAST: + current_x = 0; + current_y = current_y + 1; + break; + case flow_dir_t::SOUTH: + current_y = 0; + break; + case flow_dir_t::SOUTH_WEST: + current_x = current_x - 1; + current_y = 0; + break; + case flow_dir_t::WEST: + current_x = N - 1; + break; + case flow_dir_t::NORTH_WEST: + current_x = N - 1; + current_y = current_y - 1; + break; + default: + throw Error{ERR << "Invalid flow direction: " << static_cast(current_direction)}; + } + } + + // add the target position as the last waypoint + waypoints.push_back(request.target); + + return waypoints; +} + +template +int Pathfinder::heuristic_cost(const coord::tile &portal_pos, + const coord::tile &target_pos) { + auto portal_phys_pos = portal_pos.to_phys2(); + auto target_phys_pos = target_pos.to_phys2(); + auto delta = target_phys_pos - portal_phys_pos; + + return delta.length(); +} + +template +const std::shared_ptr> &Pathfinder::get_grid(grid_id_t id) const { + return this->grids.at(id); +} + +template +void Pathfinder::add_grid(const std::shared_ptr> &grid) { + this->grids[grid->get_id()] = grid; +} } // namespace openage::path diff --git a/libopenage/pathfinding/portal.h b/libopenage/pathfinding/portal.h index 6593eea852..6e47967c76 100644 --- a/libopenage/pathfinding/portal.h +++ b/libopenage/pathfinding/portal.h @@ -11,6 +11,8 @@ namespace openage::path { + +template class CostField; /** diff --git a/libopenage/pathfinding/portal_node.cpp b/libopenage/pathfinding/portal_node.cpp new file mode 100644 index 0000000000..78f8d211c7 --- /dev/null +++ b/libopenage/pathfinding/portal_node.cpp @@ -0,0 +1,88 @@ +// Copyright 2025-2025 the openage authors. See copying.md for legal info. + +#include "coord/phys.h" + +#include "portal_node.h" + + +namespace openage::path { + +PortalNode::PortalNode(const std::shared_ptr &portal) : + portal{portal}, + entry_sector{std::nullopt}, + future_cost{std::numeric_limits::max()}, + current_cost{std::numeric_limits::max()}, + heuristic_cost{std::numeric_limits::max()}, + was_best{false}, + prev_portal{nullptr}, + heap_node{nullptr} {} + +PortalNode::PortalNode(const std::shared_ptr &portal, + sector_id_t entry_sector, + const node_t &prev_portal) : + portal{portal}, + entry_sector{entry_sector}, + future_cost{std::numeric_limits::max()}, + current_cost{std::numeric_limits::max()}, + heuristic_cost{std::numeric_limits::max()}, + was_best{false}, + prev_portal{prev_portal}, + heap_node{nullptr} {} + +PortalNode::PortalNode(const std::shared_ptr &portal, + sector_id_t entry_sector, + const node_t &prev_portal, + int past_cost, + int heuristic_cost) : + portal{portal}, + entry_sector{entry_sector}, + future_cost{past_cost + heuristic_cost}, + current_cost{past_cost}, + heuristic_cost{heuristic_cost}, + was_best{false}, + prev_portal{prev_portal}, + heap_node{nullptr} { +} + +bool PortalNode::operator<(const PortalNode &other) const { + return this->future_cost < other.future_cost; +} + +bool PortalNode::operator==(const PortalNode &other) const { + return this->portal->get_id() == other.portal->get_id(); +} + +std::vector PortalNode::generate_backtrace() { + std::vector waypoints; + + node_t current = this->shared_from_this(); + do { + waypoints.push_back(current); + current = current->prev_portal; + } + while (current != nullptr); + + return waypoints; +} + +const PortalNode::exits_t &PortalNode::get_exits(sector_id_t entry_sector) { + ENSURE(entry_sector == this->node_sector_0 || entry_sector == this->node_sector_1, "Invalid entry sector"); + + if (this->node_sector_0 == entry_sector) { + return exits_1; + } + else { + return exits_0; + } +} + +bool PortalNode::compare_node_cost::operator()(const PortalNode::node_t &lhs, const PortalNode::node_t &rhs) const { + return *lhs < *rhs; +} + +int get_distance_cost(const coord::tile_delta &portal1_pos, const coord::tile_delta &portal2_pos) { + auto delta = portal2_pos.to_phys2() - portal1_pos.to_phys2(); + return delta.length(); +} + +} diff --git a/libopenage/pathfinding/portal_node.h b/libopenage/pathfinding/portal_node.h new file mode 100644 index 0000000000..bdfcca3dee --- /dev/null +++ b/libopenage/pathfinding/portal_node.h @@ -0,0 +1,191 @@ +// Copyright 2025-2025 the openage authors. See copying.md for legal info. + +#pragma once + + +#include +#include +#include + +#include "pathfinding/portal.h" +#include "pathfinding/types.h" + +#include "coord/tile.h" +#include "datastructure/pairing_heap.h" + +namespace openage::path { + +class Portal; + +class PortalNode; + +/** + * One navigation waypoint in a path. + */ +class PortalNode : public std::enable_shared_from_this { +public: + + using node_t = std::shared_ptr; + + /** + * Cost comparison for node_t on the pairing heap. + * + * Extracts the nodes from the shared_ptr and compares them. We have + * to use a custom comparison function because otherwise the shared_ptr + * would be compared instead of the actual node. + */ + struct compare_node_cost { + bool operator()(const PortalNode::node_t &lhs, const PortalNode::node_t &rhs) const; + }; + + using heap_t = datastructure::PairingHeap; + + using nodemap_t = std::unordered_map; + + PortalNode(const std::shared_ptr &portal); + + PortalNode(const std::shared_ptr &portal, + sector_id_t entry_sector, + const node_t &prev_portal); + + PortalNode(const std::shared_ptr &portal, + sector_id_t entry_sector, + const node_t &prev_portal, + int past_cost, + int heuristic_cost); + + /** + * Orders nodes according to their future cost value. + */ + bool operator<(const PortalNode &other) const; + + /** + * Compare the node to another one. + * They are the same if their portal is. + */ + bool operator==(const PortalNode &other) const; + + /** + * Calculates the actual movement cose to another node. + */ + int cost_to(const PortalNode &other) const; + + /** + * Create a backtrace path beginning at this node. + */ + std::vector generate_backtrace(); + + /** + * init PortalNode::exits. + */ + template + void init_exits(const nodemap_t &node_map); + + + /** + * maps node_t of a neigbhour portal to the distance cost to travel between the portals + */ + using exits_t = std::map; + + + /** + * Get the exit portals reachable via the portal when entering from a specified sector. + * + * @param entry_sector Sector from which the portal is entered. + * + * @return Exit portals nodes reachable from the portal. + */ + const exits_t &get_exits(sector_id_t entry_sector); + + /** + * The portal this node is associated to. + */ + std::shared_ptr portal; + + /** + * Sector where the portal is entered. + */ + std::optional entry_sector; + + /** + * Future cost estimation value for this node. + */ + int future_cost; + + /** + * Evaluated past cost value for the node. + * This stores the actual cost from start to this node. + */ + int current_cost; + + /** + * Heuristic cost cache. + * Calculated once, is the heuristic distance from this node + * to the goal. + */ + int heuristic_cost; + + /** + * Does this node already have an alternative path? + * If the node was once selected as the best next hop, + * this is set to true. + */ + bool was_best = false; + + /** + * Node where this one was reached by least cost. + */ + node_t prev_portal; + + /** + * Priority queue node that contains this path node. + */ + heap_t::element_t heap_node; + + /** + * First sector connected by the portal. + */ + sector_id_t node_sector_0; + + /** + * Second sector connected by the portal. + */ + sector_id_t node_sector_1; + + /** + * Exits in sector 0 reachable from the portal. + */ + exits_t exits_0; + + /** + * Exits in sector 1 reachable from the portal. + */ + exits_t exits_1; +}; + +int get_distance_cost(const coord::tile_delta &portal1_pos, const coord::tile_delta &portal2_pos); + +template +void PortalNode::init_exits(const nodemap_t &node_map) { + auto exits = this->portal->get_exits(this->node_sector_0); + for (auto &exit : exits) { + int distance_cost = get_distance_cost( + this->portal->get_exit_center(this->node_sector_0), + exit->get_entry_center(this->node_sector_1)); + + auto exit_node = node_map.at(exit->get_id()); + this->exits_1[exit_node] = distance_cost; + } + + exits = this->portal->get_exits(this->node_sector_1); + for (auto &exit : exits) { + int distance_cost = get_distance_cost( + this->portal->get_exit_center(this->node_sector_1), + exit->get_entry_center(this->node_sector_0)); + + auto exit_node = node_map.at(exit->get_id()); + this->exits_0[exit_node] = distance_cost; + } +} + +} // namespace openage::path \ No newline at end of file diff --git a/libopenage/pathfinding/sector.cpp b/libopenage/pathfinding/sector.cpp index 5897bbef84..9feb85e940 100644 --- a/libopenage/pathfinding/sector.cpp +++ b/libopenage/pathfinding/sector.cpp @@ -2,251 +2,6 @@ #include "sector.h" -#include -#include - -#include "error/error.h" - -#include "coord/tile.h" -#include "pathfinding/cost_field.h" -#include "pathfinding/definitions.h" - - namespace openage::path { -Sector::Sector(sector_id_t id, const coord::chunk &position, size_t field_size) : - id{id}, - position{position}, - cost_field{std::make_shared(field_size)} { -} - -Sector::Sector(sector_id_t id, const coord::chunk &position, const std::shared_ptr &cost_field) : - id{id}, - position{position}, - cost_field{cost_field} { -} - -const sector_id_t &Sector::get_id() const { - return this->id; -} - -const coord::chunk &Sector::get_position() const { - return this->position; -} - -const std::shared_ptr &Sector::get_cost_field() const { - return this->cost_field; -} - -const std::vector> &Sector::get_portals() const { - return this->portals; -} - -void Sector::add_portal(const std::shared_ptr &portal) { - this->portals.push_back(portal); -} - -std::vector> Sector::find_portals(const std::shared_ptr &other, - PortalDirection direction, - portal_id_t next_id) const { - ENSURE(this->cost_field->get_size() == other->get_cost_field()->get_size(), "Sector size mismatch"); - - std::vector> result; - - // cost field of the other sector - auto other_cost = other->get_cost_field(); - - // compare the edges of the sectors - size_t start = 0; - size_t end = 0; - bool passable_edge = false; - for (size_t i = 0; i < this->cost_field->get_size(); ++i) { - auto coord_this = coord::tile_delta{0, 0}; - auto coord_other = coord::tile_delta{0, 0}; - if (direction == PortalDirection::NORTH_SOUTH) { - // right edge; top to bottom - coord_this = coord::tile_delta(i, this->cost_field->get_size() - 1); - coord_other = coord::tile_delta(i, 0); - } - else if (direction == PortalDirection::EAST_WEST) { - // bottom edge; east to west - coord_this = coord::tile_delta(this->cost_field->get_size() - 1, i); - coord_other = coord::tile_delta(0, i); - } - - if (this->cost_field->get_cost(coord_this) != COST_IMPASSABLE - and other_cost->get_cost(coord_other) != COST_IMPASSABLE) { - // both sides of the edge are passable - if (not passable_edge) { - // start a new portal - start = i; - passable_edge = true; - } - // else: we already started a portal - - end = i; - if (i != this->cost_field->get_size() - 1) { - // continue to next tile unless we are at the last tile - // then we have to end the current portal - continue; - } - } - - if (passable_edge) { - // create a new portal - auto coord_start = coord::tile_delta{0, 0}; - auto coord_end = coord::tile_delta{0, 0}; - if (direction == PortalDirection::NORTH_SOUTH) { - // right edge; top to bottom - coord_start = coord::tile_delta(start, this->cost_field->get_size() - 1); - coord_end = coord::tile_delta(end, this->cost_field->get_size() - 1); - } - else if (direction == PortalDirection::EAST_WEST) { - // bottom edge; east to west - coord_start = coord::tile_delta(this->cost_field->get_size() - 1, start); - coord_end = coord::tile_delta(this->cost_field->get_size() - 1, end); - } - - result.push_back( - std::make_shared( - next_id, - this->id, - other->get_id(), - direction, - coord_start, - coord_end)); - passable_edge = false; - next_id += 1; - } - } - - return result; -} - -void Sector::connect_exits() { - if (this->portals.empty()) { - return; - } - - std::unordered_set portal_ids; - for (const auto &portal : this->portals) { - portal_ids.insert(portal->get_id()); - } - - // check all portals in the sector - std::vector> search_portals = this->portals; - while (not portal_ids.empty()) { - auto portal = search_portals.back(); - search_portals.pop_back(); - portal_ids.erase(portal->get_id()); - - auto start = portal->get_entry_start(this->id); - auto end = portal->get_entry_end(this->id); - - std::unordered_set visited; - std::deque open_list; - std::vector neighbors; - neighbors.reserve(4); - - if (portal->get_direction() == PortalDirection::NORTH_SOUTH) { - // right edge; top to bottom - for (auto i = start.se; i <= end.se; ++i) { - open_list.push_back(start.ne + i * this->cost_field->get_size()); - } - } - else if (portal->get_direction() == PortalDirection::EAST_WEST) { - // bottom edge; east to west - for (auto i = start.ne; i <= end.ne; ++i) { - open_list.push_back(i + start.se * this->cost_field->get_size()); - } - } - - // flood fill the grid to find connected portals - while (not open_list.empty()) { - auto current = open_list.front(); - open_list.pop_front(); - - if (visited.contains(current)) { - continue; - } - - // Get the x and y coordinates of the current cell - auto x = current % this->cost_field->get_size(); - auto y = current / this->cost_field->get_size(); - - // check the neighbors - if (y > 0) { - neighbors.push_back(current - this->cost_field->get_size()); - } - if (x > 0) { - neighbors.push_back(current - 1); - } - if (y < this->cost_field->get_size() - 1) { - neighbors.push_back(current + this->cost_field->get_size()); - } - if (x < this->cost_field->get_size() - 1) { - neighbors.push_back(current + 1); - } - - // add the neighbors to the open list - for (const auto &neighbor : neighbors) { - if (this->cost_field->get_cost(neighbor) != COST_IMPASSABLE) { - open_list.push_back(neighbor); - } - } - neighbors.clear(); - - // mark the current cell as visited - // TODO: Record the cost of reaching this cell - visited.insert(current); - } - - // check if the visited cells are connected to another portal - std::vector> connected_portals; - for (auto &exit : this->portals) { - if (exit->get_id() == portal->get_id()) { - // skip the current portal - continue; - } - - // get the start cell of the exit portal - // we only have to check one cell since the flood fill - // should reach any exit cell - auto exit_start = exit->get_entry_start(this->id); - auto exit_cell = exit_start.ne + exit_start.se * this->cost_field->get_size(); - - // check if the exit cell is connected to the visited cells - if (visited.contains(exit_cell)) { - connected_portals.push_back(exit); - } - } - - // set the exits for the current portal - portal->set_exits(this->id, connected_portals); - - // All connected portals share the same exits - // so we can connect them here - for (auto &connected : connected_portals) { - // make a new vector with all connected portals except the current one - std::vector> other_connected; - for (auto &other : connected_portals) { - if (other->get_id() != connected->get_id()) { - other_connected.push_back(other); - } - } - - // add the original portal as it is not in the connected portals vector - other_connected.push_back(portal); - - // set the exits for the connected portal - connected->set_exits(this->id, other_connected); - - // we don't need to food fill for this portal since we have - // found all exits, so we can remove it from the portals that - // should be searched - portal_ids.erase(connected->get_id()); - } - } -} - } // namespace openage::path diff --git a/libopenage/pathfinding/sector.h b/libopenage/pathfinding/sector.h index d684a399b6..77ce72316f 100644 --- a/libopenage/pathfinding/sector.h +++ b/libopenage/pathfinding/sector.h @@ -3,16 +3,27 @@ #pragma once #include +#include #include +#include #include #include "coord/chunk.h" +#include "coord/tile.h" + +#include "pathfinding/cost_field.h" +#include "pathfinding/definitions.h" #include "pathfinding/portal.h" #include "pathfinding/types.h" +#include "error/error.h" + namespace openage::path { + +template class CostField; + class Portal; /** @@ -21,6 +32,7 @@ class Portal; * Sectors consist of a cost field and a list of portals connecting them to adjacent * sectors. */ +template class Sector { public: /** @@ -31,8 +43,7 @@ class Sector { * @param field_size Size of the cost field. */ Sector(sector_id_t id, - const coord::chunk &position, - size_t field_size); + const coord::chunk &position); /** * Create a new sector with a specified ID and an existing cost field. @@ -43,7 +54,7 @@ class Sector { */ Sector(sector_id_t id, const coord::chunk &position, - const std::shared_ptr &cost_field); + const std::shared_ptr> &cost_field); /** * Get the ID of this sector. @@ -66,7 +77,7 @@ class Sector { * * @return Cost field of this sector. */ - const std::shared_ptr &get_cost_field() const; + const std::shared_ptr> &get_cost_field() const; /** * Get the portals connecting this sector to other sectors. @@ -117,7 +128,7 @@ class Sector { /** * Cost field of the sector. */ - std::shared_ptr cost_field; + std::shared_ptr> cost_field; /** * Portals of the sector. @@ -126,4 +137,248 @@ class Sector { }; +template +Sector::Sector(sector_id_t id, const coord::chunk &position) : + id{id}, + position{position}, + cost_field{std::make_shared>()} { +} + +template +Sector::Sector(sector_id_t id, const coord::chunk &position, const std::shared_ptr> &cost_field) : + id{id}, + position{position}, + cost_field{cost_field} { +} + +template +const sector_id_t &Sector::get_id() const { + return this->id; +} + +template +const coord::chunk &Sector::get_position() const { + return this->position; +} + +template +const std::shared_ptr> &Sector::get_cost_field() const { + return this->cost_field; +} + +template +const std::vector> &Sector::get_portals() const { + return this->portals; +} + +template +void Sector::add_portal(const std::shared_ptr &portal) { + this->portals.push_back(portal); +} + +template +std::vector> Sector::find_portals(const std::shared_ptr &other, + PortalDirection direction, + portal_id_t next_id) const { + ENSURE(this->cost_field->get_size() == other->get_cost_field()->get_size(), "Sector size mismatch"); + + std::vector> result; + + // cost field of the other sector + auto other_cost = other->get_cost_field(); + + // compare the edges of the sectors + size_t start = 0; + size_t end = 0; + bool passable_edge = false; + for (size_t i = 0; i < this->cost_field->get_size(); ++i) { + auto coord_this = coord::tile_delta{0, 0}; + auto coord_other = coord::tile_delta{0, 0}; + if (direction == PortalDirection::NORTH_SOUTH) { + // right edge; top to bottom + coord_this = coord::tile_delta(i, this->cost_field->get_size() - 1); + coord_other = coord::tile_delta(i, 0); + } + else if (direction == PortalDirection::EAST_WEST) { + // bottom edge; east to west + coord_this = coord::tile_delta(this->cost_field->get_size() - 1, i); + coord_other = coord::tile_delta(0, i); + } + + if (this->cost_field->get_cost(coord_this) != COST_IMPASSABLE + and other_cost->get_cost(coord_other) != COST_IMPASSABLE) { + // both sides of the edge are passable + if (not passable_edge) { + // start a new portal + start = i; + passable_edge = true; + } + // else: we already started a portal + + end = i; + if (i != this->cost_field->get_size() - 1) { + // continue to next tile unless we are at the last tile + // then we have to end the current portal + continue; + } + } + + if (passable_edge) { + // create a new portal + auto coord_start = coord::tile_delta{0, 0}; + auto coord_end = coord::tile_delta{0, 0}; + if (direction == PortalDirection::NORTH_SOUTH) { + // right edge; top to bottom + coord_start = coord::tile_delta(start, this->cost_field->get_size() - 1); + coord_end = coord::tile_delta(end, this->cost_field->get_size() - 1); + } + else if (direction == PortalDirection::EAST_WEST) { + // bottom edge; east to west + coord_start = coord::tile_delta(this->cost_field->get_size() - 1, start); + coord_end = coord::tile_delta(this->cost_field->get_size() - 1, end); + } + + result.push_back( + std::make_shared( + next_id, + this->id, + other->get_id(), + direction, + coord_start, + coord_end)); + passable_edge = false; + next_id += 1; + } + } + + return result; +} + +template +void Sector::connect_exits() { + if (this->portals.empty()) { + return; + } + + std::unordered_set portal_ids; + for (const auto &portal : this->portals) { + portal_ids.insert(portal->get_id()); + } + + // check all portals in the sector + std::vector> search_portals = this->portals; + while (not portal_ids.empty()) { + auto portal = search_portals.back(); + search_portals.pop_back(); + portal_ids.erase(portal->get_id()); + + auto start = portal->get_entry_start(this->id); + auto end = portal->get_entry_end(this->id); + + std::unordered_set visited; + std::deque open_list; + std::vector neighbors; + neighbors.reserve(4); + + if (portal->get_direction() == PortalDirection::NORTH_SOUTH) { + // right edge; top to bottom + for (auto i = start.se; i <= end.se; ++i) { + open_list.push_back(start.ne + i * this->cost_field->get_size()); + } + } + else if (portal->get_direction() == PortalDirection::EAST_WEST) { + // bottom edge; east to west + for (auto i = start.ne; i <= end.ne; ++i) { + open_list.push_back(i + start.se * this->cost_field->get_size()); + } + } + + // flood fill the grid to find connected portals + while (not open_list.empty()) { + auto current = open_list.front(); + open_list.pop_front(); + + if (visited.contains(current)) { + continue; + } + + // Get the x and y coordinates of the current cell + auto x = current % this->cost_field->get_size(); + auto y = current / this->cost_field->get_size(); + + // check the neighbors + if (y > 0) { + neighbors.push_back(current - this->cost_field->get_size()); + } + if (x > 0) { + neighbors.push_back(current - 1); + } + if (y < this->cost_field->get_size() - 1) { + neighbors.push_back(current + this->cost_field->get_size()); + } + if (x < this->cost_field->get_size() - 1) { + neighbors.push_back(current + 1); + } + + // add the neighbors to the open list + for (const auto &neighbor : neighbors) { + if (this->cost_field->get_cost(neighbor) != COST_IMPASSABLE) { + open_list.push_back(neighbor); + } + } + neighbors.clear(); + + // mark the current cell as visited + // TODO: Record the cost of reaching this cell + visited.insert(current); + } + + // check if the visited cells are connected to another portal + std::vector> connected_portals; + for (auto &exit : this->portals) { + if (exit->get_id() == portal->get_id()) { + // skip the current portal + continue; + } + + // get the start cell of the exit portal + // we only have to check one cell since the flood fill + // should reach any exit cell + auto exit_start = exit->get_entry_start(this->id); + auto exit_cell = exit_start.ne + exit_start.se * this->cost_field->get_size(); + + // check if the exit cell is connected to the visited cells + if (visited.contains(exit_cell)) { + connected_portals.push_back(exit); + } + } + + // set the exits for the current portal + portal->set_exits(this->id, connected_portals); + + // All connected portals share the same exits + // so we can connect them here + for (auto &connected : connected_portals) { + // make a new vector with all connected portals except the current one + std::vector> other_connected; + for (auto &other : connected_portals) { + if (other->get_id() != connected->get_id()) { + other_connected.push_back(other); + } + } + + // add the original portal as it is not in the connected portals vector + other_connected.push_back(portal); + + // set the exits for the connected portal + connected->set_exits(this->id, other_connected); + + // we don't need to food fill for this portal since we have + // found all exits, so we can remove it from the portals that + // should be searched + portal_ids.erase(connected->get_id()); + } + } +} + } // namespace openage::path diff --git a/libopenage/pathfinding/tests.cpp b/libopenage/pathfinding/tests.cpp index 2ac0960c34..62c81ad677 100644 --- a/libopenage/pathfinding/tests.cpp +++ b/libopenage/pathfinding/tests.cpp @@ -17,9 +17,14 @@ namespace openage { namespace path { namespace tests { +/** + * size of sectors. + */ +static constexpr size_t SECTOR_SIZE = 3; + void flow_field() { // Create initial cost grid - auto cost_field = std::make_shared(3); + auto cost_field = std::make_shared>(); // | 1 | 1 | 1 | // | 1 | X | 1 | @@ -29,7 +34,7 @@ void flow_field() { // Test the different field types { - auto integration_field = std::make_shared(3); + auto integration_field = std::make_shared>(); integration_field->integrate_cost(cost_field, coord::tile_delta{2, 2}); auto &int_cells = integration_field->get_cells(); @@ -71,7 +76,7 @@ void flow_field() { } // Build the flow field - auto flow_field = std::make_shared(3); + auto flow_field = std::make_shared>(); flow_field->build(integration_field); auto ff_cells = flow_field->get_cells(); @@ -84,7 +89,7 @@ void flow_field() { // Integrator test { // Integrator for managing the flow field - auto integrator = std::make_shared(); + auto integrator = std::make_shared>(); // Build the flow field auto flow_field = integrator->get(cost_field, coord::tile_delta{2, 2}).second; diff --git a/libopenage/pathfinding/types.h b/libopenage/pathfinding/types.h index 3229010a91..008bfd490b 100644 --- a/libopenage/pathfinding/types.h +++ b/libopenage/pathfinding/types.h @@ -1,4 +1,4 @@ -// Copyright 2024-2024 the openage authors. See copying.md for legal info. +// Copyright 2024-2025 the openage authors. See copying.md for legal info. #pragma once @@ -7,6 +7,7 @@ #include #include +#include "time/time.h" namespace openage::path { @@ -111,7 +112,10 @@ using sector_id_t = size_t; */ using portal_id_t = size_t; +template class FlowField; + +template class IntegrationField; /** @@ -122,6 +126,26 @@ using cache_key_t = std::pair; /** * Returnable field cache entry pair containing an integration field and a flow field. */ -using field_cache_t = std::pair, std::shared_ptr>; +template +using field_cache_t = std::pair>, std::shared_ptr>>; + +/** + * Cost stamp for a cell on a cost field. + * + * Stamps are used when cell costs are temporarily overwritten, e.g. when placing a game object + * as an obstacle over terrain. Once the obstacle is removed, the cell can be reset to its original + * value recorded by the stamp. + */ +struct cost_stamp_t { + /** + * Original cost of the stamped cell. + */ + cost_t original_cost; + + /** + * Time the cost field cell was stamped. + */ + time::time_t stamp_time; +}; } // namespace openage::path