visvoxgrid.c

/* ======================================================================================
 * VISGRID-3D — High-Performance Spatial Indexing Engine
 * ======================================================================================
 * 
 * DESCRIPTION:
 *   Visgrid-3D is a specialized, embedded-grade spatial state engine designed for 
 *   high-velocity 3D environments (Drone Swarms, Satellite Constellations, and 
 *   Autonomous Robotics). It implements a "Linearized Implicit Octree" using 
 *   Z-Order curves (Morton Codes) backed by a memory-mapped B-Tree (LMDB).
 *
 *   Unlike traditional pointer-based Octrees or BVHs, Visgrid-3D stores spatial 
 *   relationships as 64-bit integer keys. This eliminates "pointer-chasing" and 
 *   ensures that objects physically near each other in 3D space are stored on 
 *   consecutive memory pages on the SSD/RAM.
 *
 * KEY FEATURES:
 *   1. Crash-Proof Persistence (ACID):
 *      Every update is atomic. If the hardware loses power, the 3D world state is 
 *      instantly available upon reboot. Zero "loading" or "re-indexing" time.
 *
 *   2. Zero-Copy Architecture:
 *      Data is cast directly from the OS page cache via LMDB's mmap. There is zero
 *      serialization overhead and zero heap allocation during queries.
 *
 *   3. Hybrid Range Scanning:
 *      The engine uses MDB_SET_RANGE to perform sequential X-axis scans within 
 *      discrete Y/Z grid cells. This turns thousands of random B-Tree lookups 
 *      into a few highly-optimized linear disk reads.
 *
 *   4. Volumetric LOD (Level-of-Detail):
 *      The engine is density-aware. If a query volume spans more than 128 cells 
 *      horizontally/vertically, it mathematically skips to a coarser level. 
 *      This guarantees a deterministic "Stable FPS" regardless of scene complexity.
 *
 *   5. Deterministic KNN:
 *      Queries support Max-Heap sorting to return the K-closest objects to the 
 *      view center. Results are returned strictly sorted from [Closest -> Furthest].
 *
 * THEORY OF OPERATION:
 *   - The 3D world is divided into a 16-level hierarchy.
 *   - Level 0 represents the finest voxels; Level 15 represents the global bounds.
 *   - Objects are assigned a Level based on their largest dimension.
 *   - Within each level, coordinates are interleaved into a 64-bit Morton Key.
 *   - The final B-Tree key is: [4-bit Level | 60-bit Morton Code].
 *
 * NARROW PHASE (IMPLEMENTATION GUIDE):
 *   Visgrid-3D acts as a HIGH-SPEED BROAD-PHASE filter. It identifies candidate 
 *   objects that intersect the query AABB (Axis-Aligned Bounding Box). 
 *   For production use, you SHOULD implement "Narrow Phase" checks inside the 
 *   marked loop block in `db_qry_3d`:
 *     - View Frustum Culling (6-plane test)
 *     - Sphere-to-AABB intersection
 *     - Visibility/Occlusion flags
 *     - Logic-based filtering (e.g., "Only return enemies")
 *
 * CONSTRAINTS & LIMITS:
 *   - Coordinate Space: Signed 32-bit Integers (±2,147,483,648).
 *   - Object ID:        64-bit Unsigned Integer.
 *   - Max Hierarchy:    16 Levels (Level 0 cell = 256 units).
 *   - Resolution:       20-bits per axis (~1 million units per dimension).
 *   - Storage Limit:    4GB Default Map (Configurable via DB_MAP_SIZE).
 *   - Query Limit:      16,384 items per query (DB_QRY_LIMIT).
 *
 * METRICS (Expected on Modern NVMe/x86):
 *   - Update Throughput: 100,000+ upserts/sec (with db_batch_begin).
 *   - Query Latency:     <1.0ms for 10,000 object scans.
 *   - Memory Footprint:  Exactly 32-bytes per object in the spatial index.
 *   - Cache Density:     2 objects per 64-byte L1 cache line.
 *
 * ======================================================================================
 */
