Reduce memory usage for edges in A* and add tests
This commit is contained in:
Shiqing
parent
add0004a78
commit
c2b824687d
|
|
@ -164,22 +164,24 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) {
|
|||
}
|
||||
|
||||
Segment s(p_id, p_with_id);
|
||||
s.from_point = a;
|
||||
s.to_point = b;
|
||||
segments.insert(s);
|
||||
if (bidirectional) s.direction = Segment::BIDIRECTIONAL;
|
||||
|
||||
if (bidirectional) {
|
||||
SWAP(s.from, s.to);
|
||||
SWAP(s.from_point, s.to_point);
|
||||
segments.insert(s);
|
||||
Set<Segment>::Element *element = segments.find(s);
|
||||
if (element != NULL) {
|
||||
s.direction |= element->get().direction;
|
||||
if (s.direction == Segment::BIDIRECTIONAL) {
|
||||
// Both are neighbours of each other now
|
||||
a->unlinked_neighbours.remove(b->id);
|
||||
b->unlinked_neighbours.remove(a->id);
|
||||
}
|
||||
segments.erase(element);
|
||||
}
|
||||
|
||||
segments.insert(s);
|
||||
}
|
||||
|
||||
void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) {
|
||||
|
||||
Segment s(p_id, p_with_id);
|
||||
Segment t(p_with_id, p_id);
|
||||
|
||||
Point *a;
|
||||
bool a_exists = points.lookup(p_id, a);
|
||||
CRASH_COND(!a_exists);
|
||||
|
|
@ -188,23 +190,32 @@ void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) {
|
|||
bool b_exists = points.lookup(p_with_id, b);
|
||||
CRASH_COND(!b_exists);
|
||||
|
||||
bool warned = false;
|
||||
Segment s(p_id, p_with_id);
|
||||
int remove_direction = bidirectional ? (int)Segment::BIDIRECTIONAL : s.direction;
|
||||
|
||||
Set<Segment>::Element *element = segments.find(s);
|
||||
if (element != NULL) {
|
||||
// s is the new segment
|
||||
// Erase the directions to be removed
|
||||
s.direction = (element->get().direction & ~remove_direction);
|
||||
|
||||
if (segments.has(s)) {
|
||||
segments.erase(s);
|
||||
a->neighbours.remove(b->id);
|
||||
b->unlinked_neighbours.remove(a->id);
|
||||
} else {
|
||||
warned = true;
|
||||
WARN_PRINT("The edge to be removed does not exist.");
|
||||
}
|
||||
if (bidirectional) {
|
||||
b->neighbours.remove(a->id);
|
||||
if (element->get().direction != Segment::BIDIRECTIONAL) {
|
||||
a->unlinked_neighbours.remove(b->id);
|
||||
b->unlinked_neighbours.remove(a->id);
|
||||
}
|
||||
} else {
|
||||
if (s.direction == Segment::NONE)
|
||||
b->unlinked_neighbours.remove(a->id);
|
||||
else
|
||||
a->unlinked_neighbours.set(b->id, b);
|
||||
}
|
||||
|
||||
if (bidirectional && segments.has(t)) {
|
||||
segments.erase(t);
|
||||
b->neighbours.remove(a->id);
|
||||
a->unlinked_neighbours.remove(b->id);
|
||||
} else if (bidirectional && !warned) {
|
||||
WARN_PRINT("The reverse edge to be removed does not exist.");
|
||||
segments.erase(element);
|
||||
if (s.direction != Segment::NONE)
|
||||
segments.insert(s);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -242,8 +253,10 @@ PoolVector<int> AStar::get_point_connections(int p_id) {
|
|||
bool AStar::are_points_connected(int p_id, int p_with_id, bool bidirectional) const {
|
||||
|
||||
Segment s(p_id, p_with_id);
|
||||
Segment t(p_with_id, p_id);
|
||||
return segments.has(s) || (bidirectional && segments.has(t));
|
||||
const Set<Segment>::Element *element = segments.find(s);
|
||||
|
||||
return element != NULL &&
|
||||
(bidirectional || (element->get().direction & s.direction) == s.direction);
|
||||
}
|
||||
|
||||
void AStar::clear() {
|
||||
|
|
@ -297,13 +310,17 @@ Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const {
|
|||
|
||||
for (const Set<Segment>::Element *E = segments.front(); E; E = E->next()) {
|
||||
|
||||
if (!(E->get().from_point->enabled && E->get().to_point->enabled)) {
|
||||
Point *from_point = nullptr, *to_point = nullptr;
|
||||
points.lookup(E->get().u, from_point);
|
||||
points.lookup(E->get().v, to_point);
|
||||
|
||||
if (!(from_point->enabled && to_point->enabled)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Vector3 segment[2] = {
|
||||
E->get().from_point->pos,
|
||||
E->get().to_point->pos,
|
||||
from_point->pos,
|
||||
to_point->pos,
|
||||
};
|
||||
|
||||
Vector3 p = Geometry::get_closest_point_to_segment(p_point, segment);
|
||||
|
|
|
|||
|
|
@ -81,20 +81,35 @@ class AStar : public Reference {
|
|||
struct Segment {
|
||||
union {
|
||||
struct {
|
||||
int32_t from;
|
||||
int32_t to;
|
||||
int32_t u;
|
||||
int32_t v;
|
||||
};
|
||||
uint64_t key;
|
||||
};
|
||||
|
||||
Point *from_point;
|
||||
Point *to_point;
|
||||
enum {
|
||||
NONE = 0,
|
||||
FORWARD = 1,
|
||||
BACKWARD = 2,
|
||||
BIDIRECTIONAL = FORWARD | BACKWARD
|
||||
};
|
||||
unsigned char direction;
|
||||
|
||||
bool operator<(const Segment &p_s) const { return key < p_s.key; }
|
||||
Segment() { key = 0; }
|
||||
Segment() {
|
||||
key = 0;
|
||||
direction = NONE;
|
||||
}
|
||||
Segment(int p_from, int p_to) {
|
||||
from = p_from;
|
||||
to = p_to;
|
||||
if (p_from < p_to) {
|
||||
u = p_from;
|
||||
v = p_to;
|
||||
direction = FORWARD;
|
||||
} else {
|
||||
u = p_to;
|
||||
v = p_from;
|
||||
direction = BACKWARD;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -87,11 +87,145 @@ bool test_abcx() {
|
|||
return ok;
|
||||
}
|
||||
|
||||
bool test_add_remove() {
|
||||
AStar a;
|
||||
bool ok = true;
|
||||
|
||||
// Manual tests
|
||||
a.add_point(1, Vector3(0, 0, 0));
|
||||
a.add_point(2, Vector3(0, 1, 0));
|
||||
a.add_point(3, Vector3(1, 1, 0));
|
||||
a.add_point(4, Vector3(2, 0, 0));
|
||||
a.connect_points(1, 2, true);
|
||||
a.connect_points(1, 3, true);
|
||||
a.connect_points(1, 4, false);
|
||||
|
||||
ok = ok && (a.are_points_connected(2, 1) == true);
|
||||
ok = ok && (a.are_points_connected(4, 1) == true);
|
||||
ok = ok && (a.are_points_connected(2, 1, false) == true);
|
||||
ok = ok && (a.are_points_connected(4, 1, false) == false);
|
||||
|
||||
a.disconnect_points(1, 2, true);
|
||||
ok = ok && (a.get_point_connections(1).size() == 2); // 3, 4
|
||||
ok = ok && (a.get_point_connections(2).size() == 0);
|
||||
|
||||
a.disconnect_points(4, 1, false);
|
||||
ok = ok && (a.get_point_connections(1).size() == 2); // 3, 4
|
||||
ok = ok && (a.get_point_connections(4).size() == 0);
|
||||
|
||||
a.disconnect_points(4, 1, true);
|
||||
ok = ok && (a.get_point_connections(1).size() == 1); // 3
|
||||
ok = ok && (a.get_point_connections(4).size() == 0);
|
||||
|
||||
a.connect_points(2, 3, false);
|
||||
ok = ok && (a.get_point_connections(2).size() == 1); // 3
|
||||
ok = ok && (a.get_point_connections(3).size() == 1); // 1
|
||||
|
||||
a.connect_points(2, 3, true);
|
||||
ok = ok && (a.get_point_connections(2).size() == 1); // 3
|
||||
ok = ok && (a.get_point_connections(3).size() == 2); // 1, 2
|
||||
|
||||
a.disconnect_points(2, 3, false);
|
||||
ok = ok && (a.get_point_connections(2).size() == 0);
|
||||
ok = ok && (a.get_point_connections(3).size() == 2); // 1, 2
|
||||
|
||||
a.connect_points(4, 3, true);
|
||||
ok = ok && (a.get_point_connections(3).size() == 3); // 1, 2, 4
|
||||
ok = ok && (a.get_point_connections(4).size() == 1); // 3
|
||||
|
||||
a.disconnect_points(3, 4, false);
|
||||
ok = ok && (a.get_point_connections(3).size() == 2); // 1, 2
|
||||
ok = ok && (a.get_point_connections(4).size() == 1); // 3
|
||||
|
||||
a.remove_point(3);
|
||||
ok = ok && (a.get_point_connections(1).size() == 0);
|
||||
ok = ok && (a.get_point_connections(2).size() == 0);
|
||||
ok = ok && (a.get_point_connections(4).size() == 0);
|
||||
|
||||
a.add_point(0, Vector3(0, -1, 0));
|
||||
a.add_point(3, Vector3(2, 1, 0));
|
||||
// 0: (0, -1)
|
||||
// 1: (0, 0)
|
||||
// 2: (0, 1)
|
||||
// 3: (2, 1)
|
||||
// 4: (2, 0)
|
||||
|
||||
// Tests for get_closest_position_in_segment
|
||||
a.connect_points(2, 3);
|
||||
ok = ok && (a.get_closest_position_in_segment(Vector3(0.5, 0.5, 0)) == Vector3(0.5, 1, 0));
|
||||
|
||||
a.connect_points(3, 4);
|
||||
a.connect_points(0, 3);
|
||||
a.connect_points(1, 4);
|
||||
a.disconnect_points(1, 4, false);
|
||||
a.disconnect_points(4, 3, false);
|
||||
a.disconnect_points(3, 4, false);
|
||||
// Remaining edges: <2, 3>, <0, 3>, <1, 4> (directed)
|
||||
ok = ok && (a.get_closest_position_in_segment(Vector3(2, 0.5, 0)) == Vector3(1.75, 0.75, 0));
|
||||
ok = ok && (a.get_closest_position_in_segment(Vector3(-1, 0.2, 0)) == Vector3(0, 0, 0));
|
||||
ok = ok && (a.get_closest_position_in_segment(Vector3(3, 2, 0)) == Vector3(2, 1, 0));
|
||||
|
||||
int seed = 0;
|
||||
|
||||
// Random tests for connectivity checks
|
||||
for (int i = 0; i < 20000; i++) {
|
||||
seed = (seed * 1103515245 + 12345) & 0x7fffffff;
|
||||
int u = (seed / 5) % 5;
|
||||
int v = seed % 5;
|
||||
if (u == v) {
|
||||
i--;
|
||||
continue;
|
||||
}
|
||||
if (seed % 2 == 1) {
|
||||
// Add a (possibly existing) directed edge and confirm connectivity
|
||||
a.connect_points(u, v, false);
|
||||
ok = ok && (a.are_points_connected(u, v, false) == true);
|
||||
} else {
|
||||
// Remove a (possibly nonexistent) directed edge and confirm disconnectivity
|
||||
a.disconnect_points(u, v, false);
|
||||
ok = ok && (a.are_points_connected(u, v, false) == false);
|
||||
}
|
||||
}
|
||||
|
||||
// Random tests for point removal
|
||||
for (int i = 0; i < 20000; i++) {
|
||||
a.clear();
|
||||
for (int j = 0; j < 5; j++)
|
||||
a.add_point(j, Vector3(0, 0, 0));
|
||||
|
||||
// Add or remove random edges
|
||||
for (int j = 0; j < 10; j++) {
|
||||
seed = (seed * 1103515245 + 12345) & 0x7fffffff;
|
||||
int u = (seed / 5) % 5;
|
||||
int v = seed % 5;
|
||||
if (u == v) {
|
||||
j--;
|
||||
continue;
|
||||
}
|
||||
if (seed % 2 == 1)
|
||||
a.connect_points(u, v, false);
|
||||
else
|
||||
a.disconnect_points(u, v, false);
|
||||
}
|
||||
|
||||
// Remove point 0
|
||||
a.remove_point(0);
|
||||
// White box: this will check all edges remaining in the segments set
|
||||
for (int j = 1; j < 5; j++) {
|
||||
ok = ok && (a.are_points_connected(0, j, true) == false);
|
||||
}
|
||||
}
|
||||
|
||||
// It's been great work, cheers \(^ ^)/
|
||||
return ok;
|
||||
}
|
||||
|
||||
typedef bool (*TestFunc)(void);
|
||||
|
||||
TestFunc test_funcs[] = {
|
||||
test_abc,
|
||||
test_abcx,
|
||||
test_add_remove,
|
||||
NULL
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue