nx_nxa_hierarchy_correction_candidate_t332.nx source
↩ module page · 145 lines · 9709 B
1// World-space rigid correction of an existing inverse-bind skin palette.
2// Pj=Wj*inverse(Bj); C*Pj=(C*Wj)*inverse(Bj). Descendant relative pose is retained.
3// This does not choose contact angles, classify anatomy, or convert affine rigs to DQ.
4import "nx_linalg_affine_t34.nx"
5const HC_RIGID:i64=1
6// Source encoding owns the residual budget, not anatomy or visual preference.
7// For per-component uncertainty E Q30 units: one truncated product errs by
8// <2E+2, one two-product cofactor by <2*(2E+2), and each determinant term
9// by <E+2*(2E+2)+2. Summing three terms gives the admitted residual.
10// E=1 covers nearest-Q30 matrices generated from normalized quaternions.
11// E=65 covers a unit binary32 coefficient (half ULP <=64Q30) plus conversion.
12const HC_Q30_COMPONENT_ERROR:i64=1
13const HC_F32_COMPONENT_ERROR:i64=(FQ_ONE>>24)+1
14const HC_Q30_BUDGET:i64=3*(HC_Q30_COMPONENT_ERROR+2*(2*HC_Q30_COMPONENT_ERROR+2)+2)
15const HC_F32_BUDGET:i64=3*(HC_F32_COMPONENT_ERROR+2*(2*HC_F32_COMPONENT_ERROR+2)+2)
16
17const HC_BAD_GRAPH:i64=0-170
18const HC_BAD_DOMAIN:i64=0-171
19const HC_NONRIGID:i64=0-172
20const HC_STORAGE:i64=0-173
21func hc_matrix(p:*i64,j:i64)->*i64{return ((p as i64)+j*128) as *i64}
22func hc_disjoint(a:*i64,aw:i64,b:*i64,bw:i64)->i64{
23 let x:i64=a as i64;let y:i64=b as i64
24 if x<=0||y<=0||aw<1||bw<1{return 0}
25 if aw>LA_AFFINE_I64_MAX/8||bw>LA_AFFINE_I64_MAX/8{return 0}
26 if x<=y{return ((y-x)>=aw*8) as i64};return ((x-y)>=bw*8) as i64
27}
28func hc_rigid(a:*i64,budget:i64)->i64{
29 if budget<0||budget>HC_F32_BUDGET{return HC_BAD_DOMAIN}
30 var i:i64=0;while i<16{if la_affine_domain(a[i])!=0{return HC_BAD_DOMAIN};i=i+1}
31 if a[3]!=0||a[7]!=0||a[11]!=0||a[15]!=FQ_ONE{return HC_NONRIGID}
32 var c:i64=0;while c<3{var d:i64=0;while d<3{
33 var dot:i64=0;var r:i64=0;while r<3{if fq_abs(a[c*4+r])>FQ_ONE+budget{return HC_NONRIGID};dot=dot+fq_mul(a[c*4+r],a[d*4+r]);r=r+1}
34 var wanted:i64=0;if c==d{wanted=FQ_ONE};if fq_abs(dot-wanted)>budget{return HC_NONRIGID};d=d+1
35 };c=c+1}
36 let x:i64=fq_mul(a[5],a[10])-fq_mul(a[6],a[9])
37 let y:i64=fq_mul(a[6],a[8])-fq_mul(a[4],a[10])
38 let z:i64=fq_mul(a[4],a[9])-fq_mul(a[5],a[8])
39 let det:i64=fq_mul(a[0],x)+fq_mul(a[1],y)+fq_mul(a[2],z)
40 if fq_abs(det-FQ_ONE)>budget{return HC_NONRIGID};return 0
41}
42// Every pointer span is caller-owned and disjoint. Failure leaves output unchanged.
43// Parent indices need not precede children; a bounded ancestor walk validates every root/chain.
44func hc_apply(parents:*i64,n:i64,palette:*i64,kind:i64,anchor:i64,correction:*i64,budget:i64,out:*i64,out_words:i64,scratch:*i64,scratch_words:i64)->i64{
45 if n<1||n>LA_AFFINE_I64_MAX/128||anchor<0||anchor>=n||out_words!=n*16||scratch_words<n*16{return HC_BAD_DOMAIN}
46 if kind!=HC_RIGID{return HC_NONRIGID}
47 if hc_disjoint(out,n*16,scratch,n*16)==0||hc_disjoint(out,n*16,palette,n*16)==0||hc_disjoint(scratch,n*16,palette,n*16)==0{return HC_STORAGE}
48 if hc_disjoint(parents,n,out,n*16)==0||hc_disjoint(parents,n,scratch,n*16)==0||hc_disjoint(parents,n,palette,n*16)==0{return HC_STORAGE}
49 if hc_disjoint(correction,16,out,n*16)==0||hc_disjoint(correction,16,scratch,n*16)==0||hc_disjoint(correction,16,palette,n*16)==0||hc_disjoint(correction,16,parents,n)==0{return HC_STORAGE}
50 let admitted:i64=hc_rigid(correction,budget);if admitted!=0{return admitted}
51 var j:i64=0;while j<n{
52 var at:i64=j;var steps:i64=0
53 while at>=0{if at>=n||steps>=n{return HC_BAD_GRAPH};let p:i64=parents[at];if p<(0-1){return HC_BAD_GRAPH};at=p;steps=steps+1}
54 let rc:i64=hc_rigid(hc_matrix(palette,j),budget);if rc!=0{return rc};j=j+1
55 }
56 j=0;while j<n{
57 var at:i64=j;var descendant:i64=0
58 while at>=0{if at==anchor{descendant=1};at=parents[at]}
59 let dest:*i64=hc_matrix(scratch,j);let src:*i64=hc_matrix(palette,j)
60 if descendant==1{let rc:i64=la_affine_compose_q30(correction,src,dest);if rc!=0{return rc}}
61 else{var k:i64=0;while k<16{dest[k]=src[k];k=k+1}}
62 j=j+1
63 }
64 var k:i64=0;while k<n*16{out[k]=scratch[k];k=k+1};return 0
65}
66
67// Planar shader DQ rows: real[n*4],dual[n*4], Q30. Translation units are
68// caller-declared and shared by dual coefficients and correction[4..6].
69// Source row scale is preserved: normalizing each joint separately changes DQ blending.
70// The explicit rigid type is provenance, not a way to encode scale/shear in a DQ.
71const HC_DQ_DUAL_MAX:i64=LA_AFFINE_MAX/16
72func hc_qmul(a:*i64,b:*i64,o:*i64)->i64{
73 o[0]=fq_mul(a[3],b[0])+fq_mul(a[0],b[3])+fq_mul(a[1],b[2])-fq_mul(a[2],b[1])
74 o[1]=fq_mul(a[3],b[1])-fq_mul(a[0],b[2])+fq_mul(a[1],b[3])+fq_mul(a[2],b[0])
75 o[2]=fq_mul(a[3],b[2])+fq_mul(a[0],b[1])-fq_mul(a[1],b[0])+fq_mul(a[2],b[3])
76 o[3]=fq_mul(a[3],b[3])-fq_mul(a[0],b[0])-fq_mul(a[1],b[1])-fq_mul(a[2],b[2]);return 0
77}
78func hc_dq_real(q:*i64)->i64{
79 var k:i64=0;var norm:i64=0
80 while k<4{if q[k]<(0-2*FQ_ONE)||q[k]>2*FQ_ONE{return HC_BAD_DOMAIN};norm=norm+fq_mul(q[k],q[k]);k=k+1}
81 if norm<FQ_ONE/4||norm>4*FQ_ONE{return HC_BAD_DOMAIN};return 0
82}
83// Input bounds keep all products below fq_mul's 2^46 domain and all sums
84// below 2^50. Scratch includes 16 words for correction and product temporaries.
85func hc_dq_apply(parents:*i64,n:i64,rows:*i64,kind:i64,anchor:i64,correction:*i64,out:*i64,out_words:i64,scratch:*i64,scratch_words:i64)->i64{
86 if n<1||n>(LA_AFFINE_I64_MAX/64)-2||anchor<0||anchor>=n||out_words!=n*8||scratch_words<n*8+16{return HC_BAD_DOMAIN}
87 if kind!=HC_RIGID{return HC_NONRIGID}
88 if hc_disjoint(out,n*8,scratch,n*8+16)==0||hc_disjoint(out,n*8,rows,n*8)==0||hc_disjoint(scratch,n*8+16,rows,n*8)==0{return HC_STORAGE}
89 if hc_disjoint(parents,n,out,n*8)==0||hc_disjoint(parents,n,scratch,n*8+16)==0||hc_disjoint(parents,n,rows,n*8)==0{return HC_STORAGE}
90 if hc_disjoint(correction,7,out,n*8)==0||hc_disjoint(correction,7,scratch,n*8+16)==0||hc_disjoint(correction,7,rows,n*8)==0||hc_disjoint(correction,7,parents,n)==0{return HC_STORAGE}
91 if hc_dq_real(correction)!=0{return HC_BAD_DOMAIN}
92 var k:i64=4;while k<7{if correction[k]<=0-HC_DQ_DUAL_MAX||correction[k]>=HC_DQ_DUAL_MAX{return HC_BAD_DOMAIN};k=k+1}
93 var j:i64=0;while j<n{
94 var at:i64=j;var steps:i64=0
95 while at>=0{if at>=n||steps>=n{return HC_BAD_GRAPH};let parent:i64=parents[at];if parent<(0-1){return HC_BAD_GRAPH};at=parent;steps=steps+1}
96 let qr:*i64=((rows as i64)+j*32) as *i64
97 if hc_dq_real(qr)!=0{return HC_BAD_DOMAIN}
98 k=0;while k<4{let d:i64=rows[n*4+j*4+k];if d<=0-HC_DQ_DUAL_MAX||d>=HC_DQ_DUAL_MAX{return HC_BAD_DOMAIN};k=k+1};j=j+1
99 }
100 let cr:*i64=((scratch as i64)+n*64) as *i64
101 let cd:*i64=((cr as i64)+32) as *i64
102 let temp:*i64=((cr as i64)+64) as *i64
103 let product:*i64=((cr as i64)+96) as *i64
104 var norm:i64=0;k=0;while k<4{norm=norm+fq_mul(correction[k],correction[k]);k=k+1}
105 norm=fq_sqrt(norm);k=0;while k<4{cr[k]=fq_div(correction[k],norm);k=k+1}
106 temp[0]=correction[4];temp[1]=correction[5];temp[2]=correction[6];temp[3]=0
107 hc_qmul(temp,cr,cd);k=0;while k<4{cd[k]=cd[k]/2;k=k+1}
108 j=0;while j<n{
109 var at:i64=j;var descendant:i64=0;while at>=0{if at==anchor{descendant=1};at=parents[at]}
110 let r:*i64=((rows as i64)+j*32) as *i64
111 let d:*i64=((rows as i64)+(n*4+j*4)*8) as *i64
112 let rout:*i64=((scratch as i64)+j*32) as *i64
113 let dout:*i64=((scratch as i64)+(n*4+j*4)*8) as *i64
114 if descendant==1{
115 hc_qmul(cr,r,rout);hc_qmul(cr,d,dout);hc_qmul(cd,r,product)
116 k=0;while k<4{dout[k]=dout[k]+product[k];if la_affine_domain(dout[k])!=0{return HC_BAD_DOMAIN};k=k+1}
117 }else{k=0;while k<4{rout[k]=r[k];dout[k]=d[k];k=k+1}}
118 j=j+1
119 }
120 k=0;while k<n*8{out[k]=scratch[k];k=k+1};return 0
121}
122
123// Derive C from the captured source/target anchor rows, using the renderer's
124// normalized rigid transform. Inputs are planar rows; no anatomical policy is inferred.
125// Seven-word output is [quaternion xyzw, translation xyz]. All memory is caller-owned.
126func hc_dq_anchor_correction(source:*i64,target:*i64,n:i64,j:i64,out:*i64,scratch:*i64,scratch_words:i64)->i64{
127 if n<1||n>LA_AFFINE_I64_MAX/64||j<0||j>=n||scratch_words<40{return HC_BAD_DOMAIN}
128 if hc_disjoint(source,n*8,target,n*8)==0||hc_disjoint(source,n*8,out,7)==0||hc_disjoint(source,n*8,scratch,40)==0||hc_disjoint(target,n*8,out,7)==0||hc_disjoint(target,n*8,scratch,40)==0||hc_disjoint(out,7,scratch,40)==0{return HC_STORAGE}
129 let a:*i64=((source as i64)+j*32) as *i64;let b:*i64=((target as i64)+j*32) as *i64
130 if hc_dq_real(a)!=0||hc_dq_real(b)!=0{return HC_BAD_DOMAIN}
131 var k:i64=0;while k<4{if source[n*4+j*4+k]<=0-HC_DQ_DUAL_MAX/4||source[n*4+j*4+k]>=HC_DQ_DUAL_MAX/4||target[n*4+j*4+k]<=0-HC_DQ_DUAL_MAX/4||target[n*4+j*4+k]>=HC_DQ_DUAL_MAX/4{return HC_BAD_DOMAIN};k=k+1}
132 let ar:*i64=scratch;let ad:*i64=((scratch as i64)+32) as *i64
133 let br:*i64=((scratch as i64)+64) as *i64;let bd:*i64=((scratch as i64)+96) as *i64
134 let tmp:*i64=((scratch as i64)+128) as *i64;let at:*i64=((scratch as i64)+160) as *i64
135 let bt:*i64=((scratch as i64)+192) as *i64;let cr:*i64=((scratch as i64)+224) as *i64
136 let prod:*i64=((scratch as i64)+256) as *i64;let rotated:*i64=((scratch as i64)+288) as *i64
137 var an:i64=0;var bn:i64=0;k=0;while k<4{an=an+fq_mul(a[k],a[k]);bn=bn+fq_mul(b[k],b[k]);k=k+1};an=fq_sqrt(an);bn=fq_sqrt(bn)
138 k=0;while k<4{ar[k]=fq_div(a[k],an);br[k]=fq_div(b[k],bn);ad[k]=fq_div(source[n*4+j*4+k],an);bd[k]=fq_div(target[n*4+j*4+k],bn);k=k+1}
139 tmp[0]=0-ar[0];tmp[1]=0-ar[1];tmp[2]=0-ar[2];tmp[3]=ar[3];hc_qmul(ad,tmp,at);hc_qmul(br,tmp,cr)
140 tmp[0]=0-br[0];tmp[1]=0-br[1];tmp[2]=0-br[2];tmp[3]=br[3];hc_qmul(bd,tmp,bt)
141 k=0;while k<3{at[k]=2*at[k];bt[k]=2*bt[k];k=k+1};at[3]=0
142 hc_qmul(cr,at,prod);tmp[0]=0-cr[0];tmp[1]=0-cr[1];tmp[2]=0-cr[2];tmp[3]=cr[3];hc_qmul(prod,tmp,rotated)
143 k=0;while k<3{bt[k]=bt[k]-rotated[k];if bt[k]<=0-HC_DQ_DUAL_MAX||bt[k]>=HC_DQ_DUAL_MAX{return HC_BAD_DOMAIN};k=k+1}
144 k=0;while k<4{out[k]=cr[k];k=k+1};k=0;while k<3{out[4+k]=bt[k];k=k+1};return 0
145}