nx_vecmath_slerp_candidate_t181.nx source
↩ module page · 497 lines · 23422 B
1// nx_vecmath.nx -- THE SHARED INTEGER LINEAR-ALGEBRA FOUNDATION (PG2, procgen.plan, 2026-08-24).
2//
3// WHY THIS EXISTS, MEASURED NOT ASSERTED: before this file the estate's entire shared math foundation was
4// nx_itrig.nx at 1151 bytes -- it_sin4096 and it_cos4096 and nothing else. Every consumer above it re-rolled
5// its own primitives: nx_gsplat carries gs_isqrt AND a private exp-LUT, nx_bodybench carries a private acos
6// LUT. Three duplicate rulers for two operations. Worse, with no quaternion type nx_gsplat's gs_set_aniso had
7// to settle for a normal-plus-tangent SURFEL where full 3D gaussian covariance needs quaternion-plus-3-axis
8// scale -- a capability ceiling caused by a missing primitive, not by a design choice.
9//
10// SCALE IS DERIVED, NOT PICKED: VM_ONE = 4096 because that is already the estate's unit. nx_itrig emits
11// fx4096 and measures angles in rad*4096; nx_vmd decodes quaternions at V_SCALE_ROT = 4096. Adopting any
12// other scale would have made this file a converter instead of a foundation. Every other constant here is
13// either derived from VM_ONE arithmetically or carries its derivation on its own line.
14//
15// COMPOSES THE INCUMBENT: sine and cosine come from nx_itrig by import. This file does NOT re-derive Taylor
16// coefficients. VM_PI and VM_PI2 restate nx_itrig's angle constants because NishiLang const scope does not
17// cross the import, so nx_vecmath_gate carries a tooth that FAILS if the two ever disagree -- a guarded
18// duplicate, not a silent one.
19//
20// NO HEAP, NO LOCAL ARRAYS, NO ALLOCATION ANYWHERE. Every routine works in scalar locals and writes through
21// caller-owned pointers. Two reasons, both estate law: this is hot-loop code and allocating in a hot loop is
22// a measured defect; and the estate's only allocation idiom is sys_mmap, which a pure math lib must not need.
23// Every routine that could alias its output with an input reads all operands into locals FIRST.
24//
25// ALL INTEGER. The language has no float type, so every result here is bit-exact and reproducible by
26// construction. license_tier: ORIGINAL
27
28import "nx_itrig.nx"
29
30// ---- scale ----------------------------------------------------------------
31const VM_ONE: i64 = 4096 // fixed-point unit. DERIVED: nx_itrig fx4096 and nx_vmd V_SCALE_ROT.
32const VM_PI: i64 = 12868 // round(PI * VM_ONE). Guarded by tooth pi-agrees-with-itrig.
33const VM_PI2: i64 = 6434 // round(PI/2 * VM_ONE)
34const VM_TAU: i64 = 25736 // 2 * VM_PI
35
36// atan rational approximation: atan(z) ~= z / (1 + K*z*z) for |z| <= 1, K = 9/32 = 0.28125, the
37// shift-friendly single-term minimax coefficient. Its worst-case error is NOT asserted here: the gate
38// sweeps every representable angle and PRINTS the measured maximum, so the bound is a measurement this
39// file cannot silently drift away from.
40const VM_ATAN_KN: i64 = 9
41const VM_ATAN_KD: i64 = 32
42
43// slerp uses normalised-lerp when the inputs are nearly parallel, because sin(theta) -> 0 makes the
44// spherical form divide by a quantised zero. DERIVED bar: at cos(theta) this close to unity, sin(theta)
45// is under one fixed-point unit, so the division would quantise to garbage rather than degrade. For such
46// inputs nlerp and slerp agree to well under one unit, so the fallback is the correct answer, not a
47// compromise. The gate carries an anti-vacuity tooth proving the SPHERICAL branch actually runs.
48const VM_SLERP_PARALLEL: i64 = 4095 // VM_ONE - 1
49
50const VM_V3: i64 = 3
51const VM_M3: i64 = 9
52
53func vm_one() -> i64 { return VM_ONE }
54func vm_pi() -> i64 { return VM_PI }
55func vm_pi2() -> i64 { return VM_PI2 }
56func vm_tau() -> i64 { return VM_TAU }
57
58// ---- scalar ---------------------------------------------------------------
59
60func vm_abs(v: i64) -> i64 { if v < 0 { return 0 - v } return v }
61func vm_sgn(v: i64) -> i64 { if v > 0 { return 1 } if v < 0 { return 0 - 1 } return 0 }
62func vm_min(a: i64, b: i64) -> i64 { if a < b { return a } return b }
63func vm_max(a: i64, b: i64) -> i64 { if a > b { return a } return b }
64func vm_clamp(v: i64, lo: i64, hi: i64) -> i64 { if v < lo { return lo } if v > hi { return hi } return v }
65
66// Integer square root, Newton. Exact on perfect squares, floor otherwise.
67// THIS RETIRES gs_isqrt in nx_gsplat -- same algorithm, one owner.
68func vm_isqrt(v: i64) -> i64 {
69 if v <= 0 { return 0 }
70 // SEED FROM THE BIT LENGTH (2026-08-31, the hair-guide hot loop): 2^ceil(bits/2) >= sqrt(v), so the same
71 // monotone Newton descent starts at most one doubling above the root instead of at v -- ~5 divisions per
72 // call instead of ~bits/2 + 5. RESULT IDENTICAL BY CONSTRUCTION: this loop's invariant (descend while y < x
73 // from any overestimate) lands on floor(sqrt(v)) whatever the start, so every caller reads the same integer.
74 var b: i64 = 0
75 var t: i64 = v
76 while t > 0 { t = t >> 1; b = b + 1 }
77 var x: i64 = 1 << ((b + 1) / 2)
78 var y: i64 = (x + v / x) / 2
79 while y < x { x = y; y = (x + v / x) / 2 }
80 return x
81}
82
83// Fixed-point square root: v in fx -> sqrt(v) in fx. sqrt(v/ONE)*ONE == sqrt(v*ONE), so one isqrt does it.
84func vm_sqrt_fx(v: i64) -> i64 {
85 if v <= 0 { return 0 }
86 return vm_isqrt(v * VM_ONE)
87}
88
89// ---- CAPSULE PUSH-OUT: THE ONE COLLIDER -----------------------------------------------------------
90// Point (px,py,pz) against the capsule from a to b with radius r. Returns 1 and writes the SURFACE point
91// into out[0..2] when the point is inside; returns 0 and leaves out untouched when it is outside, so a
92// caller can push N points against M capsules without branching on a sentinel value.
93//
94// WHY IT LIVES HERE, AND WHY IT IS NOT A FOURTH COPY. This arithmetic already existed THREE TIMES in the
95// estate: inline in nx_softjiggle's sj_step (Q256, the pushout), as ss_capsule in nx_skullsdf (Q1024,
96// distance only) and again in that file's _hdl_build twin -- while nx_vecmath, which owns the vm_isqrt all
97// three of them call, had none. The hair chain needed a collider and would have written the FOURTH.
98// Q256 on the parameter is adopted from sj_step DELIBERATELY rather than re-derived: it is the incumbent
99// that already ships a working pushout, so migrating it here is a byte-comparable change instead of a
100// second opinion about the same geometry.
101//
102// * THE DEGENERATE CASE IS NAMED, NOT SILENT: a point lying exactly ON the axis has no direction to be
103// pushed along. It takes +x. Any fixed choice is arbitrary; leaving it undefined would strand the point
104// inside the capsule forever, which is the silent class this collider exists to remove.
105const VM_CAP_Q: i64 = 256
106func vm_capsule_push(px: i64, py: i64, pz: i64,
107 ax: i64, ay: i64, az: i64,
108 bx: i64, by: i64, bz: i64,
109 r: i64, out: *i64) -> i64 {
110 let ex: i64 = bx - ax
111 let ey: i64 = by - ay
112 let ez: i64 = bz - az
113 let den: i64 = ex*ex + ey*ey + ez*ez
114 // THE PARAMETER IS DELIBERATELY NOT QUANTISED, AND nx_haircollide_gate MEASURED WHY.
115 // It was Q256, adopted from sj_step. On an axis of length L that gives the closest point a resolution
116 // of L/256 -- 3.9 units for a 1000-unit capsule -- so for any point CLOSER TO THE AXIS THAN THAT ERROR
117 // the computed offset is dominated by AXIAL truncation noise rather than by the real radial offset, and
118 // the push slides the point ALONG the capsule instead of out of it. Worked example from the gate's own
119 // failing values: point (0,880,0), axis (0,0,0)-(0,1000,0) gives t=225.28 truncated to 225, closest
120 // point (0,878,0) instead of (0,880,0), direction +Y instead of radial, result (0,978,0) -- still
121 // exactly on the axis, still inside, contact counted as RESOLVED. Four of four segments, every tick.
122 // * A QUANTISED PARAMETER IS FINE UNTIL THE THING YOU ARE MEASURING IS SMALLER THAN ITS STEP, AND THEN
123 // IT DOES NOT DEGRADE -- IT POINTS THE WRONG WAY WHILE STILL REPORTING SUCCESS.
124 // sj_step never exposed this because its points are skin-surface against limb axes and are never within
125 // a few units of the axis; hair hanging straight down the capsule is precisely the pathological case.
126 // Exact integer division on num/den removes the step entirely and costs one divide, not a tolerance.
127 // den == 0 is the degenerate capsule (a == b), which is a SPHERE at a -- handled by leaving c at a
128 // rather than dividing by zero.
129 var cx: i64 = ax
130 var cy: i64 = ay
131 var cz: i64 = az
132 if den > 0 {
133 var num: i64 = (px-ax)*ex + (py-ay)*ey + (pz-az)*ez
134 if num < 0 { num = 0 }
135 if num > den { num = den }
136 cx = ax + ex*num/den
137 cy = ay + ey*num/den
138 cz = az + ez*num/den
139 }
140 let dx: i64 = px - cx
141 let dy: i64 = py - cy
142 let dz: i64 = pz - cz
143 let d2: i64 = dx*dx + dy*dy + dz*dz
144 if d2 >= r*r { return 0 }
145 let dist: i64 = vm_isqrt(d2)
146 var ux: i64 = VM_CAP_Q
147 var uy: i64 = 0
148 var uz: i64 = 0
149 if dist > 0 { ux = dx*VM_CAP_Q/dist; uy = dy*VM_CAP_Q/dist; uz = dz*VM_CAP_Q/dist }
150 out[0] = cx + ux*r/VM_CAP_Q
151 out[1] = cy + uy*r/VM_CAP_Q
152 out[2] = cz + uz*r/VM_CAP_Q
153 return 1
154}
155
156// ---- IEEE-754 float32 codec ---------------------------------------------------------------------
157// The estate has no float type, but NXMSH2 and glTF carry float32 positions. FOUR private decoders were
158// counted (bb_f32mul, g2_f32s, and two more in the ingest lanes) and one private encoder (g2_enc). One
159// owner here. Decode ROUNDS to nearest (bodybench's choice); gltf2mesh's copy truncated, so a consumer
160// migrating from it may see a one-unit delta -- named, not hidden. `scale` is the integer units per 1.0f:
161// 1000 gives millimetres from metres (the g2_f32mm convention), 10000 gives tenths of a millimetre.
162const VM_F32_MANT_MASK: i64 = 8388607 // 2^23 - 1
163const VM_F32_IMPLICIT: i64 = 8388608 // 2^23
164const VM_F32_EXP_BIAS: i64 = 127
165const VM_F32_MANT_BITS: i64 = 23
166const VM_F32_EXP_MASK: i64 = 255
167const VM_F32_SHIFT_MAX: i64 = 62 // a shift at or beyond this cannot be represented in i64
168
169func vm_f32_to_int(bits: i64, scale: i64) -> i64 {
170 let sign: i64 = (bits >> 31) & 1
171 let expo: i64 = (bits >> VM_F32_MANT_BITS) & VM_F32_EXP_MASK
172 if expo == 0 { return 0 }
173 let m: i64 = ((bits & VM_F32_MANT_MASK) | VM_F32_IMPLICIT) * scale
174 let e: i64 = expo - VM_F32_EXP_BIAS - VM_F32_MANT_BITS
175 var v: i64 = 0
176 if e >= 0 { if e >= VM_F32_SHIFT_MAX { return 0 } v = m << e }
177 if e < 0 {
178 let sh: i64 = 0 - e
179 if sh >= VM_F32_SHIFT_MAX { return 0 }
180 v = (m + (1 << (sh - 1))) >> sh
181 }
182 if sign == 1 { return 0 - v }
183 return v
184}
185
186// Integer/scale -> float32 bits. Lifted from g2_enc (nx_body_gen's encoder), one owner now.
187func vm_int_to_f32(v: i64, scale: i64) -> i64 {
188 if v == 0 { return 0 }
189 var neg: i64 = 0
190 var m: i64 = v
191 if m < 0 { neg = 1; m = 0 - m }
192 var e: i64 = 0
193 var num: i64 = m
194 var den: i64 = scale
195 while num >= den * 2 { den = den * 2; e = e + 1 }
196 while num < den { num = num * 2; e = e - 1 }
197 let frac: i64 = ((num - den) * VM_F32_IMPLICIT) / den
198 var bits: i64 = ((e + VM_F32_EXP_BIAS) << VM_F32_MANT_BITS) | (frac & VM_F32_MANT_MASK)
199 if neg == 1 { bits = bits | (1 << 31) }
200 return bits
201}
202
203func vm_mul_fx(a: i64, b: i64) -> i64 { return a * b / VM_ONE }
204func vm_div_fx(a: i64, b: i64) -> i64 { if b == 0 { return 0 } return a * VM_ONE / b }
205
206func vm_sin(a: i64) -> i64 { return it_sin4096(a) }
207func vm_cos(a: i64) -> i64 { return it_cos4096(a) }
208
209// tan = sin/cos, guarded. DECLARED IMPRECISION: returns 0 at the poles, which is indistinguishable from
210// tan(0). A caller that must tell them apart should test cos itself. Stated so no reader assumes exactness.
211func vm_tan(a: i64) -> i64 {
212 let c: i64 = it_cos4096(a)
213 if c == 0 { return 0 }
214 return it_sin4096(a) * VM_ONE / c
215}
216
217// atan for |z| <= VM_ONE, z in fx, result rad*VM_ONE.
218func vm_atan_core(z: i64) -> i64 {
219 let z2: i64 = z * z / VM_ONE
220 let den: i64 = VM_ONE + VM_ATAN_KN * z2 / VM_ATAN_KD
221 if den == 0 { return 0 }
222 return z * VM_ONE / den
223}
224
225// Four-quadrant atan2. y and x in any consistent unit; the ratio is what matters.
226// Result rad*VM_ONE over (-PI, PI].
227func vm_atan2(y: i64, x: i64) -> i64 {
228 if x == 0 {
229 if y > 0 { return VM_PI2 }
230 if y < 0 { return 0 - VM_PI2 }
231 return 0
232 }
233 let ax: i64 = vm_abs(x)
234 let ay: i64 = vm_abs(y)
235 var a: i64 = 0
236 if ay <= ax { a = vm_atan_core(ay * VM_ONE / ax) }
237 if ay > ax { a = VM_PI2 - vm_atan_core(ax * VM_ONE / ay) }
238 if x < 0 { a = VM_PI - a }
239 if y < 0 { a = 0 - a }
240 return a
241}
242
243// acos COMPOSED from atan2 so the estate has exactly ONE inverse-trig ruler.
244// THIS RETIRES the private acos LUT in nx_bodybench. c in fx over [-ONE, ONE], result rad*VM_ONE over [0, PI].
245func vm_acos(c0: i64) -> i64 {
246 let c: i64 = vm_clamp(c0, 0 - VM_ONE, VM_ONE)
247 let s2: i64 = VM_ONE * VM_ONE - c * c
248 var s: i64 = 0
249 if s2 > 0 { s = vm_isqrt(s2) }
250 return vm_atan2(s, c)
251}
252
253func vm_asin(s0: i64) -> i64 {
254 let s: i64 = vm_clamp(s0, 0 - VM_ONE, VM_ONE)
255 let c2: i64 = VM_ONE * VM_ONE - s * s
256 var c: i64 = 0
257 if c2 > 0 { c = vm_isqrt(c2) }
258 return vm_atan2(s, c)
259}
260
261// ---- vec3 (caller-owned *i64 of 3) ----------------------------------------
262
263func vm_v3_set(o: *i64, x: i64, y: i64, z: i64) -> i64 { o[0] = x; o[1] = y; o[2] = z; return 0 }
264func vm_v3_copy(o: *i64, a: *i64) -> i64 { o[0] = a[0]; o[1] = a[1]; o[2] = a[2]; return 0 }
265func vm_v3_add(o: *i64, a: *i64, b: *i64) -> i64 { o[0] = a[0]+b[0]; o[1] = a[1]+b[1]; o[2] = a[2]+b[2]; return 0 }
266func vm_v3_sub(o: *i64, a: *i64, b: *i64) -> i64 { o[0] = a[0]-b[0]; o[1] = a[1]-b[1]; o[2] = a[2]-b[2]; return 0 }
267func vm_v3_scale(o: *i64, a: *i64, s: i64) -> i64 { o[0] = a[0]*s/VM_ONE; o[1] = a[1]*s/VM_ONE; o[2] = a[2]*s/VM_ONE; return 0 }
268
269func vm_v3_dot(a: *i64, b: *i64) -> i64 { return (a[0]*b[0] + a[1]*b[1] + a[2]*b[2]) / VM_ONE }
270
271// Alias-safe: reads all six inputs into locals before writing o.
272func vm_v3_cross(o: *i64, a: *i64, b: *i64) -> i64 {
273 let a0: i64 = a[0]; let a1: i64 = a[1]; let a2: i64 = a[2]
274 let b0: i64 = b[0]; let b1: i64 = b[1]; let b2: i64 = b[2]
275 o[0] = (a1*b2 - a2*b1) / VM_ONE
276 o[1] = (a2*b0 - a0*b2) / VM_ONE
277 o[2] = (a0*b1 - a1*b0) / VM_ONE
278 return 0
279}
280
281func vm_v3_len2(a: *i64) -> i64 { return (a[0]*a[0] + a[1]*a[1] + a[2]*a[2]) / VM_ONE }
282func vm_v3_len(a: *i64) -> i64 { return vm_sqrt_fx(vm_v3_len2(a)) }
283
284// Returns 1 on success, 0 for the zero vector with the output ZEROED.
285// Wrong-in-the-direction-of-doing-nothing: a caller ignoring the return gets zeros, never garbage.
286func vm_v3_norm(o: *i64, a: *i64) -> i64 {
287 let l: i64 = vm_v3_len(a)
288 if l == 0 { o[0] = 0; o[1] = 0; o[2] = 0; return 0 }
289 let a0: i64 = a[0]; let a1: i64 = a[1]; let a2: i64 = a[2]
290 o[0] = a0 * VM_ONE / l
291 o[1] = a1 * VM_ONE / l
292 o[2] = a2 * VM_ONE / l
293 return 1
294}
295
296// ---- quaternion (caller-owned *i64 of 4, laid out x y z w) ----------------
297// THE PRIMITIVE THE ESTATE DID NOT HAVE. nx_gsplat's gs_set_aniso takes a normal plus a tangent because
298// there was no quaternion to take. With this, quaternion-plus-3-axis-scale covariance is expressible.
299
300func vm_q_ident(o: *i64) -> i64 { o[0] = 0; o[1] = 0; o[2] = 0; o[3] = VM_ONE; return 0 }
301func vm_q_copy(o: *i64, a: *i64) -> i64 { o[0]=a[0]; o[1]=a[1]; o[2]=a[2]; o[3]=a[3]; return 0 }
302func vm_q_conj(o: *i64, a: *i64) -> i64 {
303 let a0: i64 = a[0]; let a1: i64 = a[1]; let a2: i64 = a[2]; let a3: i64 = a[3]
304 o[0]=0-a0; o[1]=0-a1; o[2]=0-a2; o[3]=a3; return 0
305}
306func vm_q_len(a: *i64) -> i64 {
307 return vm_sqrt_fx((a[0]*a[0] + a[1]*a[1] + a[2]*a[2] + a[3]*a[3]) / VM_ONE)
308}
309
310func vm_q_norm(o: *i64, a: *i64) -> i64 {
311 let l: i64 = vm_q_len(a)
312 if l == 0 { return vm_q_ident(o) }
313 let a0: i64 = a[0]; let a1: i64 = a[1]; let a2: i64 = a[2]; let a3: i64 = a[3]
314 o[0] = a0 * VM_ONE / l
315 o[1] = a1 * VM_ONE / l
316 o[2] = a2 * VM_ONE / l
317 o[3] = a3 * VM_ONE / l
318 return 1
319}
320
321// Alias-safe: o may be a or b.
322func vm_q_mul(o: *i64, a: *i64, b: *i64) -> i64 {
323 let a0: i64 = a[0]; let a1: i64 = a[1]; let a2: i64 = a[2]; let a3: i64 = a[3]
324 let b0: i64 = b[0]; let b1: i64 = b[1]; let b2: i64 = b[2]; let b3: i64 = b[3]
325 o[0] = (a3*b0 + a0*b3 + a1*b2 - a2*b1) / VM_ONE
326 o[1] = (a3*b1 - a0*b2 + a1*b3 + a2*b0) / VM_ONE
327 o[2] = (a3*b2 + a0*b1 - a1*b0 + a2*b3) / VM_ONE
328 o[3] = (a3*b3 - a0*b0 - a1*b1 - a2*b2) / VM_ONE
329 return 0
330}
331
332// axis need not be unit; normalised here. angle in rad*VM_ONE. Returns 0 and identity on a zero axis.
333func vm_q_from_axis(o: *i64, axis: *i64, angle: i64) -> i64 {
334 let l: i64 = vm_v3_len(axis)
335 if l == 0 { vm_q_ident(o); return 0 }
336 let ax0: i64 = axis[0] * VM_ONE / l
337 let ay0: i64 = axis[1] * VM_ONE / l
338 let az0: i64 = axis[2] * VM_ONE / l
339 let h: i64 = angle / 2
340 let s: i64 = vm_sin(h)
341 o[0] = ax0 * s / VM_ONE
342 o[1] = ay0 * s / VM_ONE
343 o[2] = az0 * s / VM_ONE
344 o[3] = vm_cos(h)
345 return 1
346}
347
348// v' = v + 2w(q x v) + 2(q x (q x v)). Fully inlined so no scratch is needed and o may alias v.
349func vm_q_rotate_v3(o: *i64, q: *i64, v: *i64) -> i64 {
350 let qx: i64 = q[0]; let qy: i64 = q[1]; let qz: i64 = q[2]; let qw: i64 = q[3]
351 let vx: i64 = v[0]; let vy: i64 = v[1]; let vz: i64 = v[2]
352 let tx: i64 = 2 * (qy*vz - qz*vy) / VM_ONE
353 let ty: i64 = 2 * (qz*vx - qx*vz) / VM_ONE
354 let tz: i64 = 2 * (qx*vy - qy*vx) / VM_ONE
355 let cx: i64 = (qy*tz - qz*ty) / VM_ONE
356 let cy: i64 = (qz*tx - qx*tz) / VM_ONE
357 let cz: i64 = (qx*ty - qy*tx) / VM_ONE
358 o[0] = vx + qw*tx/VM_ONE + cx
359 o[1] = vy + qw*ty/VM_ONE + cy
360 o[2] = vz + qw*tz/VM_ONE + cz
361 return 0
362}
363
364// Spherical linear interpolation, t in [0, VM_ONE]. Takes the shortest arc.
365func vm_q_slerp(o: *i64, a: *i64, b: *i64, t: i64) -> i64 {
366 let a0: i64 = a[0]; let a1: i64 = a[1]; let a2: i64 = a[2]; let a3: i64 = a[3]
367 var b0: i64 = b[0]; var b1: i64 = b[1]; var b2: i64 = b[2]; var b3: i64 = b[3]
368 var d: i64 = (a0*b0 + a1*b1 + a2*b2 + a3*b3) / VM_ONE
369 if d < 0 { b0 = 0-b0; b1 = 0-b1; b2 = 0-b2; b3 = 0-b3; d = 0 - d }
370 var wa: i64 = VM_ONE - t
371 var wb: i64 = t
372 if d < VM_SLERP_PARALLEL {
373 let th: i64 = vm_acos(d)
374 let sth: i64 = vm_sin(th)
375 if sth != 0 {
376 wa = vm_sin(th - th * t / VM_ONE) * VM_ONE / sth
377 wb = vm_sin(th * t / VM_ONE) * VM_ONE / sth
378 }
379 }
380 o[0] = (a0*wa + b0*wb) / VM_ONE
381 o[1] = (a1*wa + b1*wb) / VM_ONE
382 o[2] = (a2*wa + b2*wb) / VM_ONE
383 o[3] = (a3*wa + b3*wb) / VM_ONE
384 return vm_q_norm(o, o)
385}
386
387// ---- mat3 (caller-owned *i64 of 9, row-major) -----------------------------
388
389func vm_m3_ident(o: *i64) -> i64 {
390 var i: i64 = 0
391 while i < VM_M3 { o[i] = 0; i = i + 1 }
392 o[0] = VM_ONE; o[4] = VM_ONE; o[8] = VM_ONE
393 return 0
394}
395
396// Alias-safe: reads both operands into locals before writing o.
397func vm_m3_mul(o: *i64, a: *i64, b: *i64) -> i64 {
398 let a0: i64=a[0]; let a1: i64=a[1]; let a2: i64=a[2]
399 let a3: i64=a[3]; let a4: i64=a[4]; let a5: i64=a[5]
400 let a6: i64=a[6]; let a7: i64=a[7]; let a8: i64=a[8]
401 let b0: i64=b[0]; let b1: i64=b[1]; let b2: i64=b[2]
402 let b3: i64=b[3]; let b4: i64=b[4]; let b5: i64=b[5]
403 let b6: i64=b[6]; let b7: i64=b[7]; let b8: i64=b[8]
404 o[0] = (a0*b0 + a1*b3 + a2*b6) / VM_ONE
405 o[1] = (a0*b1 + a1*b4 + a2*b7) / VM_ONE
406 o[2] = (a0*b2 + a1*b5 + a2*b8) / VM_ONE
407 o[3] = (a3*b0 + a4*b3 + a5*b6) / VM_ONE
408 o[4] = (a3*b1 + a4*b4 + a5*b7) / VM_ONE
409 o[5] = (a3*b2 + a4*b5 + a5*b8) / VM_ONE
410 o[6] = (a6*b0 + a7*b3 + a8*b6) / VM_ONE
411 o[7] = (a6*b1 + a7*b4 + a8*b7) / VM_ONE
412 o[8] = (a6*b2 + a7*b5 + a8*b8) / VM_ONE
413 return 0
414}
415
416func vm_m3_mulv(o: *i64, m: *i64, v: *i64) -> i64 {
417 let v0: i64 = v[0]; let v1: i64 = v[1]; let v2: i64 = v[2]
418 o[0] = (m[0]*v0 + m[1]*v1 + m[2]*v2) / VM_ONE
419 o[1] = (m[3]*v0 + m[4]*v1 + m[5]*v2) / VM_ONE
420 o[2] = (m[6]*v0 + m[7]*v1 + m[8]*v2) / VM_ONE
421 return 0
422}
423
424func vm_m3_transpose(o: *i64, m: *i64) -> i64 {
425 let m0: i64=m[0]; let m1: i64=m[1]; let m2: i64=m[2]
426 let m3: i64=m[3]; let m4: i64=m[4]; let m5: i64=m[5]
427 let m6: i64=m[6]; let m7: i64=m[7]; let m8: i64=m[8]
428 o[0]=m0; o[1]=m3; o[2]=m6
429 o[3]=m1; o[4]=m4; o[5]=m7
430 o[6]=m2; o[7]=m5; o[8]=m8
431 return 0
432}
433
434// Quaternion to rotation matrix. THE BRIDGE that lets a gaussian carry a real orientation.
435func vm_q_to_m3(o: *i64, q: *i64) -> i64 {
436 let l: i64 = vm_q_len(q)
437 if l == 0 { return vm_m3_ident(o) }
438 let qx: i64 = q[0] * VM_ONE / l
439 let qy: i64 = q[1] * VM_ONE / l
440 let qz: i64 = q[2] * VM_ONE / l
441 let qw: i64 = q[3] * VM_ONE / l
442 let xx: i64 = qx*qx/VM_ONE
443 let yy: i64 = qy*qy/VM_ONE
444 let zz: i64 = qz*qz/VM_ONE
445 let xy: i64 = qx*qy/VM_ONE
446 let xz: i64 = qx*qz/VM_ONE
447 let yz: i64 = qy*qz/VM_ONE
448 let wx: i64 = qw*qx/VM_ONE
449 let wy: i64 = qw*qy/VM_ONE
450 let wz: i64 = qw*qz/VM_ONE
451 o[0] = VM_ONE - 2*(yy+zz); o[1] = 2*(xy-wz); o[2] = 2*(xz+wy)
452 o[3] = 2*(xy+wz); o[4] = VM_ONE - 2*(xx+zz); o[5] = 2*(yz-wx)
453 o[6] = 2*(xz-wy); o[7] = 2*(yz+wx); o[8] = VM_ONE - 2*(xx+yy)
454 return 0
455}
456
457// The 3x3 covariance-shaping matrix a gaussian needs: R * diag(sx, sy, sz).
458// This is the call nx_gsplat could not make, and the reason gs_set_aniso settled for a surfel.
459func vm_q_scale_to_m3(o: *i64, q: *i64, sx: i64, sy: i64, sz: i64) -> i64 {
460 vm_q_to_m3(o, q)
461 o[0] = o[0]*sx/VM_ONE; o[1] = o[1]*sy/VM_ONE; o[2] = o[2]*sz/VM_ONE
462 o[3] = o[3]*sx/VM_ONE; o[4] = o[4]*sy/VM_ONE; o[5] = o[5]*sz/VM_ONE
463 o[6] = o[6]*sx/VM_ONE; o[7] = o[7]*sy/VM_ONE; o[8] = o[8]*sz/VM_ONE
464 return 0
465}
466
467// High-precision geodesic quaternion interpolation. Repeated normalized midpoints
468// bisect the short great-circle arc; binary fraction selection avoids low-Q12 angles.
469const VM_Q30:i64=1073741824
470const VM_Q30_BITS:i64=30
471func vm_q_norm_q30(a:*i64,out:*i64,tolerance:i64)->i64 {
472 if tolerance<0||tolerance>VM_Q30{return -1}
473 var sum:i64=0;var i:i64=0
474 while i<4{if a[i]<0-VM_Q30||a[i]>VM_Q30{return -1};sum=sum+a[i]*a[i];i=i+1}
475 let length:i64=vm_isqrt(sum);if length<=0||vm_abs(length-VM_Q30)>tolerance{return -2}
476 i=0;while i<4{out[i]=a[i]*VM_Q30/length;i=i+1};return 0
477}
478func vm_q_slerp_q30(a:*i64,b:*i64,t:i64,tolerance:i64,out:*i64,scratch:*i64)->i64 {
479 if t<0||t>VM_Q30{return -1}
480 let lo:*i64=scratch;let hi:*i64=(scratch as i64+32) as *i64;let mid:*i64=(scratch as i64+64) as *i64
481 let lowrc:i64=vm_q_norm_q30(a,lo,tolerance);if lowrc!=0{return lowrc}
482 let highrc:i64=vm_q_norm_q30(b,hi,tolerance);if highrc!=0{return highrc}
483 var dot:i64=0;var i:i64=0;while i<4{dot=dot+lo[i]*hi[i];i=i+1}
484 if dot<0{i=0;while i<4{hi[i]=0-hi[i];i=i+1}}
485 var left:i64=0;var right:i64=VM_Q30;var step:i64=0
486 while step<VM_Q30_BITS{
487 if t==left{i=0;while i<4{out[i]=lo[i];i=i+1};return 0}
488 if t==right{i=0;while i<4{out[i]=hi[i];i=i+1};return 0}
489 i=0;var sum:i64=0;while i<4{mid[i]=(lo[i]+hi[i])/2;sum=sum+mid[i]*mid[i];i=i+1}
490 let length:i64=vm_isqrt(sum);if length<=0{return -2}
491 i=0;while i<4{mid[i]=mid[i]*VM_Q30/length;i=i+1}
492 let center:i64=(left+right)/2
493 if t==center{i=0;while i<4{out[i]=mid[i];i=i+1};return 0}
494 if t<center{i=0;while i<4{hi[i]=mid[i];i=i+1};right=center}else{i=0;while i<4{lo[i]=mid[i];i=i+1};left=center}
495 step=step+1
496 };return -3
497}