QuadTree.c 21 KB

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