/* ======================================================================================
 * VISGRID-3D — High-Performance Spatial Indexing Engine (v2.2 Gold Master)
 * ======================================================================================
 * 
 * DESCRIPTION:
 *   Visgrid-3D is a specialized, embedded-grade spatial state engine designed for 
 *   high-velocity 3D environments (Drone Swarms, Satellite Constellations, and 
 *   Autonomous Robotics). It implements a "Linearized Implicit Octree" using 
 *   Z-Order curves (Morton Codes) backed by a memory-mapped B-Tree (LMDB).
 *
 *   Unlike traditional pointer-based Octrees or BVHs, Visgrid-3D stores spatial 
 *   relationships as 64-bit integer keys. This eliminates "pointer-chasing" and 
 *   ensures that objects physically near each other in 3D space are stored on 
 *   consecutive memory pages on the SSD/RAM.
 *
 * KEY FEATURES:
 *   1. Crash-Proof Persistence (ACID):
 *      Every update is atomic. If the hardware loses power, the 3D world state is 
 *      instantly available upon reboot. Zero "loading" or "re-indexing" time.
 *
 *   2. Zero-Copy Architecture:
 *      Data is cast directly from the OS page cache via LMDB's mmap. There is zero
 *      serialization overhead and zero heap allocation during queries.
 *
 *   3. Hybrid Range Scanning:
 *      The engine uses MDB_SET_RANGE to perform sequential X-axis scans within 
 *      discrete Y/Z grid cells. This turns thousands of random B-Tree lookups 
 *      into a few highly-optimized linear disk reads.
 *
 *   4. Volumetric LOD (Level-of-Detail):
 *      The engine is density-aware. If a query volume spans more than 128 cells 
 *      horizontally/vertically, it mathematically skips to a coarser level. 
 *      This guarantees a deterministic "Stable FPS" regardless of scene complexity.
 *
 *   5. Deterministic KNN:
 *      Queries support Max-Heap sorting to return the K-closest objects to the 
 *      view center. Results are returned strictly sorted from [Closest -> Furthest].
 *
 * THEORY OF OPERATION:
 *   - The 3D world is divided into a 16-level hierarchy.
 *   - Level 0 represents the finest voxels; Level 15 represents the global bounds.
 *   - Objects are assigned a Level based on their largest dimension.
 *   - Within each level, coordinates are interleaved into a 64-bit Morton Key.
 *   - The final B-Tree key is: [4-bit Level | 60-bit Morton Code].
 *
 * NARROW PHASE (IMPLEMENTATION GUIDE):
 *   Visgrid-3D acts as a HIGH-SPEED BROAD-PHASE filter. It identifies candidate 
 *   objects that intersect the query AABB (Axis-Aligned Bounding Box). 
 *   For production use, you SHOULD implement "Narrow Phase" checks inside the 
 *   marked loop block in `db_qry_3d`:
 *     - View Frustum Culling (6-plane test)
 *     - Sphere-to-AABB intersection
 *     - Visibility/Occlusion flags
 *     - Logic-based filtering (e.g., "Only return enemies")
 *
 * CONSTRAINTS & LIMITS:
 *   - Coordinate Space: Signed 32-bit Integers (±2,147,483,648).
 *   - Object ID:        64-bit Unsigned Integer.
 *   - Max Hierarchy:    16 Levels (Level 0 cell = 256 units).
 *   - Resolution:       20-bits per axis (~1 million units per dimension).
 *   - Storage Limit:    4GB Default Map (Configurable via DB_MAP_SIZE).
 *   - Query Limit:      16,384 items per query (DB_QRY_LIMIT).
 *
 * METRICS (Expected on Modern NVMe/x86):
 *   - Update Throughput: 100,000+ upserts/sec (with db_batch_begin).
 *   - Query Latency:     <1.0ms for 10,000 object scans.
 *   - Memory Footprint:  Exactly 32-bytes per object in the spatial index.
 *   - Cache Density:     2 objects per 64-byte L1 cache line.
 *
 * ======================================================================================
 */
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <lmdb.h>

#ifdef _MSC_VER
  #include <intrin.h>
#endif

