kdtree.h 7.94 KB
Newer Older
Uwe Schulzweida's avatar
Uwe Schulzweida committed
1
2
3
4
5
/*! \file kdtree.h
 * \brief Include file for the kdtree library 
 */

/*
6
  Code from: https://github.com/joergdietrich/libkdtree
Uwe Schulzweida's avatar
Uwe Schulzweida committed
7
8
9
10
11
12
  Uwe Schulzweida, 20150720: changed data pointer to unsigned int
                             changed *point to point[KD_MAX_DIM]
                             changed *location to location[KD_MAX_DIM]
                             changed *min to min[KD_MAX_DIM]
                             changed *max to max[KD_MAX_DIM]
                             _compPoints: compare index if points[axis] are equal
13
                             replace qsortR by libc:qsort (speedup 25%)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
14
15
16
17
18
19
20
21
22
23
24
*/
#ifndef _KDTREE_H_
#define _KDTREE_H_

#include <math.h>
#include <pthread.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

25
26
27
28
#define  KD_FLOAT  1
#define  KD_INT    2
#define  KD_DOUBLE 3
#define  KD_LONG   4
29

30
#define  KD_TYPE  KD_FLOAT
31
//#define  KD_TYPE  KD_INT
32

Uwe Schulzweida's avatar
Uwe Schulzweida committed
33
#if KD_TYPE == KD_INT
Uwe Schulzweida's avatar
Uwe Schulzweida committed
34
typedef int kdata_t;
35
36
#  define KDATA_SFAC     44000.
#  define KDATA_SCALE(x) ((int) lround(KDATA_SFAC*(x)))
Uwe Schulzweida's avatar
Uwe Schulzweida committed
37
#  define KDATA_INVSCALE(x) ((x)/KDATA_SFAC)
38
#  define KDATA_ABS(x)   abs(x)
39
#elif KD_TYPE == KD_FLOAT
Uwe Schulzweida's avatar
Uwe Schulzweida committed
40
typedef float kdata_t;
41
42
#  define KDATA_SCALE(x) (x)
#  define KDATA_INVSCALE(x) (x)
43
44
45
46
47
#  define KDATA_ABS(x)   fabsf(x)
#elif KD_TYPE == KD_DOUBLE
typedef double kdata_t;
#  define KDATA_SCALE(x) (x)
#  define KDATA_INVSCALE(x) (x)
48
#  define KDATA_ABS(x)   fabs(x)
49
50
51
52
53
54
#elif KD_TYPE == KD_LONG
typedef long kdata_t;
#  define KDATA_SFAC     3000000.
#  define KDATA_SCALE(x) (lround(KDATA_SFAC*(x)))
#  define KDATA_INVSCALE(x) ((x)/KDATA_SFAC)
#  define KDATA_ABS(x)   labs(x)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
55
#endif
56
57


Uwe Schulzweida's avatar
Uwe Schulzweida committed
58
59
60
#define KD_MAX_DIM 3

typedef struct kd_point {
61
    kdata_t point[KD_MAX_DIM];
Uwe Schulzweida's avatar
Uwe Schulzweida committed
62
63
64
    unsigned index;
} kd_point;

Uwe Schulzweida's avatar
Uwe Schulzweida committed
65
66

static inline
67
int qcmp(struct kd_point *a, struct kd_point *b, int axis)
Uwe Schulzweida's avatar
Uwe Schulzweida committed
68
{
69
70
  int ret = (a->point[axis] > b->point[axis]) ? 1 : (a->point[axis] < b->point[axis]) ? -1 : 0;
  if ( ret == 0 ) ret = (a->index > b->index) ? 1 : (a->index < b->index) ? -1 : 0;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
71

72
73
  return ret;
}
Uwe Schulzweida's avatar
Uwe Schulzweida committed
74
75


Uwe Schulzweida's avatar
Uwe Schulzweida committed
76
77
78
79
80
/*!
 * \struct kdNode 
 * \brief kd-tree node structure definition 
 */
