Ensure that the Octree is balanced.
This means that two adjacent nodes can only have at most one level of refinement difference. Note: This is a small breaking change for mjWarp since the order of the children was flipped. PiperOrigin-RevId: 806299445 Change-Id: Ia326a31c7a8162ae02d70c868d87779198e805be
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
2287b9f815
commit
4fc69fa64d
+146
-10
@@ -692,16 +692,18 @@ static bool boxTriangle(const Triangle& v, const double aamm[6]) {
|
||||
void mjCOctree::TaskToNode(const OctreeTask& task, OctNode& node,
|
||||
std::unordered_map<Point, int>& vert_map) {
|
||||
node.level = task.lev;
|
||||
node.parent_index = task.parent_index;
|
||||
node.child_slot = task.child_slot;
|
||||
|
||||
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];
|
||||
node.aamm[0] = task.child_slot & 1 ? (parent_aamm[3] + parent_aamm[0]) / 2 : parent_aamm[0];
|
||||
node.aamm[1] = task.child_slot & 2 ? (parent_aamm[4] + parent_aamm[1]) / 2 : parent_aamm[1];
|
||||
node.aamm[2] = task.child_slot & 4 ? (parent_aamm[5] + parent_aamm[2]) / 2 : parent_aamm[2];
|
||||
node.aamm[3] = task.child_slot & 1 ? parent_aamm[3] : (parent_aamm[0] + parent_aamm[3]) / 2;
|
||||
node.aamm[4] = task.child_slot & 2 ? parent_aamm[4] : (parent_aamm[1] + parent_aamm[4]) / 2;
|
||||
node.aamm[5] = task.child_slot & 4 ? parent_aamm[5] : (parent_aamm[2] + parent_aamm[5]) / 2;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
@@ -721,8 +723,8 @@ void mjCOctree::TaskToNode(const OctreeTask& task, OctNode& node,
|
||||
}
|
||||
|
||||
|
||||
void mjCOctree::Subdivide(std::deque<OctreeTask>& queue, const std::vector<Triangle*>& colliding,
|
||||
const OctreeTask& task, std::unordered_map<Point, int>& vert_map) {
|
||||
void mjCOctree::Subdivide(const OctreeTask& task, std::unordered_map<Point, int>& vert_map,
|
||||
std::deque<OctreeTask>* queue, const std::vector<Triangle*>& colliding) {
|
||||
for (int i = 0; i < 8; i++) {
|
||||
OctreeTask new_task;
|
||||
new_task.elements = colliding;
|
||||
@@ -733,7 +735,138 @@ void mjCOctree::Subdivide(std::deque<OctreeTask>& queue, const std::vector<Trian
|
||||
|
||||
node_.push_back(OctNode());
|
||||
TaskToNode(new_task, node_.back(), vert_map);
|
||||
queue.push_back(std::move(new_task));
|
||||
if (queue) {
|
||||
queue->push_back(std::move(new_task));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// recursively finds the adjacent ancestor neighbor region
|
||||
int mjCOctree::FindCoarseNeighbor(int node_idx, int dir) {
|
||||
if (node_idx == -1) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
int parent_idx = node_[node_idx].parent_index;
|
||||
|
||||
// if we are at the root, we have no parent and thus no siblings or external neighbors
|
||||
if (parent_idx == -1) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
int child_slot = node_[node_idx].child_slot;
|
||||
int dim = dir / 2;
|
||||
int side = dir % 2;
|
||||
int bit = 1 << dim;
|
||||
|
||||
if (side != ((child_slot & bit) != 0)) {
|
||||
// internal neighbor case: This is the successful termination of the climb
|
||||
// return the adjacent sibling node
|
||||
return node_[parent_idx].child[child_slot ^ bit];
|
||||
} else {
|
||||
// external neighbor case: Recurse up the tree
|
||||
// ask our parent to find its neighbor in the same direction
|
||||
return FindCoarseNeighbor(parent_idx, dir);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int mjCOctree::FindNeighbor(int node_idx, int dir) {
|
||||
if (node_idx == -1) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// call the helper to find the adjacent to the coarse neighbor.
|
||||
// this might be an internal node (e.g., our parent's sibling)
|
||||
int result = FindCoarseNeighbor(node_idx, dir);
|
||||
|
||||
if (result == -1) {
|
||||
// no neighbor found (either at tree boundary or some other error)
|
||||
return -1;
|
||||
}
|
||||
|
||||
// leaf descent
|
||||
double node_center[3] = {
|
||||
(node_[node_idx].aamm[0] + node_[node_idx].aamm[3]) / 2,
|
||||
(node_[node_idx].aamm[1] + node_[node_idx].aamm[4]) / 2,
|
||||
(node_[node_idx].aamm[2] + node_[node_idx].aamm[5]) / 2,
|
||||
};
|
||||
|
||||
while (node_[result].child[0] != -1) {
|
||||
double result_center[3] = {
|
||||
(node_[result].aamm[0] + node_[result].aamm[3]) / 2,
|
||||
(node_[result].aamm[1] + node_[result].aamm[4]) / 2,
|
||||
(node_[result].aamm[2] + node_[result].aamm[5]) / 2,
|
||||
};
|
||||
|
||||
// find relative octant of our node w.r.t. the neighbor's center
|
||||
int next_child_slot = 0;
|
||||
if (node_center[0] > result_center[0]) next_child_slot |= 1;
|
||||
if (node_center[1] > result_center[1]) next_child_slot |= 2;
|
||||
if (node_center[2] > result_center[2]) next_child_slot |= 4;
|
||||
|
||||
int dim = dir / 2;
|
||||
int side = dir % 2;
|
||||
int bit = 1 << dim;
|
||||
|
||||
// we need the child on the opposite side (adjacent to this node)
|
||||
int op_side = (side != 1);
|
||||
next_child_slot = (next_child_slot & ~bit) | (op_side * bit);
|
||||
result = node_[result].child[next_child_slot];
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
// refine the octree by subdividing nodes that are too coarse such that the
|
||||
// maximum level difference between adjacent nodes is at most 1.
|
||||
void mjCOctree::BalanceOctree(std::unordered_map<Point, int>& vert_map) {
|
||||
bool changed = true;
|
||||
while (changed) {
|
||||
changed = false;
|
||||
std::vector<int> leaves;
|
||||
for (int i = 0; i < nnode_; ++i) {
|
||||
if (node_[i].child[0] == -1) {
|
||||
leaves.push_back(i);
|
||||
}
|
||||
}
|
||||
|
||||
// find the nodes that are too coarse, only leaves need to be checked
|
||||
std::vector<int> leaves_to_subdivide;
|
||||
for (int leaf_idx : leaves) {
|
||||
if (node_[leaf_idx].child[0] != -1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for (int dir = 0; dir < 6; ++dir) {
|
||||
int neighbor_idx = FindNeighbor(leaf_idx, dir);
|
||||
if (neighbor_idx == -1) {
|
||||
continue;
|
||||
}
|
||||
int neighbor_level = node_[neighbor_idx].level;
|
||||
if (neighbor_level > node_[leaf_idx].level + 1) {
|
||||
leaves_to_subdivide.push_back(leaf_idx);
|
||||
}
|
||||
if (node_[leaf_idx].level > neighbor_level + 1) {
|
||||
leaves_to_subdivide.push_back(neighbor_idx);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// subdivide the nodes that are too coarse
|
||||
if (!leaves_to_subdivide.empty()) {
|
||||
changed = true;
|
||||
for (int node_idx : leaves_to_subdivide) {
|
||||
if (node_[node_idx].child[0] == -1) { // check if not already subdivided
|
||||
OctreeTask task;
|
||||
task.node_index = node_idx;
|
||||
task.lev = node_[node_idx].level;
|
||||
Subdivide(task, vert_map);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -773,8 +906,11 @@ void mjCOctree::MakeOctree(const std::vector<Triangle*>& elements, const double
|
||||
}
|
||||
|
||||
// subdivide the node
|
||||
Subdivide(queue, colliding, task, vert_map);
|
||||
Subdivide(task, vert_map, &queue, colliding);
|
||||
}
|
||||
|
||||
// store the neighbors of each node
|
||||
BalanceOctree(vert_map);
|
||||
}
|
||||
|
||||
//------------------------- class mjCDef implementation --------------------------------------------
|
||||
|
||||
Reference in New Issue
Block a user