nx_phys3d.nx source
↩ module page · 401 lines · 17914 B
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"
10import "nx_vecmath.nx"
11
12// body record: 16 i64 slots
13// [0]shape 0=sphere 1=box [1]x [2]y [3]z (fx256) [4]vx [5]vy [6]vz (fx65536)
14// [7]hx|radius [8]hy [9]hz (fx256) [10]inv_mass (fx256; 0=STATIC) [11]restitution(0..256)
15// [12]friction(0..256) [13]still_frames [14]awake(1/0) [15]spare
16const PH_MAXB: i64 = 64
17const PH_G: i64 = 178 // ~9.8 m/s^2 at 60Hz in vel-units (0.00272 m/tick * 65536)
18const PH_SLOP: i64 = 4 // penetration allowance (fx256)
19const PH_POSPCT: i64 = 128 // positional correction percent (fx256 of penetration beyond slop)
20const PH_RESTV: i64 = 400 // |vn| below this -> restitution treated as 0 (rest, standard trick)
21const PH_SLEEPV: i64 = 80 // per-axis |v| threshold for stillness
22const PH_SLEEPN: i64 = 30 // still frames before sleep
23const PH_ITERS: i64 = 6 // solver iterations per tick
24
25func ph_hdr(base: i64) -> *i64 { return base as *i64 } // [0]=nbodies [1]=tick [2..7]=spare
26func ph_body(base: i64, id: i64) -> *i64 { return (base + 64 + id * 128) as *i64 }
27// WARM-START tables (the Box2D-standard robustness kit; the cure for the parked stack rest-pen rung):
28// ACC = this tick's accumulated normal impulse per pair, WARM = last tick's (pre-loaded at first touch).
29// key = ia*PH_MAXB+ib. Copied ACC->WARM (and ACC zeroed) at the end of every step, so separated pairs
30// carry NO stale impulse.
31func ph_acc(base: i64) -> *i64 { return (base + 64 + PH_MAXB * 128) as *i64 }
32func ph_warm(base: i64) -> *i64 { return (base + 64 + PH_MAXB * 128 + PH_MAXB * PH_MAXB * 8) as *i64 }
33// per-pair TARGET VELOCITY for this tick (restitution + Baumgarte bias, captured at sweep 0). The
34// accumulator drives vn TOWARD the target, never toward 0 -- driving toward 0 clawed the bounce back
35// (sweeps 1..5 erased the restitution while overlap persisted: gate-measured apex=0) and let the warm
36// pre-load self-cancel the bias (rest sank to 42).
37func ph_tgt(base: i64) -> *i64 { return (base + 64 + PH_MAXB * 128 + PH_MAXB * PH_MAXB * 16) as *i64 }
38func ph_bytes() -> i64 { return 64 + PH_MAXB * 128 + PH_MAXB * PH_MAXB * 24 }
39
40func ph_init(base: i64) -> i64 {
41 let h: *i64 = ph_hdr(base)
42 h[0] = 0
43 h[1] = 0
44 let ac: *i64 = ph_acc(base)
45 let wm: *i64 = ph_warm(base)
46 let tg: *i64 = ph_tgt(base)
47 var i: i64 = 0
48 while i < PH_MAXB * PH_MAXB { ac[i] = 0; wm[i] = 0; tg[i] = 0; i = i + 1 }
49 return 0
50}
51
52func 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 {
53 let h: *i64 = ph_hdr(base)
54 if h[0] >= PH_MAXB { return 0 - 1 }
55 let id: i64 = h[0]
56 let b: *i64 = ph_body(base, id)
57 b[0] = shape
58 b[1] = x; b[2] = y; b[3] = z
59 b[4] = 0; b[5] = 0; b[6] = 0
60 b[7] = hx; b[8] = hy; b[9] = hz
61 b[10] = invm
62 b[11] = rest
63 b[12] = fric
64 b[13] = 0
65 b[14] = 1
66 b[15] = 0
67 h[0] = h[0] + 1
68 return id
69}
70
71func ph_set_vel(base: i64, id: i64, vx: i64, vy: i64, vz: i64) -> i64 {
72 let b: *i64 = ph_body(base, id)
73 b[4] = vx; b[5] = vy; b[6] = vz
74 b[14] = 1
75 b[13] = 0
76 return 0
77}
78
79func ph_iabs(v: i64) -> i64 { if v < 0 { return 0 - v } return v }
80func ph_min(a: i64, b: i64) -> i64 { if a < b { return a } return b }
81// deterministic integer sqrt (Newton), for sphere-sphere normals
82func ph_isqrt(v: i64) -> i64 { return vm_isqrt(v) }
83
84// resolve ONE contact: bodies a,b; unit normal scaled fx256 pointing a->b; penetration fx256 (>0 = overlap).
85// SCALE LAW (audited): v fx65536, invm fx256, n fx256. j = (256+e)*(-vn)/imsum is momentum in fx65536.
86// dv = j*n*invm => stored: j * n/256 * im/256 (exactly TWO /256 -- one per fx256 factor).
87// SLEEP LAW: a resting contact (pen<=slop AND |vn| below rest threshold) must apply NOTHING and wake NO ONE,
88// or stacks would jitter awake forever.
89func ph_contact(base: i64, ia: i64, ib: i64, nx: i64, ny: i64, nz: i64, pen: i64, docorr: i64) -> i64 {
90 let a: *i64 = ph_body(base, ia)
91 let b: *i64 = ph_body(base, ib)
92 let ima: i64 = a[10]
93 let imb: i64 = b[10]
94 let imsum: i64 = ima + imb
95 if imsum == 0 { return 0 }
96 let pc: i64 = pen - PH_SLOP
97 var rvx: i64 = b[4] - a[4]
98 var rvy: i64 = b[5] - a[5]
99 var rvz: i64 = b[6] - a[6]
100 var vn: i64 = (rvx * nx + rvy * ny + rvz * nz) / 256
101 if pc <= 0 { if vn >= 0 - PH_SLEEPV { return 0 } } // settled resting touch: no action, no wake
102 if pc <= 0 { if vn >= 0 { return 0 } } // separating with no penetration debt
103 a[14] = 1
104 b[14] = 1
105 let key: i64 = ia * PH_MAXB + ib
106 let ac: *i64 = ph_acc(base)
107 // WARM-START (first sweep): pre-apply last tick's accumulated impulse along the CURRENT normal -- the
108 // solver starts from yesterday's answer instead of re-fighting the column weight from zero. This is the
109 // literature cure for the deep-stack rest-pen (the previously parked rung).
110 if docorr == 1 {
111 let wm: *i64 = ph_warm(base)
112 let w: i64 = wm[key]
113 if w > 0 {
114 a[4] = a[4] - w * nx / 256 * ima / 256
115 a[5] = a[5] - w * ny / 256 * ima / 256
116 a[6] = a[6] - w * nz / 256 * ima / 256
117 b[4] = b[4] + w * nx / 256 * imb / 256
118 b[5] = b[5] + w * ny / 256 * imb / 256
119 b[6] = b[6] + w * nz / 256 * imb / 256
120 ac[key] = w
121 rvx = b[4] - a[4]
122 rvy = b[5] - a[5]
123 rvz = b[6] - a[6]
124 vn = (rvx * nx + rvy * ny + rvz * nz) / 256
125 }
126 }
127 // TARGET VELOCITY captured at sweep 0: restitution (+e*|vn0| on fast approach, mix=MAX) + Baumgarte
128 // bias (resting regime). Sweeps drive vn TOWARD tgt via signed accumulated deltas (clamp on the
129 // ACCUMULATOR only) -- the bounce survives the later sweeps and the bias survives the pre-load.
130 let tg: *i64 = ph_tgt(base)
131 if docorr == 1 {
132 var tv: i64 = 0
133 if vn < 0 - PH_RESTV {
134 let e: i64 = ph_imax(a[11], b[11])
135 tv = e * (0 - vn) / 256
136 } else {
137 // bias gain 77 (was 51): rest-pen equilibrium is gravity/gain ~ 178/77 ~ 2.3 beyond slop --
138 // inside the gate's r+-7 rest tolerance (51 rested at pen ~12 under the warm-started solver)
139 if pc > 0 { tv = pc * 77 }
140 }
141 tg[key] = tv
142 }
143 var jstop: i64 = (tg[key] - vn) * 256 / imsum
144 let oldacc: i64 = ac[key]
145 var newacc: i64 = oldacc + jstop
146 if newacc < 0 { newacc = 0 }
147 let dj: i64 = newacc - oldacc
148 ac[key] = newacc
149 a[4] = a[4] - dj * nx / 256 * ima / 256
150 a[5] = a[5] - dj * ny / 256 * ima / 256
151 a[6] = a[6] - dj * nz / 256 * ima / 256
152 b[4] = b[4] + dj * nx / 256 * imb / 256
153 b[5] = b[5] + dj * ny / 256 * imb / 256
154 b[6] = b[6] + dj * nz / 256 * imb / 256
155 // Coulomb friction: clamped by mu * the ACCUMULATED normal impulse (the physical cone, not per-sweep |j|)
156 let tvx: i64 = rvx - vn * nx / 256
157 let tvy: i64 = rvy - vn * ny / 256
158 let tvz: i64 = rvz - vn * nz / 256
159 let tmag: i64 = ph_isqrt(tvx * tvx + tvy * tvy + tvz * tvz)
160 if tmag > 16 {
161 let mu: i64 = ph_min(a[12], b[12])
162 var jt: i64 = tmag * 256 / imsum
163 let jmax: i64 = newacc * mu / 256
164 if jt > jmax { jt = jmax }
165 let tx: i64 = tvx * 256 / tmag
166 let ty: i64 = tvy * 256 / tmag
167 let tz: i64 = tvz * 256 / tmag
168 a[4] = a[4] + jt * tx / 256 * ima / 256
169 a[5] = a[5] + jt * ty / 256 * ima / 256
170 a[6] = a[6] + jt * tz / 256 * ima / 256
171 b[4] = b[4] - jt * tx / 256 * imb / 256
172 b[5] = b[5] - jt * ty / 256 * imb / 256
173 b[6] = b[6] - jt * tz / 256 * imb / 256
174 }
175 return 0
176}
177
178// narrowphase dispatch for a pair; generates + resolves the contact if overlapping
179func ph_pair(base: i64, ia: i64, ib: i64, docorr: i64) -> i64 {
180 let a: *i64 = ph_body(base, ia)
181 let b: *i64 = ph_body(base, ib)
182 if a[10] == 0 { if b[10] == 0 { return 0 } }
183 if a[0] == 0 { if b[0] == 0 {
184 // sphere-sphere
185 let dx: i64 = b[1] - a[1]
186 let dy: i64 = b[2] - a[2]
187 let dz: i64 = b[3] - a[3]
188 let rsum: i64 = a[7] + b[7]
189 let d2: i64 = dx * dx + dy * dy + dz * dz
190 if d2 >= rsum * rsum { return 0 }
191 var d: i64 = ph_isqrt(d2)
192 var nx: i64 = 256
193 var ny: i64 = 0
194 var nz: i64 = 0
195 if d > 0 { nx = dx * 256 / d; ny = dy * 256 / d; nz = dz * 256 / d }
196 return ph_contact(base, ia, ib, nx, ny, nz, rsum - d, docorr)
197 } }
198 // box-box AABB (and sphere treated as its bounding box vs box -- exact for axis contacts)
199 let ax0: i64 = a[1] - a[7]
200 let ax1: i64 = a[1] + a[7]
201 let ay0: i64 = a[2] - a[8]
202 let ay1: i64 = a[2] + a[8]
203 let az0: i64 = a[3] - a[9]
204 let az1: i64 = a[3] + a[9]
205 let bx0: i64 = b[1] - b[7]
206 let bx1: i64 = b[1] + b[7]
207 let by0: i64 = b[2] - b[8]
208 let by1: i64 = b[2] + b[8]
209 let bz0: i64 = b[3] - b[9]
210 let bz1: i64 = b[3] + b[9]
211 let ox: i64 = ph_min(ax1, bx1) - ph_imax(ax0, bx0)
212 let oy: i64 = ph_min(ay1, by1) - ph_imax(ay0, by0)
213 let oz: i64 = ph_min(az1, bz1) - ph_imax(az0, bz0)
214 if ox <= 0 { return 0 }
215 if oy <= 0 { return 0 }
216 if oz <= 0 { return 0 }
217 // least-penetration axis, sign from center delta (axis-aligned normal = integer-exact)
218 var nx2: i64 = 0
219 var ny2: i64 = 0
220 var nz2: i64 = 0
221 var pen: i64 = ox
222 if b[1] >= a[1] { nx2 = 256 } else { nx2 = 0 - 256 }
223 if oy < pen { pen = oy; nx2 = 0; nz2 = 0; if b[2] >= a[2] { ny2 = 256 } else { ny2 = 0 - 256 } }
224 if oz < pen { pen = oz; nx2 = 0; ny2 = 0; if b[3] >= a[3] { nz2 = 256 } else { nz2 = 0 - 256 } }
225 return ph_contact(base, ia, ib, nx2, ny2, nz2, pen, docorr)
226}
227func ph_imax(a: i64, b: i64) -> i64 { if a > b { return a } return b }
228
229// ---- PG26 (2026-09-06): CONTINUOUS COLLISION -- a fast mover advances only to its time of impact ------------------
230// A body whose displacement this tick exceeds its own radius can tunnel through a thin wall under discrete stepping
231// (the 2D sweep proved it: a 400-per-tick mover through a 4-unit wall). ph_toi is CONSERVATIVE ADVANCEMENT in
232// integers: at parameter t (Q8 of the tick) the mover's bounding sphere is at least gap(t) from every other body's
233// SURFACE, so it may advance by gap/|displacement| of the tick without penetrating; repeat until the gap closes to
234// PH_TOI_EPS or the tick is spent. Bodies the mover is ALREADY touching whose gap does not shrink over the tick (a
235// floor it rolls on, a wall it slides along) are excluded from the advancement -- otherwise a resting contact would
236// freeze every fast mover at t=0. Targets are sampled at their current position (a moving target is a declared
237// approximation, not a claim); a box mover advances as its bounding sphere (conservative, declared). The contact
238// solver then sees a touch, never an embedding. PH_CCD_OFF is the gate's neg-control switch and nothing else.
239const PH_TOI_Q: i64 = 256 // one tick in Q8
240const PH_TOI_ITERS: i64 = 16
241const PH_TOI_EPS: i64 = 2 // the gap at which advancement stops (fx256), inside PH_SLOP so the solver sees a touch
242const PH_TOI_FAR: i64 = 1000000000
243static PH_CCD_OFF: i64
244func ph_ccd_off(v: i64) -> i64 { PH_CCD_OFF = v; return 0 }
245func ph_bound_r(b: *i64) -> i64 {
246 if b[0] == 0 { return b[7] }
247 var r: i64 = b[7]
248 if b[8] > r { r = b[8] }
249 if b[9] > r { r = b[9] }
250 return r
251}
252func ph_fast(dx: i64, dy: i64, dz: i64, r: i64) -> i64 {
253 var m: i64 = ph_iabs(dx)
254 if ph_iabs(dy) > m { m = ph_iabs(dy) }
255 if ph_iabs(dz) > m { m = ph_iabs(dz) }
256 if m > r { return 1 }
257 return 0
258}
259// distance from a point to the SURFACE of body j, positive outside, 0 on or inside a box, negative inside a sphere
260func ph_surf_dist(base: i64, j: i64, px: i64, py: i64, pz: i64) -> i64 {
261 let b: *i64 = ph_body(base, j)
262 let dx: i64 = px - b[1]
263 let dy: i64 = py - b[2]
264 let dz: i64 = pz - b[3]
265 if b[0] == 0 { return ph_isqrt(dx * dx + dy * dy + dz * dz) - b[7] }
266 var qx: i64 = ph_iabs(dx) - b[7]
267 var qy: i64 = ph_iabs(dy) - b[8]
268 var qz: i64 = ph_iabs(dz) - b[9]
269 if qx < 0 { qx = 0 }
270 if qy < 0 { qy = 0 }
271 if qz < 0 { qz = 0 }
272 return ph_isqrt(qx * qx + qy * qy + qz * qz)
273}
274// the mover's gap to body j at parameter t of its displacement
275func ph_gap_at(base: i64, id: i64, j: i64, dx: i64, dy: i64, dz: i64, t: i64, r: i64) -> i64 {
276 let a: *i64 = ph_body(base, id)
277 return ph_surf_dist(base, j, a[1] + dx * t / PH_TOI_Q, a[2] + dy * t / PH_TOI_Q, a[3] + dz * t / PH_TOI_Q) - r
278}
279// time of impact for body id over displacement (dx,dy,dz) (fx256 per tick): the Q8 fraction it may advance
280func ph_toi(base: i64, id: i64, dx: i64, dy: i64, dz: i64) -> i64 {
281 let h: *i64 = ph_hdr(base)
282 let n: i64 = h[0]
283 let a: *i64 = ph_body(base, id)
284 let r: i64 = ph_bound_r(a)
285 let dl: i64 = ph_isqrt(dx * dx + dy * dy + dz * dz)
286 if dl == 0 { return PH_TOI_Q }
287 var t: i64 = 0
288 var it: i64 = 0
289 while it < PH_TOI_ITERS {
290 var dmin: i64 = PH_TOI_FAR
291 var j: i64 = 0
292 while j < n {
293 if j != id {
294 let g: i64 = ph_gap_at(base, id, j, dx, dy, dz, t, r)
295 var skip: i64 = 0
296 if g <= PH_TOI_EPS {
297 // already touching: only a contact whose gap SHRINKS over the tick can block the mover
298 let g1: i64 = ph_gap_at(base, id, j, dx, dy, dz, PH_TOI_Q, r)
299 if g1 >= g - PH_TOI_EPS { skip = 1 }
300 }
301 if skip == 0 { if g < dmin { dmin = g } }
302 }
303 j = j + 1
304 }
305 if dmin <= PH_TOI_EPS { return t }
306 if dmin >= PH_TOI_FAR { return PH_TOI_Q }
307 let dt: i64 = dmin * PH_TOI_Q / dl
308 if dt <= 0 { return t }
309 t = t + dt
310 if t >= PH_TOI_Q { return PH_TOI_Q }
311 it = it + 1
312 }
313 return t
314}
315// the deepest overlap (fx256) of body id's bounding sphere into any other body -- the gate's oracle
316func ph_max_overlap(base: i64, id: i64) -> i64 {
317 let h: *i64 = ph_hdr(base)
318 let n: i64 = h[0]
319 let a: *i64 = ph_body(base, id)
320 let r: i64 = ph_bound_r(a)
321 var worst: i64 = 0 - PH_TOI_FAR
322 var j: i64 = 0
323 while j < n {
324 if j != id {
325 let ov: i64 = r - ph_surf_dist(base, j, a[1], a[2], a[3])
326 if ov > worst { worst = ov }
327 }
328 j = j + 1
329 }
330 return worst
331}
332
333func ph_step(base: i64) -> i64 {
334 let h: *i64 = ph_hdr(base)
335 let n: i64 = h[0]
336 // integrate (semi-implicit: v then x), dynamic + awake only
337 var i: i64 = 0
338 while i < n {
339 let b: *i64 = ph_body(base, i)
340 if b[10] > 0 { if b[14] == 1 {
341 b[5] = b[5] - PH_G
342 // PG26: a body that would move farther than its own radius this tick is a FAST mover and is advanced
343 // only to its time of impact, so it can never end the tick inside another body (the discrete path is
344 // byte-identical for everything slower -- the gate pins that)
345 let dx: i64 = b[4] / 256
346 let dy: i64 = b[5] / 256
347 let dz: i64 = b[6] / 256
348 var tq: i64 = PH_TOI_Q
349 if PH_CCD_OFF == 0 { if ph_fast(dx, dy, dz, ph_bound_r(b)) == 1 { tq = ph_toi(base, i, dx, dy, dz) } }
350 b[1] = b[1] + dx * tq / PH_TOI_Q
351 b[2] = b[2] + dy * tq / PH_TOI_Q
352 b[3] = b[3] + dz * tq / PH_TOI_Q
353 } }
354 i = i + 1
355 }
356 // solve contacts, fixed order, PH_ITERS sweeps (sequential impulses)
357 var it: i64 = 0
358 while it < PH_ITERS {
359 var docorr: i64 = 0
360 if it == 0 { docorr = 1 }
361 var p: i64 = 0
362 while p < n {
363 var q: i64 = p + 1
364 while q < n {
365 ph_pair(base, p, q, docorr)
366 q = q + 1
367 }
368 p = p + 1
369 }
370 it = it + 1
371 }
372 // sleeping on POSITION stillness: integer positions FREEZE at rest (|v|<256 moves 0 cells/tick), so a
373 // frozen pos-hash for PH_SLEEPN frames = asleep. Velocity-based stillness failed: solver rounding jitter
374 // (+-tens) around the gravity/impulse balance kept resetting the counter (gate diag: top boxes awake
375 // at |vy|~38 forever). Slot [15] holds the previous position hash.
376 i = 0
377 while i < n {
378 let b2: *i64 = ph_body(base, i)
379 if b2[10] > 0 {
380 let posh: i64 = b2[1] * 31 + b2[2] * 7 + b2[3]
381 if posh == b2[15] { b2[13] = b2[13] + 1 } else { b2[13] = 0 }
382 b2[15] = posh
383 if b2[13] >= PH_SLEEPN { b2[14] = 0; b2[4] = 0; b2[5] = 0; b2[6] = 0 }
384 }
385 i = i + 1
386 }
387 // warm-start bookkeeping: this tick's accumulated impulses become next tick's pre-load; ACC zeroes so
388 // pairs that separate carry NO stale impulse.
389 let ac2: *i64 = ph_acc(base)
390 let wm2: *i64 = ph_warm(base)
391 let tg2: *i64 = ph_tgt(base)
392 var wk: i64 = 0
393 while wk < PH_MAXB * PH_MAXB {
394 wm2[wk] = ac2[wk]
395 ac2[wk] = 0
396 tg2[wk] = 0
397 wk = wk + 1
398 }
399 h[1] = h[1] + 1
400 return 0
401}