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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
|
#include "path-search.hpp"
#include "object.hpp"
#include "compat/math.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));
static constexpr auto eps = (uint32_t)math::ceil(math::sqrt((Vector2(div_size)/4).product()));
static_assert(eps > 1 && eps < TILE_SIZE2.x());
const auto pos0 = Vector2(from.local()) * TILE_SIZE2;
const auto start_bbox = bbox<float>{pos0 - Vector2(own_size/2), pos0 + Vector2(own_size)};
const auto from_offset_f = Vector2(from_offset);
const auto from_offset_len = Math::max(eps, (uint32_t)Math::ceil(from_offset_f.length()));
struct tuple
{
bbox<float> bbox;
uint32_t dist;
};
constexpr auto get_bbox = [](chunk_coords_ ch_1, local_coords pos1, Vector2b off1,
chunk_coords_ ch_2, local_coords pos2, Vector2b off2,
Vector2ub size, uint32_t dist0) -> tuple
{
constexpr auto chunk_size = iTILE_SIZE2 * TILE_MAX_DIM;
auto c = (Vector2i(ch_2.x, ch_2.y) - Vector2i(ch_1.x, ch_1.y)) * chunk_size;
auto t = (Vector2i(pos2) - Vector2i(pos1)) * iTILE_SIZE2;
auto o = Vector2i(off2) - Vector2i(off1);
auto cto = Vector2i(c + t + o);
auto dist = Math::max(1u, (uint32_t)Math::ceil(Vector2(cto).length()));
auto center0 = Vector2i(pos1) * iTILE_SIZE2 + Vector2i(off1);
auto min0 = center0 - Vector2i(size/2), max0 = min0 + Vector2i(size);
auto min1 = min0 + cto, max1 = max0 + cto;
fm_debug_assert(dist > eps);
return {
{ .min = Vector2(Math::min(min0, min1)),
.max = Vector2(Math::max(max0, max1)) },
dist0 + dist,
};
};
path_search_result result;
fm_debug_assert(result._node); // todo
auto& path = result._node->vec; path.clear();
constexpr auto div = Vector2i(subdivide_factor);
constexpr auto div_size = Vector2i(iTILE_SIZE2 / div);
constexpr auto tile_start = Vector2i(iTILE_SIZE2/-2);
astar.push(astar_edge{from, from_offset, from, from_offset}, 0);
const auto ch0 = chunk_coords_{from};
const auto bb0 = bbox_union(start_bbox, Vector2i(from.local()), {}, own_size);
if (from_offset_len >= eps && is_passable(w, ch0, bb0, own_id, p))
astar.push({from, from_offset, from, from_offset}, from_offset_len);
while (!astar.empty())
{
auto [prev, dist0] = astar.pop();
}
// 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
|
