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.h

@@ -20,6 +20,7 @@
 #include <cmath>
 #include <cstddef>
 #include <array>
+#include <deque>
 #include <functional>
 #include <string>
 #include <string_view>
@@ -251,10 +252,18 @@ struct OctNode {
   int level = 0;                       // level of the node
   std::array<int, 8> child = {-1};     // children nodes
   std::array<int, 8> vertid = {-1};    // vertex id's
-  std::array<double, 6> aabb = {0};    // bounding box
+  std::array<double, 6> aamm = {0};    // bounding box
   std::array<double, 8> coeff = {0};   // interpolation coefficients
 };
 
+struct OctreeTask {
+  std::vector<Triangle*> 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<Triangle>& elements);
-  int MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6], int lev,
-                 std::unordered_map<Point, int>& vert_map);
+  void MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6],
+                  std::unordered_map<Point, int>& vert_map);
+  void TaskToNode(const OctreeTask& task, OctNode& node, std::unordered_map<Point, int>& vert_map);
+  void Subdivide(std::deque<OctreeTask>& queue, const std::vector<Triangle*>& colliding,
+                 const OctreeTask& task, std::unordered_map<Point, int>& vert_map);
 };