nx_softtissue.nx source
↩ module page · 1022 lines · 45507 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"
41const ST_MAGIC_1229: i64 = 1229
42const ST_MAGIC_12288: i64 = 12288
43const ST_MAGIC_20972: i64 = 20972
44const ST_MAGIC_1434: i64 = 1434
45const ST_MAGIC_2654435761: i64 = 2654435761
46const ST_MAGIC_1013904223: i64 = 1013904223
47const ST_MAGIC_1442695040888963407: i64 = 1442695040888963407
48const ST_MAGIC_2246822519: i64 = 2246822519
49const ST_MAGIC_288230376151711743: i64 = 288230376151711743
50const ST_MAGIC_1050: i64 = 1050
51const ST_MAGIC_9216: i64 = 9216
52const ST_MAGIC_1024: i64 = 1024
53const ST_MAGIC_4096: i64 = 4096
54const ST_MAGIC_22528: i64 = 22528
55const ST_MAGIC_2048: i64 = 2048
56const ST_MAGIC_8192: i64 = 8192
57const ST_MAGIC_24576: i64 = 24576
58const ST_MAGIC_20480: i64 = 20480
59const ST_MAGIC_1000000: i64 = 1000000
60const ST_MAGIC_999999999: i64 = 999999999
61const ST_MAGIC_999999999999999: i64 = 999999999999999
62
63// ===== capacities =====================================================
64const ST_MAXP: i64 = 8192
65const ST_MAXT: i64 = 16384
66const ST_MAXE: i64 = 49152
67
68// ===== layers =========================================================
69const ST_LAY_MUSCLE: i64 = 0
70const ST_LAY_GLAND: i64 = 1
71const ST_LAY_ADIPOSE: i64 = 2
72const ST_LAY_SKIN: i64 = 3
73const ST_LAY_LIGAMENT: i64 = 4
74const ST_LAY_N: i64 = 5
75
76// ===== context slots ==================================================
77const ST_W_NP: i64 = 0
78const ST_W_NT: i64 = 1
79const ST_W_NE: i64 = 2
80const ST_W_PX: i64 = 3
81const ST_W_PY: i64 = 4
82const ST_W_PZ: i64 = 5
83const ST_W_QX: i64 = 6
84const ST_W_QY: i64 = 7
85const ST_W_QZ: i64 = 8
86const ST_W_VX: i64 = 9
87const ST_W_VY: i64 = 10
88const ST_W_VZ: i64 = 11
89const ST_W_IW: i64 = 12
90const ST_W_LAY: i64 = 13
91const ST_W_TET: i64 = 14
92const ST_W_TRV: i64 = 15
93const ST_W_TLA: i64 = 16
94const ST_W_TLM: i64 = 17
95const ST_W_EDG: i64 = 18
96const ST_W_ERL: i64 = 19
97const ST_W_ELA: i64 = 20
98const ST_W_ELM: i64 = 21
99const ST_W_SRF: i64 = 22
100const ST_W_TON: i64 = 23
101const ST_W_TCX: i64 = 24
102const ST_W_TCY: i64 = 25
103const ST_W_TCZ: i64 = 26
104const ST_W_TR: i64 = 27
105const ST_W_PROF:i64 = 28
106const ST_W_SLOTS: i64 = 40
107
108// ===== profile fields =================================================
109const ST_PF_A_MM: i64 = 0
110const ST_PF_B_MM: i64 = 1
111const ST_PF_C_MM: i64 = 2
112const ST_PF_GLAND_PCT: i64 = 3
113const ST_PF_LIG_ALPHA: i64 = 4
114const ST_PF_FAT_ALPHA: i64 = 5
115const ST_PF_FAT_DAMP: i64 = 6
116const ST_PF_RIB_R_MM: i64 = 7
117// Skin-envelope extensibility. The tissue is near-incompressible, so lying down forces it to
118// SPREAD, and spreading is paid for by stretching the envelope -- a lax envelope flattens
119// further than a taut one holding identical filling.
120// MEASURED CONTRIBUTION, not assumed: neutralising this row to one global value moves
121// LARGE_SOFT's lying-down flattening from 134 to 125 per-mille, so the envelope accounts for
122// roughly 7% of it while fat compliance, gland fraction and ligament strength carry the rest.
123// An earlier comment here claimed the firmness law depended on this row; the mutation run
124// disproved that, and ST_PROF_SOFT_TAUT exists to gate what the row actually does.
125const ST_PF_SKIN_ALPHA: i64 = 8
126const ST_PF_N: i64 = 9
127
128const ST_PROF_LARGE_SOFT: i64 = 0
129const ST_PROF_SMALL_FIRM: i64 = 1
130const ST_PROF_GLUTE: i64 = 2
131// LARGE_SOFT in every respect EXCEPT a taut envelope. Exists so the skin row can be gated by
132// a controlled comparison -- one variable changed, geometry and filling identical.
133const ST_PROF_SOFT_TAUT: i64 = 3
134// Any profile id at or above this is DERIVED FROM A SEED, not enumerated: st_profile()
135// forwards it to st_profile_seeded(id - ST_PROF_SEED_BASE). The four named profiles above are
136// reference points for the gate's controlled comparisons -- they are NOT the population.
137// Shipping a handful of fixed bodies would be a clone army; real bodies vary continuously in
138// size, fullness, firmness and frame, and the named rows exist only so a tooth can hold one
139// variable still. Same principle as the craft lane's 20-bit NPC genome.
140const ST_PROF_SEED_BASE: i64 = 1000
141
142const ST_Q12: i64 = 4096
143const ST_Q20: i64 = 1048576
144const ST_GRAV_CMM: i64 = 981000
145// Volume math runs on RAW cmm differences. An earlier draft pre-divided them by 256 to buy
146// overflow headroom and destroyed the constraint instead: a 10 mm cell scaled to a 6V of ~48,
147// so |grad|^2 quantised to ZERO, the denominator collapsed to the compliance term alone, and
148// a deep press produced corrections that overflowed i64. Measured headroom at raw scale:
149// 6V ~ 3.4e9, sum|grad|^2 ~ 6e13 -- five orders inside the i64 ceiling.
150const ST_GRAD_SHIFT: i64 = 1
151// Substep the compliance rows are calibrated at. alpha_tilde = alpha/dt^2, so a different
152// substep rate rescales by (ref/dt)^2 -- that is what keeps material softness a property of
153// the TISSUE and not of the frame rate.
154const ST_DT_REF_US: i64 = 4167
155
156// ===== small helpers ==================================================
157func st_abs(v: i64) -> i64 { if v < 0 { return 0 - v } return v }
158func st_min(a: i64, b: i64) -> i64 { if a < b { return a } return b }
159func st_max(a: i64, b: i64) -> i64 { if a > b { return a } return b }
160
161// Divide rounding to NEAREST, sign-symmetric. Truncation here would bias every
162// velocity toward zero and counterfeit damping we did not model.
163func st_div_r(a: i64, b: i64) -> i64 {
164 if b == 0 { return 0 }
165 if a >= 0 { return (a + b / 2) / b }
166 return 0 - ((0 - a + b / 2) / b)
167}
168
169func st_isqrt(n: i64) -> i64 {
170 if n <= 0 { return 0 }
171 var x: i64 = n
172 var c: i64 = 0
173 var d: i64 = 1
174 while d <= n / 4 { d = d * 4 }
175 while d != 0 {
176 if x >= c + d { x = x - c - d; c = c / 2 + d } else { c = c / 2 }
177 d = d / 4
178 }
179 return c
180}
181
182// ===== DATA ROWS: tissue mechanics ====================================
183// RELATIVE compliance in q12 (4096 = 1.0): alpha expressed against the constraint's own
184// stiffness scale. Our positions are cmm and our masses are unitless, so there is no absolute
185// force scale to hang an SI compliance on -- stating alpha relatively is the honest form, and
186// it is what makes a row like "adipose = 6.0" mean something a reader can reason about
187// (six times as yielding as a rigid constraint). alpha=0 is rigid; bigger is softer.
188// Tuning tissue means editing DATA here, never solver code.
189func st_layer_alpha_dist(l: i64) -> i64 {
190 if l == ST_LAY_MUSCLE { return 0 }
191 if l == ST_LAY_GLAND { return ST_MAGIC_1229 }
192 if l == ST_LAY_ADIPOSE { return ST_MAGIC_12288 }
193 if l == ST_LAY_SKIN { return ST_MAGIC_1229 }
194 if l == ST_LAY_LIGAMENT { return 0 }
195 return ST_MAGIC_1229
196}
197// Volume softness RANKING only -- it selects each tet's material (softest corner wins).
198// The volume constraint itself is solved HARD: soft tissue is ~incompressible in reality,
199// its give is shear and stretch, which is where the distance compliance above lives.
200func st_layer_alpha_vol(l: i64) -> i64 {
201 if l == ST_LAY_MUSCLE { return 0 }
202 if l == ST_LAY_GLAND { return 524 }
203 if l == ST_LAY_ADIPOSE { return ST_MAGIC_20972 }
204 if l == ST_LAY_SKIN { return 0 }
205 if l == ST_LAY_LIGAMENT { return 0 }
206 return 524
207}
208// Per-layer viscous damping q12: the fraction of velocity dissipated each substep.
209// This is what makes tissue SETTLE instead of ringing like jelly forever.
210func st_layer_damp(l: i64) -> i64 {
211 if l == ST_LAY_MUSCLE { return ST_MAGIC_1229 }
212 if l == ST_LAY_GLAND { return 491 }
213 if l == ST_LAY_ADIPOSE { return 246 }
214 if l == ST_LAY_SKIN { return 819 }
215 if l == ST_LAY_LIGAMENT { return ST_MAGIC_1434 }
216 return 491
217}
218
219// ===== DATA ROWS: body-region profiles ================================
220// The operator's law as data: "larger and softer flattens more; firmer flattens less."
221// LARGE_SOFT = big, fat-dominant, weak suspensory ligament.
222// SMALL_FIRM = smaller, gland-dominant, rigid ligament.
223// GLUTE = the same solver for buttocks: a dense near-rigid muscle base under a thick
224// high-compliance fat layer (the operator's gluteus/subcutaneous split).
225// Deterministic per-body hash. Same seed always yields the same body -- a body must be
226// reproducible from its id, or nothing downstream (save files, twins, review captures) can
227// refer to it.
228func st_hash(seed: i64, salt: i64) -> i64 {
229 var h: i64 = seed * ST_MAGIC_2654435761 + salt * ST_MAGIC_1013904223 + ST_MAGIC_1442695040888963407
230 h = h ^ (h >> 15)
231 h = h * ST_MAGIC_2246822519
232 h = h ^ (h >> 13)
233 h = h & ST_MAGIC_288230376151711743
234 if h < 0 { h = 0 - h }
235 return h
236}
237// Uniform pick in [lo,hi] inclusive.
238func st_span(seed: i64, salt: i64, lo: i64, hi: i64) -> i64 {
239 if hi <= lo { return lo }
240 return lo + st_hash(seed, salt) % (hi - lo + 1)
241}
242
243// A body derived from its seed. Fields are NOT drawn independently: a single "fullness" axis
244// (0..1000) moves size, fat compliance, envelope laxity and suspensory load together, because
245// independent draws produce implausible combinations (a tiny body with maximal fat compliance
246// and lax skin is not a person). Per-field jitter on separate salts keeps it from collapsing
247// to a one-dimensional family, and frame size (ribcage) is drawn independently of fullness --
248// a broad frame and a full bust are genuinely unrelated traits.
249// BOUNDS ARE STATED WITH UNITS and were chosen to span the range the named reference profiles
250// occupy (52..82mm half-width etc.), not by taste; widen them by measurement, never by feel.
251func st_profile_seeded(seed: i64, field: i64) -> i64 {
252 let full: i64 = st_hash(seed, 101) % 1001
253 // Size axes are DRAWN FROM THE GUARD's envelope, not from numbers written here, so a body
254 // outside the plausible range cannot be constructed at all -- a post-hoc check would only
255 // report the mistake after the geometry existed. A_MM is a semi-axis, hence the halving of
256 // the breast-diameter envelope; B_MM (cranio-caudal) has no sealed axis of its own and is
257 // derived from A by a bounded ratio, which keeps it finite too.
258 if field == ST_PF_A_MM {
259 return pg_sample(NX_AXIS_BREAST_DIAMETER, PG_IDEALIZED, full) / 2
260 }
261 if field == ST_PF_B_MM {
262 let am: i64 = pg_sample(NX_AXIS_BREAST_DIAMETER, PG_IDEALIZED, full) / 2
263 return am * (ST_MAGIC_1050 + st_span(seed, 12, 0, 150)) / 1000
264 }
265 if field == ST_PF_C_MM {
266 return pg_sample(NX_AXIS_BREAST_PROJECTION, PG_IDEALIZED, full)
267 }
268 if field == ST_PF_GLAND_PCT { return 68 - full * 42 / 1000 + st_span(seed, 14, 0, 7) }
269 if field == ST_PF_LIG_ALPHA { return full * ST_MAGIC_9216 / 1000 + st_span(seed, 15, 0, ST_MAGIC_1024) }
270 if field == ST_PF_FAT_ALPHA { return ST_MAGIC_4096 + full * ST_MAGIC_22528 / 1000 + st_span(seed, 16, 0, ST_MAGIC_2048) }
271 if field == ST_PF_FAT_DAMP { return 430 - full * 220 / 1000 + st_span(seed, 17, 0, 40) }
272 if field == ST_PF_SKIN_ALPHA { return 410 + full * ST_MAGIC_4096 / 1000 + st_span(seed, 18, 0, 410) }
273 if field == ST_PF_RIB_R_MM { return st_span(seed, 19, 118, 146) }
274 return 0
275}
276
277func st_profile(prof: i64, field: i64) -> i64 {
278 if prof >= ST_PROF_SEED_BASE { return st_profile_seeded(prof - ST_PROF_SEED_BASE, field) }
279 if prof == ST_PROF_LARGE_SOFT {
280 if field == ST_PF_A_MM { return 75 }
281 if field == ST_PF_B_MM { return 85 }
282 if field == ST_PF_C_MM { return 70 }
283 if field == ST_PF_GLAND_PCT { return 30 }
284 if field == ST_PF_LIG_ALPHA { return ST_MAGIC_8192 }
285 if field == ST_PF_FAT_ALPHA { return ST_MAGIC_24576 }
286 if field == ST_PF_FAT_DAMP { return 205 }
287 if field == ST_PF_RIB_R_MM { return 130 }
288 if field == ST_PF_SKIN_ALPHA { return ST_MAGIC_4096 }
289 }
290 if prof == ST_PROF_SMALL_FIRM {
291 if field == ST_PF_A_MM { return 62 }
292 if field == ST_PF_B_MM { return 68 }
293 if field == ST_PF_C_MM { return 55 }
294 if field == ST_PF_GLAND_PCT { return 62 }
295 if field == ST_PF_LIG_ALPHA { return 0 }
296 if field == ST_PF_FAT_ALPHA { return ST_MAGIC_4096 }
297 if field == ST_PF_FAT_DAMP { return 410 }
298 if field == ST_PF_RIB_R_MM { return 130 }
299 if field == ST_PF_SKIN_ALPHA { return 410 }
300 }
301 if prof == ST_PROF_GLUTE {
302 if field == ST_PF_A_MM { return 95 }
303 if field == ST_PF_B_MM { return 95 }
304 if field == ST_PF_C_MM { return 80 }
305 if field == ST_PF_GLAND_PCT { return 55 }
306 if field == ST_PF_LIG_ALPHA { return ST_MAGIC_2048 }
307 if field == ST_PF_FAT_ALPHA { return ST_MAGIC_20480 }
308 if field == ST_PF_FAT_DAMP { return 287 }
309 if field == ST_PF_RIB_R_MM { return 165 }
310 if field == ST_PF_SKIN_ALPHA { return ST_MAGIC_2048 }
311 }
312 if prof == ST_PROF_SOFT_TAUT {
313 if field == ST_PF_SKIN_ALPHA { return 410 }
314 return st_profile(ST_PROF_LARGE_SOFT, field)
315 }
316 return 0
317}
318
319// Depth (mm) that the supporting bone surface falls away at lateral offset xm. The ribcage
320// is a cylinder about the cranial axis, so it curves in x and NOT in y -- and that asymmetry
321// is the whole reason supine tissue spreads sideways rather than up and down. Modelling the
322// wall as a flat plane silently deletes the effect the operator asked for, so the wall is
323// geometry here, not a constant.
324func st_wall_drop(xm: i64, rib_r: i64) -> i64 {
325 if rib_r <= 0 { return 0 }
326 let ax: i64 = st_abs(xm)
327 if ax >= rib_r { return rib_r }
328 return rib_r - st_isqrt(rib_r*rib_r - ax*ax)
329}
330
331// Effective per-edge/per-tet compliance: layer row, overridden by the profile for the two
332// tissues a body-composition choice actually changes (fat softness, ligament strength).
333func st_eff_alpha_dist(W: *i64, layer: i64) -> i64 {
334 let prof: i64 = W[ST_W_PROF]
335 if layer == ST_LAY_ADIPOSE { return st_profile(prof, ST_PF_FAT_ALPHA) }
336 if layer == ST_LAY_LIGAMENT { return st_profile(prof, ST_PF_LIG_ALPHA) }
337 if layer == ST_LAY_SKIN { return st_profile(prof, ST_PF_SKIN_ALPHA) }
338 return st_layer_alpha_dist(layer)
339}
340func st_eff_damp(W: *i64, layer: i64) -> i64 {
341 if layer == ST_LAY_ADIPOSE { return st_profile(W[ST_W_PROF], ST_PF_FAT_DAMP) }
342 return st_layer_damp(layer)
343}
344
345// alpha_tilde = alpha/dt^2, scaled against this constraint's own stiffness sum.
346// dsum = sum of w|grad C|^2 for the constraint (its rigid-response scale)
347// At the reference substep the result is simply alpha_rel * dsum; halving the substep
348// quadruples it, exactly as alpha/dt^2 requires. Returning it in dsum's units lets the caller
349// write den = dsum + alpha_tilde with no further conversion -- and, crucially, lambda then
350// converges to a NON-ZERO residual, so a soft material stays soft no matter how many solver
351// iterations run. (Plain PBD drives every material to rigid as iterations rise; that bug is
352// what made "large and soft" flatten LESS than "small and firm" in the first gate run.)
353func st_alpha_tilde(alpha_q12: i64, dsum: i64, dt_us: i64) -> i64 {
354 if alpha_q12 <= 0 { return 0 }
355 if dt_us <= 0 { return 0 }
356 var at: i64 = st_div_r(alpha_q12 * dsum, ST_Q12)
357 at = st_div_r(at * ST_DT_REF_US, dt_us)
358 at = st_div_r(at * ST_DT_REF_US, dt_us)
359 return at
360}
361
362// ===== cage construction ==============================================
363// Half-ellipsoid lattice cage. Inside test runs in MILLIMETRES (a*a*b*b*c*c overflows i64
364// in cmm) -- the shape is a grid predicate, so mm resolution is exact enough.
365func st_inside(xm: i64, ym: i64, zm: i64, a: i64, b: i64, c: i64) -> i64 {
366 if zm < 0 { return 0 }
367 let lhs: i64 = xm*xm*b*b*c*c + ym*ym*a*a*c*c + zm*zm*a*a*b*b
368 let rhs: i64 = a*a*b*b*c*c
369 if lhs <= rhs { return 1 }
370 return 0
371}
372
373func st_six_vol(W: *i64, i0: i64, i1: i64, i2: i64, i3: i64) -> i64 {
374 let px: *i64 = W[ST_W_PX] as *i64
375 let py: *i64 = W[ST_W_PY] as *i64
376 let pz: *i64 = W[ST_W_PZ] as *i64
377 let ax: i64 = st_div_r(px[i1] - px[i0], ST_GRAD_SHIFT)
378 let ay: i64 = st_div_r(py[i1] - py[i0], ST_GRAD_SHIFT)
379 let az: i64 = st_div_r(pz[i1] - pz[i0], ST_GRAD_SHIFT)
380 let bx: i64 = st_div_r(px[i2] - px[i0], ST_GRAD_SHIFT)
381 let by: i64 = st_div_r(py[i2] - py[i0], ST_GRAD_SHIFT)
382 let bz: i64 = st_div_r(pz[i2] - pz[i0], ST_GRAD_SHIFT)
383 let cx: i64 = st_div_r(px[i3] - px[i0], ST_GRAD_SHIFT)
384 let cy: i64 = st_div_r(py[i3] - py[i0], ST_GRAD_SHIFT)
385 let cz: i64 = st_div_r(pz[i3] - pz[i0], ST_GRAD_SHIFT)
386 return ax * (by*cz - bz*cy) + ay * (bz*cx - bx*cz) + az * (bx*cy - by*cx)
387}
388
389// Same signed 6V but read from PREDICTED positions (the solver works on q, not p).
390func st_six_vol_q(W: *i64, i0: i64, i1: i64, i2: i64, i3: i64) -> i64 {
391 let qx: *i64 = W[ST_W_QX] as *i64
392 let qy: *i64 = W[ST_W_QY] as *i64
393 let qz: *i64 = W[ST_W_QZ] as *i64
394 let ax: i64 = st_div_r(qx[i1] - qx[i0], ST_GRAD_SHIFT)
395 let ay: i64 = st_div_r(qy[i1] - qy[i0], ST_GRAD_SHIFT)
396 let az: i64 = st_div_r(qz[i1] - qz[i0], ST_GRAD_SHIFT)
397 let bx: i64 = st_div_r(qx[i2] - qx[i0], ST_GRAD_SHIFT)
398 let by: i64 = st_div_r(qy[i2] - qy[i0], ST_GRAD_SHIFT)
399 let bz: i64 = st_div_r(qz[i2] - qz[i0], ST_GRAD_SHIFT)
400 let cx: i64 = st_div_r(qx[i3] - qx[i0], ST_GRAD_SHIFT)
401 let cy: i64 = st_div_r(qy[i3] - qy[i0], ST_GRAD_SHIFT)
402 let cz: i64 = st_div_r(qz[i3] - qz[i0], ST_GRAD_SHIFT)
403 return ax * (by*cz - bz*cy) + ay * (bz*cx - bx*cz) + az * (bx*cy - by*cx)
404}
405
406func st_add_edge(W: *i64, htab: *i64, hidx: *i64, hmask: i64,
407 a: i64, b: i64, layer: i64) -> i64 {
408 var lo: i64 = a
409 var hi: i64 = b
410 if lo > hi { lo = b; hi = a }
411 let key: i64 = lo * ST_MAXP + hi + 1
412 var h: i64 = (key * ST_MAGIC_2654435761) & hmask
413 var guard: i64 = 0
414 while guard < 64 {
415 if htab[h] == 0 {
416 let ne: i64 = W[ST_W_NE]
417 if ne >= ST_MAXE { return 0 - 1 }
418 let edg: *i64 = W[ST_W_EDG] as *i64
419 let erl: *i64 = W[ST_W_ERL] as *i64
420 let ela: *i64 = W[ST_W_ELA] as *i64
421 let px: *i64 = W[ST_W_PX] as *i64
422 let py: *i64 = W[ST_W_PY] as *i64
423 let pz: *i64 = W[ST_W_PZ] as *i64
424 edg[ne*2] = lo
425 edg[ne*2+1] = hi
426 let dx: i64 = px[hi] - px[lo]
427 let dy: i64 = py[hi] - py[lo]
428 let dz: i64 = pz[hi] - pz[lo]
429 erl[ne] = st_isqrt(dx*dx + dy*dy + dz*dz)
430 ela[ne] = layer
431 htab[h] = key
432 hidx[h] = ne
433 W[ST_W_NE] = ne + 1
434 return ne
435 }
436 if htab[h] == key {
437 // Stiffest material wins a shared edge: skin and ligament must not be softened
438 // by an adipose tet that happens to touch the same pair.
439 let ela: *i64 = W[ST_W_ELA] as *i64
440 if st_layer_alpha_dist(layer) < st_layer_alpha_dist(ela[hidx[h]]) {
441 ela[hidx[h]] = layer
442 }
443 return hidx[h]
444 }
445 h = (h + 1) & hmask
446 guard = guard + 1
447 }
448 return 0 - 1
449}
450
451// Build the cage for a profile. h_mm is the lattice cell size (the element-count dial:
452// 10 mm lands ~4-5k tets, the density the operator's blueprint calls for).
453func st_new(prof: i64, h_mm: i64) -> *i64 {
454 let W: *i64 = sys_mmap(ST_W_SLOTS * 8) as *i64
455 var s: i64 = 0
456 while s < ST_W_SLOTS { W[s] = 0; s = s + 1 }
457 W[ST_W_PROF] = prof
458 W[ST_W_PX] = sys_mmap(ST_MAXP*8) as i64
459 W[ST_W_PY] = sys_mmap(ST_MAXP*8) as i64
460 W[ST_W_PZ] = sys_mmap(ST_MAXP*8) as i64
461 W[ST_W_QX] = sys_mmap(ST_MAXP*8) as i64
462 W[ST_W_QY] = sys_mmap(ST_MAXP*8) as i64
463 W[ST_W_QZ] = sys_mmap(ST_MAXP*8) as i64
464 W[ST_W_VX] = sys_mmap(ST_MAXP*8) as i64
465 W[ST_W_VY] = sys_mmap(ST_MAXP*8) as i64
466 W[ST_W_VZ] = sys_mmap(ST_MAXP*8) as i64
467 W[ST_W_IW] = sys_mmap(ST_MAXP*8) as i64
468 W[ST_W_LAY] = sys_mmap(ST_MAXP*8) as i64
469 W[ST_W_SRF] = sys_mmap(ST_MAXP*8) as i64
470 W[ST_W_TET] = sys_mmap(ST_MAXT*4*8) as i64
471 W[ST_W_TRV] = sys_mmap(ST_MAXT*8) as i64
472 W[ST_W_TLA] = sys_mmap(ST_MAXT*8) as i64
473 W[ST_W_TLM] = sys_mmap(ST_MAXT*8) as i64
474 W[ST_W_EDG] = sys_mmap(ST_MAXE*2*8) as i64
475 W[ST_W_ERL] = sys_mmap(ST_MAXE*8) as i64
476 W[ST_W_ELA] = sys_mmap(ST_MAXE*8) as i64
477 W[ST_W_ELM] = sys_mmap(ST_MAXE*8) as i64
478
479 let a: i64 = st_profile(prof, ST_PF_A_MM)
480 let b: i64 = st_profile(prof, ST_PF_B_MM)
481 let c: i64 = st_profile(prof, ST_PF_C_MM)
482 let rib: i64 = st_profile(prof, ST_PF_RIB_R_MM)
483 if a <= 0 { return W }
484
485 let gnx: i64 = (2*a) / h_mm + 1
486 let gny: i64 = (2*b) / h_mm + 1
487 let gnz: i64 = c / h_mm + 1
488 let gtot: i64 = (gnx+1) * (gny+1) * (gnz+1)
489 let gid: *i64 = sys_mmap(gtot*8) as *i64
490 var g: i64 = 0
491 while g < gtot { gid[g] = 0 - 1; g = g + 1 }
492
493 let px: *i64 = W[ST_W_PX] as *i64
494 let py: *i64 = W[ST_W_PY] as *i64
495 let pz: *i64 = W[ST_W_PZ] as *i64
496 let iw: *i64 = W[ST_W_IW] as *i64
497 let lay: *i64 = W[ST_W_LAY] as *i64
498 let srf: *i64 = W[ST_W_SRF] as *i64
499
500 // gland core radii (percent of the outer axes) -- the firmness dial
501 let gp: i64 = st_profile(prof, ST_PF_GLAND_PCT)
502 let ga: i64 = a * gp / 100
503 let gb: i64 = b * gp / 100
504 let gc: i64 = c * gp / 100
505
506 // pass 1: particles at inside lattice points
507 var k: i64 = 0
508 while k <= gnz {
509 var j: i64 = 0
510 while j <= gny {
511 var i: i64 = 0
512 while i <= gnx {
513 let xm: i64 = 0 - a + i * h_mm
514 let ym: i64 = 0 - b + j * h_mm
515 let zm: i64 = k * h_mm
516 if st_inside(xm, ym, zm, a, b, c) == 1 {
517 let np: i64 = W[ST_W_NP]
518 if np < ST_MAXP {
519 px[np] = xm * 100
520 py[np] = ym * 100
521 pz[np] = (zm - st_wall_drop(xm, rib)) * 100
522 iw[np] = ST_Q12
523 lay[np] = ST_LAY_ADIPOSE
524 srf[np] = 0
525 gid[(k*(gny+1) + j)*(gnx+1) + i] = np
526 W[ST_W_NP] = np + 1
527 }
528 }
529 i = i + 1
530 }
531 j = j + 1
532 }
533 k = k + 1
534 }
535
536 // pass 2: layers. k==0 is the chest wall -> MUSCLE, pinned (inverse mass 0), the rigid
537 // skeletal anchor. A lattice point missing any 6-neighbour is on the boundary -> SKIN
538 // envelope. Interior points inside the gland ellipsoid -> GLAND. Rest stays ADIPOSE.
539 k = 0
540 while k <= gnz {
541 var j2: i64 = 0
542 while j2 <= gny {
543 var i2: i64 = 0
544 while i2 <= gnx {
545 let id: i64 = gid[(k*(gny+1) + j2)*(gnx+1) + i2]
546 if id >= 0 {
547 if k == 0 {
548 lay[id] = ST_LAY_MUSCLE
549 iw[id] = 0
550 } else {
551 var bnd: i64 = 0
552 if i2 == 0 { bnd = 1 }
553 if i2 == gnx { bnd = 1 }
554 if j2 == 0 { bnd = 1 }
555 if j2 == gny { bnd = 1 }
556 if k == gnz { bnd = 1 }
557 if bnd == 0 {
558 if gid[(k*(gny+1) + j2)*(gnx+1) + i2 - 1] < 0 { bnd = 1 }
559 if gid[(k*(gny+1) + j2)*(gnx+1) + i2 + 1] < 0 { bnd = 1 }
560 if gid[(k*(gny+1) + j2 - 1)*(gnx+1) + i2] < 0 { bnd = 1 }
561 if gid[(k*(gny+1) + j2 + 1)*(gnx+1) + i2] < 0 { bnd = 1 }
562 if gid[((k-1)*(gny+1) + j2)*(gnx+1) + i2] < 0 { bnd = 1 }
563 if gid[((k+1)*(gny+1) + j2)*(gnx+1) + i2] < 0 { bnd = 1 }
564 }
565 if bnd == 1 {
566 lay[id] = ST_LAY_SKIN
567 srf[id] = 1
568 } else {
569 let xm2: i64 = 0 - a + i2 * h_mm
570 let ym2: i64 = 0 - b + j2 * h_mm
571 let zm2: i64 = k * h_mm
572 if st_inside(xm2, ym2, zm2, ga, gb, gc) == 1 {
573 lay[id] = ST_LAY_GLAND
574 }
575 }
576 }
577 }
578 i2 = i2 + 1
579 }
580 j2 = j2 + 1
581 }
582 k = k + 1
583 }
584
585 // pass 3: Kuhn 6-tet subdivision of every fully-inside cell. All six tets share the
586 // main diagonal, so faces conform across neighbouring cells with no parity bookkeeping.
587 let tet: *i64 = W[ST_W_TET] as *i64
588 let trv: *i64 = W[ST_W_TRV] as *i64
589 let tla: *i64 = W[ST_W_TLA] as *i64
590 let cor: *i64 = sys_mmap(8*8) as *i64
591 let kt: *i64 = sys_mmap(24*8) as *i64
592 kt[0]=0; kt[1]=1; kt[2]=3; kt[3]=7
593 kt[4]=0; kt[5]=1; kt[6]=5; kt[7]=7
594 kt[8]=0; kt[9]=4; kt[10]=5; kt[11]=7
595 kt[12]=0; kt[13]=4; kt[14]=6; kt[15]=7
596 kt[16]=0; kt[17]=2; kt[18]=6; kt[19]=7
597 kt[20]=0; kt[21]=2; kt[22]=3; kt[23]=7
598
599 k = 0
600 while k < gnz {
601 var j3: i64 = 0
602 while j3 < gny {
603 var i3: i64 = 0
604 while i3 < gnx {
605 var ok: i64 = 1
606 var cc: i64 = 0
607 while cc < 8 {
608 let dx: i64 = cc & 1
609 let dy: i64 = (cc >> 1) & 1
610 let dz: i64 = (cc >> 2) & 1
611 let id2: i64 = gid[((k+dz)*(gny+1) + j3+dy)*(gnx+1) + i3+dx]
612 cor[cc] = id2
613 if id2 < 0 { ok = 0 }
614 cc = cc + 1
615 }
616 if ok == 1 {
617 var t: i64 = 0
618 while t < 6 {
619 let nt: i64 = W[ST_W_NT]
620 if nt < ST_MAXT {
621 var v0: i64 = cor[kt[t*4]]
622 var v1: i64 = cor[kt[t*4+1]]
623 var v2: i64 = cor[kt[t*4+2]]
624 var v3: i64 = cor[kt[t*4+3]]
625 var sv: i64 = st_six_vol(W, v0, v1, v2, v3)
626 if sv < 0 {
627 let sw: i64 = v2
628 v2 = v3
629 v3 = sw
630 sv = st_six_vol(W, v0, v1, v2, v3)
631 }
632 tet[nt*4] = v0
633 tet[nt*4+1] = v1
634 tet[nt*4+2] = v2
635 tet[nt*4+3] = v3
636 trv[nt] = sv
637 // A tet's material is its SOFTEST corner: fat between gland
638 // lobes must behave as fat, or the model reads uniformly firm.
639 var ml: i64 = ST_LAY_MUSCLE
640 var q: i64 = 0
641 while q < 4 {
642 let lq: i64 = lay[tet[nt*4+q]]
643 if st_layer_alpha_vol(lq) > st_layer_alpha_vol(ml) { ml = lq }
644 q = q + 1
645 }
646 tla[nt] = ml
647 W[ST_W_NT] = nt + 1
648 }
649 t = t + 1
650 }
651 }
652 i3 = i3 + 1
653 }
654 j3 = j3 + 1
655 }
656 k = k + 1
657 }
658
659 // pass 4: unique edges from tets
660 var hbits: i64 = 1
661 while hbits < ST_MAXE * 4 { hbits = hbits * 2 }
662 let htab: *i64 = sys_mmap(hbits*8) as *i64
663 let hidx: *i64 = sys_mmap(hbits*8) as *i64
664 var hz: i64 = 0
665 while hz < hbits { htab[hz] = 0; hidx[hz] = 0; hz = hz + 1 }
666 let hmask: i64 = hbits - 1
667
668 var tI: i64 = 0
669 while tI < W[ST_W_NT] {
670 var e1: i64 = 0
671 while e1 < 4 {
672 var e2: i64 = e1 + 1
673 while e2 < 4 {
674 let A: i64 = tet[tI*4+e1]
675 let B: i64 = tet[tI*4+e2]
676 var el: i64 = ST_LAY_ADIPOSE
677 if lay[A] == lay[B] { el = lay[A] }
678 if lay[A] == ST_LAY_SKIN { if lay[B] == ST_LAY_SKIN { el = ST_LAY_SKIN } }
679 st_add_edge(W, htab, hidx, hmask, A, B, el)
680 e2 = e2 + 1
681 }
682 e1 = e1 + 1
683 }
684 tI = tI + 1
685 }
686
687 // pass 5: Cooper's ligaments -- LONG-RANGE suspensory constraints from the chest wall to
688 // the skin envelope above it. Lattice edges are only one cell long; without these the
689 // tissue has nothing holding it up and every profile droops identically. This is the
690 // structure the firmness law actually rides on.
691 var i4: i64 = 0
692 while i4 <= gnx {
693 var j4: i64 = 0
694 while j4 <= gny {
695 let base: i64 = gid[(0*(gny+1) + j4)*(gnx+1) + i4]
696 if base >= 0 {
697 var top: i64 = 0 - 1
698 var k4: i64 = gnz
699 while k4 > 0 {
700 if top < 0 {
701 let cand: i64 = gid[(k4*(gny+1) + j4)*(gnx+1) + i4]
702 if cand >= 0 { top = cand }
703 }
704 k4 = k4 - 1
705 }
706 if top >= 0 { st_add_edge(W, htab, hidx, hmask, base, top, ST_LAY_LIGAMENT) }
707 }
708 j4 = j4 + 1
709 }
710 i4 = i4 + 1
711 }
712 return W
713}
714
715// ===== solver =========================================================
716func st_reset_state(W: *i64) -> i64 {
717 let vx: *i64 = W[ST_W_VX] as *i64
718 let vy: *i64 = W[ST_W_VY] as *i64
719 let vz: *i64 = W[ST_W_VZ] as *i64
720 var i: i64 = 0
721 while i < W[ST_W_NP] { vx[i] = 0; vy[i] = 0; vz[i] = 0; i = i + 1 }
722 return 0
723}
724
725func st_set_touch(W: *i64, on: i64, cx: i64, cy: i64, cz: i64, r: i64) -> i64 {
726 W[ST_W_TON] = on
727 W[ST_W_TCX] = cx
728 W[ST_W_TCY] = cy
729 W[ST_W_TCZ] = cz
730 W[ST_W_TR] = r
731 return 0
732}
733
734// One XPBD substep. gx/gy/gz is the gravity UNIT vector in q12 (body frame).
735func st_substep(W: *i64, gx: i64, gy: i64, gz: i64, dt_us: i64, iters: i64) -> i64 {
736 let px: *i64 = W[ST_W_PX] as *i64
737 let py: *i64 = W[ST_W_PY] as *i64
738 let pz: *i64 = W[ST_W_PZ] as *i64
739 let qx: *i64 = W[ST_W_QX] as *i64
740 let qy: *i64 = W[ST_W_QY] as *i64
741 let qz: *i64 = W[ST_W_QZ] as *i64
742 let vx: *i64 = W[ST_W_VX] as *i64
743 let vy: *i64 = W[ST_W_VY] as *i64
744 let vz: *i64 = W[ST_W_VZ] as *i64
745 let iw: *i64 = W[ST_W_IW] as *i64
746 let lay: *i64 = W[ST_W_LAY] as *i64
747 let np: i64 = W[ST_W_NP]
748
749 // gravity integration + prediction
750 let dv: i64 = ST_GRAV_CMM * dt_us / ST_MAGIC_1000000
751 var i: i64 = 0
752 while i < np {
753 if iw[i] > 0 {
754 vx[i] = vx[i] + dv * gx / ST_Q12
755 vy[i] = vy[i] + dv * gy / ST_Q12
756 vz[i] = vz[i] + dv * gz / ST_Q12
757 } else {
758 vx[i] = 0; vy[i] = 0; vz[i] = 0
759 }
760 qx[i] = px[i] + st_div_r(vx[i] * dt_us, ST_MAGIC_1000000)
761 qy[i] = py[i] + st_div_r(vy[i] * dt_us, ST_MAGIC_1000000)
762 qz[i] = pz[i] + st_div_r(vz[i] * dt_us, ST_MAGIC_1000000)
763 i = i + 1
764 }
765
766 // zero the XPBD multipliers -- lambda accumulates WITHIN a substep only
767 let elm: *i64 = W[ST_W_ELM] as *i64
768 let tlm: *i64 = W[ST_W_TLM] as *i64
769 var z: i64 = 0
770 while z < W[ST_W_NE] { elm[z] = 0; z = z + 1 }
771 z = 0
772 while z < W[ST_W_NT] { tlm[z] = 0; z = z + 1 }
773
774 let edg: *i64 = W[ST_W_EDG] as *i64
775 let erl: *i64 = W[ST_W_ERL] as *i64
776 let ela: *i64 = W[ST_W_ELA] as *i64
777 let tet: *i64 = W[ST_W_TET] as *i64
778 let trv: *i64 = W[ST_W_TRV] as *i64
779 let tla: *i64 = W[ST_W_TLA] as *i64
780
781 var it: i64 = 0
782 while it < iters {
783 // --- distance constraints -------------------------------------
784 var e: i64 = 0
785 while e < W[ST_W_NE] {
786 let A: i64 = edg[e*2]
787 let B: i64 = edg[e*2+1]
788 let wA: i64 = iw[A]
789 let wB: i64 = iw[B]
790 if wA + wB > 0 {
791 let dx: i64 = qx[A] - qx[B]
792 let dy: i64 = qy[A] - qy[B]
793 let dz: i64 = qz[A] - qz[B]
794 let L: i64 = st_isqrt(dx*dx + dy*dy + dz*dz)
795 var live: i64 = 1
796 var C: i64 = L - erl[e]
797 // Cooper's ligaments are SUSPENSORY -- rope, not strut. They resist stretch
798 // and go slack under compression. Solving them bilaterally (the first draft)
799 // planted a rigid column from chest wall to apex pointing exactly along the
800 // direction supine gravity acts, which suppressed lying-down flattening and
801 // INVERTED the firmness law: firm tissue's stiffer ligaments made it flatten
802 // more, not less. A tension-only constraint is the anatomy, and it restores
803 // both behaviours at once.
804 if ela[e] == ST_LAY_LIGAMENT { if C < 0 { live = 0 } }
805 if live == 0 { C = 0 }
806 if L > 0 && live == 1 {
807 let at: i64 = st_alpha_tilde(st_eff_alpha_dist(W, ela[e]), wA + wB, dt_us)
808 let den: i64 = wA + wB + at
809 if den > 0 {
810 let num: i64 = (0 - C) * ST_Q12 - st_div_r(at * elm[e], ST_Q12)
811 let dl: i64 = st_div_r(num * ST_Q12, den)
812 elm[e] = elm[e] + dl
813 let ux: i64 = st_div_r(dx * ST_Q12, L)
814 let uy: i64 = st_div_r(dy * ST_Q12, L)
815 let uz: i64 = st_div_r(dz * ST_Q12, L)
816 let sA: i64 = st_div_r(wA * dl, ST_Q12)
817 let sB: i64 = st_div_r(wB * dl, ST_Q12)
818 qx[A] = qx[A] + st_div_r(sA * ux, ST_Q12 * ST_Q12)
819 qy[A] = qy[A] + st_div_r(sA * uy, ST_Q12 * ST_Q12)
820 qz[A] = qz[A] + st_div_r(sA * uz, ST_Q12 * ST_Q12)
821 qx[B] = qx[B] - st_div_r(sB * ux, ST_Q12 * ST_Q12)
822 qy[B] = qy[B] - st_div_r(sB * uy, ST_Q12 * ST_Q12)
823 qz[B] = qz[B] - st_div_r(sB * uz, ST_Q12 * ST_Q12)
824 }
825 }
826 }
827 e = e + 1
828 }
829
830 // --- volume constraints (incompressibility / inversion barrier) ---
831 var t: i64 = 0
832 while t < W[ST_W_NT] {
833 let v0: i64 = tet[t*4]
834 let v1: i64 = tet[t*4+1]
835 let v2: i64 = tet[t*4+2]
836 let v3: i64 = tet[t*4+3]
837 let w0: i64 = iw[v0]
838 let w1: i64 = iw[v1]
839 let w2: i64 = iw[v2]
840 let w3: i64 = iw[v3]
841 if w0 + w1 + w2 + w3 > 0 {
842 let ax: i64 = st_div_r(qx[v1] - qx[v0], ST_GRAD_SHIFT)
843 let ay: i64 = st_div_r(qy[v1] - qy[v0], ST_GRAD_SHIFT)
844 let az: i64 = st_div_r(qz[v1] - qz[v0], ST_GRAD_SHIFT)
845 let bx: i64 = st_div_r(qx[v2] - qx[v0], ST_GRAD_SHIFT)
846 let by: i64 = st_div_r(qy[v2] - qy[v0], ST_GRAD_SHIFT)
847 let bz: i64 = st_div_r(qz[v2] - qz[v0], ST_GRAD_SHIFT)
848 let cx: i64 = st_div_r(qx[v3] - qx[v0], ST_GRAD_SHIFT)
849 let cy: i64 = st_div_r(qy[v3] - qy[v0], ST_GRAD_SHIFT)
850 let cz: i64 = st_div_r(qz[v3] - qz[v0], ST_GRAD_SHIFT)
851 let g1x: i64 = by*cz - bz*cy
852 let g1y: i64 = bz*cx - bx*cz
853 let g1z: i64 = bx*cy - by*cx
854 let g2x: i64 = cy*az - cz*ay
855 let g2y: i64 = cz*ax - cx*az
856 let g2z: i64 = cx*ay - cy*ax
857 let g3x: i64 = ay*bz - az*by
858 let g3y: i64 = az*bx - ax*bz
859 let g3z: i64 = ax*by - ay*bx
860 let g0x: i64 = 0 - g1x - g2x - g3x
861 let g0y: i64 = 0 - g1y - g2y - g3y
862 let g0z: i64 = 0 - g1z - g2z - g3z
863 let sv: i64 = ax*g1x + ay*g1y + az*g1z
864 let C: i64 = sv - trv[t]
865 let n0: i64 = st_div_r(g0x*g0x + g0y*g0y + g0z*g0z, ST_Q12)
866 let n1: i64 = st_div_r(g1x*g1x + g1y*g1y + g1z*g1z, ST_Q12)
867 let n2: i64 = st_div_r(g2x*g2x + g2y*g2y + g2z*g2z, ST_Q12)
868 let n3: i64 = st_div_r(g3x*g3x + g3y*g3y + g3z*g3z, ST_Q12)
869 let den: i64 = w0*n0 + w1*n1 + w2*n2 + w3*n3
870 if den > 0 {
871 // Hard incompressibility, solved directly for dx so no intermediate
872 // lambda has to survive fixed point. dx_i = w_i * grad_i * (-C) / den.
873 // The response scales as 1/den, and den collapses toward zero exactly as
874 // an element flattens -- so resistance climbs without bound as det F -> 0.
875 // That is the inversion barrier: pressing harder makes tissue harder,
876 // which is why a surgical press dimples instead of turning inside out.
877 let num: i64 = 0 - C
878 qx[v0] = qx[v0] + st_div_r(st_div_r(w0*g0x, ST_Q12) * num, den)
879 qy[v0] = qy[v0] + st_div_r(st_div_r(w0*g0y, ST_Q12) * num, den)
880 qz[v0] = qz[v0] + st_div_r(st_div_r(w0*g0z, ST_Q12) * num, den)
881 qx[v1] = qx[v1] + st_div_r(st_div_r(w1*g1x, ST_Q12) * num, den)
882 qy[v1] = qy[v1] + st_div_r(st_div_r(w1*g1y, ST_Q12) * num, den)
883 qz[v1] = qz[v1] + st_div_r(st_div_r(w1*g1z, ST_Q12) * num, den)
884 qx[v2] = qx[v2] + st_div_r(st_div_r(w2*g2x, ST_Q12) * num, den)
885 qy[v2] = qy[v2] + st_div_r(st_div_r(w2*g2y, ST_Q12) * num, den)
886 qz[v2] = qz[v2] + st_div_r(st_div_r(w2*g2z, ST_Q12) * num, den)
887 qx[v3] = qx[v3] + st_div_r(st_div_r(w3*g3x, ST_Q12) * num, den)
888 qy[v3] = qy[v3] + st_div_r(st_div_r(w3*g3y, ST_Q12) * num, den)
889 qz[v3] = qz[v3] + st_div_r(st_div_r(w3*g3z, ST_Q12) * num, den)
890 }
891 }
892 t = t + 1
893 }
894
895 // --- contact: a finger or an instrument, as a sphere ----------
896 // Hard projection (alpha=0): a vertex inside the probe is pushed to its surface.
897 // Neighbours follow through the distance and volume constraints above, which is what
898 // makes a smooth dimple instead of a single-vertex spike -- the propagation the
899 // operator described falls out of the cage, it is not a separate effect.
900 if W[ST_W_TON] == 1 {
901 var s: i64 = 0
902 while s < np {
903 if iw[s] > 0 {
904 let dx: i64 = qx[s] - W[ST_W_TCX]
905 let dy: i64 = qy[s] - W[ST_W_TCY]
906 let dz: i64 = qz[s] - W[ST_W_TCZ]
907 let d2: i64 = dx*dx + dy*dy + dz*dz
908 let R: i64 = W[ST_W_TR]
909 if d2 < R*R {
910 var d: i64 = st_isqrt(d2)
911 if d == 0 { d = 1 }
912 qx[s] = W[ST_W_TCX] + dx * R / d
913 qy[s] = W[ST_W_TCY] + dy * R / d
914 qz[s] = W[ST_W_TCZ] + dz * R / d
915 }
916 }
917 s = s + 1
918 }
919 }
920 it = it + 1
921 }
922
923 // velocity from the solved positions, then per-layer viscous damping
924 var u: i64 = 0
925 while u < np {
926 if iw[u] > 0 {
927 vx[u] = st_div_r((qx[u] - px[u]) * ST_MAGIC_1000000, dt_us)
928 vy[u] = st_div_r((qy[u] - py[u]) * ST_MAGIC_1000000, dt_us)
929 vz[u] = st_div_r((qz[u] - pz[u]) * ST_MAGIC_1000000, dt_us)
930 let dmp: i64 = st_eff_damp(W, lay[u])
931 vx[u] = vx[u] - st_div_r(vx[u] * dmp, ST_Q12)
932 vy[u] = vy[u] - st_div_r(vy[u] * dmp, ST_Q12)
933 vz[u] = vz[u] - st_div_r(vz[u] * dmp, ST_Q12)
934 px[u] = qx[u]
935 py[u] = qy[u]
936 pz[u] = qz[u]
937 }
938 u = u + 1
939 }
940 return 0
941}
942
943func st_run(W: *i64, gx: i64, gy: i64, gz: i64, steps: i64, dt_us: i64, iters: i64) -> i64 {
944 var s: i64 = 0
945 while s < steps { st_substep(W, gx, gy, gz, dt_us, iters); s = s + 1 }
946 return 0
947}
948
949// ===== measurement (the drape validator's own statistics) =============
950const ST_M_ASPECT: i64 = 0
951const ST_M_CENTROID: i64 = 1
952const ST_M_MINVOL: i64 = 2
953const ST_M_SUMVOL: i64 = 3
954const ST_M_KIN: i64 = 4
955const ST_M_NSURF: i64 = 5
956const ST_M_PROJ: i64 = 6
957const ST_M_N: i64 = 8
958
959// Silhouette statistics over the SKIN envelope, in exactly the terms
960// nx_breast_drape_pose.nx scores: aspect = height/width, centroid = how far the centroid
961// sits below the top of the region. Both q10, both viewed frontally (x lateral, y cranial),
962// so the bands banked for generated imagery apply unchanged to simulated geometry.
963func st_measure(W: *i64, out: *i64) -> i64 {
964 let px: *i64 = W[ST_W_PX] as *i64
965 let py: *i64 = W[ST_W_PY] as *i64
966 let pz: *i64 = W[ST_W_PZ] as *i64
967 let vx: *i64 = W[ST_W_VX] as *i64
968 let vy: *i64 = W[ST_W_VY] as *i64
969 let vz: *i64 = W[ST_W_VZ] as *i64
970 let srf: *i64 = W[ST_W_SRF] as *i64
971 var f: i64 = 0
972 while f < ST_M_N { out[f] = 0; f = f + 1 }
973
974 var minx: i64 = ST_MAGIC_999999999
975 var maxx: i64 = 0 - ST_MAGIC_999999999
976 var miny: i64 = ST_MAGIC_999999999
977 var maxy: i64 = 0 - ST_MAGIC_999999999
978 var maxz: i64 = 0 - ST_MAGIC_999999999
979 var sumy: i64 = 0
980 var n: i64 = 0
981 var i: i64 = 0
982 while i < W[ST_W_NP] {
983 if srf[i] == 1 {
984 minx = st_min(minx, px[i]); maxx = st_max(maxx, px[i])
985 miny = st_min(miny, py[i]); maxy = st_max(maxy, py[i])
986 maxz = st_max(maxz, pz[i])
987 sumy = sumy + py[i]
988 n = n + 1
989 }
990 i = i + 1
991 }
992 out[ST_M_NSURF] = n
993 if n > 0 {
994 let hgt: i64 = maxy - miny
995 let wid: i64 = maxx - minx
996 if wid > 0 { out[ST_M_ASPECT] = hgt * ST_MAGIC_1024 / wid }
997 if hgt > 0 { out[ST_M_CENTROID] = (maxy - sumy / n) * ST_MAGIC_1024 / hgt }
998 out[ST_M_PROJ] = maxz
999 }
1000
1001 var mv: i64 = ST_MAGIC_999999999999999
1002 var sv: i64 = 0
1003 let tet: *i64 = W[ST_W_TET] as *i64
1004 var t: i64 = 0
1005 while t < W[ST_W_NT] {
1006 let v: i64 = st_six_vol(W, tet[t*4], tet[t*4+1], tet[t*4+2], tet[t*4+3])
1007 mv = st_min(mv, v)
1008 sv = sv + v
1009 t = t + 1
1010 }
1011 out[ST_M_MINVOL] = mv
1012 out[ST_M_SUMVOL] = sv
1013
1014 var kin: i64 = 0
1015 var u: i64 = 0
1016 while u < W[ST_W_NP] {
1017 kin = kin + st_abs(vx[u]) + st_abs(vy[u]) + st_abs(vz[u])
1018 u = u + 1
1019 }
1020 out[ST_M_KIN] = kin
1021 return 0
1022}