nx_phys3d.nx source

1// nx_phys3d.nx -- the UNIFIED sovereign 3D rigid-body physics core (the 07-01 census's named gap; operator 2// 2026-07-03: "real 3d s class exceed physics"). ONE engine for games + sim + CAD-motion: semi-implicit Euler, 3// sphere/AABB contacts, sequential impulse solver (restitution + Coulomb friction + positional correction), 4// sleeping. ALL INTEGER (deterministic law: fixed-point pos=1/256 m, vel=1/65536 m-per-tick; 60Hz implied) -- 5// byte-identical replays by construction, no float, no libm. Base-relative so the same code runs native AND 6// in-wasm (mineworld pattern). Broadphase is O(n^2) pairs at this rung (BVH = ladder R4); shapes = sphere + 7// axis-aligned box (GJK convex = R5; rotation/orientation = R6 with joints -- stated, not hidden). 8// license_tier: ORIGINAL 9import "nx_syscalls.nx" 10 11// body record: 16 i64 slots 12// [0]shape 0=sphere 1=box [1]x [2]y [3]z (fx256) [4]vx [5]vy [6]vz (fx65536) 13// [7]hx|radius [8]hy [9]hz (fx256) [10]inv_mass (fx256; 0=STATIC) [11]restitution(0..256) 14// [12]friction(0..256) [13]still_frames [14]awake(1/0) [15]spare 15const PH_MAXB: i64 = 64 16const PH_G: i64 = 178 // ~9.8 m/s^2 at 60Hz in vel-units (0.00272 m/tick * 65536) 17const PH_SLOP: i64 = 4 // penetration allowance (fx256) 18const PH_POSPCT: i64 = 128 // positional correction percent (fx256 of penetration beyond slop) 19const PH_RESTV: i64 = 400 // |vn| below this -> restitution treated as 0 (rest, standard trick) 20const PH_SLEEPV: i64 = 80 // per-axis |v| threshold for stillness 21const PH_SLEEPN: i64 = 30 // still frames before sleep 22const PH_ITERS: i64 = 6 // solver iterations per tick 23 24func ph_hdr(base: i64) -> *i64 { return base as *i64 } // [0]=nbodies [1]=tick [2..7]=spare 25func ph_body(base: i64, id: i64) -> *i64 { return (base + 64 + id * 128) as *i64 } 26// WARM-START tables (the Box2D-standard robustness kit; the cure for the parked stack rest-pen rung): 27// ACC = this tick's accumulated normal impulse per pair, WARM = last tick's (pre-loaded at first touch). 28// key = ia*PH_MAXB+ib. Copied ACC->WARM (and ACC zeroed) at the end of every step, so separated pairs 29// carry NO stale impulse. 30func ph_acc(base: i64) -> *i64 { return (base + 64 + PH_MAXB * 128) as *i64 } 31func ph_warm(base: i64) -> *i64 { return (base + 64 + PH_MAXB * 128 + PH_MAXB * PH_MAXB * 8) as *i64 } 32// per-pair TARGET VELOCITY for this tick (restitution + Baumgarte bias, captured at sweep 0). The 33// accumulator drives vn TOWARD the target, never toward 0 -- driving toward 0 clawed the bounce back 34// (sweeps 1..5 erased the restitution while overlap persisted: gate-measured apex=0) and let the warm 35// pre-load self-cancel the bias (rest sank to 42). 36func ph_tgt(base: i64) -> *i64 { return (base + 64 + PH_MAXB * 128 + PH_MAXB * PH_MAXB * 16) as *i64 } 37func ph_bytes() -> i64 { return 64 + PH_MAXB * 128 + PH_MAXB * PH_MAXB * 24 } 38 39func ph_init(base: i64) -> i64 { 40 let h: *i64 = ph_hdr(base) 41 h[0] = 0 42 h[1] = 0 43 let ac: *i64 = ph_acc(base) 44 let wm: *i64 = ph_warm(base) 45 let tg: *i64 = ph_tgt(base) 46 var i: i64 = 0 47 while i < PH_MAXB * PH_MAXB { ac[i] = 0; wm[i] = 0; tg[i] = 0; i = i + 1 } 48 return 0 49} 50 51func ph_add(base: i64, shape: i64, x: i64, y: i64, z: i64, hx: i64, hy: i64, hz: i64, invm: i64, rest: i64, fric: i64) -> i64 { 52 let h: *i64 = ph_hdr(base) 53 if h[0] >= PH_MAXB { return 0 - 1 } 54 let id: i64 = h[0] 55 let b: *i64 = ph_body(base, id) 56 b[0] = shape 57 b[1] = x; b[2] = y; b[3] = z 58 b[4] = 0; b[5] = 0; b[6] = 0 59 b[7] = hx; b[8] = hy; b[9] = hz 60 b[10] = invm 61 b[11] = rest 62 b[12] = fric 63 b[13] = 0 64 b[14] = 1 65 b[15] = 0 66 h[0] = h[0] + 1 67 return id 68} 69 70func ph_set_vel(base: i64, id: i64, vx: i64, vy: i64, vz: i64) -> i64 { 71 let b: *i64 = ph_body(base, id) 72 b[4] = vx; b[5] = vy; b[6] = vz 73 b[14] = 1 74 b[13] = 0 75 return 0 76} 77 78func ph_iabs(v: i64) -> i64 { if v < 0 { return 0 - v } return v } 79func ph_min(a: i64, b: i64) -> i64 { if a < b { return a } return b } 80// deterministic integer sqrt (Newton), for sphere-sphere normals 81func ph_isqrt(v: i64) -> i64 { 82 if v <= 0 { return 0 } 83 var x: i64 = v 84 var y: i64 = (x + 1) / 2 85 while y < x { x = y; y = (x + v / x) / 2 } 86 return x 87} 88 89// resolve ONE contact: bodies a,b; unit normal scaled fx256 pointing a->b; penetration fx256 (>0 = overlap). 90// SCALE LAW (audited): v fx65536, invm fx256, n fx256. j = (256+e)*(-vn)/imsum is momentum in fx65536. 91// dv = j*n*invm => stored: j * n/256 * im/256 (exactly TWO /256 -- one per fx256 factor). 92// SLEEP LAW: a resting contact (pen<=slop AND |vn| below rest threshold) must apply NOTHING and wake NO ONE, 93// or stacks would jitter awake forever. 94func ph_contact(base: i64, ia: i64, ib: i64, nx: i64, ny: i64, nz: i64, pen: i64, docorr: i64) -> i64 { 95 let a: *i64 = ph_body(base, ia) 96 let b: *i64 = ph_body(base, ib) 97 let ima: i64 = a[10] 98 let imb: i64 = b[10] 99 let imsum: i64 = ima + imb 100 if imsum == 0 { return 0 } 101 let pc: i64 = pen - PH_SLOP 102 var rvx: i64 = b[4] - a[4] 103 var rvy: i64 = b[5] - a[5] 104 var rvz: i64 = b[6] - a[6] 105 var vn: i64 = (rvx * nx + rvy * ny + rvz * nz) / 256 106 if pc <= 0 { if vn >= 0 - PH_SLEEPV { return 0 } } // settled resting touch: no action, no wake 107 if pc <= 0 { if vn >= 0 { return 0 } } // separating with no penetration debt 108 a[14] = 1 109 b[14] = 1 110 let key: i64 = ia * PH_MAXB + ib 111 let ac: *i64 = ph_acc(base) 112 // WARM-START (first sweep): pre-apply last tick's accumulated impulse along the CURRENT normal -- the 113 // solver starts from yesterday's answer instead of re-fighting the column weight from zero. This is the 114 // literature cure for the deep-stack rest-pen (the previously parked rung). 115 if docorr == 1 { 116 let wm: *i64 = ph_warm(base) 117 let w: i64 = wm[key] 118 if w > 0 { 119 a[4] = a[4] - w * nx / 256 * ima / 256 120 a[5] = a[5] - w * ny / 256 * ima / 256 121 a[6] = a[6] - w * nz / 256 * ima / 256 122 b[4] = b[4] + w * nx / 256 * imb / 256 123 b[5] = b[5] + w * ny / 256 * imb / 256 124 b[6] = b[6] + w * nz / 256 * imb / 256 125 ac[key] = w 126 rvx = b[4] - a[4] 127 rvy = b[5] - a[5] 128 rvz = b[6] - a[6] 129 vn = (rvx * nx + rvy * ny + rvz * nz) / 256 130 } 131 } 132 // TARGET VELOCITY captured at sweep 0: restitution (+e*|vn0| on fast approach, mix=MAX) + Baumgarte 133 // bias (resting regime). Sweeps drive vn TOWARD tgt via signed accumulated deltas (clamp on the 134 // ACCUMULATOR only) -- the bounce survives the later sweeps and the bias survives the pre-load. 135 let tg: *i64 = ph_tgt(base) 136 if docorr == 1 { 137 var tv: i64 = 0 138 if vn < 0 - PH_RESTV { 139 let e: i64 = ph_imax(a[11], b[11]) 140 tv = e * (0 - vn) / 256 141 } else { 142 // bias gain 77 (was 51): rest-pen equilibrium is gravity/gain ~ 178/77 ~ 2.3 beyond slop -- 143 // inside the gate's r+-7 rest tolerance (51 rested at pen ~12 under the warm-started solver) 144 if pc > 0 { tv = pc * 77 } 145 } 146 tg[key] = tv 147 } 148 var jstop: i64 = (tg[key] - vn) * 256 / imsum 149 let oldacc: i64 = ac[key] 150 var newacc: i64 = oldacc + jstop 151 if newacc < 0 { newacc = 0 } 152 let dj: i64 = newacc - oldacc 153 ac[key] = newacc 154 a[4] = a[4] - dj * nx / 256 * ima / 256 155 a[5] = a[5] - dj * ny / 256 * ima / 256 156 a[6] = a[6] - dj * nz / 256 * ima / 256 157 b[4] = b[4] + dj * nx / 256 * imb / 256 158 b[5] = b[5] + dj * ny / 256 * imb / 256 159 b[6] = b[6] + dj * nz / 256 * imb / 256 160 // Coulomb friction: clamped by mu * the ACCUMULATED normal impulse (the physical cone, not per-sweep |j|) 161 let tvx: i64 = rvx - vn * nx / 256 162 let tvy: i64 = rvy - vn * ny / 256 163 let tvz: i64 = rvz - vn * nz / 256 164 let tmag: i64 = ph_isqrt(tvx * tvx + tvy * tvy + tvz * tvz) 165 if tmag > 16 { 166 let mu: i64 = ph_min(a[12], b[12]) 167 var jt: i64 = tmag * 256 / imsum 168 let jmax: i64 = newacc * mu / 256 169 if jt > jmax { jt = jmax } 170 let tx: i64 = tvx * 256 / tmag 171 let ty: i64 = tvy * 256 / tmag 172 let tz: i64 = tvz * 256 / tmag 173 a[4] = a[4] + jt * tx / 256 * ima / 256 174 a[5] = a[5] + jt * ty / 256 * ima / 256 175 a[6] = a[6] + jt * tz / 256 * ima / 256 176 b[4] = b[4] - jt * tx / 256 * imb / 256 177 b[5] = b[5] - jt * ty / 256 * imb / 256 178 b[6] = b[6] - jt * tz / 256 * imb / 256 179 } 180 return 0 181} 182 183// narrowphase dispatch for a pair; generates + resolves the contact if overlapping 184func ph_pair(base: i64, ia: i64, ib: i64, docorr: i64) -> i64 { 185 let a: *i64 = ph_body(base, ia) 186 let b: *i64 = ph_body(base, ib) 187 if a[10] == 0 { if b[10] == 0 { return 0 } } 188 if a[0] == 0 { if b[0] == 0 { 189 // sphere-sphere 190 let dx: i64 = b[1] - a[1] 191 let dy: i64 = b[2] - a[2] 192 let dz: i64 = b[3] - a[3] 193 let rsum: i64 = a[7] + b[7] 194 let d2: i64 = dx * dx + dy * dy + dz * dz 195 if d2 >= rsum * rsum { return 0 } 196 var d: i64 = ph_isqrt(d2) 197 var nx: i64 = 256 198 var ny: i64 = 0 199 var nz: i64 = 0 200 if d > 0 { nx = dx * 256 / d; ny = dy * 256 / d; nz = dz * 256 / d } 201 return ph_contact(base, ia, ib, nx, ny, nz, rsum - d, docorr) 202 } } 203 // box-box AABB (and sphere treated as its bounding box vs box -- exact for axis contacts) 204 let ax0: i64 = a[1] - a[7] 205 let ax1: i64 = a[1] + a[7] 206 let ay0: i64 = a[2] - a[8] 207 let ay1: i64 = a[2] + a[8] 208 let az0: i64 = a[3] - a[9] 209 let az1: i64 = a[3] + a[9] 210 let bx0: i64 = b[1] - b[7] 211 let bx1: i64 = b[1] + b[7] 212 let by0: i64 = b[2] - b[8] 213 let by1: i64 = b[2] + b[8] 214 let bz0: i64 = b[3] - b[9] 215 let bz1: i64 = b[3] + b[9] 216 let ox: i64 = ph_min(ax1, bx1) - ph_imax(ax0, bx0) 217 let oy: i64 = ph_min(ay1, by1) - ph_imax(ay0, by0) 218 let oz: i64 = ph_min(az1, bz1) - ph_imax(az0, bz0) 219 if ox <= 0 { return 0 } 220 if oy <= 0 { return 0 } 221 if oz <= 0 { return 0 } 222 // least-penetration axis, sign from center delta (axis-aligned normal = integer-exact) 223 var nx2: i64 = 0 224 var ny2: i64 = 0 225 var nz2: i64 = 0 226 var pen: i64 = ox 227 if b[1] >= a[1] { nx2 = 256 } else { nx2 = 0 - 256 } 228 if oy < pen { pen = oy; nx2 = 0; nz2 = 0; if b[2] >= a[2] { ny2 = 256 } else { ny2 = 0 - 256 } } 229 if oz < pen { pen = oz; nx2 = 0; ny2 = 0; if b[3] >= a[3] { nz2 = 256 } else { nz2 = 0 - 256 } } 230 return ph_contact(base, ia, ib, nx2, ny2, nz2, pen, docorr) 231} 232func ph_imax(a: i64, b: i64) -> i64 { if a > b { return a } return b } 233 234func ph_step(base: i64) -> i64 { 235 let h: *i64 = ph_hdr(base) 236 let n: i64 = h[0] 237 // integrate (semi-implicit: v then x), dynamic + awake only 238 var i: i64 = 0 239 while i < n { 240 let b: *i64 = ph_body(base, i) 241 if b[10] > 0 { if b[14] == 1 { 242 b[5] = b[5] - PH_G 243 b[1] = b[1] + b[4] / 256 244 b[2] = b[2] + b[5] / 256 245 b[3] = b[3] + b[6] / 256 246 } } 247 i = i + 1 248 } 249 // solve contacts, fixed order, PH_ITERS sweeps (sequential impulses) 250 var it: i64 = 0 251 while it < PH_ITERS { 252 var docorr: i64 = 0 253 if it == 0 { docorr = 1 } 254 var p: i64 = 0 255 while p < n { 256 var q: i64 = p + 1 257 while q < n { 258 ph_pair(base, p, q, docorr) 259 q = q + 1 260 } 261 p = p + 1 262 } 263 it = it + 1 264 } 265 // sleeping on POSITION stillness: integer positions FREEZE at rest (|v|<256 moves 0 cells/tick), so a 266 // frozen pos-hash for PH_SLEEPN frames = asleep. Velocity-based stillness failed: solver rounding jitter 267 // (+-tens) around the gravity/impulse balance kept resetting the counter (gate diag: top boxes awake 268 // at |vy|~38 forever). Slot [15] holds the previous position hash. 269 i = 0 270 while i < n { 271 let b2: *i64 = ph_body(base, i) 272 if b2[10] > 0 { 273 let posh: i64 = b2[1] * 31 + b2[2] * 7 + b2[3] 274 if posh == b2[15] { b2[13] = b2[13] + 1 } else { b2[13] = 0 } 275 b2[15] = posh 276 if b2[13] >= PH_SLEEPN { b2[14] = 0; b2[4] = 0; b2[5] = 0; b2[6] = 0 } 277 } 278 i = i + 1 279 } 280 // warm-start bookkeeping: this tick's accumulated impulses become next tick's pre-load; ACC zeroes so 281 // pairs that separate carry NO stale impulse. 282 let ac2: *i64 = ph_acc(base) 283 let wm2: *i64 = ph_warm(base) 284 let tg2: *i64 = ph_tgt(base) 285 var wk: i64 = 0 286 while wk < PH_MAXB * PH_MAXB { 287 wm2[wk] = ac2[wk] 288 ac2[wk] = 0 289 tg2[wk] = 0 290 wk = wk + 1 291 } 292 h[1] = h[1] + 1 293 return 0 294}