nx_nxa_fk.nx source

1// nx_nxa_fk.nx -- SHARED integer quaternion + FK primitives for the NXA import pipeline. 2// Extracted from nx_nxa_anim (DRY rule-15: nx_nxa_skin needs the same static-FK to compute 3// TRUE bind positions instead of placeholder propagation; nobody re-derives quat math). 4// q12 fixed-point (unit = 4096); angles arrive as millidegrees; all integer. 5// license_tier: ORIGINAL 6import "nx_itrig.nx" 7const K_MAGIC_4096: i64 = 4096 8const K_MAGIC_100000000: i64 = 100000000 9const K_MAGIC_65536: i64 = 65536 10const K_MAGIC_2048: i64 = 2048 11const K_MAGIC_57296: i64 = 57296 12 13// quat q12 multiply: r = a*b (x,y,z,w at [0..3]) 14func nf_qmul(a: *i64, b: *i64, r: *i64) -> i64 { 15 r[0] = (a[3]*b[0] + a[0]*b[3] + a[1]*b[2] - a[2]*b[1]) / K_MAGIC_4096 16 r[1] = (a[3]*b[1] - a[0]*b[2] + a[1]*b[3] + a[2]*b[0]) / K_MAGIC_4096 17 r[2] = (a[3]*b[2] + a[0]*b[1] - a[1]*b[0] + a[2]*b[3]) / K_MAGIC_4096 18 r[3] = (a[3]*b[3] - a[0]*b[0] - a[1]*b[1] - a[2]*b[2]) / K_MAGIC_4096 19 return 0 20} 21// integer sqrt (Newton) for quat norms; fixed 24 iterations, break-free by design 22func nf_isqrt(n: i64) -> i64 { 23 if n <= 0 { return 1 } 24 var x: i64 = K_MAGIC_4096 25 if n > K_MAGIC_100000000 { x = K_MAGIC_65536 } 26 var i: i64 = 0 27 while i < 24 { 28 let y: i64 = (x + n/x) / 2 29 if y > 0 { x = y } 30 i = i + 1 31 } 32 if x < 1 { x = 1 } 33 return x 34} 35// renormalize q to unit 4096 -- stops q12 truncation drift across deep FK chains 36func nf_qnorm(q: *i64) -> i64 { 37 let n2: i64 = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] 38 if n2 <= 0 { q[0]=0; q[1]=0; q[2]=0; q[3]=K_MAGIC_4096; return 0 } 39 let s: i64 = nf_isqrt(n2) 40 q[0] = q[0]*K_MAGIC_4096/s 41 q[1] = q[1]*K_MAGIC_4096/s 42 q[2] = q[2]*K_MAGIC_4096/s 43 q[3] = q[3]*K_MAGIC_4096/s 44 return 0 45} 46func nf_qconj(q: *i64, r: *i64) -> i64 { r[0]=0-q[0]; r[1]=0-q[1]; r[2]=0-q[2]; r[3]=q[3]; return 0 } 47// Euler XYZ millidegrees -> q12 quat (qz*qy*qx, FBX eOrderXYZ column form); scr = 16 words 48func nf_eul2q(mdx: i64, mdy: i64, mdz: i64, out: *i64, scr: *i64) -> i64 { 49 let hx: i64 = mdx*K_MAGIC_2048/K_MAGIC_57296 50 let hy: i64 = mdy*K_MAGIC_2048/K_MAGIC_57296 51 let hz: i64 = mdz*K_MAGIC_2048/K_MAGIC_57296 52 scr[0] = it_sin4096(hx) 53 scr[1] = 0 54 scr[2] = 0 55 scr[3] = it_cos4096(hx) 56 scr[4] = 0 57 scr[5] = it_sin4096(hy) 58 scr[6] = 0 59 scr[7] = it_cos4096(hy) 60 scr[8] = 0 61 scr[9] = 0 62 scr[10] = it_sin4096(hz) 63 scr[11] = it_cos4096(hz) 64 nf_qmul(((scr as i64)+32) as *i64, scr, ((scr as i64)+96) as *i64) 65 nf_qmul(((scr as i64)+64) as *i64, ((scr as i64)+96) as *i64, out) 66 return 0 67} 68// SHORTEST-ARC quat between two integer vectors, NO TRIG (half-angle identity): 69// q = (a x b, |a||b| + a.b) normalized. Inputs any magnitude; internally rescaled to ~4096 70// so no i64 overflow. a ~= -b (180deg) degenerates to identity -- callers use small deltas. 71func nf_qarc(ax: i64, ay: i64, az: i64, bx: i64, by: i64, bz: i64, out: *i64) -> i64 { 72 var la: i64 = nf_isqrt(ax*ax + ay*ay + az*az) 73 if la < 1 { la = 1 } 74 var lb: i64 = nf_isqrt(bx*bx + by*by + bz*bz) 75 if lb < 1 { lb = 1 } 76 let axs: i64 = ax*K_MAGIC_4096/la 77 let ays: i64 = ay*K_MAGIC_4096/la 78 let azs: i64 = az*K_MAGIC_4096/la 79 let bxs: i64 = bx*K_MAGIC_4096/lb 80 let bys: i64 = by*K_MAGIC_4096/lb 81 let bzs: i64 = bz*K_MAGIC_4096/lb 82 out[0] = (ays*bzs - azs*bys)/K_MAGIC_4096 83 out[1] = (azs*bxs - axs*bzs)/K_MAGIC_4096 84 out[2] = (axs*bys - ays*bxs)/K_MAGIC_4096 85 out[3] = K_MAGIC_4096 + (axs*bxs + ays*bys + azs*bzs)/K_MAGIC_4096 86 nf_qnorm(out) 87 return 0 88} 89// rotate integer vector v (any units) by unit q12 quat: out3 = q*(v,0)*conj(q); scr = 16 words 90func nf_qrotv(q: *i64, vx: i64, vy: i64, vz: i64, out3: *i64, scr: *i64) -> i64 { 91 scr[0] = vx 92 scr[1] = vy 93 scr[2] = vz 94 scr[3] = 0 95 nf_qconj(q, ((scr as i64)+32) as *i64) 96 nf_qmul(q, scr, ((scr as i64)+64) as *i64) 97 nf_qmul(((scr as i64)+64) as *i64, ((scr as i64)+32) as *i64, ((scr as i64)+96) as *i64) 98 out3[0] = scr[12] 99 out3[1] = scr[13] 100 out3[2] = scr[14] 101 return 0 102}