Implementation of a kd-tree. More...

#include "cuik-kdtree.h"
#include "definitions.h"
#include "vector.h"
#include "random.h"
#include <assert.h>
#include <stdio.h>
#include <string.h>

Include dependency graph for cuik-kdtree.c:

Go to the source code of this file.

Macros
#define	TRUE_MEDIAN 0
	Method to determine the split point. More...

#define	KDTREE_PRINT_INDENT(f, indent) {unsigned int k; for(k=0;k<indent;k++) fprintf(f," ");}
	Auxiliary macro to print the kd-tree. More...

Functions
TKDtree *	BuildKDTree (Trectangle ambient, unsigned int topology, unsigned int nd, unsigned int m, double r, unsigned int n, double *points, unsigned int ids)
	Initializes a kd-tree from a set of points. More...

void	SearchKDtree (double dr2, double p, double d2, unsigned int id, TKDtree kdt)
	Searches the nearest point on a KD-tree. More...

void	SearchInBallKDtree (double dr2, double p, double r2, unsigned int n, unsigned int m, unsigned int id, TKDtree kdt)
	Searches the near points on a KD-tree. More...

TKDtree *	InitKDTree (Trectangle ambient, unsigned int topology, unsigned int m, double r, unsigned int n, double *points, unsigned int ids)
	Initializes a kd-tree from a set of points. More...

void	AddPoint2KDtree (double point, unsigned int id, TKDtree *kdt)
	Adds a point to a kd-tree. More...

unsigned int	NearestNeighbour (double point, TKDtree kdt)
	Determines the nearest-neighbour for a point. More...

void	NeighboursInBall (double point, double r, unsigned int n, unsigned int *ids, TKDtree kdt)
	Neighbours in a ball. More...

void	SampleInKDtree (double point, TKDtree kdt)
	Generates a random sample. More...

double	KDtreeSamplingExpansion (TKDtree *kdt)
	Returns the sampling expansion used in the kd-tree. More...

void	PrintKDtree (FILE f, unsigned int indent, TKDtree kdt)
	Prints a kd-tree. More...

void	DeleteKDtree (TKDtree *kdt)
	Deletes a KD-tree. More...

Detailed Description

Implementation of a kd-tree. This is used to speed up the search of nearest neighbours (or neighbours in a given ball) but also for sampling.

This is an alternative to the DNN library, but here we use the kd-tree not only to search but also to sample. The DNN kd-tree is actually a forest of trees. To samle a single tree is necessary. So,instead of modifying the DNN implementation, we implemented a new kd-tree, even at the risk of being less efficient (specially when searching for neighbours).

This implemetation follows the paper

A. Yershova and S. M. LaValle, Motion Planning for Highly Costrained Spaces, Romoco, 2009.

See Also: TKDtree cuik-kdtree.h

Definition in file cuik-kdtree.c.

Macro Definition Documentation

#define TRUE_MEDIAN 0

If set to 1, the split point when building the kd-tree is computing using the median. Otherwise, we use the mean as an approximation to the median.

Using the median is supposed to produce more balanced trees.

Definition at line 45 of file cuik-kdtree.c.

#define KDTREE_PRINT_INDENT	(	f,
		indent
	)	{unsigned int k; for(k=0;k<indent;k++) fprintf(f," ");}

Indents accorging to the node height.

Parameters

f	The file where to print the kd-tree.
indent	The indentation level.

Definition at line 55 of file cuik-kdtree.c.

Referenced by PrintKDtree().

Function Documentation

TKDtree * BuildKDTree	(	Trectangle *	ambient,
		unsigned int *	topology,
		unsigned int	nd,
		unsigned int	m,
		double	r,
		unsigned int	n,
		double **	points,
		unsigned int *	ids
	)

Kd-tree constructor.

Parameters

ambient	The ambient box for the points.
topology	The topology (Real,Circle) of each dimension. Might be NULL if the topology is Real for all dimensions.
nd	Dimensionality of each point.
m	The maximum number of points in the leaves.
r	The expansion factor to use in the sampling.
n	The number of points.
points	The points.
ids	The identifiers of the points.

