nx_bvhfk_lib.nx source
↩ module page · 186 lines · 7215 B
1// nx_bvhfk_lib.nx -- SHARED BVH FORWARD-KINEMATICS CORE (fk_eval + integer matrix substrate).
2//
3// Extracted 2026-08-13 when the SECOND consumer appeared (nx_clipcheck's bvh self-clearance
4// control lane needs world positions per frame) rather than copy-pasting the FK into it --
5// Rule 15, and the standing law: EXTRACT THE FIX, DON'T RE-TYPE IT. The functions below are
6// MOVED VERBATIM from nx_bvhfk.nx; that organ's KAT (bone-length isometry proven on real
7// third-party mocap) remains the proof that this extraction is value-identical.
8//
9// SEMANTICS (BVH is specific and easy to get subtly wrong):
10// world_rot[j] = world_rot[parent] * R_local[j]
11// world_pos[j] = world_pos[parent] + world_rot[parent] * offset[j]
12// R_local is the product of that joint's rotation channels IN THE ORDER THE FILE LISTS THEM
13// (commonly ZXY, but the file decides -- never the convention).
14// The root additionally consumes its position channels as a translation.
15//
16// UNITS: positions in file units x B_SCALE (1000) -- BVH does not state its unit; the convention
17// is centimetres, so we do not claim mm. Angles arrive as millidegrees and are converted to the
18// it_* integer-trig circle (25736 units) -- never to floats.
19// license_tier: ORIGINAL No hw writes (Rule 26).
20import "nx_bvh_lib.nx"
21import "nx_itrig.nx"
22import "nx_vecmath.nx"
23
24const FK_ONE: i64 = 65536 // matrices carry fx16, not the trig table's fx12 -- see fk_ortho
25const FK_TRIGUP: i64 = 16 // it_* tables are x4096; lift to x65536 for matmul headroom
26const FK_HALF: i64 = 32768 // round-to-nearest, not truncate-toward-zero
27const FK_CIRCLE: i64 = 25736
28const FK_MDEG: i64 = 360000
29
30func fk_ang(md: i64) -> i64 {
31 var a: i64 = md * FK_CIRCLE / FK_MDEG
32 a = a % FK_CIRCLE
33 if a < 0 { a = a + FK_CIRCLE }
34 return a
35}
36
37func fk_ident(m: *i64) -> i64 {
38 var i: i64 = 0
39 while i < 9 { m[i] = 0; i = i + 1 }
40 m[0] = FK_ONE; m[4] = FK_ONE; m[8] = FK_ONE
41 return 0
42}
43
44func fk_axis(code: i64, md: i64, m: *i64) -> i64 {
45 let a: i64 = fk_ang(md)
46 let c: i64 = it_cos4096(a) * FK_TRIGUP
47 let s: i64 = it_sin4096(a) * FK_TRIGUP
48 var i: i64 = 0
49 while i < 9 { m[i] = 0; i = i + 1 }
50 if code == B_CH_XROT {
51 m[0] = FK_ONE
52 m[4] = c; m[5] = 0 - s
53 m[7] = s; m[8] = c
54 return 0
55 }
56 if code == B_CH_YROT {
57 m[4] = FK_ONE
58 m[0] = c; m[2] = s
59 m[6] = 0 - s; m[8] = c
60 return 0
61 }
62 m[8] = FK_ONE
63 m[0] = c; m[1] = 0 - s
64 m[3] = s; m[4] = c
65 return 0
66}
67
68// C = A*B in fx4096 (C must not alias A or B)
69func fk_mul(a: *i64, b: *i64, c: *i64) -> i64 {
70 var i: i64 = 0
71 while i < 3 {
72 var j: i64 = 0
73 while j < 3 {
74 var acc: i64 = 0
75 var k: i64 = 0
76 while k < 3 { acc = acc + a[i*3+k] * b[k*3+j]; k = k + 1 }
77 if acc >= 0 { c[i*3+j] = (acc + FK_HALF) / FK_ONE }
78 else { c[i*3+j] = (acc - FK_HALF) / FK_ONE }
79 j = j + 1
80 }
81 i = i + 1
82 }
83 return 0
84}
85
86// out = M * v (M fx4096, v and out in file units x1000)
87func fk_xform(m: *i64, vx: i64, vy: i64, vz: i64, out: *i64) -> i64 {
88 out[0] = (m[0]*vx + m[1]*vy + m[2]*vz) / FK_ONE
89 out[1] = (m[3]*vx + m[4]*vy + m[5]*vz) / FK_ONE
90 out[2] = (m[6]*vx + m[7]*vy + m[8]*vz) / FK_ONE
91 return 0
92}
93
94// RE-ORTHONORMALISE a composed rotation (modified Gram-Schmidt, in place).
95//
96// WHY (measured 2026-08-07, T6 on real third-party mocap): bone lengths came out LONG, and the
97// error tracked CHAIN DEPTH. A bone's length depends only on its PARENT's matrix applied to a
98// fixed offset, so a depth-correlated error can only mean the composed matrix has drifted off
99// the orthonormal manifold. The it_* table stores sin and cos quantised to 1/4096, so c*c + s*s
100// != 1 exactly; each composition multiplies that gain (~1.0008 per axis rotation) and it
101// compounds to +1.2% by depth 6. A rotation IS an isometry, so we re-impose that property after
102// every composition: error stops compounding with depth and stays bounded per joint.
103func fk_ortho(m: *i64) -> i64 {
104 // row0 <- normalise(row0)
105 var l0: i64 = fk_isqrt(m[0]*m[0] + m[1]*m[1] + m[2]*m[2])
106 if l0 <= 0 { return 0 - 1 }
107 m[0] = m[0] * FK_ONE / l0
108 m[1] = m[1] * FK_ONE / l0
109 m[2] = m[2] * FK_ONE / l0
110 // row1 <- normalise(row1 - (row1.row0) row0)
111 let d: i64 = (m[3]*m[0] + m[4]*m[1] + m[5]*m[2]) / FK_ONE
112 m[3] = m[3] - d * m[0] / FK_ONE
113 m[4] = m[4] - d * m[1] / FK_ONE
114 m[5] = m[5] - d * m[2] / FK_ONE
115 var l1: i64 = fk_isqrt(m[3]*m[3] + m[4]*m[4] + m[5]*m[5])
116 if l1 <= 0 { return 0 - 1 }
117 m[3] = m[3] * FK_ONE / l1
118 m[4] = m[4] * FK_ONE / l1
119 m[5] = m[5] * FK_ONE / l1
120 // row2 <- row0 x row1 (already unit and orthogonal by construction)
121 m[6] = (m[1]*m[5] - m[2]*m[4]) / FK_ONE
122 m[7] = (m[2]*m[3] - m[0]*m[5]) / FK_ONE
123 m[8] = (m[0]*m[4] - m[1]*m[3]) / FK_ONE
124 return 0
125}
126
127func fk_isqrt(v: i64) -> i64 { return vm_isqrt(v) }
128
129func fk_dist(ax: i64, ay: i64, az: i64, bx: i64, by: i64, bz: i64) -> i64 {
130 let dx: i64 = ax - bx
131 let dy: i64 = ay - by
132 let dz: i64 = az - bz
133 return fk_isqrt(dx*dx + dy*dy + dz*dz)
134}
135
136// Evaluate one frame. rot[j*9] fx4096 world rotation, pos[j*3] world position.
137func fk_eval(H: *i64, parent: *i64, nchan: *i64, chan0: *i64, off: *i64, chtype: *i64,
138 vals: *i64, rot: *i64, pos: *i64) -> i64 {
139 let loc: *i64 = sys_mmap(9 * 8) as *i64
140 let axm: *i64 = sys_mmap(9 * 8) as *i64
141 let tmp: *i64 = sys_mmap(9 * 8) as *i64
142 let v3: *i64 = sys_mmap(3 * 8) as *i64
143 var j: i64 = 0
144 while j < H[B_H_NJOINT] {
145 fk_ident(loc)
146 var tx: i64 = 0
147 var ty: i64 = 0
148 var tz: i64 = 0
149 var c: i64 = 0
150 while c < nchan[j] {
151 let code: i64 = chtype[chan0[j] + c]
152 let v: i64 = vals[chan0[j] + c]
153 if code == B_CH_XPOS { tx = v }
154 else { if code == B_CH_YPOS { ty = v }
155 else { if code == B_CH_ZPOS { tz = v }
156 else {
157 // rotation channels compose IN FILE ORDER -- the file decides, not convention
158 fk_axis(code, v, axm)
159 fk_mul(loc, axm, tmp)
160 var k: i64 = 0
161 while k < 9 { loc[k] = tmp[k]; k = k + 1 }
162 } } }
163 c = c + 1
164 }
165 let p: i64 = parent[j]
166 if p < 0 {
167 var k2: i64 = 0
168 while k2 < 9 { rot[j*9 + k2] = loc[k2]; k2 = k2 + 1 }
169 fk_ortho(((rot as i64) + j*9*8) as *i64)
170 pos[j*3] = off[j*3] + tx
171 pos[j*3 + 1] = off[j*3 + 1] + ty
172 pos[j*3 + 2] = off[j*3 + 2] + tz
173 } else {
174 fk_mul(((rot as i64) + p*9*8) as *i64, loc, tmp)
175 var k3: i64 = 0
176 while k3 < 9 { rot[j*9 + k3] = tmp[k3]; k3 = k3 + 1 }
177 fk_ortho(((rot as i64) + j*9*8) as *i64)
178 fk_xform(((rot as i64) + p*9*8) as *i64, off[j*3], off[j*3+1], off[j*3+2], v3)
179 pos[j*3] = pos[p*3] + v3[0]
180 pos[j*3 + 1] = pos[p*3 + 1] + v3[1]
181 pos[j*3 + 2] = pos[p*3 + 2] + v3[2]
182 }
183 j = j + 1
184 }
185 return 0
186}