nx_uv_gram_solver_candidate_t280.nx source
↩ module page · 177 lines · 11454 B
1// nx_uv_gram_solver_candidate_t280.nx -- Solves least-squares problems using a graph-based operator and compensated dot product for numerical stability.
2import "nx_nxa_corner_prepare_candidate_t280.nx"
3import "nx_numeric.nx"
4import "nx_f64_sqrt.nx"
5// Candidate native least-squares factor operator. A^T(Ax) preserves the
6// squared-residual construction; no independently clipped cotangent coefficients.
7struct UvGramSolve{
8 m:*i64,c:i64,n:i64,nt:i64,pa:i64,pb:i64,classes:*i64,corners:*i64,operatorKind:i64,boundary:*i64,rhs:*f64,
9 coeff:*f64,x:*f64,r:*f64,z:*f64,p:*f64,ap:*f64,diag:*f64,
10 storage:*u8,storageBytes:i64,sumP:*KahanState,sumE:*KahanState,
11 rho:f64,rhsNorm2:f64,initialNorm2:f64,trueNorm2:f64,tolerance2:f64,energy:f64,
12 iterations:i64,matvecs:i64,restarts:i64,result:i64
13}
14const ULS_YIELD:i64=1
15const ULS_CANCELLED:i64=2
16const ULS_NUMERIC:i64=0-20
17func uls_finite(x:f64)->i64{
18 let ptr:*i64=(&x) as *i64
19 let k:i64=nx_f64_classify(ptr[0]);if k==NX_F64_CLS_INF{return 0};if k==NX_F64_CLS_NAN{return 0};return 1
20}
21func uls_sqrt(x:f64)->f64{
22 let ptr:*i64=(&x) as *i64;var raw:i64=nx_f64_sqrt(ptr[0]);let out:*f64=(&raw) as *f64;return out[0]
23}
24// Workspace form of the existing compensated dot, avoiding per-iteration allocations.
25func uls_dot(s:*UvGramSolve,a:*f64,b:*f64)->f64{
26 nx_kahan_reset(s.sumP);nx_kahan_reset(s.sumE);var i:i64=0
27 while i<s.n*2{let p:f64=a[i]*b[i];let e:f64=nx_two_product_err(a[i],b[i]);nx_kahan_add(s.sumP,p);nx_kahan_add(s.sumE,e);i=i+1}
28 let correction:f64=s.sumE.s+s.sumE.err;nx_kahan_add(s.sumP,correction);return nx_kahan_finish(s.sumP)
29}
30// Positive edge-incidence operator, sharing the same residual/PCG owner.
31// Each interior mesh edge occurs in two faces; its constant factor of two
32// does not alter the barycentric solution after Dirichlet elimination.
33func uls_graph_apply(s:*UvGramSolve,x:*f64,y:*f64)->i64{
34 var i:i64=0;while i<s.n*2{y[i]=0.0;i=i+1};nx_kahan_reset(s.sumP)
35 var t:i64=0;while t<s.nt{var k:i64=0;while k<3{
36 let a:i64=s.corners[t*3+k];let b:i64=s.corners[t*3+(k+1)%3]
37 var au:f64=0.0;var av:f64=0.0;var bu:f64=0.0;var bv:f64=0.0
38 if s.boundary[a]==0{au=x[a];av=x[s.n+a]};if s.boundary[b]==0{bu=x[b];bv=x[s.n+b]}
39 let du:f64=au-bu;let dv:f64=av-bv
40 y[a]=y[a]+du;y[b]=y[b]-du;y[s.n+a]=y[s.n+a]+dv;y[s.n+b]=y[s.n+b]-dv
41 nx_kahan_add(s.sumP,du*du+dv*dv);k=k+1};t=t+1
42 }
43 s.energy=nx_kahan_finish(s.sumP);s.matvecs=s.matvecs+1
44 if uls_finite(s.energy)==0{return ULS_NUMERIC};if s.energy<0.0{return ULS_NUMERIC}
45 i=0;while i<s.n{if s.boundary[i]>0{y[i]=0.0;y[s.n+i]=0.0};if uls_finite(y[i])==0{return ULS_NUMERIC};if uls_finite(y[s.n+i])==0{return ULS_NUMERIC};i=i+1};return UV_OK
46}
47func uls_apply(s:*UvGramSolve,x:*f64,y:*f64)->i64{
48 if s.operatorKind==1{return uls_graph_apply(s,x,y)}
49 var i:i64=0;while i<s.n*2{y[i]=0.0;i=i+1}
50 nx_kahan_reset(s.sumP);var t:i64=0
51 while t<s.nt{
52 var real:f64=0.0;var imag:f64=0.0;var k:i64=0
53 while k<3{let v:i64=s.corners[t*3+k];let gx:f64=s.coeff[t*6+k];let gy:f64=s.coeff[t*6+3+k];real=real+gx*x[v]-gy*x[s.n+v];imag=imag+gy*x[v]+gx*x[s.n+v];k=k+1}
54 nx_kahan_add(s.sumP,real*real+imag*imag);k=0
55 while k<3{let v:i64=s.corners[t*3+k];let gx:f64=s.coeff[t*6+k];let gy:f64=s.coeff[t*6+3+k];y[v]=y[v]+gx*real+gy*imag;y[s.n+v]=y[s.n+v]-gy*real+gx*imag;k=k+1};t=t+1
56 }
57 s.energy=nx_kahan_finish(s.sumP);s.matvecs=s.matvecs+1
58 y[s.pa]=0.0;y[s.pb]=0.0;y[s.n+s.pa]=0.0;y[s.n+s.pb]=0.0
59 if uls_finite(s.energy)==0{return ULS_NUMERIC};if s.energy<0.0{return ULS_NUMERIC}
60 i=0;while i<s.n*2{if uls_finite(y[i])==0{return ULS_NUMERIC};i=i+1};return UV_OK
61}
62func uls_residual_apply(s:*UvGramSolve)->i64{
63 let rc:i64=uls_apply(s,s.x,s.ap);if rc!=UV_OK{return rc}
64 if s.operatorKind==1{var i:i64=0;while i<s.n*2{s.ap[i]=s.ap[i]-s.rhs[i];i=i+1}}
65 return UV_OK
66}
67func uls_free(s:*UvGramSolve)->i64{
68 if s==(0 as *UvGramSolve){return 0}
69 if s.rhs!=(0 as *f64){sys_munmap_direct(s.rhs as *u8,s.n*2*UV_I64)}
70 if s.boundary!=(0 as *i64){sys_munmap_direct(s.boundary as *u8,s.n*UV_I64)}
71 if s.storage!=(0 as *u8){sys_munmap_direct(s.storage,s.storageBytes)}
72 if s.sumP!=(0 as *KahanState){sys_munmap_direct(s.sumP as *u8,__size_of(KahanState))}
73 if s.sumE!=(0 as *KahanState){sys_munmap_direct(s.sumE as *u8,__size_of(KahanState))}
74 return sys_munmap_direct(s as *u8,__size_of(UvGramSolve))
75}
76func uls_begin(m:*i64,c:i64,tolerance:f64)->*UvGramSolve{
77 if m==(0 as *i64){return 0 as *UvGramSolve};if uv_stage(m)<UV_ST_CHARTS{return 0 as *UvGramSolve}
78 if c<0{return 0 as *UvGramSolve};if c>=uv_chart_count(m){return 0 as *UvGramSolve}
79 if uls_finite(tolerance)==0{return 0 as *UvGramSolve};if tolerance<=0.0{return 0 as *UvGramSolve};if tolerance>=1.0{return 0 as *UvGramSolve}
80 let s:*UvGramSolve=sys_mmap_try(__size_of(UvGramSolve)) as *UvGramSolve
81 if (s as i64)<=0{return 0 as *UvGramSolve};s.m=m;s.c=c;s.result=UV_E_NOTRUN;s.tolerance2=tolerance*tolerance
82 s.sumP=sys_mmap_try(__size_of(KahanState)) as *KahanState;s.sumE=sys_mmap_try(__size_of(KahanState)) as *KahanState
83 if (s.sumP as i64)<=0{s.sumP=0 as *KahanState;s.result=UV_E_ARGS;return s};if (s.sumE as i64)<=0{s.sumE=0 as *KahanState;s.result=UV_E_ARGS;return s}
84 let pins:*i64=sys_mmap_try(UV_MIN_PINS*UV_I64) as *i64;if (pins as i64)<=0{s.result=UV_E_ARGS;return s}
85 if uv_pins_for(m,c,pins)!=UV_MIN_PINS{sys_munmap_direct(pins as *u8,UV_MIN_PINS*UV_I64);s.result=UV_E_NOPIN;return s}
86 s.n=uvi_chart_classes(m,c)
87 let init:i64=uvi_initial(m,c,s.n,pins[0],pins[1])
88 if init!=UV_OK{sys_munmap_direct(pins as *u8,UV_MIN_PINS*UV_I64);s.result=init;return s}
89 let p0:i64=m[m[UV_O_CSTART]+c];let p1:i64=m[m[UV_O_CSTART]+c+1];s.nt=p1-p0
90 s.storageBytes=(s.n+3*s.nt+6*s.nt+12*s.n)*UV_I64
91 s.storage=sys_mmap_try(s.storageBytes);if (s.storage as i64)<=0{s.storage=0 as *u8;sys_munmap_direct(pins as *u8,UV_MIN_PINS*UV_I64);s.result=UV_E_ARGS;return s}
92 var off:i64=s.storage as i64
93 s.classes=off as *i64;off=off+s.n*UV_I64;s.corners=off as *i64;off=off+s.nt*3*UV_I64;s.coeff=off as *f64;off=off+s.nt*6*UV_I64
94 s.x=off as *f64;off=off+s.n*2*UV_I64;s.r=off as *f64;off=off+s.n*2*UV_I64;s.z=off as *f64;off=off+s.n*2*UV_I64;s.p=off as *f64;off=off+s.n*2*UV_I64;s.ap=off as *f64;off=off+s.n*2*UV_I64;s.diag=off as *f64
95 let inverse:*i64=sys_mmap_try(m[UV_H_NUV]*UV_I64) as *i64
96 if (inverse as i64)<=0{sys_munmap_direct(pins as *u8,UV_MIN_PINS*UV_I64);s.result=UV_E_ARGS;return s}
97 var i:i64=0
98 while i<s.n{
99 let cls:i64=m[m[UV_O_LIST]+i];s.classes[i]=cls;inverse[cls]=i
100 s.x[i]=__f64_from_i64(m[m[UV_O_WU]+cls]);s.x[s.n+i]=__f64_from_i64(m[m[UV_O_WV]+cls])
101 if cls==pins[0]{s.pa=i};if cls==pins[1]{s.pb=i};i=i+1
102 }
103 sys_munmap_direct(pins as *u8,UV_MIN_PINS*UV_I64)
104 s.x[s.pa]=0.0;s.x[s.n+s.pa]=0.0;s.x[s.pb]=__f64_from_i64(UV_PIN_SPAN);s.x[s.n+s.pb]=0.0
105 var p:i64=p0;var failed:i64=0
106 while p<p1{
107 let t:i64=m[m[UV_O_CORDER]+p];let row:i64=p-p0
108 let a:i64=uv_ti(m,t,0);let b:i64=uv_ti(m,t,1);let cc:i64=uv_ti(m,t,2)
109 let ax:f64=__f64_from_i64(uv_vx(m,b))-__f64_from_i64(uv_vx(m,a));let ay:f64=__f64_from_i64(uv_vy(m,b))-__f64_from_i64(uv_vy(m,a));let az:f64=__f64_from_i64(uv_vz(m,b))-__f64_from_i64(uv_vz(m,a))
110 let bx:f64=__f64_from_i64(uv_vx(m,cc))-__f64_from_i64(uv_vx(m,a));let by:f64=__f64_from_i64(uv_vy(m,cc))-__f64_from_i64(uv_vy(m,a));let bz:f64=__f64_from_i64(uv_vz(m,cc))-__f64_from_i64(uv_vz(m,a))
111 let length:f64=uls_sqrt(ax*ax+ay*ay+az*az)
112 let cx:f64=ay*bz-az*by;let cy:f64=az*bx-ax*bz;let cz:f64=ax*by-ay*bx
113 let twiceArea:f64=uls_sqrt(cx*cx+cy*cy+cz*cz)
114 if length<=0.0{failed=1;break};if twiceArea<=0.0{failed=1;break};if uls_finite(length)==0{failed=1;break};if uls_finite(twiceArea)==0{failed=1;break}
115 let x:f64=(ax*bx+ay*by+az*bz)/length;let y:f64=twiceArea/length;let scale:f64=uls_sqrt(twiceArea/2.0)/twiceArea
116 s.coeff[row*6]=(0.0-y)*scale;s.coeff[row*6+1]=y*scale;s.coeff[row*6+2]=0.0
117 s.coeff[row*6+3]=(x-length)*scale;s.coeff[row*6+4]=(0.0-x)*scale;s.coeff[row*6+5]=length*scale
118 var k:i64=0;while k<3{
119 let cls:i64=uv_corner_class(m,t,k);let v:i64=inverse[cls];s.corners[row*3+k]=v
120 let gx:f64=s.coeff[row*6+k];let gy:f64=s.coeff[row*6+3+k];let diagonal:f64=gx*gx+gy*gy
121 s.diag[v]=s.diag[v]+diagonal;s.diag[s.n+v]=s.diag[s.n+v]+diagonal;k=k+1
122 };p=p+1
123 }
124 sys_munmap_direct(inverse as *u8,m[UV_H_NUV]*UV_I64)
125 if failed==1{s.result=ULS_NUMERIC;return s}
126 s.p[s.pb]=__f64_from_i64(UV_PIN_SPAN)
127 if uls_apply(s,s.p,s.ap)!=UV_OK{s.result=ULS_NUMERIC;return s}
128 s.rhsNorm2=uls_dot(s,s.ap,s.ap)
129 if uls_residual_apply(s)!=UV_OK{s.result=ULS_NUMERIC;return s}
130 i=0;while i<s.n*2{if s.diag[i]<=0.0{s.result=ULS_NUMERIC;return s};if uls_finite(s.diag[i])==0{s.result=ULS_NUMERIC;return s};s.r[i]=0.0-s.ap[i];s.z[i]=s.r[i]/s.diag[i];s.p[i]=s.z[i];i=i+1}
131 s.initialNorm2=uls_dot(s,s.r,s.r);s.trueNorm2=s.initialNorm2;s.rho=uls_dot(s,s.r,s.z)
132 if uls_finite(s.initialNorm2)==0{s.result=ULS_NUMERIC;return s}
133 if s.initialNorm2==0.0{s.result=UV_OK;return s}
134 if s.rhsNorm2<=0.0{s.result=ULS_NUMERIC;return s};if uls_finite(s.rhsNorm2)==0{s.result=ULS_NUMERIC;return s}
135 if s.initialNorm2<=s.rhsNorm2*s.tolerance2{s.result=UV_OK;return s}
136 if s.rho<=0.0{s.result=ULS_NUMERIC};return s
137}
138func uls_step(s:*UvGramSolve,budget:i64)->i64{
139 return uls_step_controlled(s,budget,0 as *i64)
140}
141// Caller-owned cancellation word; cancellation preserves the resumable solver.
142// Cross-process signalling is an embedding contract, not implied by this pointer.
143func uls_step_controlled(s:*UvGramSolve,budget:i64,cancel:*i64)->i64{
144 if s==(0 as *UvGramSolve){return UV_E_ARGS};if budget<1{return UV_E_ARGS};if s.result!=UV_E_NOTRUN{return s.result}
145 var it:i64=0
146 while it<budget{
147 if cancel!=(0 as *i64){if cancel[0]!=0{return ULS_CANCELLED}}
148 if uls_apply(s,s.p,s.ap)!=UV_OK{s.result=ULS_NUMERIC;return s.result}
149 let pap:f64=uls_dot(s,s.p,s.ap)
150 if pap<=0.0{s.result=ULS_NUMERIC;return s.result};if uls_finite(pap)==0{s.result=ULS_NUMERIC;return s.result}
151 let alpha:f64=s.rho/pap;if uls_finite(alpha)==0{s.result=ULS_NUMERIC;return s.result}
152 var i:i64=0;while i<s.n*2{s.x[i]=s.x[i]+alpha*s.p[i];s.r[i]=s.r[i]-alpha*s.ap[i];i=i+1}
153 s.iterations=s.iterations+1
154 let norm:f64=uls_dot(s,s.r,s.r);if uls_finite(norm)==0{s.result=ULS_NUMERIC;return s.result}
155 var restart:i64=0
156 if norm<=s.rhsNorm2*s.tolerance2{
157 if uls_residual_apply(s)!=UV_OK{s.result=ULS_NUMERIC;return s.result}
158 s.trueNorm2=uls_dot(s,s.ap,s.ap)
159 if s.trueNorm2<=s.rhsNorm2*s.tolerance2{s.result=UV_OK;return UV_OK}
160 i=0;while i<s.n*2{s.r[i]=0.0-s.ap[i];i=i+1};restart=1;s.restarts=s.restarts+1
161 }
162 i=0;while i<s.n*2{s.z[i]=s.r[i]/s.diag[i];i=i+1}
163 let rho:f64=uls_dot(s,s.r,s.z);if rho<=0.0{s.result=ULS_NUMERIC;return s.result};if uls_finite(rho)==0{s.result=ULS_NUMERIC;return s.result}
164 let beta:f64=rho/s.rho
165 i=0;while i<s.n*2{if restart==1{s.p[i]=s.z[i]}else{s.p[i]=s.z[i]+beta*s.p[i]};i=i+1};s.rho=rho;it=it+1
166 }
167 if uls_residual_apply(s)!=UV_OK{s.result=ULS_NUMERIC;return s.result}
168 s.trueNorm2=uls_dot(s,s.ap,s.ap)
169 if s.trueNorm2<=s.rhsNorm2*s.tolerance2{s.result=UV_OK;return UV_OK}
170 return ULS_YIELD
171}
172func uls_export(s:*UvGramSolve)->i64{
173 if s==(0 as *UvGramSolve){return UV_E_ARGS};if s.result!=UV_OK{return UV_E_STAGE}
174 var i:i64=0
175 while i<s.n*2{let v:f64=s.x[i];if uls_finite(v)==0{return ULS_NUMERIC};if v>=9223372036854775808.0{return ULS_NUMERIC};if v<(0.0-9223372036854775808.0){return ULS_NUMERIC};i=i+1}
176 i=0;while i<s.n{let cls:i64=s.classes[i];s.m[s.m[UV_O_WU]+cls]=__f64_to_i64(s.x[i]);s.m[s.m[UV_O_WV]+cls]=__f64_to_i64(s.x[s.n+i]);i=i+1};return UV_OK
177}