From 7e048b7535d327606b88fa5d7880b1998d3f6ad2 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 12 Sep 2025 03:21:26 -0700 Subject: [PATCH] Refactor: Convert `mjCOctree::MakeOctree` from recursive to iterative. The octree construction is now performed using a breadth-first search with a `std::deque` to manage tasks. A new `OctreeTask` struct encapsulates the state for each node to be processed, and a helper function `Subdivide` is introduced for subdividing the Octree nodes. Also reduce the number of (center, size) to (min, max) conversion for the bounding box representation in order to avoid duplicated vertices in the Octree due to floating point errors. PiperOrigin-RevId: 806214441 Change-Id: Iff9015ebe4705522db743c478cef5acf1a7312b7 --- src/user/user_objects.cc | 122 ++++++++++++++++++++++++--------------- src/user/user_objects.h | 18 +++++- 2 files changed, 92 insertions(+), 48 deletions(-) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index d25ef1fa..b3781975 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -573,9 +574,12 @@ void mjCOctree::CopyChild(int* child) const { void mjCOctree::CopyAabb(mjtNum* aabb) const { for (int i = 0; i < node_.size(); ++i) { - for (int j = 0; j < 6; ++j) { - aabb[i * 6 + j] = node_[i].aabb[j]; - } + aabb[i * 6 + 0] = (node_[i].aamm[0] + node_[i].aamm[3]) / 2; + aabb[i * 6 + 1] = (node_[i].aamm[1] + node_[i].aamm[4]) / 2; + aabb[i * 6 + 2] = (node_[i].aamm[2] + node_[i].aamm[5]) / 2; + aabb[i * 6 + 3] = (node_[i].aamm[3] - node_[i].aamm[0]) / 2; + aabb[i * 6 + 4] = (node_[i].aamm[4] - node_[i].aamm[1]) / 2; + aabb[i * 6 + 5] = (node_[i].aamm[5] - node_[i].aamm[2]) / 2; } } @@ -624,7 +628,7 @@ void mjCOctree::CreateOctree(const double aamm[6]) { std::transform(elements.begin(), elements.end(), elements_ptrs.begin(), [](Triangle& triangle) { return ▵ }); std::unordered_map vert_map; - MakeOctree(elements_ptrs, box, 0, vert_map); + MakeOctree(elements_ptrs, box, vert_map); } @@ -685,23 +689,25 @@ static bool boxTriangle(const Triangle& v, const double aamm[6]) { } -int mjCOctree::MakeOctree(const std::vector& elements, const double aamm[6], int lev, - std::unordered_map& vert_map) { - node_.push_back(OctNode()); - OctNode& node = node_.back(); - node.level = lev; +void mjCOctree::TaskToNode(const OctreeTask& task, OctNode& node, + std::unordered_map& vert_map) { + node.level = task.lev; + + if (task.parent_index != -1) { + node_[task.parent_index].child[task.child_slot] = task.node_index; + const auto parent_aamm = node_[task.parent_index].aamm; + node.aamm[0] = task.child_slot & 1 ? parent_aamm[0] : (parent_aamm[3] + parent_aamm[0]) / 2; + node.aamm[1] = task.child_slot & 2 ? parent_aamm[1] : (parent_aamm[4] + parent_aamm[1]) / 2; + node.aamm[2] = task.child_slot & 4 ? parent_aamm[2] : (parent_aamm[5] + parent_aamm[2]) / 2; + node.aamm[3] = task.child_slot & 1 ? (parent_aamm[0] + parent_aamm[3]) / 2 : parent_aamm[3]; + node.aamm[4] = task.child_slot & 2 ? (parent_aamm[1] + parent_aamm[4]) / 2 : parent_aamm[4]; + node.aamm[5] = task.child_slot & 4 ? (parent_aamm[2] + parent_aamm[5]) / 2 : parent_aamm[5]; + } - // create a new node - int index = nnode_++; - std::array aabb = { - (aamm[0] + aamm[3]) / 2, (aamm[1] + aamm[4]) / 2, - (aamm[2] + aamm[5]) / 2, (aamm[3] - aamm[0]) / 2, - (aamm[4] - aamm[1]) / 2, (aamm[5] - aamm[2]) / 2}; - node.aabb = aabb; for (int i = 0; i < 8; i++) { - Point v = {{(i & 1) ? aamm[3] : aamm[0], - (i & 2) ? aamm[4] : aamm[1], - (i & 4) ? aamm[5] : aamm[2]}}; + Point v = {{(i & 1) ? node.aamm[3] : node.aamm[0], + (i & 2) ? node.aamm[4] : node.aamm[1], + (i & 4) ? node.aamm[5] : node.aamm[2]}}; auto it = vert_map.find(v); if (it != vert_map.end()) { node.vertid[i] = it->second; @@ -712,37 +718,63 @@ int mjCOctree::MakeOctree(const std::vector& elements, const double a } node.child[i] = -1; } +} - // find all triangles that intersect the current box - std::vector colliding; - for (auto* element : elements) { - if (boxTriangle(*element, aamm)) { - colliding.push_back(element); + +void mjCOctree::Subdivide(std::deque& queue, const std::vector& colliding, + const OctreeTask& task, std::unordered_map& vert_map) { + for (int i = 0; i < 8; i++) { + OctreeTask new_task; + new_task.elements = colliding; + new_task.lev = task.lev + 1; + new_task.parent_index = task.node_index; + new_task.node_index = nnode_++; + new_task.child_slot = i; + + node_.push_back(OctNode()); + TaskToNode(new_task, node_.back(), vert_map); + queue.push_back(std::move(new_task)); + } +} + + +void mjCOctree::MakeOctree(const std::vector& elements, const double aamm[6], + std::unordered_map& vert_map) { + std::deque queue; + OctreeTask initial_task; + initial_task.elements = elements; + initial_task.lev = 0; + initial_task.parent_index = -1; + initial_task.child_slot = -1; + initial_task.node_index = nnode_++; + queue.push_back(std::move(initial_task)); + + // create root node + node_.push_back(OctNode()); + OctNode& root = node_.back(); + std::copy(aamm, aamm + 6, root.aamm.data()); + TaskToNode(queue.front(), node_.back(), vert_map); + + while (!queue.empty()) { + OctreeTask task = std::move(queue.front()); + queue.pop_front(); + + // find all triangles that intersect the current box + std::vector colliding; + for (auto* element : task.elements) { + if (boxTriangle(*element, node_[task.node_index].aamm.data())) { + colliding.push_back(element); + } } - } - // return if the box is empty - if (colliding.empty() || lev >= 6) { - return index; - } + // skip if the box is empty + if (colliding.empty() || task.lev >= 6) { + continue; + } - // split the box into 8 sub-boxes - double new_aamm[8][6]; - for (int i = 0; i < 8; i++) { - new_aamm[i][0] = aabb[0] + aabb[3] * (i & 1 ? -1 : 0); - new_aamm[i][1] = aabb[1] + aabb[4] * (i & 2 ? -1 : 0); - new_aamm[i][2] = aabb[2] + aabb[5] * (i & 4 ? -1 : 0); - new_aamm[i][3] = aabb[0] + aabb[3] * (i & 1 ? 0 : 1); - new_aamm[i][4] = aabb[1] + aabb[4] * (i & 2 ? 0 : 1); - new_aamm[i][5] = aabb[2] + aabb[5] * (i & 4 ? 0 : 1); + // subdivide the node + Subdivide(queue, colliding, task, vert_map); } - - // recursive calls to create sub-boxes - for (int i = 0; i < 8; i++) { - node_[index].child[i] = MakeOctree(colliding, new_aamm[i], lev + 1, vert_map); - } - - return index; } //------------------------- class mjCDef implementation -------------------------------------------- diff --git a/src/user/user_objects.h b/src/user/user_objects.h index d9896521..9d00d11e 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -251,10 +252,18 @@ struct OctNode { int level = 0; // level of the node std::array child = {-1}; // children nodes std::array vertid = {-1}; // vertex id's - std::array aabb = {0}; // bounding box + std::array aamm = {0}; // bounding box std::array coeff = {0}; // interpolation coefficients }; +struct OctreeTask { + std::vector elements; + int lev; + int parent_index; + int child_slot; + int node_index; +}; + struct mjCOctree_ { int nnode_ = 0; int nvert_ = 0; @@ -290,8 +299,11 @@ class mjCOctree : public mjCOctree_ { private: void Make(std::vector& elements); - int MakeOctree(const std::vector& elements, const double aamm[6], int lev, - std::unordered_map& vert_map); + void MakeOctree(const std::vector& elements, const double aamm[6], + std::unordered_map& vert_map); + void TaskToNode(const OctreeTask& task, OctNode& node, std::unordered_map& vert_map); + void Subdivide(std::deque& queue, const std::vector& colliding, + const OctreeTask& task, std::unordered_map& vert_map); };