Returns: The kd-tree.

Definition at line 132 of file cuik-kdtree.c.

References TKDtree::ambient, TKDtree::boundingBox, CopyRectangle(), EnlargeRectangleWithLimits(), ExpandRectangle(), FALSE, GetRectangleLimits(), GetRectangleSplitDim(), TKDtree::height, TKDtree::id, INF, InitRectangleFromPoint(), TKDtree::isLeaf, KDTPTR, TKDtree::left, TKDtree::leftLeaf, TKDtree::m, TKDtree::n, TKDtree::nd, NEW, TKDtree::nextLeaf, NO_UINT, TKDtree::p, TKDtree::r, TKDtree::right, TKDtree::rightLeaf, TKDtree::samplingBox, SetRectangleLimits(), SetRectangleLowerLimit(), SetRectangleUpperLimit(), TKDtree::splitDim, TKDtree::splitValue, SWAP, TKDtree::topology, TRUE, and TKDtree::volume.

Referenced by AddPoint2KDtree(), and InitKDTree().

Here is the call graph for this function:

Here is the caller graph for this function:

void SearchKDtree	(	double	dr2,
		double *	p,
		double *	d2,
		unsigned int *	id,
		TKDtree *	kdt
	)

Auxiliary funciton for NearestNeighbour.

Parameters

dr2	Squared distance from the query point to the ambient box in the root of the kd-tree.
p	The query point.
d2	Squared distanace to the closest point so far.
id	The identifier associated with the nearest-neighbour so far.
kdt	The KD-tree to query.

Definition at line 364 of file cuik-kdtree.c.

References TKDtree::ambient, TKDtree::boundingBox, TKDtree::id, TKDtree::isLeaf, KDTPTR, TKDtree::left, TKDtree::n, TKDtree::nd, TKDtree::p, TKDtree::right, TKDtree::splitDim, SquaredDistanceToRectangle(), SquaredDistanceToRectangleDimension(), TKDtree::topology, and VectorSquaredDistanceTopologyMin().

Referenced by NearestNeighbour().

Here is the call graph for this function:

Here is the caller graph for this function:

void SearchInBallKDtree	(	double	dr2,
		double *	p,
		double	r2,
		unsigned int *	n,
		unsigned int *	m,
		unsigned int **	id,
		TKDtree *	kdt
	)

Auxiliary funciton for NeighboursInBall.

Parameters

dr2	Squared distance from the query point to the ambient box in the root of the kd-tree.
p	The query point.
r2	Squared search radius.
n	Number of neighbours so far.
m	Space for the neighbours allocated so far.
id	Identifiers neighbours so far.
kdt	The KD-tree to query.

Definition at line 421 of file cuik-kdtree.c.

References TKDtree::ambient, TKDtree::boundingBox, TKDtree::id, TKDtree::isLeaf, KDTPTR, TKDtree::left, MEM_DUP, TKDtree::n, TKDtree::nd, TKDtree::p, TKDtree::right, TKDtree::splitDim, SquaredDistanceToRectangle(), SquaredDistanceToRectangleDimension(), TKDtree::topology, and VectorSquaredDistanceTopologyMin().

Referenced by NeighboursInBall().

Here is the call graph for this function:

Here is the caller graph for this function:

TKDtree* InitKDTree	(	Trectangle *	ambient,
		unsigned int *	topology,
		unsigned int	m,
		double	r,
		unsigned int	n,
		double **	points,
		unsigned int *	ids
	)

Kd-tree constructor.

Parameters

ambient	The ambient box for the points.
topology	The topology (Real,Circle) of each dimension. Might be NULL if the topology is real for all dimensions.
m	The maximum number of points in the leaves.
r	The expansion factor to use in the sampling.
n	The number of points.
points	The points.
ids	The identifiers of the points.

Returns: The kd-tree.

Definition at line 544 of file cuik-kdtree.c.