/* --- Configuration --- */
#define MAX_LVLS        16          
#define MIN_CELL_SHIFT  8           
#define MIN_CELL_SIZE   (1 << MIN_CELL_SHIFT)
#define DB_MAP_SIZE     (4ULL * 1024 * 1024 * 1024) 
#define BATCH_LIMIT     1000        
#define DB_QRY_LIMIT    (16 * 1024) 
#define COORD_OFFSET    (2147483648ULL) 
#define LOD_TILE_LIMIT  64         

typedef uint64_t objid_t; 
struct spatial_row_3d {
  objid_t id;             
  int32_t minx, maxx;     
  int32_t miny, maxy;     
  int32_t minz, maxz;     
}; 
_Static_assert(sizeof(struct spatial_row_3d) == 32, "struct spatial_row_3d must be 32 bytes");
struct index_row_3d {
  uint64_t spatial_key;
  struct spatial_row_3d row;
};
struct heap_elm {
  objid_t id;
  double dist_sq;
};
struct db_ctx {
  MDB_env *env;
  MDB_dbi spatial_dbi;
  MDB_dbi index_dbi;
  MDB_txn *batch_txn;
  int batch_cnt;
  struct heap_elm qry_heap[DB_QRY_LIMIT]; 
};
static inline uint64_t
split_by_3(uint32_t a) {
  uint64_t x = a & 0x1fffff; 
  x = (x | x << 32) & 0x1f00000000ffffULL;
  x = (x | x << 16) & 0x1f0000ff0000ffULL;
  x = (x | x << 8)  & 0x100f00f00f00f00fULL;
  x = (x | x << 4)  & 0x10c30c30c30c30c3ULL;
  x = (x | x << 2)  & 0x1249249249249249ULL;
  return x;
}
static inline uint64_t
morton_encode_3d(uint32_t x, uint32_t y, uint32_t z) {
#if defined(__x86_64__) && defined(__BMI2__)
  return _pdep_u64(x, 0x9249249249249249ULL) | 
    (_pdep_u64(y, 0x9249249249249249ULL) << 1) | 
    (_pdep_u64(z, 0x9249249249249249ULL) << 2);
#else
  return split_by_3(x) | (split_by_3(y) << 1) | (split_by_3(z) << 2);
#endif
}
static inline unsigned
world_to_grid(int val, int lvl) {
  return (unsigned)(((long long)val + COORD_OFFSET) >> (MIN_CELL_SHIFT + lvl));
}
static uint64_t
get_center_key(int x, int y, int z, int w, int h, int d) {
  unsigned max_dim = (unsigned)((w > h) ? (w > d ? w : d) : (h > d ? h : d));
  int lvl = 0;
  if (max_dim > MIN_CELL_SIZE) {
    unsigned v = max_dim - 1u;
#if defined(_MSC_VER)
    unsigned long idx;
    _BitScanReverse(&idx, v);
    lvl = (int)(idx + 1u - MIN_CELL_SHIFT);
#else
    lvl = (int)((32u - __builtin_clz(v)) - MIN_CELL_SHIFT);
#endif
  }
  lvl = (lvl < 0) ? 0 : (lvl >= MAX_LVLS) ? MAX_LVLS - 1 : lvl;
  uint32_t lx = world_to_grid(x+(w>>1), lvl);
  uint32_t ly = world_to_grid(y+(h>>1), lvl);
  uint32_t lz = world_to_grid(z+(d>>1), lvl);
  uint64_t m = morton_encode_3d(lx, ly, lz);
  return ((uint64_t)lvl << 60) | (m & 0x0FFFFFFFFFFFFFFFULL);
}
static inline void
heap_push(struct heap_elm *h, int *n, struct heap_elm val) {
  int i = (*n)++;
  h[i] = val;
  while (i > 0) {
    int p = (i - 1) >> 1;
    if (h[p].dist_sq >= h[i].dist_sq) {
      break;
    }
    struct heap_elm t = h[i];
    h[i] = h[p];
    h[p] = t;
    i = p;
  }
}
static inline void
heap_replace(struct heap_elm *h, int n, struct heap_elm val) {
  int i = 0;
  while (1) {
    int l = (i << 1) + 1;
    int r = (i << 1) + 2;
    int s = i;
    if (l < n && h[l].dist_sq > h[s].dist_sq) s = l;
    if (r < n && h[r].dist_sq > h[s].dist_sq) s = r;
    if (s == i) {
      break;
    }
    h[i] = h[s]; i = s; // Shift child up
  }
  h[i] = val; // Write value once into the final hole
}
static inline void
heap_sort_results(struct heap_elm *h, int n) {
  while (n > 1) {
    struct heap_elm max_v = h[0];
    struct heap_elm last = h[--n];
    h[n] = max_v;
    heap_replace(h, n, last);
  }
}
static struct db_ctx*
db_init(const char *path) {
  struct db_ctx *ctx = calloc(1, sizeof(struct db_ctx));
  mdb_env_create(&ctx->env);
  mdb_env_set_maxdbs(ctx->env, 2);
  mdb_env_set_mapsize(ctx->env, DB_MAP_SIZE);
  mdb_env_open(ctx->env, path, MDB_NOTLS | MDB_NOSUBDIR, 0664);

  MDB_txn *txn; mdb_txn_begin(ctx->env, NULL, 0, &txn);
  mdb_dbi_open(txn, "spatial", MDB_CREATE | MDB_INTEGERKEY | MDB_DUPSORT | MDB_DUPFIXED, &ctx->spatial_dbi);
  mdb_dbi_open(txn, "index", MDB_CREATE | MDB_INTEGERKEY, &ctx->index_dbi);
  mdb_txn_commit(txn);
  return ctx;
}
static void
db_close(struct db_ctx *ctx) {
  if (ctx->batch_txn) {
    mdb_txn_commit(ctx->batch_txn);
  }
  mdb_env_close(ctx->env);
  free(ctx);
}
static void
db_batch_begin(struct db_ctx *ctx) {
  if (!ctx->batch_txn) {
    mdb_txn_begin(ctx->env, 0, 0, &ctx->batch_txn);
  }
}
static void
db_batch_commit(struct db_ctx *ctx) {
  if (ctx->batch_txn) { 
    mdb_txn_commit(ctx->batch_txn);
    ctx->batch_txn = NULL;
    ctx->batch_cnt = 0;
  }
}
static void
db_obj_del(struct db_ctx *ctx, objid_t id) {
  MDB_txn *txn = ctx->batch_txn; int local = 0;
  if (!txn) { 
    mdb_txn_begin(ctx->env, 0, 0, &txn);
    local = 1;
  }
  MDB_val k_id = { 8, &id }, v_idx;
  if (mdb_get(txn, ctx->index_dbi, &k_id, &v_idx) == 0) {
    struct index_row_3d *rec = (struct index_row_3d*)v_idx.mv_data;
    MDB_val k_sp = { 8, &rec->spatial_key }, v_sp = { 32, &rec->row };
    mdb_del(txn, ctx->spatial_dbi, &k_sp, &v_sp);
    mdb_del(txn, ctx->index_dbi, &k_id, 0);
  }
  if (local) {
    mdb_txn_commit(txn);
  }
}
static void
db_upsert(struct db_ctx *ctx, objid_t id, int32_t x, int32_t y, int32_t z, int32_t w, int32_t h, int32_t d) {
  MDB_txn *txn = ctx->batch_txn; int local = 0;
  if (!txn) {
    mdb_txn_begin(ctx->env, 0, 0, &txn);
    local = 1;
  }
  db_obj_del(ctx, id);

  uint64_t skey = get_center_key_3d(x, y, z, w, h, d);
  struct spatial_row_3d srow = { id, x, x+w, y, y+h, z, z+d };
  struct index_row_3d irow = { skey, srow };

  MDB_val k_sp = { 8, &skey }, v_sp = { 32, &srow };
  mdb_put(txn, ctx->spatial_dbi, &k_sp, &v_sp, 0);
  MDB_val k_id = { 8, &id }, v_idx = { sizeof(irow), &irow };
  mdb_put(txn, ctx->index_dbi, &k_id, &v_idx, 0);

  if (local) {
    mdb_txn_commit(txn);
  } else if (++ctx->batch_cnt >= BATCH_LIMIT) {
    mdb_txn_commit(ctx->batch_txn);
    mdb_txn_begin(ctx->env, 0, 0, &ctx->batch_txn);
    ctx->batch_cnt = 0;
  }
}
static int
db_qry(struct db_ctx *ctx, int vx, int vy, int vz, int vw, int vh, int vd, objid_t *out_ids, int max_cnt) {
  int res_cnt = 0, is_knn = (max_cnt > 0);
  int limit = is_knn ? max_cnt : -max_cnt;
  double cx = vx + vw*0.5;
  double cy = vy + vh*0.5;
  double cz = vz + vd*0.5;
  double max_d2 = 1e300;

  MDB_txn *txn; mdb_txn_begin(ctx->env, 0, MDB_RDONLY, &txn);
  MDB_cursor *cur; mdb_cursor_open(txn, ctx->spatial_dbi, &cur);
  for (int lvl = MAX_LVLS - 1; lvl >= 0; lvl--) {
    int half = (MIN_CELL_SIZE << lvl) >> 1;
    unsigned x0 = world_to_grid(vx-half, lvl), x1 = world_to_grid(vx+vw+half, lvl);
    unsigned y0 = world_to_grid(vy-half, lvl), y1 = world_to_grid(vy+vh+half, lvl);
    unsigned z0 = world_to_grid(vz-half, lvl), z1 = world_to_grid(vz+vd+half, lvl);
    if ((x1 - x0 + 1) * (y1 - y0 + 1) * (z1 - z0 + 1) > LOD_TILE_LIMIT) {
      break;
    }
    for (unsigned tz = z0; tz <= z1; tz++) {
      for (unsigned ty = y0; ty <= y1; ty++) {
        uint64_t k0 = ((uint64_t)lvl << 60) | (morton_encode_3d(x0, ty, tz) & 0x0FFFFFFFFFFFFFFFULL);
        uint64_t k1 = ((uint64_t)lvl << 60) | (morton_encode_3d(x1, ty, tz) & 0x0FFFFFFFFFFFFFFFULL);

        MDB_val k = { 8, &k0 }, v;
        if (mdb_cursor_get(cur, &k, &v, MDB_SET_RANGE) == 0) {
          while (1) {
            uint64_t fk = *(uint64_t*)k.mv_data;
            if (fk > k1) {
              break; 
            }
            struct spatial_row_3d *obj = (struct spatial_row_3d*)v.mv_data;
            if (obj->maxx >= vx && obj->minx <= vx+vw && 
              obj->maxy >= vy && obj->miny <= vy+vh && 
              obj->maxz >= vz && obj->minz <= vz+vd) {
              /* --- NARROW PHASE (Frustum/Physics) --- */
              if (is_knn) {
                double dx = (obj->minx + obj->maxx)*0.5 - cx;
                double dy = (obj->miny + obj->maxy)*0.5 - cy;
                double dz = (obj->minz + obj->maxz)*0.5 - cz;
                double d2 = dx*dx + dy*dy + dz*dz;
                if (res_cnt < limit) {
                  heap_push(ctx->qry_heap, &res_cnt, (struct heap_elm){obj->id, d2});
                  if (res_cnt == limit) max_d2 = ctx->qry_heap[0].dist_sq;
                } else if (d2 < max_d2) {
                  heap_replace(ctx->qry_heap, limit, (struct heap_elm){obj->id, d2});
                  max_d2 = ctx->qry_heap[0].dist_sq;
                }
              } else {
                if (res_cnt < limit) {
                  out_ids[res_cnt++] = obj->id;
                } else {
                 goto done;
                }
              }
            }
            if (mdb_cursor_get(cur, &k, &v, MDB_NEXT) != 0) {
              break;
            }
          }
        }
      }
    }
  }
done:
  mdb_cursor_close(cur); mdb_txn_abort(txn);
  if (is_knn) {
    heap_sort_results(ctx->qry_heap, res_cnt);
    for (int i = 0; i < res_cnt; i++) {
      out_ids[i] = ctx->qry_heap[i].id;
    }
  }
  return res_cnt;
}