nx_pose_retarget.nx source
↩ module page · 73 lines · 3723 B
1// nx_pose_retarget.nx -- RETARGET a pose track (joint world POSITIONS) onto our rig's joint ANGLES (roadmap R3,
2// the video->pose->MOTION heart: once R2 turns pixels into joint positions, this makes them DRIVE our rig ->
3// motion clips for the move library / text2motion corpus, fixing the no-mocap ceiling). Inverts the sovereign FK
4// (worldR=Ry(yaw)*Rz(elev), bones along +x -> aimed dir = (cy*ce, se, -sy*ce)): per bone aim its child, so
5// elev=asin(dir_y), yaw=atan2(-dir_z, dir_x) in the PARENT's local frame. Chain form (each bone -> next). Needs
6// integer atan2/asin, built on the it_* tables. license_tier: ORIGINAL
7import "nx_syscalls.nx"
8import "nx_skeleton.nx" // sk_localrot / sk_matmul / sk_pose / sk_bone / SK layout
9import "nx_itrig.nx" // it_sin4096 / it_cos4096 (full circle = 25736)
10import "nx_vecmath.nx"
11const K_MAGIC_25736: i64 = 25736
12const K_MAGIC_4096: i64 = 4096
13const K_MAGIC_12868: i64 = 12868
14
15func rt_isqrt(v: i64) -> i64 { return vm_isqrt(v) }
16
17// atan2(y,x) -> signed angle in (-12868, 12868] (i.e. (-pi, pi]) in the it_* unit (full circle 25736). Binary
18// search the magnitude angle in [0, pi] on the monotonic-decreasing cos table, then sign by y.
19func rt_atan2(y: i64, x: i64) -> i64 {
20 let r: i64 = rt_isqrt(x * x + y * y)
21 if r == 0 { return 0 }
22 let cx: i64 = x * K_MAGIC_4096 / r // target cos in [-K_MAGIC_4096, K_MAGIC_4096]
23 var lo: i64 = 0; var hi: i64 = K_MAGIC_12868
24 var it: i64 = 0
25 while it < 18 {
26 let mid: i64 = (lo + hi) / 2
27 if it_cos4096(mid) > cx { lo = mid } else { hi = mid } // cos decreasing -> cos>target means angle too small
28 it = it + 1
29 }
30 var a: i64 = (lo + hi) / 2
31 if y < 0 { a = 0 - a }
32 return a
33}
34// asin(s), s in [-4096,4096] (a sine value fx4096) -> angle in [-6434, 6434] (+-pi/2).
35func rt_asin(s: i64) -> i64 {
36 var ss: i64 = s
37 if ss > K_MAGIC_4096 { ss = K_MAGIC_4096 }
38 if ss < 0 - K_MAGIC_4096 { ss = 0 - K_MAGIC_4096 }
39 return rt_atan2(ss, rt_isqrt(K_MAGIC_4096 * K_MAGIC_4096 - ss * ss))
40}
41
42// retarget a CHAIN: bone i aims at bone i+1 (nbones bones, offsets along +x). targets = nbones*3 world positions
43// (i64, the SAME units the rig uses). Sets each bone's yaw/pitch; leaf (last) set to 0 (undetermined by
44// positions). Caller runs sk_update after. Returns the number of bones solved.
45func rt_retarget_chain(base: i64, targets: *i64, nbones: i64) -> i64 {
46 let pR: *i64 = sys_mmap(9 * 8) as *i64 // parent world rotation (fx256); starts = identity
47 pR[0]=256; pR[1]=0; pR[2]=0; pR[3]=0; pR[4]=256; pR[5]=0; pR[6]=0; pR[7]=0; pR[8]=256
48 let lr: *i64 = sys_mmap(9 * 8) as *i64
49 let nr: *i64 = sys_mmap(9 * 8) as *i64
50 var i: i64 = 0
51 while i < nbones - 1 {
52 let dwx: i64 = targets[(i+1)*3] - targets[i*3]
53 let dwy: i64 = targets[(i+1)*3+1] - targets[i*3+1]
54 let dwz: i64 = targets[(i+1)*3+2] - targets[i*3+2]
55 // into parent-local frame: dl = pR^T * dw (pR fx256)
56 let dlx: i64 = (pR[0]*dwx + pR[3]*dwy + pR[6]*dwz) / 256
57 let dly: i64 = (pR[1]*dwx + pR[4]*dwy + pR[7]*dwz) / 256
58 let dlz: i64 = (pR[2]*dwx + pR[5]*dwy + pR[8]*dwz) / 256
59 let mag: i64 = rt_isqrt(dlx*dlx + dly*dly + dlz*dlz)
60 var elev: i64 = 0
61 if mag > 0 { elev = rt_asin(dly * K_MAGIC_4096 / mag) }
62 let yaw: i64 = rt_atan2(0 - dlz, dlx)
63 sk_pose(base, i, yaw, elev)
64 // advance parent frame: pR = pR * localR(yaw,elev)
65 sk_localrot(yaw, elev, lr)
66 sk_matmul(pR, lr, nr)
67 var k: i64 = 0
68 while k < 9 { pR[k] = nr[k]; k = k + 1 }
69 i = i + 1
70 }
71 sk_pose(base, nbones - 1, 0, 0)
72 return nbones - 1
73}