1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
|
#include "path-search.hpp"
#include "compat/math.hpp"
#include "object.hpp"
#include <Corrade/Containers/PairStl.h>
#include <Magnum/Math/Vector2.h>
#include <Magnum/Math/Functions.h>
namespace floormat {
path_search_result path_search::Dijkstra(world& w, Vector2ub own_size, object_id own_id,
global_coords from, Vector2b from_offset,
global_coords to, Vector2b to_offset,
const pred& p)
{
fm_assert(from.x <= to.x && from.y <= to.y);
own_size = Math::max(own_size, Vector2ub(min_size));
if (from.z() != to.z()) [[unlikely]]
return {};
// todo try removing this eventually
if (from.z() != 0) [[unlikely]]
return {};
if (!is_passable(w, from, from_offset, own_size, own_id, p))
return {};
if (!is_passable(w, to, to_offset, own_size, own_id, p))
return {};
astar.clear();
astar.reserve(TILE_COUNT*(size_t)(subdivide_factor*subdivide_factor));
const auto pos0 = Vector2(from.local()) * TILE_SIZE2;
const auto start_bbox = bbox<float>{pos0 - Vector2(own_size/2), pos0 + Vector2(own_size)};
constexpr auto div = Vector2ub{subdivide_factor};
constexpr auto sub_size = Vector2i(Vector2ub(iTILE_SIZE2) / div);
constexpr auto sqrt_2 = (float)math::sqrt(2);
constexpr Vector2i directions[8] = {
iTILE_SIZE2 * Vector2i{ -1, 0 },
iTILE_SIZE2 * Vector2i{ 0, -1 },
iTILE_SIZE2 * Vector2i{ 1, 0 },
iTILE_SIZE2 * Vector2i{ 0, 1 },
iTILE_SIZE2 * Vector2i{ 1, 1 },
iTILE_SIZE2 * Vector2i{ -1, -1 },
iTILE_SIZE2 * Vector2i{ -1, 1 },
iTILE_SIZE2 * Vector2i{ 1, -1 },
};
const auto bb0 = bbox_union(start_bbox, Vector2i(from.local()), {}, own_size);
//if (is_passable(w, chunk_coords_{from}, bb0.min, bb0.max, own_id, p))
// push_heap(_astar.Q, {from, from_offset, from, {}, 0});
path_search_result result;
fm_debug_assert(result._node); // todo
auto& path = result._node->vec; path.clear();
{
auto [pos2, _offset2] = object::normalize_coords(from, from_offset, {});
}
for (const auto& dir : directions)
{
auto pos = object::normalize_coords(from, from_offset, dir);
}
while (!astar.empty())
{
}
auto [cmin, cmax] = Math::minmax(Vector2i(from.chunk()) - Vector2i(1, 1),
Vector2i(to.chunk()) + Vector2i(1, 1));
// todo...
return result;
}
path_search_result path_search::Dijkstra(world& w, const object& obj,
global_coords to, Vector2b to_offset,
const pred& p)
{
constexpr auto full_tile = Vector2ub(iTILE_SIZE2*3/4);
auto size = Math::max(obj.bbox_size, full_tile);
// todo fixme
// if bbox_offset is added to obj's offset, then all coordinates in the paths are shifted by bbox_offset.
// maybe add handling to subtract bbox_offset from the returned path.
// for that it needs to be passed into callees separately.
fm_assert(obj.bbox_offset.isZero());
return Dijkstra(w, size, obj.id, obj.coord, obj.offset, to, to_offset, p);
}
} // namespace floormat
|
