1 #include <extUtil.h>
2
3 typedef struct {
4 double r;
5 int i;
6 }Rsort;
7
8 int compareRsort(Rsort *a, Rsort *b);
9
10 /*
11 ! identifies the position id of px in an ordered array
12 ! x of real numbers;
13 !
14 ! id is such that x(id).le.px and x(id+1).gt.px
15 !
16 */
ident(x,px,n)17 int ident(x,px,n)
18 int n;
19 double *x,px;
20 {
21 int id=0, n2, m;
22 if(n == 0) return(0);
23
24 /* Parameter adjustments */
25 --x;
26
27 n2=n+1;
28 do
29 {
30 m=(n2+id)/2;
31 if(px >= x[m]) { id=m; }
32 else { n2=m; }
33 if((n2-id) == 1) return(id);
34 }while(1);
35 }
36
37
38 /*
39 c determines the k closest nodes out of n with coordinates in
40 c xo,yo,zo to the point with coordinates (xp,yp,zp);
41 c
42 c order xo, yo and zo before the first call to near2d and
43 c store the results with the corresponding permutation array
44 c in x,y,z and nx,ny,nz, respectively
45 */
46 /*
47 C DSORT sorts array DX and optionally makes the same interchanges in
48 C array IY. The array DX may be sorted in increasing order or
49 C decreasing order. A slightly modified quicksort algorithm is used.
50 C
51 C SUBROUTINE DSORT (DX, IY, N, KFLAG)
52 C Description of Parameters
53 C DX - array of values to be sorted (usually abscissas)
54 C IY - array to be (optionally) carried along
55 C N - number of values in array DX to be sorted
56 C KFLAG - control parameter
57 C = 2 means sort DX in increasing order and carry IY along.
58 C = 1 means sort DX in increasing order (ignoring IY)
59 C = -1 means sort DX in decreasing order (ignoring IY)
60 C = -2 means sort DX in decreasing order and carry IY along.
61 */
62
near3d(xo,yo,zo,x,y,z,nx,ny,nz,xp,yp,zp,n,node,k)63 void near3d(xo,yo,zo,x,y,z,nx,ny,nz,xp,yp,zp,n,node,k)
64 int *nx,*ny,*nz,n,*node,k;
65 double *xo,*yo,*zo,*x,*y,*z,xp,yp,zp;
66 {
67 int i,j,m,ks,it,iz,idx,idy,idz,nboundary;
68 double aad,aanod,aamin,xr,yr,zr;
69 double xx,yy,zz,rr,aaw,aao,aas,aan,aau;
70 Rsort *rsort=NULL;
71
72 int kindx;
73
74 if(k>n) k=n;
75 //if(k>100) k=100;
76 kindx=k-1;
77
78 idx=ident(x,xp,n);
79 idy=ident(y,yp,n);
80 idz=ident(z,zp,n);
81 /* according to original fortran idx etc for a field starting at '1' */
82 /*
83 printf("ident: %d %d %d\n", idx,idy,idz);
84 printf("1: %f %f %f\n", x[idx-1],y[idy-1],z[idz-1]);
85 printf("p: %f %f %f\n", xp,yp,zp);
86 printf("2: %f %f %f\n", x[idx],y[idy],z[idz]);
87 */
88
89 /* DETERMINATION OF THE k NEAREST POINTS */
90 if ( (rsort = (Rsort *)realloc((Rsort *)rsort, (n+1) * sizeof(Rsort))) == NULL )
91 printf("ERROR: realloc failed: Rsort\n\n" );
92
93 /* abs dist from p to o */
94 for(i=0; i<k; i++)
95 {
96 xr=xo[i]-xp;
97 yr=yo[i]-yp;
98 zr=zo[i]-zp;
99 rsort[i].r=sqrt(xr*xr+yr*yr+zr*zr);
100 rsort[i].i=i;
101 }
102 qsort( rsort, k, sizeof(Rsort), (void *)compareRsort );
103
104 i=1; // nr of loops?
105 ks=0;
106
107 loop:;
108
109 iz=0;
110
111 // west
112
113 m=idx-i; // closest xo to xp - nr_of_loops +1 == one point above xp
114 if(m<0) // goto "east" if no lower point is there
115 {
116 aaw=rsort[kindx].r; // k has to be reduced by 1 (field runs to k-1)
117 goto l4;
118 }
119 xx=xo[nx[m]]; // coorinates of point o above p in dir x
120 yy=yo[nx[m]];
121 zz=zo[nx[m]];
122 aaw=xx-xp; // x distance
123 yr=yy-yp;
124 zr=zz-zp;
125 rr=sqrt(aaw*aaw+yr*yr+zr*zr); // abs distance
126
127 if(rr >= rsort[kindx].r) goto l4; // if abs_dist >= "k"closest distance (k has to be reduced by 1 (field runs to k-1))
128 it=iz+k;
129 for( j=0; j<it; j++) if(nx[m] == rsort[j].i) goto l4;
130 //printf("we %d %d i:%d r:%f\n", i,iz,nx[m],rr);
131 rsort[iz+k].r=rr;
132 rsort[iz+k].i=nx[m];
133 iz++;
134
135 // east
136 l4:;
137
138 m=idx+i;
139 if(m > n)
140 {
141 aao=rsort[kindx].r;
142 goto l6;
143 }
144 m--;
145 xx=xo[nx[m]];
146 yy=yo[nx[m]];
147 zz=zo[nx[m]];
148 aao=xx-xp;
149 yr=yy-yp;
150 zr=zz-zp;
151 rr=sqrt(aao*aao+yr*yr+zr*zr);
152
153 if(rr >= rsort[kindx].r) goto l6;
154 it=iz+k;
155 for( j=0; j<it; j++) if(nx[m] == rsort[j].i) goto l6;
156 //printf("ea %d %d i:%d r:%f\n", i,iz,nx[m],rr);
157 rsort[iz+k].r=rr;
158 rsort[iz+k].i=nx[m];
159 iz++;
160
161 // south
162 l6:;
163
164 m=idy-i;
165 if(m < 0)
166 {
167 aas=rsort[kindx].r;
168 goto l5;
169 }
170
171 xx=xo[ny[m]];
172 yy=yo[ny[m]];
173 zz=zo[ny[m]];
174 xr=xx-xp;
175 aas=yy-yp;
176 zr=zz-zp;
177 rr=sqrt(xr*xr+aas*aas+zr*zr);
178
179 if(rr >= rsort[kindx].r) goto l5;
180 it=iz+k;
181 for( j=0; j<it; j++) if(ny[m] == rsort[j].i) goto l5;
182 //printf("so %d %d i:%d r:%f\n", i,iz,ny[m],rr);
183 rsort[iz+k].r=rr;
184 rsort[iz+k].i=ny[m];
185 iz++;
186
187 // north
188 l5:;
189
190 m=idy+i;
191 if(m>n)
192 {
193 aan=rsort[kindx].r;
194 goto l7;
195 }
196 m--;
197 xx=xo[ny[m]];
198 yy=yo[ny[m]];
199 zz=zo[ny[m]];
200 xr=xx-xp;
201 aan=yy-yp;
202 zr=zz-zp;
203 rr=sqrt(xr*xr+aan*aan+zr*zr);
204
205 if(rr >= rsort[kindx].r) goto l7;
206 it=iz+k;
207 for(j=0; j<it; j++) if(ny[m] == rsort[j].i) goto l7;
208 //printf("no %d %d i:%d r:%f\n", i,iz,ny[m],rr);
209 rsort[iz+k].r=rr;
210 rsort[iz+k].i=ny[m];
211 iz++;
212
213 // up
214 l7:;
215
216 m=idz-i;
217 if(m < 0)
218 {
219 aau=rsort[kindx].r;
220 goto l20;
221 }
222
223 xx=xo[nz[m]];
224 yy=yo[nz[m]];
225 zz=zo[nz[m]];
226 xr=xx-xp;
227 yr=yy-yp;
228 aau=zz-zp;
229 rr=sqrt(xr*xr+yr*yr+aau*aau);
230
231 if(rr >= rsort[kindx].r) goto l20;
232 it=iz+k;
233 for(j=0; j<it; j++) if(nz[m] == rsort[j].i) goto l20;
234 //printf("up %d %d i:%d r:%f\n", i,iz,nz[m],rr);
235 rsort[iz+k].r=rr;
236 rsort[iz+k].i=nz[m];
237 iz++;
238
239 // down
240 l20:;
241
242 m=idz+i;
243 if(m > n)
244 {
245 aad=rsort[kindx].r;
246 goto l22;
247 }
248 m--;
249 xx=xo[nz[m]];
250 yy=yo[nz[m]];
251 zz=zo[nz[m]];
252 xr=xx-xp;
253 yr=yy-yp;
254 aad=zz-zp;
255 rr=sqrt(xr*xr+yr*yr+aad*aad);
256
257 if(rr >=rsort[kindx].r) goto l22;
258 it=iz+k;
259 for(j=0; j<it; j++) if(nz[m] == rsort[j].i) goto l22;
260 //printf("do %d %d i:%d r:%f\n", i,iz,nz[m],rr);
261 rsort[iz+k].r=rr;
262 rsort[iz+k].i=nz[m];
263 iz++;
264
265 l22:;
266
267 aamin=MAX_FLOAT;
268 aanod=sqrt(aan*aan+aao*aao+aad*aad);
269 if(aanod<aamin) aamin=aanod;
270 aanod=sqrt(aan*aan+aao*aao+aau*aau);
271 if(aanod<aamin) aamin=aanod;
272 aanod=sqrt(aas*aas+aao*aao+aad*aad);
273 if(aanod<aamin) aamin=aanod;
274 aanod=sqrt(aas*aas+aao*aao+aau*aau);
275 if(aanod<aamin) aamin=aanod;
276 aanod=sqrt(aas*aas+aaw*aaw+aad*aad);
277 if(aanod<aamin) aamin=aanod;
278 aanod=sqrt(aas*aas+aaw*aaw+aau*aau);
279 if(aanod<aamin) aamin=aanod;
280 aanod=sqrt(aan*aan+aaw*aaw+aad*aad);
281 if(aanod<aamin) aamin=aanod;
282 aanod=sqrt(aan*aan+aaw*aaw+aau*aau);
283 if(aanod<aamin) aamin=aanod;
284
285 if(iz != 0)
286 {
287 iz+=k;
288 qsort( rsort, iz, sizeof(Rsort), (void *)compareRsort );
289 }
290
291
292 nboundary=ks;
293 for( j=nboundary; j<k; j++)
294 {
295 if(rsort[j].r <= aamin)
296 {
297 ks=j+1;
298 if(ks==k) goto endloop;
299 }
300 else
301 {
302 i++;
303 goto loop;
304 }
305 }
306
307
308 goto loop;
309 endloop:;
310
311 //for(i=0; i<k; i++) printf("n:%d r:%f orig: %f %f %f p:%f %f %f\n",rsort[i].i,rsort[i].r, xo[rsort[i].i],yo[rsort[i].i],zo[rsort[i].i], xp,yp,zp);
312 for(i=0; i<k; i++) node[i]=rsort[i].i;
313 free(rsort);
314 return;
315 }
316
317
318