#include "dc_octree.h"
#include <cstddef>
#include <unordered_map>
#include <cassert>
// ---------------------------------------------------------------------------
// Density cache — avoids redundant evaluations at shared corners.
// Key: packed voxel coords (x, y, z) as uint64_t.
// ---------------------------------------------------------------------------
// Use pack_voxel_key() from dc_octree.h
struct DensityCache {
std::unordered_map<uint64_t, float> map;
float sample(int voxel[3], float voxel_scale, float origin[3],
DensityFunc density_fn, void* userdata) {
uint64_t key = pack_voxel_key(voxel[0], voxel[1], voxel[2]);
auto it = map.find(key);
if (it != map.end()) return it->second;
float wx = origin[0] + voxel[0] * voxel_scale;
float wy = origin[1] + voxel[1] * voxel_scale;
float wz = origin[2] + voxel[2] * voxel_scale;
float d = density_fn(wx, wy, wz, userdata);
map[key] = d;
return d;
}
};
// Build corner bitmask and cache density values.
// bit i = 1 if corner i has positive density (solid).
// densities_out[8] receives the actual density values (may be nullptr).
static uint8_t build_corners(
int min[3], int size,
float voxel_scale, float origin[3],
DensityFunc density_fn, void* userdata,
DensityCache& cache, float* densities_out = nullptr)
{
uint8_t corners = 0;
for (int i = 0; i < 8; i++) {
int pos[3] = {
min[0] + CHILD_MIN_OFFSETS[i][0] * size,
min[1] + CHILD_MIN_OFFSETS[i][1] * size,
min[2] + CHILD_MIN_OFFSETS[i][2] * size
};
float d = cache.sample(pos, voxel_scale, origin, density_fn, userdata);
if (densities_out) densities_out[i] = d;
if (d > 0.0f) {
corners |= (1 << i);
}
}
return corners;
}
// Only prune at leaf level (size=1) to prevent aliasing.
// With the density grid, octree construction uses fast grid lookups
// so intermediate pruning isn't needed for performance.
static constexpr int MAX_PRUNE_SIZE = 1;
static OctreeNode* construct_recursive(
int min[3], int size,
float voxel_scale, float origin[3],
DensityFunc density_fn, void* userdata,
DensityCache& cache)
{
// At leaf level or small cells, check corners and prune uniform regions.
if (size <= MAX_PRUNE_SIZE) {
float densities[8];
uint8_t corners = build_corners(min, size, voxel_scale, origin,
density_fn, userdata, cache, densities);
if (corners == 0x00 || corners == 0xFF) {
return nullptr;
}
if (size == 1) {
OctreeNode* leaf = new OctreeNode();
leaf->type = OctreeNode::LEAF;
leaf->min[0] = min[0];
leaf->min[1] = min[1];
leaf->min[2] = min[2];
leaf->size = size;
leaf->corners = corners;
for (int i = 0; i < 8; i++) leaf->corner_densities[i] = densities[i];
leaf->qef.clear();
return leaf;
}
// size 2-4: sign change exists, must subdivide to find leaves
}
// Subdivide into 8 children.
int half = size / 2;
OctreeNode* children[8] = {};
bool any_child = false;
for (int i = 0; i < 8; i++) {
int child_min[3] = {
min[0] + CHILD_MIN_OFFSETS[i][0] * half,
min[1] + CHILD_MIN_OFFSETS[i][1] * half,
min[2] + CHILD_MIN_OFFSETS[i][2] * half
};
children[i] = construct_recursive(child_min, half, voxel_scale, origin,
density_fn, userdata, cache);
if (children[i]) {
any_child = true;
}
}
if (!any_child) {
return nullptr;
}
OctreeNode* node = new OctreeNode();
node->type = OctreeNode::INTERNAL;
node->min[0] = min[0];
node->min[1] = min[1];
node->min[2] = min[2];
node->size = size;
node->corners = 0; // Internal nodes don't need corner data
for (int i = 0; i < 8; i++) {
node->children[i] = children[i];
}
return node;
}
OctreeNode* construct_octree(
int min[3], int size,
float voxel_scale,
float origin[3],
DensityFunc density_fn,
void* userdata)
{
assert(size > 0 && (size & (size - 1)) == 0 && "size must be power of 2");
DensityCache cache;
return construct_recursive(min, size, voxel_scale, origin,
density_fn, userdata, cache);
}
// ---------------------------------------------------------------------------
// Grid-direct octree construction — bypasses DensityCache hash map entirely.
// Reads density values directly from DensityGrid::at() by integer coords.
// ---------------------------------------------------------------------------
static uint8_t build_corners_grid(
int min[3], int size, const DensityGrid& grid,
float* densities_out = nullptr)
{
uint8_t corners = 0;
for (int i = 0; i < 8; i++) {
int x = min[0] + CHILD_MIN_OFFSETS[i][0] * size;
int y = min[1] + CHILD_MIN_OFFSETS[i][1] * size;
int z = min[2] + CHILD_MIN_OFFSETS[i][2] * size;
float d = grid.at(x, y, z);
if (densities_out) densities_out[i] = d;
if (d > 0.0f) corners |= (1 << i);
}
return corners;
}
static OctreeNode* construct_recursive_grid(
int min[3], int size, const DensityGrid& grid)
{
if (size <= MAX_PRUNE_SIZE) {
float densities[8];
uint8_t corners = build_corners_grid(min, size, grid, densities);
if (corners == 0x00 || corners == 0xFF) return nullptr;
if (size == 1) {
OctreeNode* leaf = new OctreeNode();
leaf->type = OctreeNode::LEAF;
leaf->min[0] = min[0]; leaf->min[1] = min[1]; leaf->min[2] = min[2];
leaf->size = size;
leaf->corners = corners;
for (int i = 0; i < 8; i++) leaf->corner_densities[i] = densities[i];
leaf->qef.clear();
return leaf;
}
}
int half = size / 2;
OctreeNode* children[8] = {};
bool any_child = false;
for (int i = 0; i < 8; i++) {
int child_min[3] = {
min[0] + CHILD_MIN_OFFSETS[i][0] * half,
min[1] + CHILD_MIN_OFFSETS[i][1] * half,
min[2] + CHILD_MIN_OFFSETS[i][2] * half
};
children[i] = construct_recursive_grid(child_min, half, grid);
if (children[i]) any_child = true;
}
if (!any_child) return nullptr;
OctreeNode* node = new OctreeNode();
node->type = OctreeNode::INTERNAL;
node->min[0] = min[0]; node->min[1] = min[1]; node->min[2] = min[2];
node->size = size;
node->corners = 0;
for (int i = 0; i < 8; i++) node->children[i] = children[i];
return node;
}
OctreeNode* construct_octree_grid(int min[3], int size, const DensityGrid& grid) {
assert(size > 0 && (size & (size - 1)) == 0 && "size must be power of 2");
return construct_recursive_grid(min, size, grid);
}
// ---------------------------------------------------------------------------
// Pool-allocated grid octree construction — same logic as construct_recursive_grid
// but allocates from a flat arena for better cache locality.
// ---------------------------------------------------------------------------
static OctreeNode* construct_recursive_grid_pooled(
int min[3], int size, const DensityGrid& grid,
Pool<OctreeNode>& pool)
{
if (size <= MAX_PRUNE_SIZE) {
float densities[8];
uint8_t corners = build_corners_grid(min, size, grid, densities);
if (corners == 0x00 || corners == 0xFF) return nullptr;
if (size == 1) {
OctreeNode* leaf = pool.alloc();
leaf->type = OctreeNode::LEAF;
leaf->min[0] = min[0]; leaf->min[1] = min[1]; leaf->min[2] = min[2];
leaf->size = size;
leaf->corners = corners;
for (int i = 0; i < 8; i++) leaf->corner_densities[i] = densities[i];
leaf->qef.clear();
return leaf;
}
}
int half = size / 2;
OctreeNode* children[8] = {};
bool any_child = false;
for (int i = 0; i < 8; i++) {
int child_min[3] = {
min[0] + CHILD_MIN_OFFSETS[i][0] * half,
min[1] + CHILD_MIN_OFFSETS[i][1] * half,
min[2] + CHILD_MIN_OFFSETS[i][2] * half
};
children[i] = construct_recursive_grid_pooled(child_min, half, grid, pool);
if (children[i]) any_child = true;
}
if (!any_child) return nullptr;
OctreeNode* node = pool.alloc();
node->type = OctreeNode::INTERNAL;
node->min[0] = min[0]; node->min[1] = min[1]; node->min[2] = min[2];
node->size = size;
node->corners = 0;
for (int i = 0; i < 8; i++) node->children[i] = children[i];
return node;
}
OctreeNode* construct_octree_grid_pooled(
int min[3], int size, const DensityGrid& grid,
Pool<OctreeNode>& pool)
{
assert(size > 0 && (size & (size - 1)) == 0);
return construct_recursive_grid_pooled(min, size, grid, pool);
}
void destroy_octree(OctreeNode* node)
{
if (!node) return;
for (int i = 0; i < 8; i++) {
destroy_octree(node->children[i]);
}
delete node;
}
int count_leaves(const OctreeNode* node)
{
if (!node) return 0;
if (node->type == OctreeNode::LEAF || node->type == OctreeNode::PSEUDO) {
return 1;
}
int count = 0;
for (int i = 0; i < 8; i++) {
count += count_leaves(node->children[i]);
}
return count;
}