nx_pose_retarget.nx source
↩ module page · 72 lines · 3798 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)
10const K_MAGIC_25736: i64 = 25736
11const K_MAGIC_4096: i64 = 4096
12const K_MAGIC_12868: i64 = 12868
13
14func rt_isqrt(v: i64) -> i64 { if v <= 0 { return 0 } var x: i64 = v; var y: i64 = (x + 1) / 2; while y < x { x = y; y = (x + v / x) / 2 } return x }
15
16// atan2(y,x) -> signed angle in (-12868, 12868] (i.e. (-pi, pi]) in the it_* unit (full circle 25736). Binary
17// search the magnitude angle in [0, pi] on the monotonic-decreasing cos table, then sign by y.
18func rt_atan2(y: i64, x: i64) -> i64 {
19 let r: i64 = rt_isqrt(x * x + y * y)
20 if r == 0 { return 0 }
21 let cx: i64 = x * K_MAGIC_4096 / r // target cos in [-K_MAGIC_4096, K_MAGIC_4096]
22 var lo: i64 = 0; var hi: i64 = K_MAGIC_12868
23 var it: i64 = 0
24 while it < 18 {
25 let mid: i64 = (lo + hi) / 2
26 if it_cos4096(mid) > cx { lo = mid } else { hi = mid } // cos decreasing -> cos>target means angle too small
27 it = it + 1
28 }
29 var a: i64 = (lo + hi) / 2
30 if y < 0 { a = 0 - a }
31 return a
32}
33// asin(s), s in [-4096,4096] (a sine value fx4096) -> angle in [-6434, 6434] (+-pi/2).
34func rt_asin(s: i64) -> i64 {
35 var ss: i64 = s
36 if ss > K_MAGIC_4096 { ss = K_MAGIC_4096 }
37 if ss < 0 - K_MAGIC_4096 { ss = 0 - K_MAGIC_4096 }
38 return rt_atan2(ss, rt_isqrt(K_MAGIC_4096 * K_MAGIC_4096 - ss * ss))
39}
40
41// retarget a CHAIN: bone i aims at bone i+1 (nbones bones, offsets along +x). targets = nbones*3 world positions
42// (i64, the SAME units the rig uses). Sets each bone's yaw/pitch; leaf (last) set to 0 (undetermined by
43// positions). Caller runs sk_update after. Returns the number of bones solved.
44func rt_retarget_chain(base: i64, targets: *i64, nbones: i64) -> i64 {
45 let pR: *i64 = sys_mmap(9 * 8) as *i64 // parent world rotation (fx256); starts = identity
46 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
47 let lr: *i64 = sys_mmap(9 * 8) as *i64
48 let nr: *i64 = sys_mmap(9 * 8) as *i64
49 var i: i64 = 0
50 while i < nbones - 1 {
51 let dwx: i64 = targets[(i+1)*3] - targets[i*3]
52 let dwy: i64 = targets[(i+1)*3+1] - targets[i*3+1]
53 let dwz: i64 = targets[(i+1)*3+2] - targets[i*3+2]
54 // into parent-local frame: dl = pR^T * dw (pR fx256)
55 let dlx: i64 = (pR[0]*dwx + pR[3]*dwy + pR[6]*dwz) / 256
56 let dly: i64 = (pR[1]*dwx + pR[4]*dwy + pR[7]*dwz) / 256
57 let dlz: i64 = (pR[2]*dwx + pR[5]*dwy + pR[8]*dwz) / 256
58 let mag: i64 = rt_isqrt(dlx*dlx + dly*dly + dlz*dlz)
59 var elev: i64 = 0
60 if mag > 0 { elev = rt_asin(dly * K_MAGIC_4096 / mag) }
61 let yaw: i64 = rt_atan2(0 - dlz, dlx)
62 sk_pose(base, i, yaw, elev)
63 // advance parent frame: pR = pR * localR(yaw,elev)
64 sk_localrot(yaw, elev, lr)
65 sk_matmul(pR, lr, nr)
66 var k: i64 = 0
67 while k < 9 { pR[k] = nr[k]; k = k + 1 }
68 i = i + 1
69 }
70 sk_pose(base, nbones - 1, 0, 0)
71 return nbones - 1
72}