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
21extern real distance_cropped(real *x, int dim, int i, int j);
22
23struct node_data_struct {
24 real node_weight;
25 real *coord;
26 real id;
27 void *data;
28};
29
30typedef struct node_data_struct *node_data;
31
32
33static 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
45void 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
52real node_data_get_weight(void *d){
53 node_data nd = (node_data) d;
54 return nd->node_weight;
55}
56
57real* node_data_get_coord(void *d){
58 node_data nd = (node_data) d;
59 return nd->coord;
60}
61
62int 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
70void 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
80void 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
126void 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
144static 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}
154static 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
165static 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
278static 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
317void 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}
347QuadTree 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
402QuadTree 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
421void 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
438static 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
455QuadTree 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}
477static 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
554QuadTree 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
560static 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}
615static 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
654void 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
674static 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
727void 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