nx_bvh_nearest_candidate_t284.nx source
↩ module page · 109 lines · 5001 B
1// Nearest-surface query arithmetic for existing NxBvh/NxMesh.
2// Independent of hair. Finite integer inputs are converted with existing IEEE owner.
3// Outputs xyz + squared distance; caller owns four f64 words and scratch.
4// A degenerate triangle falls back to its three segments, never a fabricated plane.
5import "nx_bvh.nx"
6import "nx_f64_cvt.nx"
7func bq_f(v:i64)->f64{return nx_i64_to_f64(v) as f64}
8func bq_dist(px:f64,py:f64,pz:f64,x:f64,y:f64,z:f64)->f64{
9 let dx:f64=px-x;let dy:f64=py-y;let dz:f64=pz-z;return dx*dx+dy*dy+dz*dz
10}
11func bq_store(out:*f64,px:f64,py:f64,pz:f64,x:f64,y:f64,z:f64)->i64{
12 out[0]=x;out[1]=y;out[2]=z;out[3]=bq_dist(px,py,pz,x,y,z);return 0
13}
14func bq_segment(out:*f64,px:f64,py:f64,pz:f64,ax:f64,ay:f64,az:f64,bx:f64,by:f64,bz:f64)->i64{
15 let dx:f64=bx-ax;let dy:f64=by-ay;let dz:f64=bz-az
16 let den:f64=dx*dx+dy*dy+dz*dz;var t:f64=0.0
17 if den>0.0{t=((px-ax)*dx+(py-ay)*dy+(pz-az)*dz)/den}
18 if t<0.0{t=0.0};if t>1.0{t=1.0}
19 return bq_store(out,px,py,pz,ax+t*dx,ay+t*dy,az+t*dz)
20}
21func bq_copy_best(out:*f64,c:*f64)->i64{
22 if c[3]<out[3]{var a:i64=0;while a<4{out[a]=c[a];a=a+1}}
23 return 0
24}
25func bq_triangle(out:*f64,scratch:*f64,px:f64,py:f64,pz:f64,
26 ax:f64,ay:f64,az:f64,bx:f64,by:f64,bz:f64,cx:f64,cy:f64,cz:f64)->i64{
27 let ux:f64=bx-ax;let uy:f64=by-ay;let uz:f64=bz-az
28 let vx:f64=cx-ax;let vy:f64=cy-ay;let vz:f64=cz-az
29 let wx:f64=px-ax;let wy:f64=py-ay;let wz:f64=pz-az
30 // Cross-product area avoids subtracting two nearly equal squared dot products.
31 let nx:f64=uy*vz-uz*vy;let ny:f64=uz*vx-ux*vz;let nz:f64=ux*vy-uy*vx
32 let den:f64=nx*nx+ny*ny+nz*nz
33 if den>0.0{
34 let sx:f64=wy*vz-wz*vy;let sy:f64=wz*vx-wx*vz;let sz:f64=wx*vy-wy*vx
35 let tx:f64=uy*wz-uz*wy;let ty:f64=uz*wx-ux*wz;let tz:f64=ux*wy-uy*wx
36 let s:f64=(sx*nx+sy*ny+sz*nz)/den;let t:f64=(tx*nx+ty*ny+tz*nz)/den
37 if s>=0.0{if t>=0.0{if s+t<=1.0{
38 bq_store(out,px,py,pz,ax+s*ux+t*vx,ay+s*uy+t*vy,az+s*uz+t*vz)
39 return 0
40 }}}
41 }
42 bq_segment(out,px,py,pz,ax,ay,az,bx,by,bz)
43 bq_segment(scratch,px,py,pz,bx,by,bz,cx,cy,cz);bq_copy_best(out,scratch)
44 bq_segment(scratch,px,py,pz,cx,cy,cz,ax,ay,az);bq_copy_best(out,scratch)
45 if den<=0.0{return 1}
46 return 0
47}
48
49func bq_box_distance(n:*NxBvhNode,px:f64,py:f64,pz:f64)->f64{
50 var dx:f64=0.0;var dy:f64=0.0;var dz:f64=0.0
51 if px<bq_f(n.mnx){dx=bq_f(n.mnx)-px};if px>bq_f(n.mxx){dx=px-bq_f(n.mxx)}
52 if py<bq_f(n.mny){dy=bq_f(n.mny)-py};if py>bq_f(n.mxy){dy=py-bq_f(n.mxy)}
53 if pz<bq_f(n.mnz){dz=bq_f(n.mnz)-pz};if pz>bq_f(n.mxz){dz=pz-bq_f(n.mxz)}
54 return dx*dx+dy*dy+dz*dz
55}
56func bq_mesh_triangle(m:*NxMesh,ti:i64,out:*f64,scratch:*f64,px:f64,py:f64,pz:f64)->i64{
57 let a:i64=m.indices[ti*3]*4;let b:i64=m.indices[ti*3+1]*4;let c:i64=m.indices[ti*3+2]*4
58 return bq_triangle(out,scratch,px,py,pz,
59 bq_f(m.verts[a]),bq_f(m.verts[a+1]),bq_f(m.verts[a+2]),
60 bq_f(m.verts[b]),bq_f(m.verts[b+1]),bq_f(m.verts[b+2]),
61 bq_f(m.verts[c]),bq_f(m.verts[c+1]),bq_f(m.verts[c+2]))
62}
63// m and b must describe the same validated immutable geometry epoch.
64// Workspace sizes derive from b.n_nodes; no per-query allocations.
65// out[0..3] xyz,distance²; meta[0..3] triangle,nodes,triangles,degenerate tests.
66// Failure means no qualified result even if scratch/output has partial values.
67func nx_bvh_nearest_query(b:*NxBvh,m:*NxMesh,x:i64,y:i64,z:i64,
68 stack:*i64,stack_words:i64,out:*f64,out_words:i64,scratch:*f64,scratch_words:i64,meta:*i64,meta_words:i64)->i64{
69 if (b as i64)==0||(m as i64)==0{return 0-1}
70 if (stack as i64)==0||(out as i64)==0||(scratch as i64)==0||(meta as i64)==0{return 0-1}
71 if b.n_nodes<1||b.n_tris<1||b.n_tris!=m.n_tris{return 0-1}
72 if stack_words<b.n_nodes||out_words<4||scratch_words<8||meta_words<4{return 0-2}
73 let px:f64=bq_f(x);let py:f64=bq_f(y);let pz:f64=bq_f(z)
74 if nx_f64_to_i64(px as i64)!=x||nx_f64_to_i64(py as i64)!=y||nx_f64_to_i64(pz as i64)!=z{return 0-4}
75 var i:i64=0;while i<4{meta[i]=0;i=i+1}
76 meta[0]=0-1
77 let candidate:*f64=scratch;let local:*f64=((scratch as i64)+32) as *f64
78 var top:i64=1;stack[0]=0
79 while top>0{
80 top=top-1;let ni:i64=stack[top]
81 if ni<0||ni>=b.n_nodes{return 0-3}
82 let node:*NxBvhNode=nx_bvh_node_at(b,ni);meta[1]=meta[1]+1
83 if meta[1]>b.n_nodes{return 0-3}
84 var inspect:i64=1
85 if meta[0]>=0{if bq_box_distance(node,px,py,pz)>out[3]{inspect=0}}
86 if inspect==1{
87 if node.count>0{
88 if node.left_or_first<0||node.count>b.n_tris-node.left_or_first{return 0-3}
89 i=0
90 while i<node.count{
91 let ti:i64=b.tri_order[node.left_or_first+i]
92 if ti<0||ti>=m.n_tris{return 0-3}
93 let kind:i64=bq_mesh_triangle(m,ti,candidate,local,px,py,pz)
94 meta[2]=meta[2]+1;meta[3]=meta[3]+kind
95 var choose:i64=0
96 if meta[0]<0{choose=1}else{if candidate[3]<out[3]{choose=1}}
97 if choose==1{var k:i64=0;while k<4{out[k]=candidate[k];k=k+1};meta[0]=ti}
98 i=i+1
99 }
100 }else{
101 if node.count<0{return 0-3}
102 if top>stack_words-2{return 0-2}
103 stack[top]=node.left_or_first;stack[top+1]=node.left_or_first+1;top=top+2
104 }
105 }
106 }
107 if meta[0]<0{return 0-3}
108 return 0
109}