Commit 33c688d2 authored by Uwe Schulzweida's avatar Uwe Schulzweida
Browse files

added kdtreelib

parent 5f406aa5
......@@ -586,6 +586,15 @@ src/kdtree.c -text
src/kdtree.h -text
src/kdtree_fast.c -text
src/kdtree_fast.h -text
src/kdtreelib/COPYING.DJ -text
src/kdtreelib/kdtree.h -text
src/kdtreelib/kdtree_cartesian.c -text
src/kdtreelib/kdtree_common.c -text
src/kdtreelib/kdtree_spherical.c -text
src/kdtreelib/pmergesort.c -text
src/kdtreelib/pqueue.c -text
src/kdtreelib/pqueue.h -text
src/kdtreelib/qsort.c -text
src/kvlist.c -text
src/kvlist.h -text
src/list.c -text
......
This is the file "copying.dj". It does NOT apply to any sources or
binaries copyrighted by UCB Berkeley, the Free Software Foundation, or
any other agency besides DJ Delorie and others who have agreed to
allow their sources to be distributed under these terms.
Copyright Information for sources and executables that are marked
Copyright (C) DJ Delorie
7 Kim Lane
Rochester NH 03867-2954
This document is Copyright (C) DJ Delorie and may be distributed
verbatim, but changing it is not allowed.
Source code copyright DJ Delorie is distributed under the terms of the
GNU General Public Licence, with the following exceptions:
* Sources used to build crt0.o, gcrt0.o, libc.a, libdbg.a, and
libemu.a are distributed under the terms of the GNU Library General
Public License, rather than the GNU GPL.
* Any existing copyright or authorship information in any given source
file must remain intact. If you modify a source file, a notice to that
effect must be added to the authorship information in the source file.
* Runtime binaries, as provided by DJ in DJGPP, may be distributed
without sources ONLY if the recipient is given sufficient information
to obtain a copy of djgpp themselves. This primarily applies to
go32-v2.exe, emu387.dxe, and stubedit.exe.
* Runtime objects and libraries, as provided by DJ in DJGPP, when
linked into an application, may be distributed without sources ONLY
if the recipient is given sufficient information to obtain a copy of
djgpp themselves. This primarily applies to crt0.o and libc.a.
-----
Changes to source code copyright BSD, FSF, or others, by DJ Delorie
fall under the terms of the original copyright. Such files usually
have multiple copyright notices in them.
A copy of the files "COPYING" and "COPYING.LIB" are included with this
document. If you did not receive a copy of these files, you may
obtain one from whence this document was obtained, or by writing:
Free Software Foundation
59 Temple Place - Suite 330
Boston, MA 02111-1307
USA
/*! \file kdtree.h
* \brief Include file for the kdtree library
*/
/*
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
*/
#ifndef _KDTREE_H_
#define _KDTREE_H_
#include <math.h>
#include <pthread.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#define KD_MAX_DIM 3
typedef struct kd_point {
float point[KD_MAX_DIM];
unsigned index;
} kd_point;
/*!
* \struct kdNode
* \brief kd-tree node structure definition
*/
typedef struct kdNode {
float location[KD_MAX_DIM]; /*!<vector to the node's location */
float min[KD_MAX_DIM]; /*!<vector to the min coordinates of the hyperrectangle */
float max[KD_MAX_DIM]; /*!<vector to the max coordinates of the hyperrectangle */
int split; /*!<axis along which the tree bifurcates */
unsigned index; /*!<optional index value */
struct kdNode *left; /*!<the left child of the tree node */
struct kdNode *right; /*!<the right child of the tree node */
} kdNode;
/*!
* \struct resItem
* \brief result items, member of a priority queue
*/
typedef struct resItem {
struct kdNode *node; /*!<pointer to a kdNode */
float dist_sq; /*!<distance squared as the priority */
} resItem;
/*!
* \struct pqueue
* \brief priority queue (min-max heap)
*/
typedef struct pqueue {
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 */
struct resItem **d; /*!<pointer to an array of result items */
} pqueue;
/*!
* \struct kd_thread_data
* \brief arguments passed to the threaded kd-Tree construction
*/
typedef struct kd_thread_data {
int max_threads;
struct kd_point *points;
unsigned long nPoints;
float min[KD_MAX_DIM];
float max[KD_MAX_DIM];
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 */
float kd_dist_sq(float *x, float *y, int dim);
float kd_sqr(float a);
float kd_min(float x, float y);
/* Spherical */
float kd_sph_orth_dist(float *p1, float *p2, int split);
float kd_sph_dist_sq(float *x, float *y);
float kd_sph_dist(float *x, float *y);
float kd_sph_bearing(float *p1, float *p2);
float kd_sph_xtd(float *p1, float *p2, float *p3);
/* 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,
float *min, float *max, int dim, int axis);
void kd_destroyTree(struct kdNode *node);
struct kd_thread_data *kd_buildArg(struct kd_point *points,
unsigned long nPoints,
float *min, float *max,
int depth, int max_threads,
int dim);
struct kdNode *kd_buildTree(struct kd_point *points, unsigned long nPoints,
float *min, float *max, int dim, int max_threads);
void *kd_doBuildTree(void *threadarg);
struct kdNode *kd_sph_buildTree(struct kd_point *points,
unsigned long nPoints,
float *min, float *max,
int max_threads);
void *kd_sph_doBuildTree(void *threadarg);
/* Functions for range searches
* Cartesian
*/
int kd_isPointInRect(struct kdNode *node, float *min, float *max, int dim);
int kd_isRectInRect(struct kdNode *node, float *min, float *max, int dim);
int kd_rectOverlapsRect(struct kdNode *node, float *min, float *max, int dim);
struct pqueue *kd_ortRangeSearch(struct kdNode *node, float *min, float *max,
int dim);
int kd_doOrtRangeSearch(struct kdNode *node, float *min, float *max, int dim,
struct pqueue *res);
struct kdNode *kd_nearest(struct kdNode *node, float *p, float *max_dist_sq,
int dim);
struct pqueue *kd_qnearest(struct kdNode *node, float *p,
float *max_dist_sq, unsigned int q, int dim);
int kd_doQnearest(struct kdNode *node, float *p,
float *max_dist_sq, unsigned int q, int dim,
struct pqueue *res);
struct pqueue *kd_range(struct kdNode *node, float *p, float *max_dist_sq,
int dim, int ordered);
int kd_doRange(struct kdNode *node, float *p, float *max_dist_sq,
int dim, struct pqueue *res, int ordered);
/* spherical */
int kd_sph_isPointInRect(struct kdNode *node, float *min, float *max);
int kd_sph_isRectInRect(struct kdNode *node, float *min, float *max);
int kd_sph_rectOverlapsRect(struct kdNode *node, float *min, float *max);
struct pqueue *kd_sph_ortRangeSearch(struct kdNode *node, float *min,
float *max);
int kd_sph_doOrtRangeSearch(struct kdNode *node, float *min, float *max,
struct pqueue *res);
struct kdNode *kd_sph_nearest(struct kdNode *node, float *p,
float *max_dist_sq);
struct pqueue *kd_sph_qnearest(struct kdNode *node, float *p,
float *max_dist_sq, unsigned int q);
int kd_sph_doQnearest(struct kdNode *node, float *p,
float *max_dist_sq, unsigned int q, struct pqueue *res);
struct pqueue *kd_sph_range(struct kdNode *node, float *p, float *max_dist_sq,
int ordered);
int kd_sph_doRange(struct kdNode *node, float *p, float *max_dist_sq,
struct pqueue *res, int ordered);
/* Functions for results heaps */
int kd_insertResTree(struct kdNode *node, struct pqueue *res);
#endif /* _KDTREE_H_ */
/*!\file kdtree_cartesian.c
* \brief Routines specific to the n-dimensional cartesian kd-tree
*
*/
#include "kdtree.h"
#include "pqueue.h"
/* ********************************************************************
general utility functions
********************************************************************* */
inline float
kd_dist_sq(float *x, float *y, int dim)
{
int i;
float dsq = 0;
if (!x || !y)
return -1;
for(i = 0; i < dim; i++)
dsq += kd_sqr(x[i] - y[i]);
return dsq;
}
inline float
kd_min(float x, float y)
{
return x < y ? x : y;
}
/* ******************************************************************
Functions for building and destroying trees
******************************************************************** */
/*! \brief build kd-tree structure
*
* \param points an array of kd_points (struct with position vector
* and data container).
*
* \param nPoints the length of the points array.
*
* \param constr a pointer to a void *constructor() function to include
* the data container in the tree; optional, can be NULL
*
* \param destr a pointer to a void destructor() function to free()
* the data containers in the tree; optional, can be NULL, but should
* be given if the constr argument is non-NULL.
*
* \param min a vector with the minimum positions of the corners of the
* hyperrectangle containing the data.
*
* \param max a vector with the maximum positions of the corners of
* the hyperrectangle containing the data.
*
* \param dim the dimensionality of the data.
*
* \param max_threads the maximal number of threads spawned for
* construction of the tree. The threads will be unbalanced if this is
* not a power of 2.
*
* \return root node of the tree
*/
struct kdNode *
kd_buildTree(struct kd_point *points, unsigned long nPoints,
float *min, float *max, int dim, int max_threads)
{
struct kd_thread_data *my_data;
struct kdNode *tree;
my_data = kd_buildArg(points, nPoints, min, max, 0, max_threads, dim);
tree = kd_doBuildTree(my_data);
free(my_data);
return tree;
}
/* *******************************************************************
Functions for range searches
*******************************************************************
*/
/* Returns 1 if node is a point in the hyperrectangle defined by
minimum and maximum vectors min and max. */
int
kd_isPointInRect(struct kdNode *node, float *min, float *max, int dim)
{
int i;
if (node == NULL)
return 0;
for(i = 0; i < dim; i++) {
if (node->location[i] < min[i] || node->location[i] > max[i])
return 0;
}
return 1;
}
/* Returns 1 if the hyperrectangle of node is fully contained within
the HR described by the minimum and maximum vectors min and
max. Returns 0 otherwise. */
int
kd_isRectInRect(struct kdNode *node, float *min, float *max, int dim)
{
int i;
if (node == NULL)
return 0;
for(i = 0; i < dim; i++) {
if (node->min[i] < min[i] || node->max[i] > max[i])
return 0;
}
return 1;
}
/* Returns 1 if the hyperrectangle of node overlaps the HR described
the minimum and maximum vectors min and max. Returns 0
otherwise. */
int
kd_rectOverlapsRect(struct kdNode *node, float *min, float *max, int dim)
{
int i;
if (node == NULL)
return 0;
for(i = 0; i < dim; i++) {
if ((node->min[i] > max[i] || node->max[i] < min[i]))
return 0;
}
return 1;
}
/*!
* \brief Perform orthogonal range search (get all points in a
* hyperrectangle).
*
* \param node the root node of tree to be searched.
*
* \param min a vector with the minimum positions of the corners of the
* hyperrectangle containing the data.
*
* \param max a vector with the maximum positions of the corners of
* the hyperrectangle containing the data.
*
* \param dim the dimension of the data.
*
* \return Pointer to a priority queue, NULL in case of problems.
*/
struct pqueue *
kd_ortRangeSearch(struct kdNode *node, float *min, float *max, int dim)
{
struct pqueue *res;
uint32_t i;
if ((res = pqinit(NULL, 1)) == NULL)
return NULL;
if (!kd_doOrtRangeSearch(node, min, max, dim, res)) {
for(i = 0; i < res->size; i++) {
free(res->d[i]);
}
free(res->d);
free(res);
return NULL;
}
return res;
}
/* This is the orthogonal range search. Returns 1 if okay, 0 in case
of problems. */
int
kd_doOrtRangeSearch(struct kdNode *node, float *min, float *max,
int dim, struct pqueue *res)
{
if (node == NULL)
return 1;
if (kd_isleaf(node) && kd_isPointInRect(node, min, max, dim)) {
return kd_insertResTree(node, res);
} else {
if (kd_isRectInRect(node->left, min, max, dim)) {
if (!kd_insertResTree(node->left, res))
return 0;
} else {
if (kd_rectOverlapsRect(node->left, min, max, dim))
if (!kd_doOrtRangeSearch(node->left, min, max, dim, res))
return 0;
}
if (kd_isRectInRect(node->right, min, max, dim)) {
if (!kd_insertResTree(node->right, res))
return 0;
} else {
if (kd_rectOverlapsRect(node->right, min, max, dim))
if (!kd_doOrtRangeSearch(node->right, min, max, dim, res))
return 0;
}
}
return 1;
}
/*!
* \brief Find the nearest neighbor of a point.
*
* \param node the root node of the tree to be searched.
*
* \param p a vector to the point whose nearest neighbor is sought.
*
* \param max_dist_sq the square of the maximum distance to the
* nearest neighbor.
*
* \param dim the dimension of the data.
*
* \return A pointer to node containing the nearest neighbor.
* max_dist_sq is set to the square of the distance to the nearest
* neigbor.
*/
struct kdNode *
kd_nearest(struct kdNode *node, float *p, float *max_dist_sq, int dim)
{
struct kdNode *nearer, *further, *nearest = NULL, *tmp, *tmp_nearest;
float dist_sq, tmp_dist_sq, dx;
if (!node)
return NULL;
if (p[node->split] < node->location[node->split]) {
nearer = node->left;
further = node->right;
} else {
nearer = node->right;
further = node->left;
}
tmp = kd_nearest(nearer, p, max_dist_sq, dim);
if (tmp)
nearest = tmp;
else
nearest = node;
dist_sq = kd_dist_sq(nearest->location, p, dim);
if (*max_dist_sq > dist_sq)
*max_dist_sq = dist_sq;
if (!further)
return nearest;
dx = kd_min(fabs(p[node->split] - further->min[node->split]),
fabs(p[node->split] - further->max[node->split]));
if (*max_dist_sq > kd_sqr(dx)) {
/*
* some part of the further hyper-rectangle is in the search
* radius, search the further node
*/
tmp = kd_nearest(further, p, max_dist_sq, dim);
if (tmp)
tmp_nearest = tmp;
else
tmp_nearest = further;
tmp_dist_sq = kd_dist_sq(tmp_nearest->location, p, dim);
if (tmp_dist_sq < dist_sq) {
nearest = tmp_nearest;
dist_sq = tmp_dist_sq;
*max_dist_sq = kd_min(dist_sq, *max_dist_sq);
}
}
return nearest;
}
/*!
* \brief Return the q nearest-neighbors to a point.
*
* \param node the root node of the tree to be searched.
*
* \param p a vector to the point whose nearest neighbors are sought.
*
* \param max_dist_sq the square of the maximum distance to the
* nearest neighbors.
*
* \param q the maximum number of points to be retured.
*
* \param dim the dimension of the data.
*
* \return A pointer to a priority queue of the points found, or NULL
* in case of problems.
*/
struct pqueue *
kd_qnearest(struct kdNode *node, float *p,
float *max_dist_sq, unsigned int q, int dim)
{
struct pqueue *res;
uint32_t i;
if ((res = pqinit(NULL, q + 2)) == NULL)
return NULL;
if (!kd_doQnearest(node, p, max_dist_sq, q + 1, dim, res)) {
for(i = 0; i < res->size; i++) {
free(res->d[i]);
}
free(res->d);
free(res);
return NULL;
}
return res;
}
/* *
* This is the q nearest-neighbor search.
*
* This is a modification of the range search in which the maximum
* search radius is decreased to the maximum of the queue as soon as
* the queue is filled.
*
* return 1 if okay, zero in case of problems
*/
int
kd_doQnearest(struct kdNode *node, float *p,
float *max_dist_sq, unsigned int q, int dim, struct pqueue *res)
{
struct kdNode *nearer, *further;
struct resItem *point, *item;
float dist_sq, dx;
if (!node)
return 1;
dist_sq = kd_dist_sq(node->location, p, dim);
if (dist_sq < *max_dist_sq && kd_isleaf(node)) {
if ((point = kd_malloc(sizeof(struct resItem), "kd_doQnearest: "))
== NULL)
return 0;
point->node = node;
point->dist_sq = dist_sq;
pqinsert(res, point);
}
if (res->size > q) {
pqremove_max(res, &item);
free(item);
if (res->size > 1) {
/*
* Only inspect the queue if there are items left
*/
pqpeek_max(res, &item);
*max_dist_sq = item->dist_sq;
} else {
/*
* Nothing was found within the max search radius
*/
*max_dist_sq = 0;
}
}
if (p[node->split] < node->location[node->split]) {
nearer = node->left;
further = node->right;
} else {
nearer = node->right;
further = node->left;
}
if (!kd_doQnearest(nearer, p, max_dist_sq, q, dim, res))
return 0;
if (!further)
return 1;
dx = kd_min(fabs(p[node->split] - further->min[node->split]),
fabs(p[node->split] - further->max[node->split]));
if (*max_dist_sq > kd_sqr(dx)) {