talezero/integration
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

PlaneMap::scan is now working, with unit tests.

Browse Source
This commit is contained in:
jyelon
2022-07-13 01:08:54 -04:00
parent d54d6e4433
commit 69efb12c26
6 changed files with 444 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

483
luprex/core/cpp/planemap.cpp
View File

@@ -121,6 +121,10 @@ static constexpr ChildBits child_bit(int i) {
return (uint64_t(1) << i);
}
static constexpr ChildBits child_bit(int x, int y, int z) {
return child_bit((x << 4) | (y << 2) | z);
}
static constexpr uint8_t node_get_level(NodeID node) {
return node >> 48;
}
@@ -155,14 +159,15 @@ static constexpr ChildBits node_childbit(NodeID node) {
return child_bit(node_childindex(node));
}
static constexpr NodeID node_nthchild(NodeID node, int i) {
static constexpr NodeID node_child(NodeID node, uint16_t x, uint16_t y, uint16_t z) {
int level = node_get_level(node);
uint16_t x = (i >> 4)&3;
uint16_t y = (i >> 2)&3;
uint16_t z = (i >> 0)&3;
return ((node & node_lxyz(0, 0xFFFF, 0xFFFF, 0xFFFF)) << 2) | node_lxyz(level - 1, x, y, z);
}
static constexpr NodeID node_nthchild(NodeID node, int i) {
return node_child(node, (i >> 4) & 3, (i >> 2) & 3, (i >> 0) & 3);
}
static void print_node_id(NodeID node, std::ostream *os) {
int level = node_get_level(node);
@@ -201,6 +206,16 @@ struct NodeInfo {
}
};
template <int LEVEL>
struct TreeLevel {
constexpr static int child() { return LEVEL - 1; }
};
template <>
struct TreeLevel<0> {
constexpr static int child() { return 0; }
};
// Class PlaneTree. Everything here is 'public', but this class
// is only visible inside this one C++ file.
class PlaneTree : public eng::opnew {
@@ -241,6 +256,27 @@ public:
int way_outside_bbox_;
public:
// The following state is initialized whenever we do a scan.
// It is only relevant during the scan.
const PlaneScan *scan_config_;
IdVector *scan_result_;
int scan_bbxlo_[9], scan_bbxhi_[9];
int scan_bbylo_[9], scan_bbyhi_[9];
int scan_bbzlo_[9], scan_bbzhi_[9];
public:
ChildBits get_internal_node(NodeID id) const {
auto iter = internal_nodes_.find(id);
if (iter == internal_nodes_.end()) return 0;
return iter->second;
}
PlaneItem *get_leaf_node(NodeID id) const {
auto iter = leaf_nodes_.find(id);
if (iter == leaf_nodes_.end()) return 0;
return iter->second;
}
// Untrack all planeitems. This is for unit testing and for destructors. We
// don't use PlaneItem::untrack, because that would create problems with
// removing items from a list while iterating over that list.
@@ -337,9 +373,10 @@ public:
uint8_t parentlevel = childlevel + 1;
NodeID parent = node_parent(child);
auto iter = internal_nodes_.find(parent);
assert(iter != internal_nodes_.end());
iter->second &= (~node_childbit(child));
if (iter->second == 0) {
internal_nodes_.erase(parent);
internal_nodes_.erase(iter);
if (parentlevel < 8) remove_child_from_childbits(parent);
}
}
@@ -385,64 +422,35 @@ public:
item->plane_ = tree->plane_;
}
// Scan a planetree.
void scan(const PlaneScan &scan, IdVector *result) {
// Convert the bounding box to integral coordinates.
NodeInfo bblo(scale_, scan.lo_.x, scan.lo_.y, scan.lo_.z);
NodeInfo bbhi(scale_, scan.hi_.x, scan.hi_.y, scan.hi_.z);
// NOT IMPLEMENTED YET
void print_indented_internal_node(NodeID node, ChildBits cb, std::ostream *os) {
int level = node_get_level(node);
int indent = 8 - level;
(*os) << "|";
for (int i = 0; i < indent; i++) (*os) << " ";
print_node_id(node, os);
if ((cb == 0) && (level != 8)) {
(*os) << " (invalid empty node)";
}
}
void print_planeitem_ids(PlaneItem *first, std::ostream *os) {
void print_indented_leaf_node(NodeID node, PlaneItem *first, std::ostream *os) {
(*os) << "| ";
print_node_id(node, os);
(*os) << " ";
util::IdVector ids;
if (first == nullptr) {
(*os) << "invalid null list";
return;
}
PlaneItem *pi = first;
while (true) {
PlaneItem *next = pi->next_;
ids.push_back(pi->id());
if (next == first) break;
pi = next;
}
collect_planeitem_ids(first, &ids);
std::sort(ids.begin(), ids.end());
if (ids.size() > 0) {
(*os) << ids[0];
}
for (int i = 1; i < int(ids.size()); i++) {
(*os) << "," << ids[i];
}
util::print_id_vector(ids, os);
}
void print_tree_r(NodeID node, std::ostream *os) {
int level = node_get_level(node);
int indent = 8 - level;
if (level == 0) {
(*os) << "| ";
print_node_id(node, os);
(*os) << " ";
auto iter = leaf_nodes_.find(node);
if (iter == leaf_nodes_.end()) {
(*os) << "no such leaf";
} else {
print_planeitem_ids(iter->second, os);
}
print_indented_leaf_node(node, get_leaf_node(node), os);
} else {
auto iter = internal_nodes_.find(node);
ChildBits cb = 0;
if (iter != internal_nodes_.end()) {
cb = iter->second;
}
ChildBits cb = get_internal_node(node);
if ((level & 1) == 0) {
(*os) << "|";
for (int i = 0; i < indent; i++) (*os) << " ";
print_node_id(node, os);
if ((cb == 0) && (level != 8)) {
(*os) << " (invalid empty node)";
}
print_indented_internal_node(node, cb, os);
}
for (int i = 0; i < 64; i++) {
if (cb & child_bit(i)) {
@@ -454,6 +462,174 @@ public:
}
}
void calculate_search_bboxes(const PlaneScan &scan) {
// Convert the scan's bounding box to integral coordinates.
NodeInfo bblo(scale_, scan.lo_.x, scan.lo_.y, scan.lo_.z);
NodeInfo bbhi(scale_, scan.hi_.x, scan.hi_.y, scan.hi_.z);
// Calculate the bounding box at each level of the tree.
NodeID ibblo = bblo.node;
NodeID ibbhi = bbhi.node;
for (int i = 0; i <= 8; i++) {
scan_bbxlo_[i] = node_get_x(ibblo);
scan_bbylo_[i] = node_get_y(ibblo);
scan_bbzlo_[i] = node_get_z(ibblo);
scan_bbxhi_[i] = node_get_x(ibbhi);
scan_bbyhi_[i] = node_get_y(ibbhi);
scan_bbzhi_[i] = node_get_z(ibbhi);
ibblo = node_parent(ibblo);
ibbhi = node_parent(ibbhi);
}
}
// Calculate the size of the search bboxes.
int64_t scan_xsize(int i) const { return 1 + scan_bbxhi_[i] - scan_bbxlo_[i]; }
int64_t scan_ysize(int i) const { return 1 + scan_bbyhi_[i] - scan_bbylo_[i]; }
int64_t scan_zsize(int i) const { return 1 + scan_bbzhi_[i] - scan_bbzlo_[i]; }
eng::string search_bboxes_debug_string(const PlaneScan &scan) {
calculate_search_bboxes(scan);
eng::ostringstream oss;
for (int i = 8; i >= 0; i -= 2) {
auto fmt = util::hex.width((8 - i) / 2);
oss << "|Level " << i << " ";
oss << fmt.val(scan_bbxlo_[i]) << ","
<< fmt.val(scan_bbylo_[i]) << ","
<< fmt.val(scan_bbzlo_[i]) << " - "
<< fmt.val(scan_bbxhi_[i]) << ","
<< fmt.val(scan_bbyhi_[i]) << ","
<< fmt.val(scan_bbzhi_[i]);
}
return oss.str();
}
static inline void scan_push_id(int64_t id, int64_t special, IdVector *result) {
if (id != special) {
result->push_back(id);
}
}
static void scan_planeitem(PlaneItem *pi, const PlaneScan &scan, IdVector *result) {
switch (scan.shape_) {
case PlaneScan::BOX: {
if ((pi->x() >= scan.lo_.x) && (pi->x() <= scan.hi_.x) &&
(pi->y() >= scan.lo_.y) && (pi->y() <= scan.hi_.y) &&
(pi->z() >= scan.lo_.z) && (pi->z() <= scan.hi_.z)) {
scan_push_id(pi->id(), scan.special_, result);
}
break;
}
case PlaneScan::SPHERE: {
float dx = (pi->x() - scan.center_.x) * scan.invradius_.x;
float dy = (pi->y() - scan.center_.y) * scan.invradius_.y;
float dz = (pi->z() - scan.center_.z) * scan.invradius_.z;
if (dx*dx + dy*dy + dz*dz <= 1.0) {
scan_push_id(pi->id(), scan.special_, result);
}
break;
}
case PlaneScan::CYLINDER: {
if ((pi->z() >= scan.lo_.z) && (pi->z() <= scan.hi_.z)) {
float dx = (pi->x() - scan.center_.x) * scan.invradius_.x;
float dy = (pi->y() - scan.center_.y) * scan.invradius_.y;
if (dx*dx + dy*dy <= 1.0) {
scan_push_id(pi->id(), scan.special_, result);
}
}
break;
}
}
}
// Recursive part of the planetree scan.
// Note: template expansion terminates because
// TreeLevel<0>::child returns zero again.
template <int LEVEL>
void scan_node(NodeID node, std::ostream *debug) {
if (LEVEL == 0) {
auto iter = leaf_nodes_.find(node);
assert (iter != leaf_nodes_.end());
PlaneItem *first = iter->second;
assert(first != nullptr);
if (debug != nullptr) {
print_indented_leaf_node(node, first, debug);
}
PlaneItem *pi = first;
while (true) {
PlaneItem *next = pi->next_;
scan_planeitem(pi, *scan_config_, scan_result_);
if (next == first) break;
pi = next;
}
} else {
constexpr int CHILDLEVEL = TreeLevel<LEVEL>::child();
NodeID firstchild = node_nthchild(node, 0);
int xlo = std::max(0, scan_bbxlo_[CHILDLEVEL] - int(node_get_x(firstchild)));
int ylo = std::max(0, scan_bbylo_[CHILDLEVEL] - int(node_get_y(firstchild)));
int zlo = std::max(0, scan_bbzlo_[CHILDLEVEL] - int(node_get_z(firstchild)));
int xhi = std::min(3, scan_bbxhi_[CHILDLEVEL] - int(node_get_x(firstchild)));
int yhi = std::min(3, scan_bbyhi_[CHILDLEVEL] - int(node_get_y(firstchild)));
int zhi = std::min(3, scan_bbzhi_[CHILDLEVEL] - int(node_get_z(firstchild)));
auto iter = internal_nodes_.find(node);
assert (iter != internal_nodes_.end());
ChildBits cb = iter->second;
assert (cb != 0);
if (debug != nullptr) {
print_indented_internal_node(node, cb, debug);
}
for (int x = xlo; x <= xhi; x++) {
for (int y = ylo; y <= yhi; y++) {
for (int z = zlo; z <= zhi; z++) {
if (cb & child_bit(x, y, z)) {
NodeID child = node_child(node, x, y, z);
scan_node<CHILDLEVEL>(child, debug);
}
}
}
}
}
}
// Scan a planetree.
void scan(const PlaneScan &sc, IdVector *result, std::ostream *debug) {
scan_config_ = &sc;
scan_result_ = result;
calculate_search_bboxes(sc);
// We must only call 'scan_node' on nodes that actually exist.
// So we check if the tree is empty, and if so, we don't scan the
// root node.
if (!internal_nodes_.empty()) {
NodeID root = node_lxyz(8, 0, 0, 0);
scan_node<8>(root, debug);
}
}
eng::string scan_steps_debug_string(const PlaneScan &sc) {
eng::ostringstream oss;
IdVector result;
scan(sc, &result, &oss);
oss << "|Result: ";
std::sort(result.begin(), result.end());
util::print_id_vector(result, &oss);
return oss.str();
}
void collect_planeitem_ids(PlaneItem *first, IdVector *ids) {
if (first != nullptr) {
PlaneItem *pi = first;
while (true) {
PlaneItem *next = pi->next_;
ids->push_back(pi->id());
if (next == first) break;
pi = next;
}
}
}
eng::string tree_debug_string() {
eng::ostringstream oss;
print_tree_r(node_lxyz(8,0,0,0), &oss);
@@ -579,48 +755,46 @@ PlaneMap::PlaneMap() : default_radius_(32768.0) {}
PlaneMap::~PlaneMap() {}
IdVector PlaneMap::scan(const PlaneScan &scan) const {
IdVector PlaneMap::scan(const PlaneScan &sc) const {
IdVector result;
int startpos = 0;
if (scan.special_ != 0) {
if (!scan.omit_special_) {
result.push_back(scan.special_);
if (sc.special_ != 0) {
if (!sc.omit_special_) {
result.push_back(sc.special_);
startpos = 1;
}
}
if (scan.omit_nowhere_ && (scan.plane_ == "nowhere")) {
if (sc.omit_nowhere_ && (sc.plane_ == "nowhere")) {
return result;
}
auto piter = planes_.find(scan.plane_);
auto piter = planes_.find(sc.plane_);
if (piter != planes_.end()) {
const std::unique_ptr<PlaneTree> &tree = piter->second;
tree->scan(scan, &result);
tree->scan(sc, &result, nullptr);
}
if (scan.sorted_) {
if (sc.sorted_) {
std::sort(result.begin() + startpos, result.end());
}
return result;
}
eng::string PlaneMap::tree_debug_string(const eng::string &plane) const {
auto iter = planes_.find(plane);
if (iter == planes_.end()) {
return "no such plane";
} else {
return iter->second->tree_debug_string();
}
eng::string PlaneMap::tree_debug_string(const eng::string &plane) {
return PlaneTree::get(this, plane)->tree_debug_string();
}
eng::string PlaneMap::outliers_debug_string(const eng::string &plane) const {
auto iter = planes_.find(plane);
if (iter == planes_.end()) {
return "no such plane";
} else {
return iter->second->outliers_debug_string();
}
eng::string PlaneMap::outliers_debug_string(const eng::string &plane) {
return PlaneTree::get(this, plane)->outliers_debug_string();
}
eng::string PlaneMap::search_bboxes_debug_string(const eng::string &plane, const PlaneScan &scan) {
return PlaneTree::get(this, plane)->search_bboxes_debug_string(scan);
}
eng::string PlaneMap::scan_steps_debug_string(const eng::string &plane, const PlaneScan &scan) {
return PlaneTree::get(this, plane)->scan_steps_debug_string(scan);
}
void PlaneMap::untrack_all() {
@@ -629,11 +803,11 @@ void PlaneMap::untrack_all() {
}
}
static eng::string tdb(const PlaneMap &pm) {
static eng::string tdb(PlaneMap &pm) {
return pm.tree_debug_string("p");
}
static eng::string odb(const PlaneMap &pm) {
static eng::string odb(PlaneMap &pm) {
return pm.outliers_debug_string("p");
}
@@ -644,6 +818,7 @@ static eng::string odb(const PlaneMap &pm) {
LuaDefine(unittests_planemap, "", "some unit tests") {
PlaneMap pm;
PlaneItem pi123, pi456;
PlaneScan scan;
pi123.set_id(123);
pi456.set_id(456);
@@ -827,6 +1002,162 @@ LuaDefine(unittests_planemap, "", "some unit tests") {
"| L2:802,000,802"
"| L0:8023,0000,8027 456");
// Test the calculation of search bboxes.
// The two corners are deliberately not in low-high order.
scan.clear();
scan.set_bbox_given_two_corners(util::XYZ(0x23, 0x97, 0x103),
util::XYZ(0x309, 0x412, 0x27));
LuaAssertStrEq(L, pm.search_bboxes_debug_string("p", scan),
"|Level 8 0,0,0 - 0,0,0"
"|Level 6 8,8,8 - 8,8,8"
"|Level 4 80,80,80 - 83,84,81"
"|Level 2 802,809,802 - 830,841,810"
"|Level 0 8023,8097,8027 - 8309,8412,8103");
// TESTS OF SCANNING
// Store a single object in the map to scan.
pm.untrack_all();
pi123.set_pos("p", 0x12, 0x34, 0x45);
pi123.track(&pm);
// Set up a scan with a radius of zero, centered
// right on the one object. Check the bboxes to make
// sure they only include the one cell.
scan.clear();
scan.set_bbox_given_center_radius(util::XYZ(0x12, 0x34, 0x45), 0.0);
LuaAssertStrEq(L, pm.search_bboxes_debug_string("p", scan),
"|Level 8 0,0,0 - 0,0,0"
"|Level 6 8,8,8 - 8,8,8"
"|Level 4 80,80,80 - 80,80,80"
"|Level 2 801,803,804 - 801,803,804"
"|Level 0 8012,8034,8045 - 8012,8034,8045");
// Run the scan with radius zero. It should find the one object.
LuaAssertStrEq(L, pm.scan_steps_debug_string("p", scan),
"|L8:root"
"| L7:2,2,2"
"| L6:8,8,8"
"| L5:20,20,20"
"| L4:80,80,80"
"| L3:200,200,201"
"| L2:801,803,804"
"| L1:2004,200d,2011"
"| L0:8012,8034,8045 123"
"|Result: 123");
// Bump the scan over a half-unit. It should have the same
// bboxes as before, since a half-unit isn't enough to shift
// from one cell to the next.
scan.clear();
scan.set_bbox_given_center_radius(util::XYZ(0x12 + 0.5, 0x34, 0x45), 0.0);
LuaAssertStrEq(L, pm.search_bboxes_debug_string("p", scan),
"|Level 8 0,0,0 - 0,0,0"
"|Level 6 8,8,8 - 8,8,8"
"|Level 4 80,80,80 - 80,80,80"
"|Level 2 801,803,804 - 801,803,804"
"|Level 0 8012,8034,8045 - 8012,8034,8045");
// Run the scan with radius zero, but one half-step away
// from the object. It should encounter the cell containing the
// one object, but the object should get removed in the final
// filtering step.
LuaAssertStrEq(L, pm.scan_steps_debug_string("p", scan),
"|L8:root"
"| L7:2,2,2"
"| L6:8,8,8"
"| L5:20,20,20"
"| L4:80,80,80"
"| L3:200,200,201"
"| L2:801,803,804"
"| L1:2004,200d,2011"
"| L0:8012,8034,8045 123"
"|Result: ");
// Next, expand the scan radius to huge. Examine the bboxes
// to make sure they cover the entire PlaneTree.
scan.clear();
scan.set_bbox_given_center_radius(util::XYZ(0x12, 0x34, 0x45), 100000.0);
LuaAssertStrEq(L, pm.search_bboxes_debug_string("p", scan),
"|Level 8 0,0,0 - 0,0,0"
"|Level 6 0,0,0 - f,f,f"
"|Level 4 00,00,00 - ff,ff,ff"
"|Level 2 000,000,000 - fff,fff,fff"
"|Level 0 0000,0000,0000 - ffff,ffff,ffff");
// Walk the tree using the expansive search bboxes. It should
// find the one object, and it should still only traverse the same
// cells, because those are the only cells that exist.
LuaAssertStrEq(L, pm.scan_steps_debug_string("p", scan),
"|L8:root"
"| L7:2,2,2"
"| L6:8,8,8"
"| L5:20,20,20"
"| L4:80,80,80"
"| L3:200,200,201"
"| L2:801,803,804"
"| L1:2004,200d,2011"
"| L0:8012,8034,8045 123"
"|Result: 123");
// Add another object to the tree. Then, scan again
// using the expansive search. It should find both objects.
pi456.set_pos("p", 0x14, 0x35, 0x30);
pi456.track(&pm);
LuaAssertStrEq(L, pm.scan_steps_debug_string("p", scan),
"|L8:root"
"| L7:2,2,2"
"| L6:8,8,8"
"| L5:20,20,20"
"| L4:80,80,80"
"| L3:200,200,200"
"| L2:801,803,803"
"| L1:2005,200d,200c"
"| L0:8014,8035,8030 456"
"| L3:200,200,201"
"| L2:801,803,804"
"| L1:2004,200d,2011"
"| L0:8012,8034,8045 123"
"|Result: 123,456");
// Set up a tree with a single object that's outside
// the tree's bounding box (an outlier). Print the tree
// to verify that the object ended up on the edge.
pm.untrack_all();
pi123.set_pos("p", 0x100000, 0x16, 0x23);
pi123.track(&pm);
LuaAssertStrEq(L, tdb(pm),
"|L8:root"
"| L6:f,8,8"
"| L4:ff,80,80"
"| L2:fff,801,802"
"| L0:ffff,8016,8023 123");
// Now set up a scan around the target outlier. Print out
// the scan bboxes. It should be scanning the edge cell that
// contains the outlier. It also contains a few other cells
// because the radius is nonzero.
scan.clear();
scan.set_bbox_given_center_radius(util::XYZ(0x100000, 0x16, 0x23), 0.2);
LuaAssertStrEq(L, pm.search_bboxes_debug_string("p", scan),
"|Level 8 0,0,0 - 0,0,0"
"|Level 6 f,8,8 - f,8,8"
"|Level 4 ff,80,80 - ff,80,80"
"|Level 2 fff,801,802 - fff,801,802"
"|Level 0 ffff,8015,8022 - ffff,8016,8023");
// Confirm that the scan finds the outlier.
LuaAssertStrEq(L, pm.scan_steps_debug_string("p", scan),
"|L8:root"
"| L7:3,2,2"
"| L6:f,8,8"
"| L5:3f,20,20"
"| L4:ff,80,80"
"| L3:3ff,200,200"
"| L2:fff,801,802"
"| L1:3fff,2005,2008"
"| L0:ffff,8016,8023 123"
"|Result: 123");
return 0;
}