nx_softtissue.nx source
↩ module page · 1590 lines · 78002 B
1// nx_softtissue.nx -- SOVEREIGN soft-tissue solver: XPBD over a tetrahedral physics cage
2// with per-layer compliance. THE tier our body physics did not have.
3//
4// WHY THIS EXISTS (measured, 2026-07-30): the rigmesh viewer drives breasts with a 2-mass
5// EMA spring (a damped oscillator with a calibrated gain). An oscillator has no VOLUME, no
6// LAYERS and no relationship to gravity DIRECTION -- so it physically cannot reproduce the
7// one behavior a human notices instantly: when a woman lies down, soft heavy tissue FLATTENS
8// and spreads toward the chest wall, and firm tissue does so much less. Our own ecosystem
9// already MEASURES that behavior -- nx_breast_drape_pose.nx bands LAYING_BACK at
10// aspect 716..921 / centroid 410..614 against STANDING_FACING 1024..1331 / 614..768 -- so a
11// ruler existed for a simulator that did not. This organ is the simulator; that validator is
12// its oracle (see nx_softtissue_gate.nx). Same solver serves buttocks, thigh and abdomen:
13// the tissue differs only by PROFILE ROWS, never by code (rule 11, rule 25).
14//
15// METHOD (XPBD, Macklin et al. -- position-based, so it cannot blow up like a force solver):
16// per substep: predict -> solve constraints N times -> derive velocity -> damp
17// constraints: distance (edges) | volume (Neo-Hookean incompressibility, C = 6V - 6V0)
18// | contact (sphere/capsule = a finger or a surgical instrument)
19// | attachment (chest-wall particles pinned, inverse mass 0)
20// compliance: alpha_tilde = alpha / dt^2, per LAYER, from data rows. alpha 0 = rigid
21// (muscle, Cooper's ligament), alpha large = soft (adipose).
22//
23// WHY IT CANNOT INVERT: the volume constraint's correction magnitude scales as 1/(6V) as the
24// element collapses, so resistance rises without bound as det F -> 0. Tissue gets HARDER the
25// harder it is pressed -- the same qualitative guarantee the Neo-Hookean strain-energy
26// barrier gives, which is why a deep press dimples instead of exploding. Proven by tooth, not
27// asserted: the gate presses a capsule 60% into the cage and asserts every 6V stays > 0.
28//
29// UNITS: positions in cmm (0.01 mm) -- the NXA VERT unit, so cage and mesh share a frame.
30// H=171530 cmm = 1.7153 m. Scalars q12 (4096=1.0), compliance q20 (1048576=1.0).
31// Velocity cmm/s. dt in microseconds. Frame: +x lateral, +y cranial, +z anterior.
32// Gravity is a UNIT VECTOR in that body frame -- standing (0,-1,0), supine (0,0,-1).
33// That single input is what makes lying-down flattening emergent, not scripted.
34// QUANTIZATION (honest): positions are integer cmm, so one substep's displacement carries
35// ~1 cmm of rounding. st_div_r rounds to NEAREST (not toward zero) so the error does
36// not bias into fake damping -- a truncating divide would silently drain energy and
37// read as viscosity we did not model.
38// license_tier: ORIGINAL No hw writes (Rule 26).
39import "nx_syscalls.nx"
40import "nx_proportion_guard.nx"
41import "nx_vecmath.nx"
42const ST_MAGIC_1229: i64 = 1229
43const ST_MAGIC_12288: i64 = 12288
44const ST_MAGIC_20972: i64 = 20972
45const ST_MAGIC_1434: i64 = 1434
46const ST_MAGIC_2654435761: i64 = 2654435761
47const ST_MAGIC_1013904223: i64 = 1013904223
48const ST_MAGIC_1442695040888963407: i64 = 1442695040888963407
49const ST_MAGIC_2246822519: i64 = 2246822519
50const ST_MAGIC_288230376151711743: i64 = 288230376151711743
51const ST_MAGIC_1050: i64 = 1050
52const ST_MAGIC_9216: i64 = 9216
53const ST_MAGIC_1024: i64 = 1024
54const ST_MAGIC_4096: i64 = 4096
55const ST_MAGIC_22528: i64 = 22528
56const ST_MAGIC_2048: i64 = 2048
57const ST_MAGIC_8192: i64 = 8192
58const ST_MAGIC_24576: i64 = 24576
59const ST_MAGIC_20480: i64 = 20480
60const ST_MAGIC_1000000: i64 = 1000000
61const ST_MAGIC_999999999: i64 = 999999999
62const ST_MAGIC_999999999999999: i64 = 999999999999999
63
64// ===== capacities =====================================================
65const ST_MAXP: i64 = 8192
66const ST_MAXT: i64 = 16384
67const ST_MAXE: i64 = 49152
68
69// ===== layers =========================================================
70const ST_LAY_MUSCLE: i64 = 0
71const ST_LAY_GLAND: i64 = 1
72const ST_LAY_ADIPOSE: i64 = 2
73const ST_LAY_SKIN: i64 = 3
74const ST_LAY_LIGAMENT: i64 = 4
75const ST_LAY_N: i64 = 5
76
77// ===== context slots ==================================================
78const ST_W_NP: i64 = 0
79const ST_W_NT: i64 = 1
80const ST_W_NE: i64 = 2
81const ST_W_PX: i64 = 3
82const ST_W_PY: i64 = 4
83const ST_W_PZ: i64 = 5
84const ST_W_QX: i64 = 6
85const ST_W_QY: i64 = 7
86const ST_W_QZ: i64 = 8
87const ST_W_VX: i64 = 9
88const ST_W_VY: i64 = 10
89const ST_W_VZ: i64 = 11
90const ST_W_IW: i64 = 12
91const ST_W_LAY: i64 = 13
92const ST_W_TET: i64 = 14
93const ST_W_TRV: i64 = 15
94const ST_W_TLA: i64 = 16
95const ST_W_TLM: i64 = 17
96const ST_W_EDG: i64 = 18
97const ST_W_ERL: i64 = 19
98const ST_W_ELA: i64 = 20
99const ST_W_ELM: i64 = 21
100const ST_W_SRF: i64 = 22
101const ST_W_TON: i64 = 23
102const ST_W_TCX: i64 = 24
103const ST_W_TCY: i64 = 25
104const ST_W_TCZ: i64 = 26
105const ST_W_TR: i64 = 27
106const ST_W_PROF:i64 = 28
107// CONTACT READBACK (2026-08-25). The solver already RESOLVES contact; it never REPORTED it, so nothing
108// downstream could tell firm tissue from soft and no feedback loop could close. These two accumulate the
109// hard projection the contact constraint performs, summed over every iteration of a substep and zeroed at
110// its start, which is the constraint impulse for that substep.
111// UNITS, STATED HONESTLY: iw is a uniform q12, so every free particle is UNIT MASS and the solver carries
112// NO KILOGRAM ANYWHERE. An absolute newton is therefore NOT derivable here and this file does not pretend
113// otherwise -- it would need a density model the profile rows do not carry, which is a named follow-on.
114// What IS exact: depth in cmm, the engaged-particle count, and an impulse in cmm/s against unit mass. The
115// two properties a haptic loop actually needs are unit-INDEPENDENT and provable -- force rises with depth,
116// and the stiff layer reads harder than the soft one at equal depth.
117const ST_W_CDEP: i64 = 29 // sum of push-out distance over the substep, cmm
118const ST_W_CN: i64 = 30 // particles the probe engaged on the last iteration
119// CONTACT FRICTION (2026-08-25). mu in per-256, or ST_MU_UNSET meaning NO CITED COEFFICIENT EXISTS.
120// CITE OR ABSTAIN, APPLIED TO PHYSICS. A friction coefficient is a material fact, not a tunable, and the
121// estate has no cited macroscopic skin-on-skin Coulomb mu whose bytes anyone here has read. Two open-access
122// papers were fetched and pinned on 2026-08-25 and NEITHER supplies one: cmp_softbody_skinfriction.html is
123// a Frictiometer FR 770 device index over 633 subjects (an instrument reading, not a Coulomb mu), and
124// cmp_softbody_skinmicro.html reports MICROASPERITY values of 0.004 and 0.001 from topography alone. The
125// widely quoted 0.2-0.5 dry / >1 moist band comes from a review this estate has NOT read the bytes of.
126// So the DEFAULT IS UNSET AND FRICTION IS SIMPLY NOT APPLIED. A caller that wants it must pass a number and
127// own where it came from. Shipping a plausible 0.3 here would have been exactly the invented constant this
128// whole lane exists to remove -- and it would have been invisible, because friction always looks plausible.
129const ST_W_MU: i64 = 31
130const ST_MU_UNSET: i64 = 0 - 1
131// Probe displacement PENDING for the next substep, cmm. Coulomb friction resists the tangential slip of
132// the particle RELATIVE TO THE SURFACE IT TOUCHES; measuring the particle's own world-space motion instead
133// damps it against the room, which is a different physical claim and a wrong one. The probe only ever moves
134// between st_set_touch calls, so its displacement is accumulated there and CONSUMED by the next substep --
135// which is why these hold a pending delta rather than a previous position.
136const ST_W_TDX: i64 = 32
137const ST_W_TDY: i64 = 33
138const ST_W_TDZ: i64 = 34
139// SB10 self-contact readback. SCT is the PAIR COUNT ACTUALLY TESTED, and it is a first-class output
140// rather than a diagnostic: an all-pairs pass over two 368-particle surfaces is 135,424 tests per
141// iteration, so whether this rung is affordable is a number the caller must be able to read, not a
142// property anyone should assume. A cost you cannot measure is a cost you will discover in production.
143const ST_W_SCN: i64 = 35
144const ST_W_SCT: i64 = 36
145// PEAK pairs in contact across the passes. The residual count (SCN) answers "is it resolved" and is
146// meant to fall to zero; on its own it cannot distinguish a solved contact from one that never
147// happened. The peak answers "how much contact was there" -- the size of the patch -- which is what a
148// haptic or therapeutic consumer actually wants and what tells a reader whether a passing test
149// exercised a contact patch or a single grazing pair.
150const ST_W_SCP: i64 = 37
151// SB16 engorgement: the PRISTINE rest state, snapshotted lazily on the first inflate so every
152// later call is ABSOLUTE rather than compounding. Without these, "inflate to 1.2" applied twice
153// means 1.44 and there is no way back to 1.0 -- an actuator you cannot return to rest is a one-way
154// door, and arousal is by definition a state the body leaves again.
155const ST_W_ERL0: i64 = 38
156const ST_W_TRV0: i64 = 39
157const ST_W_SLOTS: i64 = 44
158
159// ===== profile fields =================================================
160const ST_PF_A_MM: i64 = 0
161const ST_PF_B_MM: i64 = 1
162const ST_PF_C_MM: i64 = 2
163const ST_PF_GLAND_PCT: i64 = 3
164const ST_PF_LIG_ALPHA: i64 = 4
165const ST_PF_FAT_ALPHA: i64 = 5
166const ST_PF_FAT_DAMP: i64 = 6
167const ST_PF_RIB_R_MM: i64 = 7
168// Skin-envelope extensibility. The tissue is near-incompressible, so lying down forces it to
169// SPREAD, and spreading is paid for by stretching the envelope -- a lax envelope flattens
170// further than a taut one holding identical filling.
171// MEASURED CONTRIBUTION, not assumed: neutralising this row to one global value moves
172// LARGE_SOFT's lying-down flattening from 134 to 125 per-mille, so the envelope accounts for
173// roughly 7% of it while fat compliance, gland fraction and ligament strength carry the rest.
174// An earlier comment here claimed the firmness law depended on this row; the mutation run
175// disproved that, and ST_PROF_SOFT_TAUT exists to gate what the row actually does.
176const ST_PF_SKIN_ALPHA: i64 = 8
177const ST_PF_N: i64 = 9
178
179const ST_PROF_LARGE_SOFT: i64 = 0
180const ST_PROF_SMALL_FIRM: i64 = 1
181const ST_PROF_GLUTE: i64 = 2
182// LARGE_SOFT in every respect EXCEPT a taut envelope. Exists so the skin row can be gated by
183// a controlled comparison -- one variable changed, geometry and filling identical.
184const ST_PROF_SOFT_TAUT: i64 = 3
185// Any profile id at or above this is DERIVED FROM A SEED, not enumerated: st_profile()
186// forwards it to st_profile_seeded(id - ST_PROF_SEED_BASE). The four named profiles above are
187// reference points for the gate's controlled comparisons -- they are NOT the population.
188// Shipping a handful of fixed bodies would be a clone army; real bodies vary continuously in
189// size, fullness, firmness and frame, and the named rows exist only so a tooth can hold one
190// variable still. Same principle as the craft lane's 20-bit NPC genome.
191const ST_PROF_SEED_BASE: i64 = 1000
192
193const ST_Q12: i64 = 4096
194const ST_Q20: i64 = 1048576
195const ST_GRAV_CMM: i64 = 981000
196// Volume math runs on RAW cmm differences. An earlier draft pre-divided them by 256 to buy
197// overflow headroom and destroyed the constraint instead: a 10 mm cell scaled to a 6V of ~48,
198// so |grad|^2 quantised to ZERO, the denominator collapsed to the compliance term alone, and
199// a deep press produced corrections that overflowed i64. Measured headroom at raw scale:
200// 6V ~ 3.4e9, sum|grad|^2 ~ 6e13 -- five orders inside the i64 ceiling.
201const ST_GRAD_SHIFT: i64 = 1
202// Substep the compliance rows are calibrated at. alpha_tilde = alpha/dt^2, so a different
203// substep rate rescales by (ref/dt)^2 -- that is what keeps material softness a property of
204// the TISSUE and not of the frame rate.
205const ST_DT_REF_US: i64 = 4167
206
207// ===== small helpers ==================================================
208func st_abs(v: i64) -> i64 { if v < 0 { return 0 - v } return v }
209func st_min(a: i64, b: i64) -> i64 { if a < b { return a } return b }
210func st_max(a: i64, b: i64) -> i64 { if a > b { return a } return b }
211
212// Divide rounding to NEAREST, sign-symmetric. Truncation here would bias every
213// velocity toward zero and counterfeit damping we did not model.
214func st_div_r(a: i64, b: i64) -> i64 {
215 if b == 0 { return 0 }
216 if a >= 0 { return (a + b / 2) / b }
217 return 0 - ((0 - a + b / 2) / b)
218}
219
220func st_isqrt(n: i64) -> i64 { return vm_isqrt(n) }
221
222// ===== DATA ROWS: tissue mechanics ====================================
223// RELATIVE compliance in q12 (4096 = 1.0): alpha expressed against the constraint's own
224// stiffness scale. Our positions are cmm and our masses are unitless, so there is no absolute
225// force scale to hang an SI compliance on -- stating alpha relatively is the honest form, and
226// it is what makes a row like "adipose = 6.0" mean something a reader can reason about
227// (six times as yielding as a rigid constraint). alpha=0 is rigid; bigger is softer.
228// Tuning tissue means editing DATA here, never solver code.
229// SB4, 2026-08-25: THE GLAND/ADIPOSE RATIO IS NOW CITED, NOT TUNED.
230// The two constants below used to be ST_MAGIC_1229 and ST_MAGIC_12288 -- hand-tuned numbers that
231// nx_magic apply had RENAMED rather than sourced. A MAGIC NUMBER GIVEN A NAME IS STILL A MAGIC
232// NUMBER: the grep goes quiet and the arbitrariness is untouched. Their implied stiffness ratio was
233// 12288/1229 = 9.998 (compliance is inverse stiffness), against a published within-study band of
234// 1.420..5.698 -- 1.75x above the TOP of the literature.
235//
236// WHY A RATIO AND NOT MODULI: elastography moduli for the same tissue span ~50x across studies
237// (adipose 0.33 to 17.1 kPa in the source review's own table). That spread is METHOD, not biology,
238// so absolute values are not transferable into a solver and none is banked. The WITHIN-STUDY ratio
239// is what survives, because the method cancels. Band + the four ratios live in
240// knowledge/gamefeel_oracle.conf as tissue_gland_adipose_ratio_permil.
241//
242// WHY ADIPOSE IS THE ANCHOR, AND IT IS A MEASUREMENT: fixing a ratio leaves the absolute SCALE free,
243// and this solver has no referent for it (no kilogram -- alpha is declared RELATIVE by the header
244// above). Censusing the distance-constraint population of our own cages, adipose carries 4666/5833,
245// 1922/2990 and 7088/9711 edges (80%, 64%, 73%) while gland carries 43, 372 and 1007 (0.7%, 12%,
246// 10%). Holding adipose fixed therefore preserves the organ's gross compliance essentially exactly
247// -- on ST_PROF_LARGE_SOFT the population-weighted mean moves 9926 -> 9960, 0.34%. Anchoring on
248// gland instead would have rescaled the whole organ to fix a 0.7% minority.
249const ST_GA_RATIO_PERMIL: i64 = 2106 // median of the four cited within-study MRE ratios
250const ST_ALPHA_ADIPOSE: i64 = 12288 // THE ANCHOR: unchanged, for the measured reason above
251// DERIVED at every call rather than precomputed, so the ratio cannot silently drift from the conf.
252func st_alpha_gland() -> i64 { return ST_ALPHA_ADIPOSE * 1000 / ST_GA_RATIO_PERMIL }
253
254func st_layer_alpha_dist(l: i64) -> i64 {
255 if l == ST_LAY_MUSCLE { return 0 }
256 if l == ST_LAY_GLAND { return st_alpha_gland() }
257 if l == ST_LAY_ADIPOSE { return ST_ALPHA_ADIPOSE }
258 // SKIN IS UNCITED AND DELIBERATELY UNCHANGED. No within-study skin/adipose pairing was found in
259 // the mirrored source, so its value stays where it was and is NOT claimed to be validated. Its
260 // relative position (stiffer than the new gland) is at least directionally consistent with the
261 // literature's stated skin range, but a direction is not a citation and this is named as a gap.
262 if l == ST_LAY_SKIN { return ST_MAGIC_1229 }
263 // MUSCLE AND LIGAMENT STAY RIGID, AND FOR LIGAMENT THAT IS NOW CITED RATHER THAN ASSUMED:
264 // gamefeel_oracle.conf tissue_cooper_kpa is 1000..2000 kPa against glandular tissue at ~2.45 kPa
265 // -- 400x to 800x stiffer, so alpha=0 is the correct limit and not a placeholder. Muscle has no
266 // mirrored within-study value here and its rigidity remains an assumption, named as one.
267 if l == ST_LAY_LIGAMENT { return 0 }
268 return ST_MAGIC_1229
269}
270// Volume softness RANKING only -- it selects each tet's material (softest corner wins).
271// The volume constraint itself is solved HARD: soft tissue is ~incompressible in reality,
272// its give is shear and stretch, which is where the distance compliance above lives.
273func st_layer_alpha_vol(l: i64) -> i64 {
274 if l == ST_LAY_MUSCLE { return 0 }
275 if l == ST_LAY_GLAND { return 524 }
276 if l == ST_LAY_ADIPOSE { return ST_MAGIC_20972 }
277 if l == ST_LAY_SKIN { return 0 }
278 if l == ST_LAY_LIGAMENT { return 0 }
279 return 524
280}
281// Per-layer viscous damping q12: the fraction of velocity dissipated each substep.
282// This is what makes tissue SETTLE instead of ringing like jelly forever.
283func st_layer_damp(l: i64) -> i64 {
284 if l == ST_LAY_MUSCLE { return ST_MAGIC_1229 }
285 if l == ST_LAY_GLAND { return 491 }
286 if l == ST_LAY_ADIPOSE { return 246 }
287 if l == ST_LAY_SKIN { return 819 }
288 if l == ST_LAY_LIGAMENT { return ST_MAGIC_1434 }
289 return 491
290}
291
292// ===== DATA ROWS: body-region profiles ================================
293// The operator's law as data: "larger and softer flattens more; firmer flattens less."
294// LARGE_SOFT = big, fat-dominant, weak suspensory ligament.
295// SMALL_FIRM = smaller, gland-dominant, rigid ligament.
296// GLUTE = the same solver for buttocks: a dense near-rigid muscle base under a thick
297// high-compliance fat layer (the operator's gluteus/subcutaneous split).
298// Deterministic per-body hash. Same seed always yields the same body -- a body must be
299// reproducible from its id, or nothing downstream (save files, twins, review captures) can
300// refer to it.
301func st_hash(seed: i64, salt: i64) -> i64 {
302 var h: i64 = seed * ST_MAGIC_2654435761 + salt * ST_MAGIC_1013904223 + ST_MAGIC_1442695040888963407
303 h = h ^ (h >> 15)
304 h = h * ST_MAGIC_2246822519
305 h = h ^ (h >> 13)
306 h = h & ST_MAGIC_288230376151711743
307 if h < 0 { h = 0 - h }
308 return h
309}
310// Uniform pick in [lo,hi] inclusive.
311func st_span(seed: i64, salt: i64, lo: i64, hi: i64) -> i64 {
312 if hi <= lo { return lo }
313 return lo + st_hash(seed, salt) % (hi - lo + 1)
314}
315
316// A body derived from its seed. Fields are NOT drawn independently: a single "fullness" axis
317// (0..1000) moves size, fat compliance, envelope laxity and suspensory load together, because
318// independent draws produce implausible combinations (a tiny body with maximal fat compliance
319// and lax skin is not a person). Per-field jitter on separate salts keeps it from collapsing
320// to a one-dimensional family, and frame size (ribcage) is drawn independently of fullness --
321// a broad frame and a full bust are genuinely unrelated traits.
322// BOUNDS ARE STATED WITH UNITS and were chosen to span the range the named reference profiles
323// occupy (52..82mm half-width etc.), not by taste; widen them by measurement, never by feel.
324func st_profile_seeded(seed: i64, field: i64) -> i64 {
325 let full: i64 = st_hash(seed, 101) % 1001
326 // Size axes are DRAWN FROM THE GUARD's envelope, not from numbers written here, so a body
327 // outside the plausible range cannot be constructed at all -- a post-hoc check would only
328 // report the mistake after the geometry existed. A_MM is a semi-axis, hence the halving of
329 // the breast-diameter envelope; B_MM (cranio-caudal) has no sealed axis of its own and is
330 // derived from A by a bounded ratio, which keeps it finite too.
331 if field == ST_PF_A_MM {
332 return pg_sample(NX_AXIS_BREAST_DIAMETER, PG_IDEALIZED, full) / 2
333 }
334 if field == ST_PF_B_MM {
335 let am: i64 = pg_sample(NX_AXIS_BREAST_DIAMETER, PG_IDEALIZED, full) / 2
336 return am * (ST_MAGIC_1050 + st_span(seed, 12, 0, 150)) / 1000
337 }
338 if field == ST_PF_C_MM {
339 return pg_sample(NX_AXIS_BREAST_PROJECTION, PG_IDEALIZED, full)
340 }
341 if field == ST_PF_GLAND_PCT { return 68 - full * 42 / 1000 + st_span(seed, 14, 0, 7) }
342 if field == ST_PF_LIG_ALPHA { return full * ST_MAGIC_9216 / 1000 + st_span(seed, 15, 0, ST_MAGIC_1024) }
343 if field == ST_PF_FAT_ALPHA { return ST_MAGIC_4096 + full * ST_MAGIC_22528 / 1000 + st_span(seed, 16, 0, ST_MAGIC_2048) }
344 if field == ST_PF_FAT_DAMP { return 430 - full * 220 / 1000 + st_span(seed, 17, 0, 40) }
345 if field == ST_PF_SKIN_ALPHA { return 410 + full * ST_MAGIC_4096 / 1000 + st_span(seed, 18, 0, 410) }
346 if field == ST_PF_RIB_R_MM { return st_span(seed, 19, 118, 146) }
347 return 0
348}
349
350func st_profile(prof: i64, field: i64) -> i64 {
351 if prof >= ST_PROF_SEED_BASE { return st_profile_seeded(prof - ST_PROF_SEED_BASE, field) }
352 if prof == ST_PROF_LARGE_SOFT {
353 if field == ST_PF_A_MM { return 75 }
354 if field == ST_PF_B_MM { return 85 }
355 if field == ST_PF_C_MM { return 70 }
356 if field == ST_PF_GLAND_PCT { return 30 }
357 if field == ST_PF_LIG_ALPHA { return ST_MAGIC_8192 }
358 if field == ST_PF_FAT_ALPHA { return ST_MAGIC_24576 }
359 if field == ST_PF_FAT_DAMP { return 205 }
360 if field == ST_PF_RIB_R_MM { return 130 }
361 if field == ST_PF_SKIN_ALPHA { return ST_MAGIC_4096 }
362 }
363 if prof == ST_PROF_SMALL_FIRM {
364 if field == ST_PF_A_MM { return 62 }
365 if field == ST_PF_B_MM { return 68 }
366 if field == ST_PF_C_MM { return 55 }
367 if field == ST_PF_GLAND_PCT { return 62 }
368 if field == ST_PF_LIG_ALPHA { return 0 }
369 if field == ST_PF_FAT_ALPHA { return ST_MAGIC_4096 }
370 if field == ST_PF_FAT_DAMP { return 410 }
371 if field == ST_PF_RIB_R_MM { return 130 }
372 if field == ST_PF_SKIN_ALPHA { return 410 }
373 }
374 if prof == ST_PROF_GLUTE {
375 if field == ST_PF_A_MM { return 95 }
376 if field == ST_PF_B_MM { return 95 }
377 if field == ST_PF_C_MM { return 80 }
378 if field == ST_PF_GLAND_PCT { return 55 }
379 if field == ST_PF_LIG_ALPHA { return ST_MAGIC_2048 }
380 if field == ST_PF_FAT_ALPHA { return ST_MAGIC_20480 }
381 if field == ST_PF_FAT_DAMP { return 287 }
382 if field == ST_PF_RIB_R_MM { return 165 }
383 if field == ST_PF_SKIN_ALPHA { return ST_MAGIC_2048 }
384 }
385 if prof == ST_PROF_SOFT_TAUT {
386 if field == ST_PF_SKIN_ALPHA { return 410 }
387 return st_profile(ST_PROF_LARGE_SOFT, field)
388 }
389 return 0
390}
391
392// Depth (mm) that the supporting bone surface falls away at lateral offset xm. The ribcage
393// is a cylinder about the cranial axis, so it curves in x and NOT in y -- and that asymmetry
394// is the whole reason supine tissue spreads sideways rather than up and down. Modelling the
395// wall as a flat plane silently deletes the effect the operator asked for, so the wall is
396// geometry here, not a constant.
397func st_wall_drop(xm: i64, rib_r: i64) -> i64 {
398 if rib_r <= 0 { return 0 }
399 let ax: i64 = st_abs(xm)
400 if ax >= rib_r { return rib_r }
401 return rib_r - st_isqrt(rib_r*rib_r - ax*ax)
402}
403
404// Effective per-edge/per-tet compliance: layer row, overridden by the profile for the two
405// tissues a body-composition choice actually changes (fat softness, ligament strength).
406func st_eff_alpha_dist(W: *i64, layer: i64) -> i64 {
407 let prof: i64 = W[ST_W_PROF]
408 if layer == ST_LAY_ADIPOSE { return st_profile(prof, ST_PF_FAT_ALPHA) }
409 if layer == ST_LAY_LIGAMENT { return st_profile(prof, ST_PF_LIG_ALPHA) }
410 if layer == ST_LAY_SKIN { return st_profile(prof, ST_PF_SKIN_ALPHA) }
411 return st_layer_alpha_dist(layer)
412}
413func st_eff_damp(W: *i64, layer: i64) -> i64 {
414 if layer == ST_LAY_ADIPOSE { return st_profile(W[ST_W_PROF], ST_PF_FAT_DAMP) }
415 return st_layer_damp(layer)
416}
417
418// alpha_tilde = alpha/dt^2, scaled against this constraint's own stiffness sum.
419// dsum = sum of w|grad C|^2 for the constraint (its rigid-response scale)
420// At the reference substep the result is simply alpha_rel * dsum; halving the substep
421// quadruples it, exactly as alpha/dt^2 requires. Returning it in dsum's units lets the caller
422// write den = dsum + alpha_tilde with no further conversion -- and, crucially, lambda then
423// converges to a NON-ZERO residual, so a soft material stays soft no matter how many solver
424// iterations run. (Plain PBD drives every material to rigid as iterations rise; that bug is
425// what made "large and soft" flatten LESS than "small and firm" in the first gate run.)
426func st_alpha_tilde(alpha_q12: i64, dsum: i64, dt_us: i64) -> i64 {
427 if alpha_q12 <= 0 { return 0 }
428 if dt_us <= 0 { return 0 }
429 var at: i64 = st_div_r(alpha_q12 * dsum, ST_Q12)
430 at = st_div_r(at * ST_DT_REF_US, dt_us)
431 at = st_div_r(at * ST_DT_REF_US, dt_us)
432 return at
433}
434
435// ===== cage construction ==============================================
436// Half-ellipsoid lattice cage. Inside test runs in MILLIMETRES (a*a*b*b*c*c overflows i64
437// in cmm) -- the shape is a grid predicate, so mm resolution is exact enough.
438func st_inside(xm: i64, ym: i64, zm: i64, a: i64, b: i64, c: i64) -> i64 {
439 if zm < 0 { return 0 }
440 let lhs: i64 = xm*xm*b*b*c*c + ym*ym*a*a*c*c + zm*zm*a*a*b*b
441 let rhs: i64 = a*a*b*b*c*c
442 if lhs <= rhs { return 1 }
443 return 0
444}
445
446func st_six_vol(W: *i64, i0: i64, i1: i64, i2: i64, i3: i64) -> i64 {
447 let px: *i64 = W[ST_W_PX] as *i64
448 let py: *i64 = W[ST_W_PY] as *i64
449 let pz: *i64 = W[ST_W_PZ] as *i64
450 let ax: i64 = st_div_r(px[i1] - px[i0], ST_GRAD_SHIFT)
451 let ay: i64 = st_div_r(py[i1] - py[i0], ST_GRAD_SHIFT)
452 let az: i64 = st_div_r(pz[i1] - pz[i0], ST_GRAD_SHIFT)
453 let bx: i64 = st_div_r(px[i2] - px[i0], ST_GRAD_SHIFT)
454 let by: i64 = st_div_r(py[i2] - py[i0], ST_GRAD_SHIFT)
455 let bz: i64 = st_div_r(pz[i2] - pz[i0], ST_GRAD_SHIFT)
456 let cx: i64 = st_div_r(px[i3] - px[i0], ST_GRAD_SHIFT)
457 let cy: i64 = st_div_r(py[i3] - py[i0], ST_GRAD_SHIFT)
458 let cz: i64 = st_div_r(pz[i3] - pz[i0], ST_GRAD_SHIFT)
459 return ax * (by*cz - bz*cy) + ay * (bz*cx - bx*cz) + az * (bx*cy - by*cx)
460}
461
462// Same signed 6V but read from PREDICTED positions (the solver works on q, not p).
463func st_six_vol_q(W: *i64, i0: i64, i1: i64, i2: i64, i3: i64) -> i64 {
464 let qx: *i64 = W[ST_W_QX] as *i64
465 let qy: *i64 = W[ST_W_QY] as *i64
466 let qz: *i64 = W[ST_W_QZ] as *i64
467 let ax: i64 = st_div_r(qx[i1] - qx[i0], ST_GRAD_SHIFT)
468 let ay: i64 = st_div_r(qy[i1] - qy[i0], ST_GRAD_SHIFT)
469 let az: i64 = st_div_r(qz[i1] - qz[i0], ST_GRAD_SHIFT)
470 let bx: i64 = st_div_r(qx[i2] - qx[i0], ST_GRAD_SHIFT)
471 let by: i64 = st_div_r(qy[i2] - qy[i0], ST_GRAD_SHIFT)
472 let bz: i64 = st_div_r(qz[i2] - qz[i0], ST_GRAD_SHIFT)
473 let cx: i64 = st_div_r(qx[i3] - qx[i0], ST_GRAD_SHIFT)
474 let cy: i64 = st_div_r(qy[i3] - qy[i0], ST_GRAD_SHIFT)
475 let cz: i64 = st_div_r(qz[i3] - qz[i0], ST_GRAD_SHIFT)
476 return ax * (by*cz - bz*cy) + ay * (bz*cx - bx*cz) + az * (bx*cy - by*cx)
477}
478
479func st_add_edge(W: *i64, htab: *i64, hidx: *i64, hmask: i64,
480 a: i64, b: i64, layer: i64) -> i64 {
481 var lo: i64 = a
482 var hi: i64 = b
483 if lo > hi { lo = b; hi = a }
484 let key: i64 = lo * ST_MAXP + hi + 1
485 var h: i64 = (key * ST_MAGIC_2654435761) & hmask
486 var guard: i64 = 0
487 while guard < 64 {
488 if htab[h] == 0 {
489 let ne: i64 = W[ST_W_NE]
490 if ne >= ST_MAXE { return 0 - 1 }
491 let edg: *i64 = W[ST_W_EDG] as *i64
492 let erl: *i64 = W[ST_W_ERL] as *i64
493 let ela: *i64 = W[ST_W_ELA] as *i64
494 let px: *i64 = W[ST_W_PX] as *i64
495 let py: *i64 = W[ST_W_PY] as *i64
496 let pz: *i64 = W[ST_W_PZ] as *i64
497 edg[ne*2] = lo
498 edg[ne*2+1] = hi
499 let dx: i64 = px[hi] - px[lo]
500 let dy: i64 = py[hi] - py[lo]
501 let dz: i64 = pz[hi] - pz[lo]
502 erl[ne] = st_isqrt(dx*dx + dy*dy + dz*dz)
503 ela[ne] = layer
504 htab[h] = key
505 hidx[h] = ne
506 W[ST_W_NE] = ne + 1
507 return ne
508 }
509 if htab[h] == key {
510 // Stiffest material wins a shared edge: skin and ligament must not be softened
511 // by an adipose tet that happens to touch the same pair.
512 let ela: *i64 = W[ST_W_ELA] as *i64
513 if st_layer_alpha_dist(layer) < st_layer_alpha_dist(ela[hidx[h]]) {
514 ela[hidx[h]] = layer
515 }
516 return hidx[h]
517 }
518 h = (h + 1) & hmask
519 guard = guard + 1
520 }
521 return 0 - 1
522}
523
524// Build the cage for a profile. h_mm is the lattice cell size (the element-count dial:
525// 10 mm lands ~4-5k tets, the density the operator's blueprint calls for).
526func st_new(prof: i64, h_mm: i64) -> *i64 {
527 let W: *i64 = sys_mmap(ST_W_SLOTS * 8) as *i64
528 var s: i64 = 0
529 while s < ST_W_SLOTS { W[s] = 0; s = s + 1 }
530 W[ST_W_PROF] = prof
531 W[ST_W_PX] = sys_mmap(ST_MAXP*8) as i64
532 W[ST_W_PY] = sys_mmap(ST_MAXP*8) as i64
533 W[ST_W_PZ] = sys_mmap(ST_MAXP*8) as i64
534 W[ST_W_QX] = sys_mmap(ST_MAXP*8) as i64
535 W[ST_W_QY] = sys_mmap(ST_MAXP*8) as i64
536 W[ST_W_QZ] = sys_mmap(ST_MAXP*8) as i64
537 W[ST_W_VX] = sys_mmap(ST_MAXP*8) as i64
538 W[ST_W_VY] = sys_mmap(ST_MAXP*8) as i64
539 W[ST_W_VZ] = sys_mmap(ST_MAXP*8) as i64
540 W[ST_W_IW] = sys_mmap(ST_MAXP*8) as i64
541 // friction defaults to UNSET, never to zero -- see st_set_friction for why those are different answers
542 W[ST_W_MU] = ST_MU_UNSET
543 W[ST_W_LAY] = sys_mmap(ST_MAXP*8) as i64
544 W[ST_W_SRF] = sys_mmap(ST_MAXP*8) as i64
545 W[ST_W_TET] = sys_mmap(ST_MAXT*4*8) as i64
546 W[ST_W_TRV] = sys_mmap(ST_MAXT*8) as i64
547 W[ST_W_TLA] = sys_mmap(ST_MAXT*8) as i64
548 W[ST_W_TLM] = sys_mmap(ST_MAXT*8) as i64
549 W[ST_W_EDG] = sys_mmap(ST_MAXE*2*8) as i64
550 W[ST_W_ERL] = sys_mmap(ST_MAXE*8) as i64
551 W[ST_W_ELA] = sys_mmap(ST_MAXE*8) as i64
552 W[ST_W_ELM] = sys_mmap(ST_MAXE*8) as i64
553
554 let a: i64 = st_profile(prof, ST_PF_A_MM)
555 let b: i64 = st_profile(prof, ST_PF_B_MM)
556 let c: i64 = st_profile(prof, ST_PF_C_MM)
557 let rib: i64 = st_profile(prof, ST_PF_RIB_R_MM)
558 if a <= 0 { return W }
559
560 let gnx: i64 = (2*a) / h_mm + 1
561 let gny: i64 = (2*b) / h_mm + 1
562 let gnz: i64 = c / h_mm + 1
563 let gtot: i64 = (gnx+1) * (gny+1) * (gnz+1)
564 let gid: *i64 = sys_mmap(gtot*8) as *i64
565 var g: i64 = 0
566 while g < gtot { gid[g] = 0 - 1; g = g + 1 }
567
568 let px: *i64 = W[ST_W_PX] as *i64
569 let py: *i64 = W[ST_W_PY] as *i64
570 let pz: *i64 = W[ST_W_PZ] as *i64
571 let iw: *i64 = W[ST_W_IW] as *i64
572 let lay: *i64 = W[ST_W_LAY] as *i64
573 let srf: *i64 = W[ST_W_SRF] as *i64
574
575 // gland core radii (percent of the outer axes) -- the firmness dial
576 let gp: i64 = st_profile(prof, ST_PF_GLAND_PCT)
577 let ga: i64 = a * gp / 100
578 let gb: i64 = b * gp / 100
579 let gc: i64 = c * gp / 100
580
581 // pass 1: particles at inside lattice points
582 var k: i64 = 0
583 while k <= gnz {
584 var j: i64 = 0
585 while j <= gny {
586 var i: i64 = 0
587 while i <= gnx {
588 let xm: i64 = 0 - a + i * h_mm
589 let ym: i64 = 0 - b + j * h_mm
590 let zm: i64 = k * h_mm
591 if st_inside(xm, ym, zm, a, b, c) == 1 {
592 let np: i64 = W[ST_W_NP]
593 if np < ST_MAXP {
594 px[np] = xm * 100
595 py[np] = ym * 100
596 pz[np] = (zm - st_wall_drop(xm, rib)) * 100
597 iw[np] = ST_Q12
598 lay[np] = ST_LAY_ADIPOSE
599 srf[np] = 0
600 gid[(k*(gny+1) + j)*(gnx+1) + i] = np
601 W[ST_W_NP] = np + 1
602 }
603 }
604 i = i + 1
605 }
606 j = j + 1
607 }
608 k = k + 1
609 }
610
611 // pass 2: layers. k==0 is the chest wall -> MUSCLE, pinned (inverse mass 0), the rigid
612 // skeletal anchor. A lattice point missing any 6-neighbour is on the boundary -> SKIN
613 // envelope. Interior points inside the gland ellipsoid -> GLAND. Rest stays ADIPOSE.
614 k = 0
615 while k <= gnz {
616 var j2: i64 = 0
617 while j2 <= gny {
618 var i2: i64 = 0
619 while i2 <= gnx {
620 let id: i64 = gid[(k*(gny+1) + j2)*(gnx+1) + i2]
621 if id >= 0 {
622 if k == 0 {
623 lay[id] = ST_LAY_MUSCLE
624 iw[id] = 0
625 } else {
626 var bnd: i64 = 0
627 if i2 == 0 { bnd = 1 }
628 if i2 == gnx { bnd = 1 }
629 if j2 == 0 { bnd = 1 }
630 if j2 == gny { bnd = 1 }
631 if k == gnz { bnd = 1 }
632 if bnd == 0 {
633 if gid[(k*(gny+1) + j2)*(gnx+1) + i2 - 1] < 0 { bnd = 1 }
634 if gid[(k*(gny+1) + j2)*(gnx+1) + i2 + 1] < 0 { bnd = 1 }
635 if gid[(k*(gny+1) + j2 - 1)*(gnx+1) + i2] < 0 { bnd = 1 }
636 if gid[(k*(gny+1) + j2 + 1)*(gnx+1) + i2] < 0 { bnd = 1 }
637 if gid[((k-1)*(gny+1) + j2)*(gnx+1) + i2] < 0 { bnd = 1 }
638 if gid[((k+1)*(gny+1) + j2)*(gnx+1) + i2] < 0 { bnd = 1 }
639 }
640 if bnd == 1 {
641 lay[id] = ST_LAY_SKIN
642 srf[id] = 1
643 } else {
644 let xm2: i64 = 0 - a + i2 * h_mm
645 let ym2: i64 = 0 - b + j2 * h_mm
646 let zm2: i64 = k * h_mm
647 if st_inside(xm2, ym2, zm2, ga, gb, gc) == 1 {
648 lay[id] = ST_LAY_GLAND
649 }
650 }
651 }
652 }
653 i2 = i2 + 1
654 }
655 j2 = j2 + 1
656 }
657 k = k + 1
658 }
659
660 // pass 3: Kuhn 6-tet subdivision of every fully-inside cell. All six tets share the
661 // main diagonal, so faces conform across neighbouring cells with no parity bookkeeping.
662 let tet: *i64 = W[ST_W_TET] as *i64
663 let trv: *i64 = W[ST_W_TRV] as *i64
664 let tla: *i64 = W[ST_W_TLA] as *i64
665 let cor: *i64 = sys_mmap(8*8) as *i64
666 let kt: *i64 = sys_mmap(24*8) as *i64
667 kt[0]=0; kt[1]=1; kt[2]=3; kt[3]=7
668 kt[4]=0; kt[5]=1; kt[6]=5; kt[7]=7
669 kt[8]=0; kt[9]=4; kt[10]=5; kt[11]=7
670 kt[12]=0; kt[13]=4; kt[14]=6; kt[15]=7
671 kt[16]=0; kt[17]=2; kt[18]=6; kt[19]=7
672 kt[20]=0; kt[21]=2; kt[22]=3; kt[23]=7
673
674 k = 0
675 while k < gnz {
676 var j3: i64 = 0
677 while j3 < gny {
678 var i3: i64 = 0
679 while i3 < gnx {
680 var ok: i64 = 1
681 var cc: i64 = 0
682 while cc < 8 {
683 let dx: i64 = cc & 1
684 let dy: i64 = (cc >> 1) & 1
685 let dz: i64 = (cc >> 2) & 1
686 let id2: i64 = gid[((k+dz)*(gny+1) + j3+dy)*(gnx+1) + i3+dx]
687 cor[cc] = id2
688 if id2 < 0 { ok = 0 }
689 cc = cc + 1
690 }
691 if ok == 1 {
692 var t: i64 = 0
693 while t < 6 {
694 let nt: i64 = W[ST_W_NT]
695 if nt < ST_MAXT {
696 var v0: i64 = cor[kt[t*4]]
697 var v1: i64 = cor[kt[t*4+1]]
698 var v2: i64 = cor[kt[t*4+2]]
699 var v3: i64 = cor[kt[t*4+3]]
700 var sv: i64 = st_six_vol(W, v0, v1, v2, v3)
701 if sv < 0 {
702 let sw: i64 = v2
703 v2 = v3
704 v3 = sw
705 sv = st_six_vol(W, v0, v1, v2, v3)
706 }
707 tet[nt*4] = v0
708 tet[nt*4+1] = v1
709 tet[nt*4+2] = v2
710 tet[nt*4+3] = v3
711 trv[nt] = sv
712 // A tet's material is its SOFTEST corner: fat between gland
713 // lobes must behave as fat, or the model reads uniformly firm.
714 var ml: i64 = ST_LAY_MUSCLE
715 var q: i64 = 0
716 while q < 4 {
717 let lq: i64 = lay[tet[nt*4+q]]
718 if st_layer_alpha_vol(lq) > st_layer_alpha_vol(ml) { ml = lq }
719 q = q + 1
720 }
721 tla[nt] = ml
722 W[ST_W_NT] = nt + 1
723 }
724 t = t + 1
725 }
726 }
727 i3 = i3 + 1
728 }
729 j3 = j3 + 1
730 }
731 k = k + 1
732 }
733
734 // pass 4: unique edges from tets
735 var hbits: i64 = 1
736 while hbits < ST_MAXE * 4 { hbits = hbits * 2 }
737 let htab: *i64 = sys_mmap(hbits*8) as *i64
738 let hidx: *i64 = sys_mmap(hbits*8) as *i64
739 var hz: i64 = 0
740 while hz < hbits { htab[hz] = 0; hidx[hz] = 0; hz = hz + 1 }
741 let hmask: i64 = hbits - 1
742
743 var tI: i64 = 0
744 while tI < W[ST_W_NT] {
745 var e1: i64 = 0
746 while e1 < 4 {
747 var e2: i64 = e1 + 1
748 while e2 < 4 {
749 let A: i64 = tet[tI*4+e1]
750 let B: i64 = tet[tI*4+e2]
751 var el: i64 = ST_LAY_ADIPOSE
752 if lay[A] == lay[B] { el = lay[A] }
753 if lay[A] == ST_LAY_SKIN { if lay[B] == ST_LAY_SKIN { el = ST_LAY_SKIN } }
754 st_add_edge(W, htab, hidx, hmask, A, B, el)
755 e2 = e2 + 1
756 }
757 e1 = e1 + 1
758 }
759 tI = tI + 1
760 }
761
762 // pass 5: Cooper's ligaments -- LONG-RANGE suspensory constraints from the chest wall to
763 // the skin envelope above it. Lattice edges are only one cell long; without these the
764 // tissue has nothing holding it up and every profile droops identically. This is the
765 // structure the firmness law actually rides on.
766 var i4: i64 = 0
767 while i4 <= gnx {
768 var j4: i64 = 0
769 while j4 <= gny {
770 let base: i64 = gid[(0*(gny+1) + j4)*(gnx+1) + i4]
771 if base >= 0 {
772 var top: i64 = 0 - 1
773 var k4: i64 = gnz
774 while k4 > 0 {
775 if top < 0 {
776 let cand: i64 = gid[(k4*(gny+1) + j4)*(gnx+1) + i4]
777 if cand >= 0 { top = cand }
778 }
779 k4 = k4 - 1
780 }
781 if top >= 0 { st_add_edge(W, htab, hidx, hmask, base, top, ST_LAY_LIGAMENT) }
782 }
783 j4 = j4 + 1
784 }
785 i4 = i4 + 1
786 }
787 return W
788}
789
790// ===== solver =========================================================
791func st_reset_state(W: *i64) -> i64 {
792 let vx: *i64 = W[ST_W_VX] as *i64
793 let vy: *i64 = W[ST_W_VY] as *i64
794 let vz: *i64 = W[ST_W_VZ] as *i64
795 var i: i64 = 0
796 while i < W[ST_W_NP] { vx[i] = 0; vy[i] = 0; vz[i] = 0; i = i + 1 }
797 return 0
798}
799
800func st_set_touch(W: *i64, on: i64, cx: i64, cy: i64, cz: i64, r: i64) -> i64 {
801 // Only when the probe was ALREADY touching: the first activation is a teleport from nowhere, and
802 // charging that to friction would drag the entire surface sideways at the instant of contact.
803 if W[ST_W_TON] == 1 {
804 if on == 1 {
805 W[ST_W_TDX] = W[ST_W_TDX] + (cx - W[ST_W_TCX])
806 W[ST_W_TDY] = W[ST_W_TDY] + (cy - W[ST_W_TCY])
807 W[ST_W_TDZ] = W[ST_W_TDZ] + (cz - W[ST_W_TCZ])
808 }
809 }
810 W[ST_W_TON] = on
811 W[ST_W_TCX] = cx
812 W[ST_W_TCY] = cy
813 W[ST_W_TCZ] = cz
814 W[ST_W_TR] = r
815 return 0
816}
817
818// One XPBD substep. gx/gy/gz is the gravity UNIT vector in q12 (body frame).
819func st_substep(W: *i64, gx: i64, gy: i64, gz: i64, dt_us: i64, iters: i64) -> i64 {
820 // contact readback is PER SUBSTEP: zero here, accumulate across this substep's iterations. Zeroing
821 // per ITERATION would report only the last correction and read as a converging contact getting
822 // weaker; zeroing never would make it a lifetime total that only ever rises. Neither is the impulse.
823 W[ST_W_CDEP] = 0
824 W[ST_W_CN] = 0
825 let px: *i64 = W[ST_W_PX] as *i64
826 let py: *i64 = W[ST_W_PY] as *i64
827 let pz: *i64 = W[ST_W_PZ] as *i64
828 let qx: *i64 = W[ST_W_QX] as *i64
829 let qy: *i64 = W[ST_W_QY] as *i64
830 let qz: *i64 = W[ST_W_QZ] as *i64
831 let vx: *i64 = W[ST_W_VX] as *i64
832 let vy: *i64 = W[ST_W_VY] as *i64
833 let vz: *i64 = W[ST_W_VZ] as *i64
834 let iw: *i64 = W[ST_W_IW] as *i64
835 let lay: *i64 = W[ST_W_LAY] as *i64
836 let np: i64 = W[ST_W_NP]
837
838 // gravity integration + prediction
839 let dv: i64 = ST_GRAV_CMM * dt_us / ST_MAGIC_1000000
840 var i: i64 = 0
841 while i < np {
842 if iw[i] > 0 {
843 vx[i] = vx[i] + dv * gx / ST_Q12
844 vy[i] = vy[i] + dv * gy / ST_Q12
845 vz[i] = vz[i] + dv * gz / ST_Q12
846 } else {
847 vx[i] = 0; vy[i] = 0; vz[i] = 0
848 }
849 qx[i] = px[i] + st_div_r(vx[i] * dt_us, ST_MAGIC_1000000)
850 qy[i] = py[i] + st_div_r(vy[i] * dt_us, ST_MAGIC_1000000)
851 qz[i] = pz[i] + st_div_r(vz[i] * dt_us, ST_MAGIC_1000000)
852 i = i + 1
853 }
854
855 // zero the XPBD multipliers -- lambda accumulates WITHIN a substep only
856 let elm: *i64 = W[ST_W_ELM] as *i64
857 let tlm: *i64 = W[ST_W_TLM] as *i64
858 var z: i64 = 0
859 while z < W[ST_W_NE] { elm[z] = 0; z = z + 1 }
860 z = 0
861 while z < W[ST_W_NT] { tlm[z] = 0; z = z + 1 }
862
863 let edg: *i64 = W[ST_W_EDG] as *i64
864 let erl: *i64 = W[ST_W_ERL] as *i64
865 let ela: *i64 = W[ST_W_ELA] as *i64
866 let tet: *i64 = W[ST_W_TET] as *i64
867 let trv: *i64 = W[ST_W_TRV] as *i64
868 let tla: *i64 = W[ST_W_TLA] as *i64
869
870 var it: i64 = 0
871 while it < iters {
872 // --- distance constraints -------------------------------------
873 var e: i64 = 0
874 while e < W[ST_W_NE] {
875 let A: i64 = edg[e*2]
876 let B: i64 = edg[e*2+1]
877 let wA: i64 = iw[A]
878 let wB: i64 = iw[B]
879 if wA + wB > 0 {
880 let dx: i64 = qx[A] - qx[B]
881 let dy: i64 = qy[A] - qy[B]
882 let dz: i64 = qz[A] - qz[B]
883 let L: i64 = st_isqrt(dx*dx + dy*dy + dz*dz)
884 var live: i64 = 1
885 var C: i64 = L - erl[e]
886 // Cooper's ligaments are SUSPENSORY -- rope, not strut. They resist stretch
887 // and go slack under compression. Solving them bilaterally (the first draft)
888 // planted a rigid column from chest wall to apex pointing exactly along the
889 // direction supine gravity acts, which suppressed lying-down flattening and
890 // INVERTED the firmness law: firm tissue's stiffer ligaments made it flatten
891 // more, not less. A tension-only constraint is the anatomy, and it restores
892 // both behaviours at once.
893 if ela[e] == ST_LAY_LIGAMENT { if C < 0 { live = 0 } }
894 if live == 0 { C = 0 }
895 if L > 0 && live == 1 {
896 let at: i64 = st_alpha_tilde(st_eff_alpha_dist(W, ela[e]), wA + wB, dt_us)
897 let den: i64 = wA + wB + at
898 if den > 0 {
899 let num: i64 = (0 - C) * ST_Q12 - st_div_r(at * elm[e], ST_Q12)
900 let dl: i64 = st_div_r(num * ST_Q12, den)
901 elm[e] = elm[e] + dl
902 let ux: i64 = st_div_r(dx * ST_Q12, L)
903 let uy: i64 = st_div_r(dy * ST_Q12, L)
904 let uz: i64 = st_div_r(dz * ST_Q12, L)
905 let sA: i64 = st_div_r(wA * dl, ST_Q12)
906 let sB: i64 = st_div_r(wB * dl, ST_Q12)
907 qx[A] = qx[A] + st_div_r(sA * ux, ST_Q12 * ST_Q12)
908 qy[A] = qy[A] + st_div_r(sA * uy, ST_Q12 * ST_Q12)
909 qz[A] = qz[A] + st_div_r(sA * uz, ST_Q12 * ST_Q12)
910 qx[B] = qx[B] - st_div_r(sB * ux, ST_Q12 * ST_Q12)
911 qy[B] = qy[B] - st_div_r(sB * uy, ST_Q12 * ST_Q12)
912 qz[B] = qz[B] - st_div_r(sB * uz, ST_Q12 * ST_Q12)
913 }
914 }
915 }
916 e = e + 1
917 }
918
919 // --- volume constraints (incompressibility / inversion barrier) ---
920 var t: i64 = 0
921 while t < W[ST_W_NT] {
922 let v0: i64 = tet[t*4]
923 let v1: i64 = tet[t*4+1]
924 let v2: i64 = tet[t*4+2]
925 let v3: i64 = tet[t*4+3]
926 let w0: i64 = iw[v0]
927 let w1: i64 = iw[v1]
928 let w2: i64 = iw[v2]
929 let w3: i64 = iw[v3]
930 if w0 + w1 + w2 + w3 > 0 {
931 let ax: i64 = st_div_r(qx[v1] - qx[v0], ST_GRAD_SHIFT)
932 let ay: i64 = st_div_r(qy[v1] - qy[v0], ST_GRAD_SHIFT)
933 let az: i64 = st_div_r(qz[v1] - qz[v0], ST_GRAD_SHIFT)
934 let bx: i64 = st_div_r(qx[v2] - qx[v0], ST_GRAD_SHIFT)
935 let by: i64 = st_div_r(qy[v2] - qy[v0], ST_GRAD_SHIFT)
936 let bz: i64 = st_div_r(qz[v2] - qz[v0], ST_GRAD_SHIFT)
937 let cx: i64 = st_div_r(qx[v3] - qx[v0], ST_GRAD_SHIFT)
938 let cy: i64 = st_div_r(qy[v3] - qy[v0], ST_GRAD_SHIFT)
939 let cz: i64 = st_div_r(qz[v3] - qz[v0], ST_GRAD_SHIFT)
940 let g1x: i64 = by*cz - bz*cy
941 let g1y: i64 = bz*cx - bx*cz
942 let g1z: i64 = bx*cy - by*cx
943 let g2x: i64 = cy*az - cz*ay
944 let g2y: i64 = cz*ax - cx*az
945 let g2z: i64 = cx*ay - cy*ax
946 let g3x: i64 = ay*bz - az*by
947 let g3y: i64 = az*bx - ax*bz
948 let g3z: i64 = ax*by - ay*bx
949 let g0x: i64 = 0 - g1x - g2x - g3x
950 let g0y: i64 = 0 - g1y - g2y - g3y
951 let g0z: i64 = 0 - g1z - g2z - g3z
952 let sv: i64 = ax*g1x + ay*g1y + az*g1z
953 let C: i64 = sv - trv[t]
954 let n0: i64 = st_div_r(g0x*g0x + g0y*g0y + g0z*g0z, ST_Q12)
955 let n1: i64 = st_div_r(g1x*g1x + g1y*g1y + g1z*g1z, ST_Q12)
956 let n2: i64 = st_div_r(g2x*g2x + g2y*g2y + g2z*g2z, ST_Q12)
957 let n3: i64 = st_div_r(g3x*g3x + g3y*g3y + g3z*g3z, ST_Q12)
958 let den: i64 = w0*n0 + w1*n1 + w2*n2 + w3*n3
959 if den > 0 {
960 // Hard incompressibility, solved directly for dx so no intermediate
961 // lambda has to survive fixed point. dx_i = w_i * grad_i * (-C) / den.
962 // The response scales as 1/den, and den collapses toward zero exactly as
963 // an element flattens -- so resistance climbs without bound as det F -> 0.
964 // That is the inversion barrier: pressing harder makes tissue harder,
965 // which is why a surgical press dimples instead of turning inside out.
966 let num: i64 = 0 - C
967 qx[v0] = qx[v0] + st_div_r(st_div_r(w0*g0x, ST_Q12) * num, den)
968 qy[v0] = qy[v0] + st_div_r(st_div_r(w0*g0y, ST_Q12) * num, den)
969 qz[v0] = qz[v0] + st_div_r(st_div_r(w0*g0z, ST_Q12) * num, den)
970 qx[v1] = qx[v1] + st_div_r(st_div_r(w1*g1x, ST_Q12) * num, den)
971 qy[v1] = qy[v1] + st_div_r(st_div_r(w1*g1y, ST_Q12) * num, den)
972 qz[v1] = qz[v1] + st_div_r(st_div_r(w1*g1z, ST_Q12) * num, den)
973 qx[v2] = qx[v2] + st_div_r(st_div_r(w2*g2x, ST_Q12) * num, den)
974 qy[v2] = qy[v2] + st_div_r(st_div_r(w2*g2y, ST_Q12) * num, den)
975 qz[v2] = qz[v2] + st_div_r(st_div_r(w2*g2z, ST_Q12) * num, den)
976 qx[v3] = qx[v3] + st_div_r(st_div_r(w3*g3x, ST_Q12) * num, den)
977 qy[v3] = qy[v3] + st_div_r(st_div_r(w3*g3y, ST_Q12) * num, den)
978 qz[v3] = qz[v3] + st_div_r(st_div_r(w3*g3z, ST_Q12) * num, den)
979 }
980 }
981 t = t + 1
982 }
983
984 // --- contact: a finger or an instrument, as a sphere ----------
985 // Hard projection (alpha=0): a vertex inside the probe is pushed to its surface.
986 // Neighbours follow through the distance and volume constraints above, which is what
987 // makes a smooth dimple instead of a single-vertex spike -- the propagation the
988 // operator described falls out of the cage, it is not a separate effect.
989 if W[ST_W_TON] == 1 {
990 var s: i64 = 0
991 var cn: i64 = 0
992 while s < np {
993 if iw[s] > 0 {
994 let dx: i64 = qx[s] - W[ST_W_TCX]
995 let dy: i64 = qy[s] - W[ST_W_TCY]
996 let dz: i64 = qz[s] - W[ST_W_TCZ]
997 let d2: i64 = dx*dx + dy*dy + dz*dz
998 let R: i64 = W[ST_W_TR]
999 if d2 < R*R {
1000 var d: i64 = st_isqrt(d2)
1001 if d == 0 { d = 1 }
1002 // THE READBACK: (R - d) is exactly the distance this projection moved the
1003 // particle, i.e. the correction the contact constraint had to apply. Summed
1004 // over every iteration of the substep it IS the constraint impulse, which is
1005 // why it is accumulated here rather than measured after the fact.
1006 W[ST_W_CDEP] = W[ST_W_CDEP] + (R - d)
1007 cn = cn + 1
1008 let dnorm: i64 = R - d
1009 qx[s] = W[ST_W_TCX] + dx * R / d
1010 qy[s] = W[ST_W_TCY] + dy * R / d
1011 qz[s] = W[ST_W_TCZ] + dz * R / d
1012 // COULOMB FRICTION, POSITIONAL FORM. nx_phys3d already clamps a tangential IMPULSE
1013 // by mu times the ACCUMULATED NORMAL impulse (jmax = newacc*mu/256); the position-
1014 // based analogue clamps the tangential CORRECTION by mu times the NORMAL correction,
1015 // which is the quantity the contact readback above already computes. Same cone, same
1016 // per-256 mu, ported rather than re-derived so there is one friction law here.
1017 // Skipped entirely when mu is UNSET: no citation, no force.
1018 if W[ST_W_MU] >= 0 {
1019 if dnorm > 0 {
1020 let nqx: i64 = dx*1024/d
1021 let nqy: i64 = dy*1024/d
1022 let nqz: i64 = dz*1024/d
1023 // RELATIVE slip: the particle's displacement MINUS the probe's.
1024 let mvx: i64 = (qx[s] - px[s]) - W[ST_W_TDX]
1025 let mvy: i64 = (qy[s] - py[s]) - W[ST_W_TDY]
1026 let mvz: i64 = (qz[s] - pz[s]) - W[ST_W_TDZ]
1027 let mdotn: i64 = (mvx*nqx + mvy*nqy + mvz*nqz)/1024
1028 let tgx: i64 = mvx - mdotn*nqx/1024
1029 let tgy: i64 = mvy - mdotn*nqy/1024
1030 let tgz: i64 = mvz - mdotn*nqz/1024
1031 let tmag: i64 = st_isqrt(tgx*tgx + tgy*tgy + tgz*tgz)
1032 if tmag > 0 {
1033 // Macklin's form: the correction OPPOSES the slip, scaled by
1034 // min(1, mu*d/|slip|). Below the cone the WHOLE slip is removed and the
1035 // particle sticks to the probe; above it only the cone's worth goes and
1036 // it slides. mu=0 therefore removes NOTHING, which is what frictionless
1037 // means. The first cut clamped the ALLOWED SLIDE instead of the
1038 // CORRECTION, which made mu=0 the stickiest setting available -- caught
1039 // by nx_contactfriction_gate on its first ever run, RED 3/5.
1040 let fcap: i64 = dnorm*W[ST_W_MU]/256
1041 var fx: i64 = tgx
1042 var fy: i64 = tgy
1043 var fz: i64 = tgz
1044 if tmag > fcap {
1045 // Integer division truncates toward zero, so a partial correction is
1046 // very slightly UNDER-applied. Declared rather than hidden: it errs
1047 // toward less friction, the one direction that cannot invent grip
1048 // the coefficient never asked for.
1049 fx = tgx*fcap/tmag
1050 fy = tgy*fcap/tmag
1051 fz = tgz*fcap/tmag
1052 }
1053 qx[s] = qx[s] - fx
1054 qy[s] = qy[s] - fy
1055 qz[s] = qz[s] - fz
1056 }
1057 }
1058 }
1059 }
1060 }
1061 s = s + 1
1062 }
1063 W[ST_W_CN] = cn
1064 // The probe moved once, so its displacement is spent once. Leaving it set would re-apply the
1065 // same drag on every following substep and manufacture friction from a stationary probe.
1066 W[ST_W_TDX] = 0
1067 W[ST_W_TDY] = 0
1068 W[ST_W_TDZ] = 0
1069 }
1070 it = it + 1
1071 }
1072
1073 // velocity from the solved positions, then per-layer viscous damping
1074 var u: i64 = 0
1075 while u < np {
1076 if iw[u] > 0 {
1077 vx[u] = st_div_r((qx[u] - px[u]) * ST_MAGIC_1000000, dt_us)
1078 vy[u] = st_div_r((qy[u] - py[u]) * ST_MAGIC_1000000, dt_us)
1079 vz[u] = st_div_r((qz[u] - pz[u]) * ST_MAGIC_1000000, dt_us)
1080 let dmp: i64 = st_eff_damp(W, lay[u])
1081 vx[u] = vx[u] - st_div_r(vx[u] * dmp, ST_Q12)
1082 vy[u] = vy[u] - st_div_r(vy[u] * dmp, ST_Q12)
1083 vz[u] = vz[u] - st_div_r(vz[u] * dmp, ST_Q12)
1084 px[u] = qx[u]
1085 py[u] = qy[u]
1086 pz[u] = qz[u]
1087 }
1088 u = u + 1
1089 }
1090 return 0
1091}
1092
1093func st_run(W: *i64, gx: i64, gy: i64, gz: i64, steps: i64, dt_us: i64, iters: i64) -> i64 {
1094 var s: i64 = 0
1095 while s < steps { st_substep(W, gx, gy, gz, dt_us, iters); s = s + 1 }
1096 return 0
1097}
1098
1099// ===== measurement (the drape validator's own statistics) =============
1100const ST_M_ASPECT: i64 = 0
1101const ST_M_CENTROID: i64 = 1
1102const ST_M_MINVOL: i64 = 2
1103const ST_M_SUMVOL: i64 = 3
1104const ST_M_KIN: i64 = 4
1105const ST_M_NSURF: i64 = 5
1106const ST_M_PROJ: i64 = 6
1107const ST_M_N: i64 = 8
1108
1109// Silhouette statistics over the SKIN envelope, in exactly the terms
1110// nx_breast_drape_pose.nx scores: aspect = height/width, centroid = how far the centroid
1111// sits below the top of the region. Both q10, both viewed frontally (x lateral, y cranial),
1112// so the bands banked for generated imagery apply unchanged to simulated geometry.
1113// ===== CONTACT READBACK ACCESSORS =========================================================
1114// st_contact_depth is the raw truth: total push-out in cmm for the last substep. Everything else is
1115// derived from it, so there is one measured quantity and no second ruler.
1116// ---------------------------------------------------------------------------------------------
1117// SB10 SELF-CONTACT: two tissue surfaces that cannot pass through each other.
1118//
1119// SCOPE, STATED SO IT CANNOT BE OVERREAD: this resolves NON-INTERPENETRATION between two cages. It
1120// deliberately does NOT apply the Coulomb law from st_set_friction. That is not an oversight and not a
1121// half-measure -- bundling contact and friction into one rung is exactly the compound bar that kept the
1122// old SB3 from ever closing, and mu is UNSET estate-wide today for want of a citation, so a friction
1123// term here would provably do nothing while making the rung look larger than it is.
1124//
1125// ⚠ THE CONTACT DISTANCE MUST BE SIZED FROM st_surface_spacing(), NOT CHOSEN. Below the mesh's own
1126// particle spacing this routine can only ever catch the single closest pair -- contacts degenerate to
1127// points, and two surfaces can slip BETWEEN particles with no pair ever coming within range. Measured
1128// on ST_PROF_LARGE_SOFT: a 300 cmm distance gave a peak of ONE pair on every pass.
1129//
1130// ⚠ WHAT THIS CANNOT SEE, MEASURED 2026-08-25 AND DECLARED RATHER THAN DISCOVERED LATER. Contact is
1131// detected by SURFACE-PARTICLE PROXIMITY, so it resolves GRAZING contact -- two surfaces meeting -- and
1132// is blind to DEEP INTERLEAVING, where one cage's shell has passed bodily inside the other. Pushing a
1133// cage 600 cmm past touching took the measured closest approach from 288 to 390 cmm and the contact
1134// count from thousands to ZERO, because once B's surface sits inside A's volume the nearest A-SURFACE
1135// particle is far away. st_sc_min_sep inherits this exactly: it is a surface-to-surface distance and is
1136// NOT a penetration depth, so it is non-monotonic once bodies interleave. Escaping a tangle needs a
1137// signed distance field or continuous collision detection and is a strictly harder rung than this one.
1138// ★A CONTACT ROUTINE THAT CANNOT SEE THE TANGLED CASE REPORTS IT AS "NOT TOUCHING", WHICH IS THE MOST
1139// REASSURING POSSIBLE WAY TO BE WRONG. Callers must prevent tunnelling by stepping small enough that
1140// surfaces meet before they cross; this routine cannot rescue them afterwards.
1141//
1142// TWO PHASES, CHOSEN FROM THE BOUND. Broad phase: bounding boxes over each cage's SURFACE particles,
1143// padded by the contact distance. Two bodies that are not near each other cost SIX comparisons and zero
1144// pair tests. Narrow phase: all-pairs, but only over particles inside the BOX INTERSECTION -- two
1145// surfaces in contact overlap in a small region, so the quadratic term is paid on the contact patch
1146// rather than on the bodies. st_sc_tested() reports what was actually paid.
1147
1148// Mean distance from each SURFACE particle to its nearest surface neighbour, cmm. THE NATURAL UNIT OF
1149// THIS MESH: a contact distance below it can only ever catch the single closest pair, so contacts
1150// degenerate to points and -- the part that matters -- two surfaces can pass BETWEEN particles without
1151// any pair ever coming within range. Callers should size their contact distance from this rather than
1152// choosing one, which is why it is exported rather than left inside the gate that discovered it.
1153func st_surface_spacing(W: *i64) -> i64 {
1154 let px: *i64 = W[ST_W_PX] as *i64
1155 let py: *i64 = W[ST_W_PY] as *i64
1156 let pz: *i64 = W[ST_W_PZ] as *i64
1157 let srf: *i64 = W[ST_W_SRF] as *i64
1158 let np: i64 = W[ST_W_NP]
1159 var sum: i64 = 0
1160 var n: i64 = 0
1161 var i: i64 = 0
1162 while i < np {
1163 if srf[i] == 1 {
1164 var best: i64 = 0
1165 var have: i64 = 0
1166 var j: i64 = 0
1167 while j < np {
1168 if srf[j] == 1 {
1169 if j != i {
1170 let dx: i64 = px[i] - px[j]
1171 let dy: i64 = py[i] - py[j]
1172 let dz: i64 = pz[i] - pz[j]
1173 let d: i64 = st_isqrt(dx*dx + dy*dy + dz*dz)
1174 if have == 0 {
1175 best = d
1176 have = 1
1177 }
1178 if d < best { best = d }
1179 }
1180 }
1181 j = j + 1
1182 }
1183 if have == 1 {
1184 sum = sum + best
1185 n = n + 1
1186 }
1187 }
1188 i = i + 1
1189 }
1190 if n == 0 { return 0 }
1191 return sum/n
1192}
1193
1194// Axis-aligned bounds over the SURFACE particles, padded by `pad`.
1195// out[0..5] = minx,maxx,miny,maxy,minz,maxz. Returns 1 if the cage has any surface at all, else 0 --
1196// an empty surface must be a REFUSAL, because a zero-pair run would otherwise report a clean pass.
1197func st_sc_bounds(W: *i64, pad: i64, out: *i64) -> i64 {
1198 let px: *i64 = W[ST_W_PX] as *i64
1199 let py: *i64 = W[ST_W_PY] as *i64
1200 let pz: *i64 = W[ST_W_PZ] as *i64
1201 let srf: *i64 = W[ST_W_SRF] as *i64
1202 let np: i64 = W[ST_W_NP]
1203 var have: i64 = 0
1204 var i: i64 = 0
1205 while i < np {
1206 if srf[i] == 1 {
1207 if have == 0 {
1208 out[0] = px[i]
1209 out[1] = px[i]
1210 out[2] = py[i]
1211 out[3] = py[i]
1212 out[4] = pz[i]
1213 out[5] = pz[i]
1214 have = 1
1215 }
1216 if px[i] < out[0] { out[0] = px[i] }
1217 if px[i] > out[1] { out[1] = px[i] }
1218 if py[i] < out[2] { out[2] = py[i] }
1219 if py[i] > out[3] { out[3] = py[i] }
1220 if pz[i] < out[4] { out[4] = pz[i] }
1221 if pz[i] > out[5] { out[5] = pz[i] }
1222 }
1223 i = i + 1
1224 }
1225 if have == 0 { return 0 }
1226 out[0] = out[0] - pad
1227 out[1] = out[1] + pad
1228 out[2] = out[2] - pad
1229 out[3] = out[3] + pad
1230 out[4] = out[4] - pad
1231 out[5] = out[5] + pad
1232 return 1
1233}
1234
1235// Resolve interpenetration between two cages at separation `dist` (cmm), `iters` projection passes.
1236// Corrections are split by inverse mass, so a pinned particle takes none and its partner takes all --
1237// splitting evenly regardless would let a fixed anchor be shoved by the tissue resting against it.
1238// Velocities are corrected by the position change over dt so the contact is COUPLED rather than a
1239// cosmetic post-step nudge; without that the next substep re-drives the surfaces straight back in.
1240// Returns the number of pairs found in contact on the final pass.
1241func st_self_contact(WA: *i64, WB: *i64, dist: i64, iters: i64, dt_us: i64) -> i64 {
1242 WA[ST_W_SCN] = 0
1243 WA[ST_W_SCT] = 0
1244 if dist <= 0 { return 0 }
1245 if dt_us <= 0 { return 0 }
1246 let ba: *i64 = sys_mmap(8*8) as *i64
1247 let bb: *i64 = sys_mmap(8*8) as *i64
1248 if st_sc_bounds(WA, dist, ba) == 0 { return 0 }
1249 if st_sc_bounds(WB, dist, bb) == 0 { return 0 }
1250 // BROAD PHASE. Any separating axis ends it for six comparisons and no pair tests.
1251 if ba[1] < bb[0] { return 0 }
1252 if bb[1] < ba[0] { return 0 }
1253 if ba[3] < bb[2] { return 0 }
1254 if bb[3] < ba[2] { return 0 }
1255 if ba[5] < bb[4] { return 0 }
1256 if bb[5] < ba[4] { return 0 }
1257 // The overlap box: the only region where a pair can possibly be in contact.
1258 var oxl: i64 = ba[0]
1259 if bb[0] > oxl { oxl = bb[0] }
1260 var oxh: i64 = ba[1]
1261 if bb[1] < oxh { oxh = bb[1] }
1262 var oyl: i64 = ba[2]
1263 if bb[2] > oyl { oyl = bb[2] }
1264 var oyh: i64 = ba[3]
1265 if bb[3] < oyh { oyh = bb[3] }
1266 var ozl: i64 = ba[4]
1267 if bb[4] > ozl { ozl = bb[4] }
1268 var ozh: i64 = ba[5]
1269 if bb[5] < ozh { ozh = bb[5] }
1270
1271 let apx: *i64 = WA[ST_W_PX] as *i64
1272 let apy: *i64 = WA[ST_W_PY] as *i64
1273 let apz: *i64 = WA[ST_W_PZ] as *i64
1274 let avx: *i64 = WA[ST_W_VX] as *i64
1275 let avy: *i64 = WA[ST_W_VY] as *i64
1276 let avz: *i64 = WA[ST_W_VZ] as *i64
1277 let aiw: *i64 = WA[ST_W_IW] as *i64
1278 let asf: *i64 = WA[ST_W_SRF] as *i64
1279 let bpx: *i64 = WB[ST_W_PX] as *i64
1280 let bpy: *i64 = WB[ST_W_PY] as *i64
1281 let bpz: *i64 = WB[ST_W_PZ] as *i64
1282 let bvx: *i64 = WB[ST_W_VX] as *i64
1283 let bvy: *i64 = WB[ST_W_VY] as *i64
1284 let bvz: *i64 = WB[ST_W_VZ] as *i64
1285 let biw: *i64 = WB[ST_W_IW] as *i64
1286 let bsf: *i64 = WB[ST_W_SRF] as *i64
1287 let na: i64 = WA[ST_W_NP]
1288 let nb: i64 = WB[ST_W_NP]
1289
1290 var tested: i64 = 0
1291 var hits: i64 = 0
1292 var peak: i64 = 0
1293 var it: i64 = 0
1294 while it < iters {
1295 hits = 0
1296 var i: i64 = 0
1297 while i < na {
1298 if asf[i] == 1 {
1299 if apx[i] >= oxl {
1300 if apx[i] <= oxh {
1301 if apy[i] >= oyl {
1302 if apy[i] <= oyh {
1303 if apz[i] >= ozl {
1304 if apz[i] <= ozh {
1305 var j: i64 = 0
1306 while j < nb {
1307 if bsf[j] == 1 {
1308 if bpx[j] >= oxl {
1309 if bpx[j] <= oxh {
1310 if bpy[j] >= oyl {
1311 if bpy[j] <= oyh {
1312 if bpz[j] >= ozl {
1313 if bpz[j] <= ozh {
1314 tested = tested + 1
1315 var dx: i64 = apx[i] - bpx[j]
1316 var dy: i64 = apy[i] - bpy[j]
1317 var dz: i64 = apz[i] - bpz[j]
1318 let d2: i64 = dx*dx + dy*dy + dz*dz
1319 if d2 < dist*dist {
1320 var d: i64 = st_isqrt(d2)
1321 if d == 0 {
1322 // Exactly coincident particles have no direction to
1323 // separate along. Pick one rather than divide by zero:
1324 // any axis is as good as any other and the next pass
1325 // refines it.
1326 d = 1
1327 dx = 1
1328 dy = 0
1329 dz = 0
1330 }
1331 let pen: i64 = dist - d
1332 hits = hits + 1
1333 let wa: i64 = aiw[i]
1334 let wb: i64 = biw[j]
1335 let wsum: i64 = wa + wb
1336 if wsum > 0 {
1337 let ca: i64 = pen*wa/wsum
1338 let cb: i64 = pen*wb/wsum
1339 let oax: i64 = apx[i]
1340 let oay: i64 = apy[i]
1341 let oaz: i64 = apz[i]
1342 let obx: i64 = bpx[j]
1343 let oby: i64 = bpy[j]
1344 let obz: i64 = bpz[j]
1345 apx[i] = apx[i] + dx*ca/d
1346 apy[i] = apy[i] + dy*ca/d
1347 apz[i] = apz[i] + dz*ca/d
1348 bpx[j] = bpx[j] - dx*cb/d
1349 bpy[j] = bpy[j] - dy*cb/d
1350 bpz[j] = bpz[j] - dz*cb/d
1351 avx[i] = avx[i] + (apx[i]-oax)*1000000/dt_us
1352 avy[i] = avy[i] + (apy[i]-oay)*1000000/dt_us
1353 avz[i] = avz[i] + (apz[i]-oaz)*1000000/dt_us
1354 bvx[j] = bvx[j] + (bpx[j]-obx)*1000000/dt_us
1355 bvy[j] = bvy[j] + (bpy[j]-oby)*1000000/dt_us
1356 bvz[j] = bvz[j] + (bpz[j]-obz)*1000000/dt_us
1357 }
1358 }
1359 }
1360 }
1361 }
1362 }
1363 }
1364 }
1365 }
1366 j = j + 1
1367 }
1368 }
1369 }
1370 }
1371 }
1372 }
1373 }
1374 }
1375 i = i + 1
1376 }
1377 if hits > peak { peak = hits }
1378 it = it + 1
1379 }
1380 WA[ST_W_SCN] = hits
1381 WA[ST_W_SCT] = tested
1382 WA[ST_W_SCP] = peak
1383 return hits
1384}
1385
1386// Pairs still in contact after the final projection pass. Zero means resolved.
1387func st_sc_hits(W: *i64) -> i64 { return W[ST_W_SCN] }
1388// The LARGEST number of pairs in contact on any pass -- the size of the contact patch. See ST_W_SCP:
1389// a residual of zero is equally true of a contact that resolved and of one that never existed.
1390func st_sc_peak(W: *i64) -> i64 { return W[ST_W_SCP] }
1391// Pair tests actually paid for. THE COST IS AN OUTPUT, not an assumption -- see the note on ST_W_SCT.
1392func st_sc_tested(W: *i64) -> i64 { return W[ST_W_SCT] }
1393
1394// Smallest surface-to-surface separation between two cages, cmm. Negative is overlap depth.
1395// The gate's oracle: a claim that two bodies do not interpenetrate has to be a MEASUREMENT of the
1396// closest approach, never the contact routine's own opinion of how well it did.
1397func st_sc_min_sep(WA: *i64, WB: *i64) -> i64 {
1398 let apx: *i64 = WA[ST_W_PX] as *i64
1399 let apy: *i64 = WA[ST_W_PY] as *i64
1400 let apz: *i64 = WA[ST_W_PZ] as *i64
1401 let asf: *i64 = WA[ST_W_SRF] as *i64
1402 let bpx: *i64 = WB[ST_W_PX] as *i64
1403 let bpy: *i64 = WB[ST_W_PY] as *i64
1404 let bpz: *i64 = WB[ST_W_PZ] as *i64
1405 let bsf: *i64 = WB[ST_W_SRF] as *i64
1406 var best: i64 = 0
1407 var have: i64 = 0
1408 var i: i64 = 0
1409 while i < WA[ST_W_NP] {
1410 if asf[i] == 1 {
1411 var j: i64 = 0
1412 while j < WB[ST_W_NP] {
1413 if bsf[j] == 1 {
1414 let dx: i64 = apx[i] - bpx[j]
1415 let dy: i64 = apy[i] - bpy[j]
1416 let dz: i64 = apz[i] - bpz[j]
1417 let d: i64 = st_isqrt(dx*dx + dy*dy + dz*dz)
1418 if have == 0 { best = d
1419 have = 1 }
1420 if d < best { best = d }
1421 }
1422 j = j + 1
1423 }
1424 }
1425 i = i + 1
1426 }
1427 return best
1428}
1429
1430// ---------------------------------------------------------------------------------------------
1431// SB16 ENGORGEMENT: TISSUE THAT SWELLS BECAUSE ITS REST STATE GREW, NOT BECAUSE ITS SKIN WAS PUSHED.
1432//
1433// WHY THE OBVIOUS APPROACH IS WRONG, AND THE OTHER BOARD ALREADY SAID SO. procgen's arousal row
1434// states its own blocker in as many words: "a pure normal push is not volume-conserving, which the
1435// conservation gate above would refuse." That is exactly right. Displacing surface vertices along
1436// their normals moves the SKIN while leaving the rest lengths and rest volumes untouched, so every
1437// constraint in the solver is now violated and the next substep pulls it all back. The tissue does
1438// not swell; it fights, and our conservation gate refuses it -- correctly.
1439//
1440// Engorgement is fluid entering tissue, so the REST SHAPE ITSELF grows. Change the rest state and
1441// the solver finds the new equilibrium on its own, with every constraint satisfied throughout.
1442// ISOTROPIC LINEAR GROWTH BY s SCALES REST LENGTHS BY s AND REST VOLUMES BY s^3 -- that is
1443// geometry, not a tuning choice, and getting it wrong (scaling both by s) would leave the distance
1444// and volume constraints demanding different shapes and the tissue permanently stressed at rest.
1445//
1446// ⚠ WHAT THIS IS NOT: it is not a claim about how MUCH any tissue engorges, or how fast. No such
1447// band is mirrored in this estate, so the caller supplies the factor and this organ supplies only
1448// the mechanism -- the same cite-or-abstain split the friction coefficient ships under.
1449func st_inflate_rest_volume(W: *i64, layer: i64, linear_permil: i64) -> i64 {
1450 if linear_permil <= 0 { return 0 - 1 }
1451 let ne: i64 = W[ST_W_NE]
1452 let nt: i64 = W[ST_W_NT]
1453 // Lazy snapshot: only a cage that is actually inflated pays the memory.
1454 if W[ST_W_ERL0] == 0 {
1455 let e0: *i64 = sys_mmap(ST_MAXE*8) as *i64
1456 let t0: *i64 = sys_mmap(ST_MAXT*8) as *i64
1457 let erlS: *i64 = W[ST_W_ERL] as *i64
1458 let trvS: *i64 = W[ST_W_TRV] as *i64
1459 var i: i64 = 0
1460 while i < ne { e0[i] = erlS[i]; i = i + 1 }
1461 var j: i64 = 0
1462 while j < nt { t0[j] = trvS[j]; j = j + 1 }
1463 W[ST_W_ERL0] = e0 as i64
1464 W[ST_W_TRV0] = t0 as i64
1465 }
1466 let erl: *i64 = W[ST_W_ERL] as *i64
1467 let erl0: *i64 = W[ST_W_ERL0] as *i64
1468 let ela: *i64 = W[ST_W_ELA] as *i64
1469 let trv: *i64 = W[ST_W_TRV] as *i64
1470 let trv0: *i64 = W[ST_W_TRV0] as *i64
1471 let tla: *i64 = W[ST_W_TLA] as *i64
1472 var touched: i64 = 0
1473 var e: i64 = 0
1474 while e < ne {
1475 if ela[e] == layer {
1476 erl[e] = erl0[e] * linear_permil / 1000
1477 touched = touched + 1
1478 }
1479 e = e + 1
1480 }
1481 var t: i64 = 0
1482 while t < nt {
1483 if tla[t] == layer {
1484 // s^3 applied in THREE stages, never as one multiply: trv can be large and s^3 at a
1485 // 2x factor is 8e9, so a single product would overflow silently and shrink the tissue
1486 // it was asked to grow. Staged, each intermediate stays the same order as trv itself.
1487 var v: i64 = trv0[t] * linear_permil / 1000
1488 v = v * linear_permil / 1000
1489 v = v * linear_permil / 1000
1490 trv[t] = v
1491 }
1492 t = t + 1
1493 }
1494 return touched
1495}
1496
1497// Is this cage's rest state currently inflated away from pristine? -1 until the first inflate.
1498func st_inflated(W: *i64) -> i64 {
1499 if W[ST_W_ERL0] == 0 { return 0 - 1 }
1500 return 1
1501}
1502
1503// Set the Coulomb friction coefficient for the contact probe, in per-256 (256 = mu 1.0).
1504// PASS ST_MU_UNSET TO TURN FRICTION OFF, WHICH IS THE DEFAULT AND THE HONEST STATE TODAY. There is a
1505// third answer here on purpose: unset is not zero. mu=0 is a MEASURED frictionless surface and slides;
1506// unset means NOBODY HAS CITED A COEFFICIENT and the solver declines to invent one. Collapsing those two
1507// would let an absent citation masquerade as a physical result, which is the whole failure this guards.
1508func st_set_friction(W: *i64, mu256: i64) -> i64 { W[ST_W_MU] = mu256; return 0 }
1509func st_friction(W: *i64) -> i64 { return W[ST_W_MU] }
1510
1511func st_contact_depth(W: *i64) -> i64 { return W[ST_W_CDEP] }
1512// how many particles the probe engaged. A force with a zero count is not a light touch, it is NO touch --
1513// the caller needs to tell those apart, and a bare magnitude cannot.
1514func st_contact_n(W: *i64) -> i64 { return W[ST_W_CN] }
1515// Impulse against UNIT MASS, in cmm/s: depth per unit time. One division, well conditioned.
1516func st_contact_impulse(W: *i64, dt_us: i64) -> i64 {
1517 if dt_us <= 0 { return 0 }
1518 return W[ST_W_CDEP]*ST_MAGIC_1000000/dt_us
1519}
1520// Force against UNIT MASS, in cmm/s^2. Derived from the impulse by a SECOND division rather than by
1521// squaring the timestep in one expression, which would overflow i64 on a small dt at a deep press.
1522// NOT NEWTONS, and deliberately not called that: the solver's inverse mass is a uniform q12, so there is
1523// no kilogram in this system to convert with. Absolute force needs a density model the profile rows do
1524// not carry. What this IS good for is comparison -- deeper reads harder, stiffer reads harder -- which is
1525// exactly what a haptic loop and a firmness judgement consume.
1526func st_contact_force(W: *i64, dt_us: i64) -> i64 {
1527 if dt_us <= 0 { return 0 }
1528 return st_contact_impulse(W, dt_us)*ST_MAGIC_1000000/dt_us
1529}
1530
1531func st_measure(W: *i64, out: *i64) -> i64 {
1532 let px: *i64 = W[ST_W_PX] as *i64
1533 let py: *i64 = W[ST_W_PY] as *i64
1534 let pz: *i64 = W[ST_W_PZ] as *i64
1535 let vx: *i64 = W[ST_W_VX] as *i64
1536 let vy: *i64 = W[ST_W_VY] as *i64
1537 let vz: *i64 = W[ST_W_VZ] as *i64
1538 let srf: *i64 = W[ST_W_SRF] as *i64
1539 var f: i64 = 0
1540 while f < ST_M_N { out[f] = 0; f = f + 1 }
1541
1542 var minx: i64 = ST_MAGIC_999999999
1543 var maxx: i64 = 0 - ST_MAGIC_999999999
1544 var miny: i64 = ST_MAGIC_999999999
1545 var maxy: i64 = 0 - ST_MAGIC_999999999
1546 var maxz: i64 = 0 - ST_MAGIC_999999999
1547 var sumy: i64 = 0
1548 var n: i64 = 0
1549 var i: i64 = 0
1550 while i < W[ST_W_NP] {
1551 if srf[i] == 1 {
1552 minx = st_min(minx, px[i]); maxx = st_max(maxx, px[i])
1553 miny = st_min(miny, py[i]); maxy = st_max(maxy, py[i])
1554 maxz = st_max(maxz, pz[i])
1555 sumy = sumy + py[i]
1556 n = n + 1
1557 }
1558 i = i + 1
1559 }
1560 out[ST_M_NSURF] = n
1561 if n > 0 {
1562 let hgt: i64 = maxy - miny
1563 let wid: i64 = maxx - minx
1564 if wid > 0 { out[ST_M_ASPECT] = hgt * ST_MAGIC_1024 / wid }
1565 if hgt > 0 { out[ST_M_CENTROID] = (maxy - sumy / n) * ST_MAGIC_1024 / hgt }
1566 out[ST_M_PROJ] = maxz
1567 }
1568
1569 var mv: i64 = ST_MAGIC_999999999999999
1570 var sv: i64 = 0
1571 let tet: *i64 = W[ST_W_TET] as *i64
1572 var t: i64 = 0
1573 while t < W[ST_W_NT] {
1574 let v: i64 = st_six_vol(W, tet[t*4], tet[t*4+1], tet[t*4+2], tet[t*4+3])
1575 mv = st_min(mv, v)
1576 sv = sv + v
1577 t = t + 1
1578 }
1579 out[ST_M_MINVOL] = mv
1580 out[ST_M_SUMVOL] = sv
1581
1582 var kin: i64 = 0
1583 var u: i64 = 0
1584 while u < W[ST_W_NP] {
1585 kin = kin + st_abs(vx[u]) + st_abs(vy[u]) + st_abs(vz[u])
1586 u = u + 1
1587 }
1588 out[ST_M_KIN] = kin
1589 return 0
1590}