1 | /* $Id$ $Revision$ */ |
2 | /* vim:set shiftwidth=4 ts=8: */ |
3 | |
4 | /************************************************************************* |
5 | * Copyright (c) 2011 AT&T Intellectual Property |
6 | * All rights reserved. This program and the accompanying materials |
7 | * are made available under the terms of the Eclipse Public License v1.0 |
8 | * which accompanies this distribution, and is available at |
9 | * http://www.eclipse.org/legal/epl-v10.html |
10 | * |
11 | * Contributors: See CVS logs. Details at http://www.graphviz.org/ |
12 | *************************************************************************/ |
13 | |
14 | #include "general.h" |
15 | #include "geom.h" |
16 | #include "arith.h" |
17 | #include "math.h" |
18 | #include "LinkedList.h" |
19 | #include "QuadTree.h" |
20 | |
21 | extern real distance_cropped(real *x, int dim, int i, int j); |
22 | |
23 | struct node_data_struct { |
24 | real node_weight; |
25 | real *coord; |
26 | real id; |
27 | void *data; |
28 | }; |
29 | |
30 | typedef struct node_data_struct *node_data; |
31 | |
32 | |
33 | static node_data node_data_new(int dim, real weight, real *coord, int id){ |
34 | node_data nd; |
35 | int i; |
36 | nd = MALLOC(sizeof(struct node_data_struct)); |
37 | nd->node_weight = weight; |
38 | nd->coord = MALLOC(sizeof(real)*dim); |
39 | nd->id = id; |
40 | for (i = 0; i < dim; i++) nd->coord[i] = coord[i]; |
41 | nd->data = NULL; |
42 | return nd; |
43 | } |
44 | |
45 | void node_data_delete(void *d){ |
46 | node_data nd = (node_data) d; |
47 | FREE(nd->coord); |
48 | /*delete outside if (nd->data) FREE(nd->data);*/ |
49 | FREE(nd); |
50 | } |
51 | |
52 | real node_data_get_weight(void *d){ |
53 | node_data nd = (node_data) d; |
54 | return nd->node_weight; |
55 | } |
56 | |
57 | real* node_data_get_coord(void *d){ |
58 | node_data nd = (node_data) d; |
59 | return nd->coord; |
60 | } |
61 | |
62 | int node_data_get_id(void *d){ |
63 | node_data nd = (node_data) d; |
64 | return nd->id; |
65 | } |
66 | |
67 | #define node_data_get_data(d) (((node_data) (d))->data) |
68 | |
69 | |
70 | void check_or_realloc_arrays(int dim, int *nsuper, int *nsupermax, real **center, real **supernode_wgts, real **distances){ |
71 | |
72 | if (*nsuper >= *nsupermax) { |
73 | *nsupermax = *nsuper + MAX(10, (int) 0.2*(*nsuper)); |
74 | *center = REALLOC(*center, sizeof(real)*(*nsupermax)*dim); |
75 | *supernode_wgts = REALLOC(*supernode_wgts, sizeof(real)*(*nsupermax)); |
76 | *distances = REALLOC(*distances, sizeof(real)*(*nsupermax)); |
77 | } |
78 | } |
79 | |
80 | void QuadTree_get_supernodes_internal(QuadTree qt, real bh, real *point, int nodeid, int *nsuper, int *nsupermax, real **center, real **supernode_wgts, real **distances, real *counts, int *flag){ |
81 | SingleLinkedList l; |
82 | real *coord, dist; |
83 | int dim, i; |
84 | |
85 | (*counts)++; |
86 | |
87 | if (!qt) return; |
88 | dim = qt->dim; |
89 | l = qt->l; |
90 | if (l){ |
91 | while (l){ |
92 | check_or_realloc_arrays(dim, nsuper, nsupermax, center, supernode_wgts, distances); |
93 | if (node_data_get_id(SingleLinkedList_get_data(l)) != nodeid){ |
94 | coord = node_data_get_coord(SingleLinkedList_get_data(l)); |
95 | for (i = 0; i < dim; i++){ |
96 | (*center)[dim*(*nsuper)+i] = coord[i]; |
97 | } |
98 | (*supernode_wgts)[*nsuper] = node_data_get_weight(SingleLinkedList_get_data(l)); |
99 | (*distances)[*nsuper] = point_distance(point, coord, dim); |
100 | (*nsuper)++; |
101 | } |
102 | l = SingleLinkedList_get_next(l); |
103 | } |
104 | } |
105 | |
106 | if (qt->qts){ |
107 | dist = point_distance(qt->center, point, dim); |
108 | if (qt->width < bh*dist){ |
109 | check_or_realloc_arrays(dim, nsuper, nsupermax, center, supernode_wgts, distances); |
110 | for (i = 0; i < dim; i++){ |
111 | (*center)[dim*(*nsuper)+i] = qt->average[i]; |
112 | } |
113 | (*supernode_wgts)[*nsuper] = qt->total_weight; |
114 | (*distances)[*nsuper] = point_distance(qt->average, point, dim); |
115 | (*nsuper)++; |
116 | } else { |
117 | for (i = 0; i < 1<<dim; i++){ |
118 | QuadTree_get_supernodes_internal(qt->qts[i], bh, point, nodeid, nsuper, nsupermax, center, |
119 | supernode_wgts, distances, counts, flag); |
120 | } |
121 | } |
122 | } |
123 | |
124 | } |
125 | |
126 | void QuadTree_get_supernodes(QuadTree qt, real bh, real *point, int nodeid, int *nsuper, |
127 | int *nsupermax, real **center, real **supernode_wgts, real **distances, double *counts, int *flag){ |
128 | int dim = qt->dim; |
129 | |
130 | (*counts) = 0; |
131 | |
132 | *nsuper = 0; |
133 | |
134 | *flag = 0; |
135 | *nsupermax = 10; |
136 | if (!*center) *center = MALLOC(sizeof(real)*(*nsupermax)*dim); |
137 | if (!*supernode_wgts) *supernode_wgts = MALLOC(sizeof(real)*(*nsupermax)); |
138 | if (!*distances) *distances = MALLOC(sizeof(real)*(*nsupermax)); |
139 | QuadTree_get_supernodes_internal(qt, bh, point, nodeid, nsuper, nsupermax, center, supernode_wgts, distances, counts, flag); |
140 | |
141 | } |
142 | |
143 | |
144 | static real *get_or_assign_node_force(real *force, int i, SingleLinkedList l, int dim){ |
145 | |
146 | real *f = (real*) node_data_get_data(SingleLinkedList_get_data(l)); |
147 | |
148 | if (!f){ |
149 | node_data_get_data(SingleLinkedList_get_data(l)) = &(force[i*dim]); |
150 | f = (real*) node_data_get_data(SingleLinkedList_get_data(l)); |
151 | } |
152 | return f; |
153 | } |
154 | static real *get_or_alloc_force_qt(QuadTree qt, int dim){ |
155 | int i; |
156 | real *force = (real*) qt->data; |
157 | if (!force){ |
158 | qt->data = MALLOC(sizeof(real)*dim); |
159 | force = (real*) qt->data; |
160 | for (i = 0; i < dim; i++) force[i] = 0.; |
161 | } |
162 | return force; |
163 | } |
164 | |
165 | static void QuadTree_repulsive_force_interact(QuadTree qt1, QuadTree qt2, real *x, real *force, real bh, real p, real KP, real *counts){ |
166 | /* calculate the all to all reopulsive force and accumulate on each node of the quadtree if an interaction is possible. |
167 | force[i*dim+j], j=1,...,dim is the force on node i |
168 | */ |
169 | SingleLinkedList l1, l2; |
170 | real *x1, *x2, dist, wgt1, wgt2, f, *f1, *f2, w1, w2; |
171 | int dim, i, j, i1, i2, k; |
172 | QuadTree qt11, qt12; |
173 | |
174 | if (!qt1 || !qt2) return; |
175 | assert(qt1->n > 0 && qt2->n > 0); |
176 | dim = qt1->dim; |
177 | |
178 | l1 = qt1->l; |
179 | l2 = qt2->l; |
180 | |
181 | /* far enough, calculate repulsive force */ |
182 | dist = point_distance(qt1->average, qt2->average, dim); |
183 | if (qt1->width + qt2->width < bh*dist){ |
184 | counts[0]++; |
185 | x1 = qt1->average; |
186 | w1 = qt1->total_weight; |
187 | f1 = get_or_alloc_force_qt(qt1, dim); |
188 | x2 = qt2->average; |
189 | w2 = qt2->total_weight; |
190 | f2 = get_or_alloc_force_qt(qt2, dim); |
191 | assert(dist > 0); |
192 | for (k = 0; k < dim; k++){ |
193 | if (p == -1){ |
194 | f = w1*w2*KP*(x1[k] - x2[k])/(dist*dist); |
195 | } else { |
196 | f = w1*w2*KP*(x1[k] - x2[k])/pow(dist, 1.- p); |
197 | } |
198 | f1[k] += f; |
199 | f2[k] -= f; |
200 | } |
201 | return; |
202 | } |
203 | |
204 | |
205 | /* both at leaves, calculate repulsive force */ |
206 | if (l1 && l2){ |
207 | while (l1){ |
208 | x1 = node_data_get_coord(SingleLinkedList_get_data(l1)); |
209 | wgt1 = node_data_get_weight(SingleLinkedList_get_data(l1)); |
210 | i1 = node_data_get_id(SingleLinkedList_get_data(l1)); |
211 | f1 = get_or_assign_node_force(force, i1, l1, dim); |
212 | l2 = qt2->l; |
213 | while (l2){ |
214 | x2 = node_data_get_coord(SingleLinkedList_get_data(l2)); |
215 | wgt2 = node_data_get_weight(SingleLinkedList_get_data(l2)); |
216 | i2 = node_data_get_id(SingleLinkedList_get_data(l2)); |
217 | f2 = get_or_assign_node_force(force, i2, l2, dim); |
218 | if ((qt1 == qt2 && i2 < i1) || i1 == i2) { |
219 | l2 = SingleLinkedList_get_next(l2); |
220 | continue; |
221 | } |
222 | counts[1]++; |
223 | dist = distance_cropped(x, dim, i1, i2); |
224 | for (k = 0; k < dim; k++){ |
225 | if (p == -1){ |
226 | f = wgt1*wgt2*KP*(x1[k] - x2[k])/(dist*dist); |
227 | } else { |
228 | f = wgt1*wgt2*KP*(x1[k] - x2[k])/pow(dist, 1.- p); |
229 | } |
230 | f1[k] += f; |
231 | f2[k] -= f; |
232 | } |
233 | l2 = SingleLinkedList_get_next(l2); |
234 | } |
235 | l1 = SingleLinkedList_get_next(l1); |
236 | } |
237 | return; |
238 | } |
239 | |
240 | |
241 | /* identical, split one */ |
242 | if (qt1 == qt2){ |
243 | for (i = 0; i < 1<<dim; i++){ |
244 | qt11 = qt1->qts[i]; |
245 | for (j = i; j < 1<<dim; j++){ |
246 | qt12 = qt1->qts[j]; |
247 | QuadTree_repulsive_force_interact(qt11, qt12, x, force, bh, p, KP, counts); |
248 | } |
249 | } |
250 | } else { |
251 | /* split the one with bigger box, or one not at the last level */ |
252 | if (qt1->width > qt2->width && !l1){ |
253 | for (i = 0; i < 1<<dim; i++){ |
254 | qt11 = qt1->qts[i]; |
255 | QuadTree_repulsive_force_interact(qt11, qt2, x, force, bh, p, KP, counts); |
256 | } |
257 | } else if (qt2->width > qt1->width && !l2){ |
258 | for (i = 0; i < 1<<dim; i++){ |
259 | qt11 = qt2->qts[i]; |
260 | QuadTree_repulsive_force_interact(qt11, qt1, x, force, bh, p, KP, counts); |
261 | } |
262 | } else if (!l1){/* pick one that is not at the last level */ |
263 | for (i = 0; i < 1<<dim; i++){ |
264 | qt11 = qt1->qts[i]; |
265 | QuadTree_repulsive_force_interact(qt11, qt2, x, force, bh, p, KP, counts); |
266 | } |
267 | } else if (!l2){ |
268 | for (i = 0; i < 1<<dim; i++){ |
269 | qt11 = qt2->qts[i]; |
270 | QuadTree_repulsive_force_interact(qt11, qt1, x, force, bh, p, KP, counts); |
271 | } |
272 | } else { |
273 | assert(0); /* can be both at the leaf level since that should be catched at the beginning of this func. */ |
274 | } |
275 | } |
276 | } |
277 | |
278 | static void QuadTree_repulsive_force_accumulate(QuadTree qt, real *force, real *counts){ |
279 | /* push down forces on cells into the node level */ |
280 | real wgt, wgt2; |
281 | real *f, *f2; |
282 | SingleLinkedList l = qt->l; |
283 | int i, k, dim; |
284 | QuadTree qt2; |
285 | |
286 | dim = qt->dim; |
287 | wgt = qt->total_weight; |
288 | f = get_or_alloc_force_qt(qt, dim); |
289 | assert(wgt > 0); |
290 | counts[2]++; |
291 | |
292 | if (l){ |
293 | while (l){ |
294 | i = node_data_get_id(SingleLinkedList_get_data(l)); |
295 | f2 = get_or_assign_node_force(force, i, l, dim); |
296 | wgt2 = node_data_get_weight(SingleLinkedList_get_data(l)); |
297 | wgt2 = wgt2/wgt; |
298 | for (k = 0; k < dim; k++) f2[k] += wgt2*f[k]; |
299 | l = SingleLinkedList_get_next(l); |
300 | } |
301 | return; |
302 | } |
303 | |
304 | for (i = 0; i < 1<<dim; i++){ |
305 | qt2 = qt->qts[i]; |
306 | if (!qt2) continue; |
307 | assert(qt2->n > 0); |
308 | f2 = get_or_alloc_force_qt(qt2, dim); |
309 | wgt2 = qt2->total_weight; |
310 | wgt2 = wgt2/wgt; |
311 | for (k = 0; k < dim; k++) f2[k] += wgt2*f[k]; |
312 | QuadTree_repulsive_force_accumulate(qt2, force, counts); |
313 | } |
314 | |
315 | } |
316 | |
317 | void QuadTree_get_repulsive_force(QuadTree qt, real *force, real *x, real bh, real p, real KP, real *counts, int *flag){ |
318 | /* get repulsice force by a more efficient algortihm: we consider two cells, if they are well separated, we |
319 | calculate the overall repulsive force on the cell level, if not well separated, we divide one of the cell. |
320 | If both cells are at the leaf level, we calcuaulate repulsicve force among individual nodes. Finally |
321 | we accumulate forces at the cell levels to the node level |
322 | qt: the quadtree |
323 | x: current coordinates for node i is x[i*dim+j], j = 0, ..., dim-1 |
324 | force: the repulsice force, an array of length dim*nnodes, the force for node i is at force[i*dim+j], j = 0, ..., dim - 1 |
325 | bh: Barnes-Hut coefficient. If width_cell1+width_cell2 < bh*dist_between_cells, we treat each cell as a supernode. |
326 | p: the repulsive force power |
327 | KP: pow(K, 1 - p) |
328 | counts: array of size 4. |
329 | . counts[0]: number of cell-cell interaction |
330 | . counts[1]: number of cell-node interaction |
331 | . counts[2]: number of total cells in the quadtree |
332 | . Al normalized by dividing by number of nodes |
333 | */ |
334 | int n = qt->n, dim = qt->dim, i; |
335 | |
336 | for (i = 0; i < 4; i++) counts[i] = 0; |
337 | |
338 | *flag = 0; |
339 | |
340 | for (i = 0; i < dim*n; i++) force[i] = 0; |
341 | |
342 | QuadTree_repulsive_force_interact(qt, qt, x, force, bh, p, KP, counts); |
343 | QuadTree_repulsive_force_accumulate(qt, force, counts); |
344 | for (i = 0; i < 4; i++) counts[i] /= n; |
345 | |
346 | } |
347 | QuadTree QuadTree_new_from_point_list(int dim, int n, int max_level, real *coord, real *weight){ |
348 | /* form a new QuadTree data structure from a list of coordinates of n points |
349 | coord: of length n*dim, point i sits at [i*dim, i*dim+dim - 1] |
350 | weight: node weight of lentgth n. If NULL, unit weight assumed. |
351 | */ |
352 | real *xmin, *xmax, *center, width; |
353 | QuadTree qt = NULL; |
354 | int i, k; |
355 | |
356 | xmin = MALLOC(sizeof(real)*dim); |
357 | xmax = MALLOC(sizeof(real)*dim); |
358 | center = MALLOC(sizeof(real)*dim); |
359 | if (!xmin || !xmax || !center) { |
360 | FREE(xmin); |
361 | FREE(xmax); |
362 | FREE(center); |
363 | return NULL; |
364 | } |
365 | |
366 | for (i = 0; i < dim; i++) xmin[i] = coord[i]; |
367 | for (i = 0; i < dim; i++) xmax[i] = coord[i]; |
368 | |
369 | for (i = 1; i < n; i++){ |
370 | for (k = 0; k < dim; k++){ |
371 | xmin[k] = MIN(xmin[k], coord[i*dim+k]); |
372 | xmax[k] = MAX(xmax[k], coord[i*dim+k]); |
373 | } |
374 | } |
375 | |
376 | width = xmax[0] - xmin[0]; |
377 | for (i = 0; i < dim; i++) { |
378 | center[i] = (xmin[i] + xmax[i])*0.5; |
379 | width = MAX(width, xmax[i] - xmin[i]); |
380 | } |
381 | if (width == 0) width = 0.00001;/* if we only have one point, width = 0! */ |
382 | width *= 0.52; |
383 | qt = QuadTree_new(dim, center, width, max_level); |
384 | |
385 | if (weight){ |
386 | for (i = 0; i < n; i++){ |
387 | qt = QuadTree_add(qt, &(coord[i*dim]), weight[i], i); |
388 | } |
389 | } else { |
390 | for (i = 0; i < n; i++){ |
391 | qt = QuadTree_add(qt, &(coord[i*dim]), 1, i); |
392 | } |
393 | } |
394 | |
395 | |
396 | FREE(xmin); |
397 | FREE(xmax); |
398 | FREE(center); |
399 | return qt; |
400 | } |
401 | |
402 | QuadTree QuadTree_new(int dim, real *center, real width, int max_level){ |
403 | QuadTree q; |
404 | int i; |
405 | q = MALLOC(sizeof(struct QuadTree_struct)); |
406 | q->dim = dim; |
407 | q->n = 0; |
408 | q->center = MALLOC(sizeof(real)*dim); |
409 | for (i = 0; i < dim; i++) q->center[i] = center[i]; |
410 | assert(width > 0); |
411 | q->width = width; |
412 | q->total_weight = 0; |
413 | q->average = NULL; |
414 | q->qts = NULL; |
415 | q->l = NULL; |
416 | q->max_level = max_level; |
417 | q->data = NULL; |
418 | return q; |
419 | } |
420 | |
421 | void QuadTree_delete(QuadTree q){ |
422 | int i, dim; |
423 | if (!q) return; |
424 | dim = q->dim; |
425 | FREE(q->center); |
426 | FREE(q->average); |
427 | if (q->data) FREE(q->data); |
428 | if (q->qts){ |
429 | for (i = 0; i < 1<<dim; i++){ |
430 | QuadTree_delete(q->qts[i]); |
431 | } |
432 | FREE(q->qts); |
433 | } |
434 | SingleLinkedList_delete(q->l, node_data_delete); |
435 | FREE(q); |
436 | } |
437 | |
438 | static int QuadTree_get_quadrant(int dim, real *center, real *coord){ |
439 | /* find the quadrant that a point of coordinates coord is going into with reference to the center. |
440 | if coord - center == {+,-,+,+} = {1,0,1,1}, then it will sit in the i-quadrant where |
441 | i's binary representation is 1011 (that is, decimal 11). |
442 | */ |
443 | int d = 0, i; |
444 | |
445 | for (i = dim - 1; i >= 0; i--){ |
446 | if (coord[i] - center[i] < 0){ |
447 | d = 2*d; |
448 | } else { |
449 | d = 2*d+1; |
450 | } |
451 | } |
452 | return d; |
453 | } |
454 | |
455 | QuadTree QuadTree_new_in_quadrant(int dim, real *center, real width, int max_level, int i){ |
456 | /* a new quadtree in quadrant i of the original cell. The original cell is centered at 'center". |
457 | The new cell have width "width". |
458 | */ |
459 | QuadTree qt; |
460 | int k; |
461 | |
462 | qt = QuadTree_new(dim, center, width, max_level); |
463 | center = qt->center;/* right now this has the center for the parent */ |
464 | for (k = 0; k < dim; k++){/* decompose child id into binary, if {1,0}, say, then |
465 | add {width/2, -width/2} to the parents' center |
466 | to get the child's center. */ |
467 | if (i%2 == 0){ |
468 | center[k] -= width; |
469 | } else { |
470 | center[k] += width; |
471 | } |
472 | i = (i - i%2)/2; |
473 | } |
474 | return qt; |
475 | |
476 | } |
477 | static QuadTree QuadTree_add_internal(QuadTree q, real *coord, real weight, int id, int level){ |
478 | int i, dim = q->dim, ii; |
479 | node_data nd = NULL; |
480 | |
481 | int max_level = q->max_level; |
482 | int idd; |
483 | |
484 | /* Make sure that coord is within bounding box */ |
485 | for (i = 0; i < q->dim; i++) { |
486 | if (coord[i] < q->center[i] - q->width - 1.e5*MACHINEACC*q->width || coord[i] > q->center[i] + q->width + 1.e5*MACHINEACC*q->width) { |
487 | #ifdef DEBUG_PRINT |
488 | fprintf(stderr,"coordinate %f is outside of the box:{%f, %f}, \n(q->center[i] - q->width) - coord[i] =%g, coord[i]-(q->center[i] + q->width) = %g\n" ,coord[i], (q->center[i] - q->width), (q->center[i] + q->width), |
489 | (q->center[i] - q->width) - coord[i], coord[i]-(q->center[i] + q->width)); |
490 | #endif |
491 | //return NULL; |
492 | } |
493 | } |
494 | |
495 | if (q->n == 0){ |
496 | /* if this node is empty right now */ |
497 | q->n = 1; |
498 | q->total_weight = weight; |
499 | q->average = MALLOC(sizeof(real)*dim); |
500 | for (i = 0; i < q->dim; i++) q->average[i] = coord[i]; |
501 | nd = node_data_new(q->dim, weight, coord, id); |
502 | assert(!(q->l)); |
503 | q->l = SingleLinkedList_new(nd); |
504 | } else if (level < max_level){ |
505 | /* otherwise open up into 2^dim quadtrees unless the level is too high */ |
506 | q->total_weight += weight; |
507 | for (i = 0; i < q->dim; i++) q->average[i] = ((q->average[i])*q->n + coord[i])/(q->n + 1); |
508 | if (!q->qts){ |
509 | q->qts = MALLOC(sizeof(QuadTree)*(1<<dim)); |
510 | for (i = 0; i < 1<<dim; i++) q->qts[i] = NULL; |
511 | }/* done adding new quadtree, now add points to them */ |
512 | |
513 | /* insert the old node (if exist) and the current node into the appropriate child quadtree */ |
514 | ii = QuadTree_get_quadrant(dim, q->center, coord); |
515 | assert(ii < 1<<dim && ii >= 0); |
516 | if (q->qts[ii] == NULL) q->qts[ii] = QuadTree_new_in_quadrant(q->dim, q->center, (q->width)/2, max_level, ii); |
517 | |
518 | q->qts[ii] = QuadTree_add_internal(q->qts[ii], coord, weight, id, level + 1); |
519 | assert(q->qts[ii]); |
520 | |
521 | if (q->l){ |
522 | idd = node_data_get_id(SingleLinkedList_get_data(q->l)); |
523 | assert(q->n == 1); |
524 | coord = node_data_get_coord(SingleLinkedList_get_data(q->l)); |
525 | weight = node_data_get_weight(SingleLinkedList_get_data(q->l)); |
526 | ii = QuadTree_get_quadrant(dim, q->center, coord); |
527 | assert(ii < 1<<dim && ii >= 0); |
528 | |
529 | if (q->qts[ii] == NULL) q->qts[ii] = QuadTree_new_in_quadrant(q->dim, q->center, (q->width)/2, max_level, ii); |
530 | |
531 | q->qts[ii] = QuadTree_add_internal(q->qts[ii], coord, weight, idd, level + 1); |
532 | assert(q->qts[ii]); |
533 | |
534 | /* delete the old node data on parent */ |
535 | SingleLinkedList_delete(q->l, node_data_delete); |
536 | q->l = NULL; |
537 | } |
538 | |
539 | (q->n)++; |
540 | } else { |
541 | assert(!(q->qts)); |
542 | /* level is too high, append data in the linked list */ |
543 | (q->n)++; |
544 | q->total_weight += weight; |
545 | for (i = 0; i < q->dim; i++) q->average[i] = ((q->average[i])*q->n + coord[i])/(q->n + 1); |
546 | nd = node_data_new(q->dim, weight, coord, id); |
547 | assert(q->l); |
548 | q->l = SingleLinkedList_prepend(q->l, nd); |
549 | } |
550 | return q; |
551 | } |
552 | |
553 | |
554 | QuadTree QuadTree_add(QuadTree q, real *coord, real weight, int id){ |
555 | if (!q) return q; |
556 | return QuadTree_add_internal(q, coord, weight, id, 0); |
557 | |
558 | } |
559 | |
560 | static void draw_polygon(FILE *fp, int dim, real *center, real width){ |
561 | /* pliot the enclosing square */ |
562 | if (dim < 2 || dim > 3) return; |
563 | fprintf(fp,"(*in c*){Line[{" ); |
564 | |
565 | if (dim == 2){ |
566 | fprintf(fp, "{%f, %f}" , center[0] + width, center[1] + width); |
567 | fprintf(fp, ",{%f, %f}" , center[0] - width, center[1] + width); |
568 | fprintf(fp, ",{%f, %f}" , center[0] - width, center[1] - width); |
569 | fprintf(fp, ",{%f, %f}" , center[0] + width, center[1] - width); |
570 | fprintf(fp, ",{%f, %f}" , center[0] + width, center[1] + width); |
571 | } else if (dim == 3){ |
572 | /* top */ |
573 | fprintf(fp,"{" ); |
574 | fprintf(fp, "{%f, %f, %f}" , center[0] + width, center[1] + width, center[2] + width); |
575 | fprintf(fp, ",{%f, %f, %f}" , center[0] - width, center[1] + width, center[2] + width); |
576 | fprintf(fp, ",{%f, %f, %f}" , center[0] - width, center[1] - width, center[2] + width); |
577 | fprintf(fp, ",{%f, %f, %f}" , center[0] + width, center[1] - width, center[2] + width); |
578 | fprintf(fp, ",{%f, %f, %f}" , center[0] + width, center[1] + width, center[2] + width); |
579 | fprintf(fp,"}," ); |
580 | /* bot */ |
581 | fprintf(fp,"{" ); |
582 | fprintf(fp, "{%f, %f, %f}" , center[0] + width, center[1] + width, center[2] - width); |
583 | fprintf(fp, ",{%f, %f, %f}" , center[0] - width, center[1] + width, center[2] - width); |
584 | fprintf(fp, ",{%f, %f, %f}" , center[0] - width, center[1] - width, center[2] - width); |
585 | fprintf(fp, ",{%f, %f, %f}" , center[0] + width, center[1] - width, center[2] - width); |
586 | fprintf(fp, ",{%f, %f, %f}" , center[0] + width, center[1] + width, center[2] - width); |
587 | fprintf(fp,"}," ); |
588 | /* for sides */ |
589 | fprintf(fp,"{" ); |
590 | fprintf(fp, "{%f, %f, %f}" , center[0] + width, center[1] + width, center[2] - width); |
591 | fprintf(fp, ",{%f, %f, %f}" , center[0] + width, center[1] + width, center[2] + width); |
592 | fprintf(fp,"}," ); |
593 | |
594 | fprintf(fp,"{" ); |
595 | fprintf(fp, "{%f, %f, %f}" , center[0] - width, center[1] + width, center[2] - width); |
596 | fprintf(fp, ",{%f, %f, %f}" , center[0] - width, center[1] + width, center[2] + width); |
597 | fprintf(fp,"}," ); |
598 | |
599 | fprintf(fp,"{" ); |
600 | fprintf(fp, "{%f, %f, %f}" , center[0] + width, center[1] - width, center[2] - width); |
601 | fprintf(fp, ",{%f, %f, %f}" , center[0] + width, center[1] - width, center[2] + width); |
602 | fprintf(fp,"}," ); |
603 | |
604 | fprintf(fp,"{" ); |
605 | fprintf(fp, "{%f, %f, %f}" , center[0] - width, center[1] - width, center[2] - width); |
606 | fprintf(fp, ",{%f, %f, %f}" , center[0] - width, center[1] - width, center[2] + width); |
607 | fprintf(fp,"}" ); |
608 | } |
609 | |
610 | |
611 | fprintf(fp, "}]}(*end C*)" ); |
612 | |
613 | |
614 | } |
615 | static void QuadTree_print_internal(FILE *fp, QuadTree q, int level){ |
616 | /* dump a quad tree in Mathematica format. */ |
617 | SingleLinkedList l, l0; |
618 | real *coord; |
619 | int i, dim; |
620 | |
621 | if (!q) return; |
622 | |
623 | draw_polygon(fp, q->dim, q->center, q->width); |
624 | dim = q->dim; |
625 | |
626 | l0 = l = q->l; |
627 | if (l){ |
628 | printf(",(*a*) {Red," ); |
629 | while (l){ |
630 | if (l != l0) printf("," ); |
631 | coord = node_data_get_coord(SingleLinkedList_get_data(l)); |
632 | fprintf(fp, "(*node %d*) Point[{" , node_data_get_id(SingleLinkedList_get_data(l))); |
633 | for (i = 0; i < dim; i++){ |
634 | if (i != 0) printf("," ); |
635 | fprintf(fp, "%f" ,coord[i]); |
636 | } |
637 | fprintf(fp, "}]" ); |
638 | l = SingleLinkedList_get_next(l); |
639 | } |
640 | fprintf(fp, "}" ); |
641 | } |
642 | |
643 | if (q->qts){ |
644 | for (i = 0; i < 1<<dim; i++){ |
645 | fprintf(fp, ",(*b*){" ); |
646 | QuadTree_print_internal(fp, q->qts[i], level + 1); |
647 | fprintf(fp, "}" ); |
648 | } |
649 | } |
650 | |
651 | |
652 | } |
653 | |
654 | void QuadTree_print(FILE *fp, QuadTree q){ |
655 | if (!fp) return; |
656 | if (q->dim == 2){ |
657 | fprintf(fp, "Graphics[{" ); |
658 | } else if (q->dim == 3){ |
659 | fprintf(fp, "Graphics3D[{" ); |
660 | } else { |
661 | return; |
662 | } |
663 | QuadTree_print_internal(fp, q, 0); |
664 | if (q->dim == 2){ |
665 | fprintf(fp, "}, PlotRange -> All, Frame -> True, FrameTicks -> True]\n" ); |
666 | } else { |
667 | fprintf(fp, "}, PlotRange -> All]\n" ); |
668 | } |
669 | } |
670 | |
671 | |
672 | |
673 | |
674 | static void QuadTree_get_nearest_internal(QuadTree qt, real *x, real *y, real *min, int *imin, int tentative, int *flag){ |
675 | /* get the narest point years to {x[0], ..., x[dim]} and store in y.*/ |
676 | SingleLinkedList l; |
677 | real *coord, dist; |
678 | int dim, i, iq = -1; |
679 | real qmin; |
680 | real *point = x; |
681 | |
682 | *flag = 0; |
683 | if (!qt) return; |
684 | dim = qt->dim; |
685 | l = qt->l; |
686 | if (l){ |
687 | while (l){ |
688 | coord = node_data_get_coord(SingleLinkedList_get_data(l)); |
689 | dist = point_distance(point, coord, dim); |
690 | if(*min < 0 || dist < *min) { |
691 | *min = dist; |
692 | *imin = node_data_get_id(SingleLinkedList_get_data(l)); |
693 | for (i = 0; i < dim; i++) y[i] = coord[i]; |
694 | } |
695 | l = SingleLinkedList_get_next(l); |
696 | } |
697 | } |
698 | |
699 | if (qt->qts){ |
700 | dist = point_distance(qt->center, point, dim); |
701 | if (*min >= 0 && (dist - sqrt((real) dim) * qt->width > *min)){ |
702 | return; |
703 | } else { |
704 | if (tentative){/* quick first approximation*/ |
705 | qmin = -1; |
706 | for (i = 0; i < 1<<dim; i++){ |
707 | if (qt->qts[i]){ |
708 | dist = point_distance(qt->qts[i]->average, point, dim); |
709 | if (dist < qmin || qmin < 0){ |
710 | qmin = dist; iq = i; |
711 | } |
712 | } |
713 | } |
714 | assert(iq >= 0); |
715 | QuadTree_get_nearest_internal(qt->qts[iq], x, y, min, imin, tentative, flag); |
716 | } else { |
717 | for (i = 0; i < 1<<dim; i++){ |
718 | QuadTree_get_nearest_internal(qt->qts[i], x, y, min, imin, tentative, flag); |
719 | } |
720 | } |
721 | } |
722 | } |
723 | |
724 | } |
725 | |
726 | |
727 | void QuadTree_get_nearest(QuadTree qt, real *x, real *ymin, int *imin, real *min, int *flag){ |
728 | |
729 | *flag = 0; |
730 | *min = -1; |
731 | |
732 | QuadTree_get_nearest_internal(qt, x, ymin, min, imin, TRUE, flag); |
733 | |
734 | QuadTree_get_nearest_internal(qt, x, ymin, min, imin, FALSE, flag); |
735 | |
736 | |
737 | } |
738 | |