typedef struct kdNode {
81
82
    struct kdNode *left;          /*!<the left child of the tree node */
    struct kdNode *right;         /*!<the right child of the tree node */
83
84
85
    kdata_t location[KD_MAX_DIM]; /*!<vector to the node's location */
    kdata_t min[KD_MAX_DIM];      /*!<vector to the min coordinates of the hyperrectangle */
    kdata_t max[KD_MAX_DIM];      /*!<vector to the max coordinates of the hyperrectangle */
86
87
    int split;                    /*!<axis along which the tree bifurcates */
    unsigned index;               /*!<optional index value */
Uwe Schulzweida's avatar
Uwe Schulzweida committed
88
89
90
91
92
93
94
95
} kdNode;

/*!
 * \struct resItem
 * \brief result items, member of a priority queue 
 */
typedef struct resItem {
    struct kdNode *node;        /*!<pointer to a kdNode */
96
    kdata_t dist_sq;            /*!<distance squared as the priority */
Uwe Schulzweida's avatar
Uwe Schulzweida committed
97
98
99
100
101
102
103
} resItem;

/*!
 * \struct pqueue
 * \brief priority queue (min-max heap)
 */
typedef struct pqueue {
104
    struct resItem **d;         /*!<pointer to an array of result items */
Uwe Schulzweida's avatar
Uwe Schulzweida committed
105
106
107
108
109
110
111
112
113
114
115
    uint32_t size;              /*!<current length of the queue */
    uint32_t avail;             /*!<currently allocated queue elements */
    uint32_t step;              /*!<step size in which new elements are allocated */
} pqueue;

/*!
 * \struct kd_thread_data
 * \brief arguments passed to the threaded kd-Tree construction
 */
typedef struct kd_thread_data {
    struct kd_point *points;
116
117
    kdata_t min[KD_MAX_DIM];
    kdata_t max[KD_MAX_DIM];
118
119
    unsigned long nPoints;
    int max_threads;
Uwe Schulzweida's avatar
Uwe Schulzweida committed
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
    int depth;
    int dim;
} kd_thread_data;


#define KD_ORDERED   (1)
#define KD_UNORDERED (0)

/* functions for the priority queue */
struct pqueue *pqinit(struct pqueue *q, uint32_t n);
int pqinsert(struct pqueue *q, struct resItem *d);
struct resItem **pqremove_min(struct pqueue *q, struct resItem **d);
struct resItem **pqremove_max(struct pqueue *q, struct resItem **d);
struct resItem **pqpeek_min(struct pqueue *q, struct resItem **d);
struct resItem **pqpeek_max(struct pqueue *q, struct resItem **d);


/* general utility functions */
void *kd_malloc(size_t size, const char *msg);
int kd_isleaf(struct kdNode *n);
/* Cartesian */
141
142
kdata_t kd_sqr(kdata_t a);
kdata_t kd_min(kdata_t x, kdata_t y);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
143
/* Spherical */
144
145
146
147
148
kdata_t kd_sph_orth_dist(kdata_t *p1, kdata_t *p2, int split);
kdata_t kd_sph_dist_sq(kdata_t *x, kdata_t *y);
kdata_t kd_sph_dist(kdata_t *x, kdata_t *y);
kdata_t kd_sph_bearing(kdata_t *p1, kdata_t *p2);
kdata_t kd_sph_xtd(kdata_t *p1, kdata_t *p2, kdata_t *p3);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
149
150
151
152
153
154
155
156

/* helper functions for debugging */
void kd_printNode(struct kdNode *node);
void kd_printTree(struct kdNode *node);

/* Functions for building and destroying trees */
void kd_freeNode(kdNode * node);
struct kdNode *kd_allocNode(struct kd_point *points, unsigned long pivot,
157
                            kdata_t *min, kdata_t *max, int dim, int axis);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
