nx_body_rest_frame_candidate_t345.nx source
↩ module page · 40 lines · 3513 B
1// Segment rest registration. Input joints retain source units; caller declares units per metre and frame axes.
2import "nx_body_rest_registration_candidate_t345.nx"
3import "nx_nxa_affine_palette_t34.nx"
4import "nx_vecmath.nx"
5const BRF_COMPONENT_MAX:i64=1073741824
6// Rescale before squaring: each normalized intermediate is <=Q20, so three squares fit signed64.
7func brf_normalize(x:i64,y:i64,z:i64,out:*i64)->i64{
8 if x<0-BRF_COMPONENT_MAX||x>BRF_COMPONENT_MAX||y<0-BRF_COMPONENT_MAX||y>BRF_COMPONENT_MAX||z<0-BRF_COMPONENT_MAX||z>BRF_COMPONENT_MAX{return BR_RANGE}
9 var largest:i64=vm_abs(x);if vm_abs(y)>largest{largest=vm_abs(y)};if vm_abs(z)>largest{largest=vm_abs(z)};if largest==0{return BR_SCHEMA}
10 let a:i64=x*FBB_Q/largest;let b:i64=y*FBB_Q/largest;let c:i64=z*FBB_Q/largest;let length:i64=vm_isqrt(a*a+b*b+c*c)
11 out[0]=a*FBB_Q/length;out[1]=b*FBB_Q/length;out[2]=c*FBB_Q/length;return 0
12}
13// Column-major frame: origin at parent, Y spans parent-to-joint, X/Z unit transverse axes.
14// Root origin is itself with caller-declared unit Y. Explicit referenceZ selects roll; parallel axes refuse.
15// Validated graph is internal input. Output unchanged on any refusal; scratch is25 caller-owned i64.
16func brf_segment(rows:*i64,n:i64,index:i64,units:i64,root_y:*i64,reference_z:*i64,out:*i64,scratch:*i64)->i64{
17 if index<0||index>=n||units<1||units>BRF_COMPONENT_MAX{return BR_RANGE}
18 let row:*i64=((rows as i64)+index*BR_ROW_BYTES) as *i64
19 var parent:i64=index;if row[1]>=0{parent=br_joint_index(rows,n,row[1]);if parent<0{return BR_HIERARCHY}}
20 let origin:*i64=((rows as i64)+parent*BR_ROW_BYTES) as *i64
21 var k:i64=3;while k<6{if row[k]<0-BRF_COMPONENT_MAX/2||row[k]>BRF_COMPONENT_MAX/2||origin[k]<0-BRF_COMPONENT_MAX/2||origin[k]>BRF_COMPONENT_MAX/2{return BR_RANGE};k=k+1}
22 let y:*i64=((scratch as i64)+128) as *i64;let z:*i64=((scratch as i64)+152) as *i64;let x:*i64=((scratch as i64)+176) as *i64
23 var dx:i64=row[3]-origin[3];var dy:i64=row[4]-origin[4];var dz:i64=row[5]-origin[5]
24 if row[1]<0{dx=root_y[0];dy=root_y[1];dz=root_y[2]}
25 var rc:i64=brf_normalize(dx,dy,dz,y);if rc!=0{return rc};rc=brf_normalize(reference_z[0],reference_z[1],reference_z[2],z);if rc!=0{return rc}
26 rc=brf_normalize((y[1]*z[2]-y[2]*z[1])/FBB_Q,(y[2]*z[0]-y[0]*z[2])/FBB_Q,(y[0]*z[1]-y[1]*z[0])/FBB_Q,x);if rc!=0{return rc}
27 z[0]=(x[1]*y[2]-x[2]*y[1])/FBB_Q;z[1]=(x[2]*y[0]-x[0]*y[2])/FBB_Q;z[2]=(x[0]*y[1]-x[1]*y[0])/FBB_Q
28 rc=brf_normalize(z[0],z[1],z[2],z);if rc!=0{return rc}
29 k=0;while k<16{scratch[k]=0;k=k+1};scratch[15]=FBB_Q
30 k=0;while k<3{scratch[k]=x[k];scratch[8+k]=z[k];scratch[12+k]=origin[3+k]*FBB_Q/units;scratch[4+k]=y[k];k=k+1}
31 if row[1]>=0{scratch[4]=dx*FBB_Q/units;scratch[5]=dy*FBB_Q/units;scratch[6]=dz*FBB_Q/units}
32 k=0;while k<16{if scratch[k]<=0-LA_AFFINE_MAX/AF_Q20_TO_Q30||scratch[k]>=LA_AFFINE_MAX/AF_Q20_TO_Q30{return BR_RANGE};k=k+1}
33 k=0;while k<16{out[k]=scratch[k];k=k+1};return 0
34}
35// Reuse checked inverse-bind and normal owners; no quaternion approximation of scaled rest segments.
36func brf_delta(reference:*i64,target:*i64,out:*i64,normal:*i64,budget:i64,scratch:*i64,report:*i64)->i64{
37 let inverse:*i64=scratch;let delta:*i64=((scratch as i64)+128) as *i64;let norm:*i64=((scratch as i64)+256) as *i64
38 var rc:i64=af_inverse_bind(reference,inverse,budget,report);if rc==0{rc=af_palette_matrix(target,inverse,delta)};if rc==0{rc=af_normal_matrix(delta,norm,budget,report)}
39 if rc==0{var i:i64=0;while i<16{out[i]=delta[i];i=i+1};i=0;while i<9{normal[i]=norm[i];i=i+1}};return rc
40}