References ANGLE_ACCURACY, BuildKDTree(), GetRectangleDim(), GetRectangleLimits(), M_PI, SetRectangleLimits(), and TOPOLOGY_S.

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

void AddPoint2KDtree	(	double *	point,
		unsigned int	id,
		TKDtree **	kdt
	)

Adds a point to a kd-tree.

Parameters

point	The point to add to the kd-tree.
id	The identifier associated with the point.
kdt	The kd-tree to update.

Definition at line 582 of file cuik-kdtree.c.

References AddPoint2KDtree(), TKDtree::ambient, TKDtree::boundingBox, BuildKDTree(), CopyRectangle(), DeleteKDtree(), EnlargeRectangleWithLimits(), ExpandRectangle(), TKDtree::height, TKDtree::id, INF, InitRectangleFromPoint(), TKDtree::isLeaf, KDTPTR, TKDtree::left, TKDtree::leftLeaf, TKDtree::m, TKDtree::n, TKDtree::nd, NEW, TKDtree::nextLeaf, TKDtree::p, TKDtree::r, TKDtree::right, TKDtree::rightLeaf, TKDtree::samplingBox, TKDtree::splitDim, TKDtree::splitValue, TKDtree::topology, VectorSquaredDistanceTopologyMin(), and TKDtree::volume.

Referenced by AddPoint2KDtree(), and main().

Here is the call graph for this function:

Here is the caller graph for this function:

unsigned int NearestNeighbour	(	double *	point,
		TKDtree *	kdt
	)

Uses the kd-tree to find the nearest-neighbour of a given point.

Parameters

point	The query point.
kdt	The KD-tree to query.

Returns: The identifier associated with the nearest-neighbour.

Definition at line 714 of file cuik-kdtree.c.

References INF, TKDtree::n, NO_UINT, and SearchKDtree().

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

void NeighboursInBall	(	double *	point,
		double	r,
		unsigned int *	n,
		unsigned int **	ids,
		TKDtree *	kdt
	)

Determines all the points in a given ball.

Parameters

point	The center of the query ball.
r	The radius of the query ball.
n	The nuber of output points.
ids	The identifiers for the output points.
kdt	The KD-tree to query.

Definition at line 730 of file cuik-kdtree.c.

References TKDtree::n, NEW, and SearchInBallKDtree().

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

void SampleInKDtree	(	double *	point,
		TKDtree *	kdt
	)

Geneates a random sample in the volume covered by the kd-tree expanded by a given factor ('r' given in the kd-tree construction) with uniform distribution.

Parameters

point	The sampled point.
kdt	The kd-tree

See Also: InitKDTree

Definition at line 747 of file cuik-kdtree.c.

References TKDtree::isLeaf, KDTPTR, TKDtree::left, randomDouble(), RandomPointInRectangle(), TKDtree::right, SampleInKDtree(), TKDtree::samplingBox, and TKDtree::volume.

Referenced by SampleInKDtree().

Here is the call graph for this function:

Here is the caller graph for this function:

double KDtreeSamplingExpansion ( TKDtree * kdt )

Returns the sampling expansion used in the kd-tree.

Parameters

kdt	The kd-tree to query.

Returns: The sampling expansion of the kd-tree.

Definition at line 764 of file cuik-kdtree.c.

References TKDtree::r.

void PrintKDtree	(	FILE *	f,
		unsigned int	indent,
		TKDtree *	kdt
	)

Prints a kd-tree. Used for debug.

Parameters

f	The file where to print the kd-tree. Typically stdout or stderr.
indent	Indent level for the first level of the tree. The rest of levels are progressively indented.
kdt	The kd-tree to print.

Definition at line 769 of file cuik-kdtree.c.

References TKDtree::boundingBox, TKDtree::id, TKDtree::isLeaf, KDTPTR, KDTREE_PRINT_INDENT, TKDtree::left, TKDtree::n, TKDtree::nd, TKDtree::p, PrintKDtree(), PrintRectangle(), TKDtree::right, TKDtree::splitDim, and TKDtree::splitValue.