158
159
160
void kd_destroyTree(struct kdNode *node);
struct kd_thread_data *kd_buildArg(struct kd_point *points,
                                   unsigned long nPoints,
161
                                   kdata_t *min, kdata_t *max,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
162
163
164
                                   int depth, int max_threads,
                                   int dim);
struct kdNode *kd_buildTree(struct kd_point *points, unsigned long nPoints,
165
                            kdata_t *min, kdata_t *max, int dim, int max_threads);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
166
void *kd_doBuildTree(void *threadarg);
167
168
struct kdNode *kd_sph_buildTree(struct kd_point *points, unsigned long nPoints,
                                kdata_t *min, kdata_t *max, int max_threads);
Uwe Schulzweida's avatar
Uwe Schulzweida committed
169
170
171
172
173
void *kd_sph_doBuildTree(void *threadarg);

/* Functions for range searches 
 * Cartesian
 */
174
175
176
177
int kd_isPointInRect(struct kdNode *node, kdata_t *min, kdata_t *max, int dim);
int kd_isRectInRect(struct kdNode *node, kdata_t *min, kdata_t *max, int dim);
int kd_rectOverlapsRect(struct kdNode *node, kdata_t *min, kdata_t *max, int dim);
struct pqueue *kd_ortRangeSearch(struct kdNode *node, kdata_t *min, kdata_t *max,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
178
                                 int dim);
179
int kd_doOrtRangeSearch(struct kdNode *node, kdata_t *min, kdata_t *max, int dim,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
180
                        struct pqueue *res);
181
struct kdNode *kd_nearest(struct kdNode *node, kdata_t *p, kdata_t *max_dist_sq,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
182
                          int dim);
183
184
185
186
struct pqueue *kd_qnearest(struct kdNode *node, kdata_t *p,
                           kdata_t *max_dist_sq, unsigned int q, int dim);
int kd_doQnearest(struct kdNode *node, kdata_t *p,
                  kdata_t *max_dist_sq, unsigned int q, int dim,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
187
                  struct pqueue *res);
188
struct pqueue *kd_range(struct kdNode *node, kdata_t *p, kdata_t *max_dist_sq,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
189
                        int dim, int ordered);
190
int kd_doRange(struct kdNode *node, kdata_t *p, kdata_t *max_dist_sq,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
191
192
               int dim, struct pqueue *res, int ordered);
/* spherical */
193
194
195
196
197
198
int kd_sph_isPointInRect(struct kdNode *node, kdata_t *min, kdata_t *max);
int kd_sph_isRectInRect(struct kdNode *node, kdata_t *min, kdata_t *max);
int kd_sph_rectOverlapsRect(struct kdNode *node, kdata_t *min, kdata_t *max);
struct pqueue *kd_sph_ortRangeSearch(struct kdNode *node, kdata_t *min,
                                     kdata_t *max);
int kd_sph_doOrtRangeSearch(struct kdNode *node, kdata_t *min, kdata_t *max,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
199
                            struct pqueue *res);
200
201
202
203
204
205
206
struct kdNode *kd_sph_nearest(struct kdNode *node, kdata_t *p,
                              kdata_t *max_dist_sq);
struct pqueue *kd_sph_qnearest(struct kdNode *node, kdata_t *p,
                               kdata_t *max_dist_sq, unsigned int q);
int kd_sph_doQnearest(struct kdNode *node, kdata_t *p,
                      kdata_t *max_dist_sq, unsigned int q, struct pqueue *res);
struct pqueue *kd_sph_range(struct kdNode *node, kdata_t *p, kdata_t *max_dist_sq,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
207
                            int ordered);
208
int kd_sph_doRange(struct kdNode *node, kdata_t *p, kdata_t *max_dist_sq,
Uwe Schulzweida's avatar
Uwe Schulzweida committed
209
210
211
212
213
214
215
                   struct pqueue *res, int ordered);

/* Functions for results heaps */
int kd_insertResTree(struct kdNode *node, struct pqueue *res);


#endif                          /* _KDTREE_H_ */