From c2b824687d5e18028de5b71d71cf5be478bf838e Mon Sep 17 00:00:00 2001 From: Shiqing Date: Tue, 16 Jul 2019 01:01:50 +0800 Subject: [PATCH] Reduce memory usage for edges in A* and add tests --- core/math/a_star.cpp | 75 ++++++++++++--------- core/math/a_star.h | 29 +++++++-- main/tests/test_astar.cpp | 134 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 202 insertions(+), 36 deletions(-) diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp index 482d7d8cd5..b390bdd976 100644 --- a/core/math/a_star.cpp +++ b/core/math/a_star.cpp @@ -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::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::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 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::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::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); diff --git a/core/math/a_star.h b/core/math/a_star.h index 94e42483f1..dba89f1db2 100644 --- a/core/math/a_star.h +++ b/core/math/a_star.h @@ -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; + } } }; diff --git a/main/tests/test_astar.cpp b/main/tests/test_astar.cpp index d34ff0d95e..b2002cd5d9 100644 --- a/main/tests/test_astar.cpp +++ b/main/tests/test_astar.cpp @@ -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 };