Data-SpatialHash-Shared
view release on metacpan or search on metacpan
uint32_t free_head; /* 28 */
uint64_t total_size; /* 32 */
uint64_t reader_slots_off; /* 40 */
uint64_t buckets_off; /* 48 */
uint64_t bitmap_off; /* 56 */
uint64_t entries_off; /* 64 */
uint32_t rwlock; /* 72 */
uint32_t rwlock_waiters; /* 76 */
uint32_t rwlock_writers_waiting; /* 80 */
uint32_t _pad0; /* 84 */
uint64_t stat_ops; /* 88 */
double world[3]; /* 96 toroidal wrap extents per axis (0 = no wrap) */
uint32_t flags; /* 120 bit0 = SPH_FLAG_WRAP */
uint32_t _pad0b; /* 124 */
double sphere_radius; /* 128 body radius for geo methods (0 = geo disabled) */
uint8_t _pad1[120]; /* 136..255 */
};
typedef struct SphHeader SphHeader;
#define SPH_FLAG_WRAP 1u
_Static_assert(sizeof(SpatialEntry) == 48, "SpatialEntry must be 48 bytes");
_Static_assert(sizeof(SphHeader) == 256, "SphHeader must be 256 bytes");
/* ---- Process-local handle ---- */
typedef struct SpatialHandle {
SphHeader *hdr;
SphReaderSlot *reader_slots; /* SPH_READER_SLOTS entries */
uint32_t *buckets;
uint64_t *bitmap;
SpatialEntry *entries;
uint32_t bitmap_words;
size_t mmap_size;
char *path; /* backing file path (strdup'd) */
int notify_fd;
int backing_fd; /* memfd fd to close on destroy, -1 otherwise */
uint32_t my_slot_idx; /* UINT32_MAX if all slots taken (no recovery for this handle) */
uint32_t cached_pid; /* getpid() cached at last slot claim */
uint32_t cached_fork_gen; /* sph_fork_gen value at last slot claim */
double world[3]; /* cached wrap extents (0 = no wrap on axis) */
int64_t wrap_cells[3]; /* ceil(world/cell) per axis, 0 = no wrap */
int wrap; /* nonzero if any axis wraps */
} SpatialHandle;
/* ================================================================
* Hashing + cell helpers
* ================================================================ */
static inline uint32_t sph_next_pow2(uint32_t v) {
if (v < 2) return 1;
return 1u << (32 - __builtin_clz(v - 1));
}
/* Largest magnitude a cell index may take. Clamping to +/-2^62 keeps cells far
* inside int64 range, so the cell-box and knn-shell loops -- which expand from
* center +/- g over a span already capped at SPH_MAX_QUERY_CELLS -- can never
* overflow int64. 2^62 is far beyond any coordinate a double represents exactly
* (2^53), so nothing reachable is lost. */
#define SPH_CELL_LIMIT ((int64_t)1 << 62)
/* Floor one coordinate to its integer cell. Defined for every double: NaN/Inf
* map to cell 0, and the result is clamped to +/-SPH_CELL_LIMIT, both to avoid
* the UB of converting an out-of-range double to int64_t and to bound the
* query loops (see SPH_CELL_LIMIT). */
static inline int64_t sph_floor_cell(double v, double cs) {
double d = floor(v / cs);
if (!isfinite(d)) return 0;
if (d >= (double)SPH_CELL_LIMIT) return SPH_CELL_LIMIT;
if (d <= -(double)SPH_CELL_LIMIT) return -SPH_CELL_LIMIT;
return (int64_t)d;
}
/* positive modulo of a cell index into [0, n); n <= 0 means "no wrap on this axis" */
static inline int64_t sph_wrap_cell(int64_t c, int64_t n) {
if (n <= 0) return c;
int64_t m = c % n;
return m < 0 ? m + n : m;
}
/* shortest per-axis separation; with wrap, the minimum-image (toroidal) distance */
static inline double sph_axis_delta(double a, double b, double world) {
double d = fabs(a - b);
return (world > 0.0 && d > world * 0.5) ? world - d : d;
}
/* raw integer cell of a position (no wrap) -- used for box-corner span math */
static inline void sph_cell_raw(const SpatialHandle *h, const double pos[3], int64_t cell[3]) {
double cs = h->hdr->cell_size;
cell[0] = sph_floor_cell(pos[0], cs);
cell[1] = sph_floor_cell(pos[1], cs);
cell[2] = sph_floor_cell(pos[2], cs);
}
/* storage cell: the raw cell wrapped into [0,nx) per axis when toroidal, so a
point near x=0 and one near x=world land in adjacent cells (0 and nx-1) */
static inline void sph_cell_of(const SpatialHandle *h, const double pos[3], int64_t cell[3]) {
sph_cell_raw(h, pos, cell);
if (h->wrap) {
cell[0] = sph_wrap_cell(cell[0], h->wrap_cells[0]);
cell[1] = sph_wrap_cell(cell[1], h->wrap_cells[1]);
cell[2] = sph_wrap_cell(cell[2], h->wrap_cells[2]);
}
}
static inline uint32_t sph_bucket_of_cell(const SpatialHandle *h, const int64_t cell[3]) {
return (uint32_t)(XXH3_64bits(cell, 3 * sizeof(int64_t)) & (h->hdr->num_buckets - 1));
}
static inline int sph_cell_eq(const int64_t a[3], const int64_t b[3]) {
return a[0] == b[0] && a[1] == b[1] && a[2] == b[2];
}
/* ================================================================
* Futex-based write-preferring read-write lock
* with reader-slot dead-process recovery
* ================================================================ */
#define SPH_RWLOCK_SPIN_LIMIT 32
#define SPH_LOCK_TIMEOUT_SEC 2 /* FUTEX_WAIT timeout for stale lock detection */
static inline void sph_rwlock_spin_pause(void) {
#if defined(__x86_64__) || defined(__i386__)
__asm__ volatile("pause" ::: "memory");
#elif defined(__aarch64__)
__asm__ volatile("yield" ::: "memory");
#else
__asm__ volatile("" ::: "memory");
}
}
}
return 1;
}
/* ================================================================
* Geo helpers (lat/lon/alt <-> xyz); R = body radius (hdr->sphere_radius).
* Angles in radians: lat in [-pi/2, pi/2], lon in (-pi, pi].
* ================================================================ */
static inline void sph_geo_to_xyz(double R, double lat, double lon, double alt, double out[3]) {
double r = R + alt;
double cl = cos(lat);
out[0] = r * cl * cos(lon);
out[1] = r * cl * sin(lon);
out[2] = r * sin(lat);
}
static inline void sph_geo_of_xyz(double R, const double in[3], double *lat, double *lon, double *alt) {
double r = sqrt(in[0]*in[0] + in[1]*in[1] + in[2]*in[2]);
double s = (r > 0.0) ? in[2] / r : 0.0;
if (s > 1.0) s = 1.0; else if (s < -1.0) s = -1.0; /* guard asin domain vs rounding */
*lat = asin(s);
*lon = atan2(in[1], in[0]); /* 0 at the poles */
*alt = r - R;
}
/* ================================================================
* Cube-sphere cell IDs (stateless; a tiny S2-like scheme).
* Equal-angle (tangent-warp) projection of a direction onto one of 6 cube
* faces, row-major i,j at a level. Packed id: level(5) face(3) i(24) j(24).
* Faces: 0=+X 1=-X 2=+Y 3=-Y 4=+Z 5=-Z. Per face the major axis carries the
* sign and the other two coords are (s,t); reconstruction is d=(face basis).
* ================================================================ */
#define SPH_CUBE_MAX_LEVEL 24
#define SPH_PI_4 0.78539816339744830961 /* pi/4 */
static inline int sph_cube_level(uint64_t id) { return (int)((id >> 51) & 0x1Fu); }
static inline int sph_cube_face (uint64_t id) { return (int)((id >> 48) & 0x7u); }
/* well-formed id: no stray high bits, level <= MAX, face < 6, i,j < 2^level */
static inline int sph_cube_valid(uint64_t id) {
if (id >> 56) return 0;
int level = sph_cube_level(id);
if (level > SPH_CUBE_MAX_LEVEL) return 0;
if (sph_cube_face(id) > 5) return 0;
uint64_t N = (uint64_t)1 << level;
uint64_t i = (id >> 24) & 0xFFFFFFu, j = id & 0xFFFFFFu;
return (i < N && j < N);
}
/* direction (need not be unit) -> cell id at level [0, SPH_CUBE_MAX_LEVEL] */
static inline uint64_t sph_cube_cell(const double dir[3], int level) {
double x = dir[0], y = dir[1], z = dir[2];
double ax = fabs(x), ay = fabs(y), az = fabs(z);
int face; double mag, s, t;
if (ax >= ay && ax >= az) { mag = ax; face = (x >= 0) ? 0 : 1; s = y / mag; t = z / mag; }
else if (ay >= az) { mag = ay; face = (y >= 0) ? 2 : 3; s = x / mag; t = z / mag; }
else { mag = az; face = (z >= 0) ? 4 : 5; s = x / mag; t = y / mag; }
double u = atan(s) / SPH_PI_4; /* equal-angle warp to [-1,1] */
double v = atan(t) / SPH_PI_4;
if (!isfinite(u)) u = 0.0; /* dir==0 -> s,t NaN; keep defined */
if (!isfinite(v)) v = 0.0;
int64_t N = (int64_t)1 << level;
int64_t i = (int64_t)floor((u + 1.0) * 0.5 * (double)N);
int64_t j = (int64_t)floor((v + 1.0) * 0.5 * (double)N);
if (i < 0) i = 0; else if (i >= N) i = N - 1;
if (j < 0) j = 0; else if (j >= N) j = N - 1;
return ((uint64_t)level << 51) | ((uint64_t)face << 48)
| ((uint64_t)i << 24) | (uint64_t)j;
}
/* reconstruct a face-basis direction from (face, s, t): the major axis carries
the face sign, the two minor coords are (s, t). Inverse of the face/s/t split
in sph_cube_cell; shared by sph_cube_center and sph_cube_neighbors. */
static inline void sph_face_dir(int face, double s, double t, double d[3]) {
switch (face) {
case 0: d[0]= 1; d[1]= s; d[2]= t; break;
case 1: d[0]=-1; d[1]= s; d[2]= t; break;
case 2: d[0]= s; d[1]= 1; d[2]= t; break;
case 3: d[0]= s; d[1]=-1; d[2]= t; break;
case 4: d[0]= s; d[1]= t; d[2]= 1; break;
default: d[0]= s; d[1]= t; d[2]=-1; break;
}
}
/* cell id -> its centre as a unit direction */
static inline void sph_cube_center(uint64_t id, double out[3]) {
int level = sph_cube_level(id), face = sph_cube_face(id);
int64_t N = (int64_t)1 << level;
int64_t i = (int64_t)((id >> 24) & 0xFFFFFFu), j = (int64_t)(id & 0xFFFFFFu);
double u = ((double)i + 0.5) / (double)N * 2.0 - 1.0;
double v = ((double)j + 0.5) / (double)N * 2.0 - 1.0;
double s = tan(u * SPH_PI_4), t = tan(v * SPH_PI_4);
double d[3];
sph_face_dir(face, s, t, d);
double n = sqrt(d[0]*d[0] + d[1]*d[1] + d[2]*d[2]);
out[0] = d[0]/n; out[1] = d[1]/n; out[2] = d[2]/n;
}
/* parent at level-1 (returns 0 at level 0); children at level+1 (0 at MAX) */
static inline int sph_cube_parent(uint64_t id, uint64_t *out) {
int level = sph_cube_level(id);
if (level == 0) return 0;
int64_t i = (int64_t)((id >> 24) & 0xFFFFFFu), j = (int64_t)(id & 0xFFFFFFu);
*out = ((uint64_t)(level - 1) << 51) | ((uint64_t)sph_cube_face(id) << 48)
| ((uint64_t)(i >> 1) << 24) | (uint64_t)(j >> 1);
return 1;
}
static inline int sph_cube_children(uint64_t id, uint64_t out[4]) {
int level = sph_cube_level(id);
if (level >= SPH_CUBE_MAX_LEVEL) return 0;
uint64_t face = (uint64_t)sph_cube_face(id);
int64_t i = (int64_t)((id >> 24) & 0xFFFFFFu), j = (int64_t)(id & 0xFFFFFFu);
int k = 0;
for (int di = 0; di < 2; di++) for (int dj = 0; dj < 2; dj++)
out[k++] = ((uint64_t)(level + 1) << 51) | (face << 48)
| ((uint64_t)(2*i + di) << 24) | (uint64_t)(2*j + dj);
return 1;
}
/* 4 edge-adjacent neighbors (seam-aware) by perturb-and-reproject: step one
}
for (int64_t dy = -g + 1; dy <= g - 1; dy++) {
SPH_KNN_PROCESS(cx - g, cy + dy, cz + dz);
SPH_KNN_PROCESS(cx + g, cy + dy, cz + dz);
}
}
}
#undef SPH_KNN_PROCESS
/* stop once full and the next shell cannot contain anything closer:
min distance from the query point to shell g+1 is g*cell_size. */
if (n >= k) { double bound = (double)g * cs; if (bound*bound >= heap[0].d2) break; }
/* stop once every live entry has been examined: nothing left to find. */
if (seen >= total) break;
}
} /* end !wrap */
qsort(heap, n, sizeof(sph_cand_t), sph_cmp_d2); /* nearest-first */
for (uint32_t i = 0; i < n; i++)
if (!sph_collect_push(out, heap[i].val)) { free(heap); return 0; }
free(heap);
return 1;
}
/* ================================================================
* Collision-pair emitters (callers hold the read lock)
* ================================================================ */
/* emit returns 1 to continue, 0 to abort (collector OOM). */
typedef int (*sph_pair_cb)(int64_t va, int64_t vb, void *ctx);
static int sph_pair_to_collect(int64_t va, int64_t vb, void *ctx) {
sph_collect_t *c = (sph_collect_t *)ctx;
return sph_collect_push(c, va) && sph_collect_push(c, vb);
}
/* Emit every unordered pair once. fixed_r >= 0: pairs with min-image centre
distance < fixed_r. fixed_r < 0: collision mode -- distance < radius_a+radius_b.
Enumeration/distance are 3D when any entry has a non-zero z (entries are
bucketed by their full 3D cell) or the world wraps in z, else 2D.
Returns SPH_Q_OK / OOM / TOOBIG. */
static int sph_pairs(SpatialHandle *h, double fixed_r, sph_pair_cb emit, void *ctx) {
double cs = h->hdr->cell_size;
uint32_t me = h->hdr->max_entries;
int collide = (fixed_r < 0.0);
/* one pass: largest radius (collision reach) + whether any entry is 3D, so the
enumeration covers the real z-cell range rather than only z-cell 0 */
double maxr = 0.0; int has3d = 0;
for (uint32_t i = 0; i < me; i++) {
if (!sph_is_live(h, i)) continue;
if (collide && h->entries[i].radius > maxr) maxr = h->entries[i].radius;
if (h->entries[i].pos[2] != 0.0) has3d = 1;
}
if (collide) { if (maxr <= 0.0) return SPH_Q_OK; } /* no radii -> points never collide */
else if (!(fixed_r > 0.0)) return SPH_Q_OK; /* non-positive radius -> nothing */
int dims = (h->world[2] > 0.0 || has3d) ? 3 : 2;
for (uint32_t a = 0; a < me; a++) {
if (!sph_is_live(h, a)) continue;
const double *pa = h->entries[a].pos;
double ra = h->entries[a].radius;
double reach_d = ceil((collide ? (ra + maxr) : fixed_r) / cs);
/* defensive: the API validates radii, but a corrupt stored radius could make this
NaN, which would slip past the cap check below (NaN >= X is false) into the cast */
if (!(reach_d >= 0)) reach_d = 0;
if (reach_d >= (double)SPH_MAX_QUERY_CELLS) return SPH_Q_TOOBIG; /* avoid int64 overflow */
int64_t reach = (int64_t)reach_d;
int64_t ac[3]; sph_cell_raw(h, pa, ac);
int64_t cnt[3] = { 1, 1, 1 };
uint64_t cells = 1;
for (int i = 0; i < dims; i++) {
uint64_t span = (uint64_t)(2 * reach + 1);
if (h->wrap_cells[i] > 0 && span > (uint64_t)h->wrap_cells[i]) span = (uint64_t)h->wrap_cells[i];
if (span >= (uint64_t)SPH_MAX_QUERY_CELLS) return SPH_Q_TOOBIG;
if (cells > (uint64_t)SPH_MAX_QUERY_CELLS / span) return SPH_Q_TOOBIG;
cells *= span; cnt[i] = (int64_t)span;
}
int64_t b0 = ac[0]-reach, b1 = ac[1]-reach, b2 = ac[2]-reach;
for (int64_t i0 = 0; i0 < cnt[0]; i0++) {
int64_t C0 = h->wrap ? sph_wrap_cell(b0+i0, h->wrap_cells[0]) : b0+i0;
for (int64_t i1 = 0; i1 < cnt[1]; i1++) {
int64_t C1 = h->wrap ? sph_wrap_cell(b1+i1, h->wrap_cells[1]) : b1+i1;
for (int64_t i2 = 0; i2 < cnt[2]; i2++) { /* cnt[2]==1 when dims==2 */
int64_t C2 = (dims == 3) ? (h->wrap ? sph_wrap_cell(b2+i2, h->wrap_cells[2]) : b2+i2) : 0;
int64_t C[3] = { C0, C1, C2 };
uint32_t bkt = sph_bucket_of_cell(h, C);
for (uint32_t idx = h->buckets[bkt]; idx != SPH_NONE; idx = h->entries[idx].next) {
if (idx <= a) continue; /* unordered: emit each pair once */
int64_t ec[3]; sph_cell_of(h, h->entries[idx].pos, ec);
if (!sph_cell_eq(ec, C)) continue; /* hash-collision guard */
double thr = collide ? (ra + h->entries[idx].radius) : fixed_r;
if (sph_dist2(h, pa, h->entries[idx].pos, dims) < thr*thr)
if (!emit(h->entries[a].value, h->entries[idx].value, ctx)) return SPH_Q_OOM;
}
}
}
}
}
return SPH_Q_OK;
}
/* ================================================================
* Lifecycle helpers: clear, chain stats
* ================================================================ */
static void sph_clear_locked(SpatialHandle *h) {
uint32_t me = h->hdr->max_entries, nb = h->hdr->num_buckets;
for (uint32_t b = 0; b < nb; b++) h->buckets[b] = SPH_NONE;
memset(h->bitmap, 0, (size_t)h->bitmap_words * sizeof(uint64_t));
for (uint32_t i = 0; i < me; i++) h->entries[i].next = (i+1 < me) ? i+1 : SPH_NONE;
h->hdr->free_head = 0; /* max_entries >= 1 (validated at create) */
h->hdr->count = 0;
}
static void sph_chain_stats(SpatialHandle *h, uint32_t *occupied, uint32_t *max_chain,
uint32_t *max_cell) {
uint32_t occ = 0, mx = 0, mxc = 0, nb = h->hdr->num_buckets;
for (uint32_t b = 0; b < nb; b++) {
uint32_t len = 0;
for (uint32_t idx = h->buckets[b]; idx != SPH_NONE; idx = h->entries[idx].next) {
len++;
/* per-cell occupancy: count chain entries sharing idx's cell (entries of
one cell always hash to one bucket, so a cell is a subset of a chain) */
int64_t ci[3]; sph_cell_of(h, h->entries[idx].pos, ci);
uint32_t cc = 0;
( run in 1.414 second using v1.01-cache-2.11-cpan-9581c071862 )