kdtree_common.cc 7.03 KB
Newer Older
Uwe Schulzweida's avatar
Uwe Schulzweida committed
1
2
3
4
5
6
7
8
9
/*!\file kdtree_common.c
 * \brief Routines common to the Cartesian and spherical kd-tree versions
 *
 */

#include "kdtree.h"
#include "pqueue.h"


10
extern int pmergesort(struct kd_point *base, size_t nmemb, int axis, int max_threads);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
11
12
13
14
15
16
17
18


/* ********************************************************************

   general utility functions 

   ********************************************************************* */

19
void *
Uwe Schulzweida's avatar
Uwe Schulzweida committed
20
21
kd_malloc(size_t size, const char *msg)
{
22
23
24
25
  void *ptr;
  if ((ptr = malloc(size)) == NULL)
    perror(msg);
  return ptr;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
26
27
}

28
29
kdata_t
kd_sqr(kdata_t x)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
30
{
31
  return (x < 0 || x > 0) ? x * x : 0;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
32
33
}

34
int
Uwe Schulzweida's avatar
Uwe Schulzweida committed
35
36
kd_isleaf(struct kdNode *n)
{
37
  return (n->left || n->right) ? 0 : 1;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
38
39
40
41
42
43
44
45
46
47
48
49
50
}

/* end utility functions */

/* *******************************************************************
   
   Helper functions for debugging

   ******************************************************************* */

void
kd_printNode(struct kdNode *node)
{
51
52
53
54
55
56
  if (!node) {
    fprintf(stderr, "Node is empty.\n");
    return;
  }
  printf("Node %p at (%f, %f)\n", (void *) node, node->location[0],
         node->location[1]);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
57

58
59
60
61
  printf("Split axis: %d\n", node->split);
  printf("Corners: (%f, %f)\t(%f, %f)\n", node->min[0], node->min[1],
         node->max[0], node->max[1]);
  printf("Children: %p\t%p\n", (void *) node->left, (void *) node->right);
62
  printf("Index: %zu\n", node->index);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
63

64
  printf("\n");
Uwe Schulzweida's avatar
Uwe Schulzweida committed
65
66
67
68
69
}

void
kd_printTree(struct kdNode *node)
{
70
  if ( node == NULL ) return;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
71

72
73
74
75
76
77
  kd_printTree(node->left);

  if ( kd_isleaf(node) )
    printf("%f\t%f\n", node->location[0], node->location[1]);

  kd_printTree(node->right);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
78
79
80
81
82
83
84
85
86
87
}

/* End helper functions */

/* ******************************************************************
   
   Functions for building and destroying trees 

   ******************************************************************** */

88
void *kd_doBuildTree(void *threadarg)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
89
{
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
  kdata_t tmpMinLeft[KD_MAX_DIM], tmpMaxLeft[KD_MAX_DIM], tmpMinRight[KD_MAX_DIM], tmpMaxRight[KD_MAX_DIM];
  struct kdNode *node;
  pthread_t threads[2];
  pthread_attr_t attr;
  struct kd_thread_data *argleft, *argright;
  struct kd_thread_data *my_data = (struct kd_thread_data *) threadarg;

  struct kd_point *points = my_data->points;
  unsigned long nPoints = my_data->nPoints;
  kdata_t *min = my_data->min;
  kdata_t *max = my_data->max;
  int depth = my_data->depth;
  int max_threads = my_data->max_threads;
  int dim = my_data->dim;

  pthread_attr_init(&attr);
  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);

  int sortaxis = depth % dim;

  if (nPoints == 1) {
    if ((node = kd_allocNode(points, 0, min, max, sortaxis, dim)) == NULL)
      return NULL;
    node->index = points[0].index;
    return node;
  }

  /*
   * If this iteration is allowed to start more threads, we first
   * use them to parallelize the sorting 
   */
  pmergesort(points, nPoints, sortaxis, max_threads);

  unsigned long pivot = nPoints / 2;
  if ((node = kd_allocNode(points, pivot, min, max, sortaxis, dim)) == NULL)
    return NULL;

  memcpy(tmpMinLeft, min, dim * sizeof(kdata_t));
  memcpy(tmpMaxLeft, max, dim * sizeof(kdata_t));
  tmpMaxLeft[sortaxis] = node->location[sortaxis];
  argleft = kd_buildArg(points, pivot, tmpMinLeft, tmpMaxLeft,
                        depth + 1, max_threads / 2, dim);
  if (!argleft) {
    kd_destroyTree(node);
    return NULL;
  }

  if (max_threads > 1) {
    pthread_create(&threads[0], &attr, kd_doBuildTree, (void *) argleft);
  } else {
    node->left = (kdNode *)kd_doBuildTree((void *) argleft);
    free(argleft);
    if (!node->left) {
      kd_destroyTree(node);
      return NULL;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
145
    }
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
  }

  memcpy(tmpMinRight, min, dim * sizeof(kdata_t));
  memcpy(tmpMaxRight, max, dim * sizeof(kdata_t));
  tmpMinRight[sortaxis] = node->location[sortaxis];
  argright = kd_buildArg(&points[pivot], nPoints - pivot,
                         tmpMinRight, tmpMaxRight, depth + 1,
                         max_threads / 2, dim);
  if (!argright) {
    kd_destroyTree(node);
    return NULL;
  }

  if (max_threads > 1) {
    pthread_create(&threads[1], &attr, kd_doBuildTree, (void *) argright);
  } else {
    node->right = (kdNode *)kd_doBuildTree((void *) argright);
    free(argright);
    if (!node->right) {
      kd_destroyTree(node);
      return NULL;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
167
    }
168
169
170
171
172
173
174
175
176
177
  }

  if (max_threads > 1) {
    pthread_join(threads[0], (void **) (&node->left));
    free(argleft);
    pthread_join(threads[1], (void **) (&node->right));
    free(argright);
    if (!node->left || !node->right) {
      kd_destroyTree(node);
      return NULL;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
178
    }
179
  }
