kdtree_common.cc 7.01 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
  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;
98
  size_t nPoints = my_data->nPoints;
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
  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);

123
  size_t pivot = nPoints / 2;
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
  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
}


struct kd_thread_data *
kd_buildArg(struct kd_point *points,
195
            size_t 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

struct kdNode *
222
kd_allocNode(struct kd_point *points, size_t 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
}