Uwe Schulzweida's avatar
Uwe Schulzweida committed
180

181
  return (void *) node;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
182
183
184
185
186
187
}


void
kd_freeNode(kdNode *node)
{
188
189
  if ( node ) free(node);
  return;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
190
191
192
193
194
195
}


struct kd_thread_data *
kd_buildArg(struct kd_point *points,
            unsigned long nPoints,
196
            kdata_t *min, kdata_t *max,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
197
198
            int depth, int max_threads, int dim)
{
199
200
201
202
203
204
205
206
207
208
209
210
211
212
  struct kd_thread_data *d;

  if ((d = (kd_thread_data *)kd_malloc(sizeof(kd_thread_data), "kd_thread_data")) == NULL)
    return NULL;

  d->points = points;
  d->nPoints = nPoints;
  memcpy(d->min, min, dim*sizeof(kdata_t));
  memcpy(d->max, max, dim*sizeof(kdata_t));
  d->depth = depth;
  d->max_threads = max_threads;
  d->dim = dim;

  return d;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
213
214
}

Uwe Schulzweida's avatar
Uwe Schulzweida committed
215
216
217
218
219
//#define TEST_BIGMEM 1
#ifdef  TEST_BIGMEM
size_t num_nodes = 0;
struct kdNode *mem_pool = NULL;
#endif
Uwe Schulzweida's avatar
Uwe Schulzweida committed
220
221
222

struct kdNode *
kd_allocNode(struct kd_point *points, unsigned long pivot,
223
             kdata_t *min, kdata_t *max, int axis, int dim)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
224
{
225
226
  struct kdNode *node;

Uwe Schulzweida's avatar
Uwe Schulzweida committed
227
228
229
230
#ifdef  TEST_BIGMEM
  if ( mem_pool == NULL ) mem_pool = (kdNode *) malloc(2*25920000*sizeof(kdNode)); // 2*gridsize
  node = &mem_pool[num_nodes++];
#else
231
232
  if ((node = (kdNode *)kd_malloc(sizeof(kdNode), "kd_allocNode (node): ")) == NULL)
    return NULL;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
233
#endif
234
235
236
237
238
239
240
241
242

  node->split = axis;
  memcpy(node->location, points[pivot].point, dim * sizeof(kdata_t));
  memcpy(node->min, min, dim * sizeof(kdata_t));
  memcpy(node->max, max, dim * sizeof(kdata_t));
  node->left = node->right = NULL;
  node->index = 0;

  return node;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
243
244
245
246
247
248
249
250
251
252
253
254
255
256
}

/*!
 * \brief free the kd-tree data structure, 
 *
 * \param node the root node of the tree to be destroyed
 *
 * \param *destr a pointer to the destructor function for the data container.
 *
 * \return This function does not return a value
 */
void
kd_destroyTree(struct kdNode *node)
{
257
  if ( node == NULL ) return;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
258

Uwe Schulzweida's avatar
Uwe Schulzweida committed
259
260
261
262
263
264
265
#ifdef  TEST_BIGMEM
  if ( mem_pool )
    {
      free(mem_pool);
      num_nodes = 0;
    }
#else
266
267
268
  kd_destroyTree(node->left);
  kd_destroyTree(node->right);
  kd_freeNode(node);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
269
#endif
Uwe Schulzweida's avatar
Uwe Schulzweida committed
270
271
272
273
274
275
276
277
278
279
}

/* end of tree construction and destruction */

/* Functions dealing with result heaps */

/* Insert the sub-tree starting at node into the result heap res */
int
kd_insertResTree(struct kdNode *node, struct pqueue *res)
{
280
  if ( node == NULL ) return 1;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
281

282
  if ( !kd_insertResTree(node->left, res) ) return 0;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
283

284
285
286
287
288
  if ( kd_isleaf(node) )
    {
      struct resItem *point;
      if ((point = (struct resItem *)kd_malloc(sizeof(struct resItem), "kd_insertResTree: "))
          == NULL)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
289
        return 0;
290
291
292
293

      point->node = node;
      point->dist_sq = -1;
      pqinsert(res, point);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
294
    }
295
296
297
298
  
  if ( !kd_insertResTree(node->right, res) ) return 0;

  return 1;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
299
}