Index: /branches/pap_mops/psLib/src/types/psTree.c =================================================================== --- /branches/pap_mops/psLib/src/types/psTree.c (revision 25179) +++ /branches/pap_mops/psLib/src/types/psTree.c (revision 25180) @@ -14,4 +14,6 @@ #include "psTree.h" +//#define INPUT_CHECK // Check inputs for functions that may be in a tight loop? + // XXX Upgrades: @@ -84,5 +86,5 @@ } -psTree *psTreeAlloc(int dim, int maxLeafContents, long numNodes) +psTree *psTreeAlloc(int dim, int maxLeafContents, psTreeType type, long numNodes) { psAssert(dim > 0, "Dimensionality (%d) must be positive", dim); @@ -95,4 +97,5 @@ tree->dim = dim; tree->maxLeafContents = maxLeafContents; + tree->type = type; tree->numNodes = numNodes; @@ -115,5 +118,5 @@ -psTree *psTreePlant(int dim, int maxLeafContents, ...) +psTree *psTreePlant(int dim, int maxLeafContents, psTreeType type, ...) { PS_ASSERT_INT_POSITIVE(dim, NULL); @@ -122,5 +125,5 @@ // Parse coordinate list va_list args; // Variable argument list - va_start(args, maxLeafContents); + va_start(args, type); psArray *coords = psArrayAlloc(dim); // Array of coordinates long numData = 0; // Number of data points @@ -148,5 +151,5 @@ } - psTree *tree = psTreeAlloc(dim, maxLeafContents, numNodes); + psTree *tree = psTreeAlloc(dim, maxLeafContents, type, numNodes); tree->data = psTreeCoordArrayAlloc(numData, dim); tree->numData = numData; @@ -365,5 +368,5 @@ psTreeNode *psTreeLeaf(const psTree *tree, const psVector *coords) { -#if 1 // Might be in a tight loop +#ifdef INPUT_CHECK // Might be in a tight loop PS_ASSERT_TREE_NON_NULL(tree, NULL); PS_ASSERT_VECTOR_NON_NULL(coords, NULL); @@ -403,20 +406,40 @@ { int dim = tree->dim; // Dimensionality - switch (dim) { - case 2: - return PS_SQR(coords->data.F64[0] - tree->data->F64[index][0]) + - PS_SQR(coords->data.F64[1] - tree->data->F64[index][1]); - case 3: - return PS_SQR(coords->data.F64[0] - tree->data->F64[index][0]) + - PS_SQR(coords->data.F64[1] - tree->data->F64[index][1]) + - PS_SQR(coords->data.F64[2] - tree->data->F64[index][2]); - default: { - double distance2 = 0.0; // Distance of interest - for (int i = 0; i < dim; i++) { - distance2 += PS_SQR(coords->data.F64[i] - tree->data->F64[index][i]); + switch (tree->type) { + case PS_TREE_EUCLIDEAN: + switch (dim) { + case 2: + return PS_SQR(coords->data.F64[0] - tree->data->F64[index][0]) + + PS_SQR(coords->data.F64[1] - tree->data->F64[index][1]); + case 3: + return PS_SQR(coords->data.F64[0] - tree->data->F64[index][0]) + + PS_SQR(coords->data.F64[1] - tree->data->F64[index][1]) + + PS_SQR(coords->data.F64[2] - tree->data->F64[index][2]); + default: { + double distance2 = 0.0; // Distance of interest + for (int i = 0; i < dim; i++) { + distance2 += PS_SQR(coords->data.F64[i] - tree->data->F64[index][i]); + } + return distance2; } - return distance2; - } - } + } + break; + case PS_TREE_SPHERICAL: + switch (dim) { + case 2: { + double dphi = coords->data.F64[0] - tree->data->F64[index][0]; + double dlambda = coords->data.F64[1] - tree->data->F64[index][1]; + double sindphi = sin(dphi / 2.0); + double sindlambda = sin(dlambda / 2.0); + return PS_SQR(sindphi) + + cos(coords->data.F64[0]) * cos(coords->data.F64[0]) * PS_SQR(sindlambda); + } + default: + psAbort("Spherical distances not supported for more than 2 dimensions"); + } + default: + psAbort("Unrecognised type: %x", tree->type); + } + return NAN; } @@ -430,10 +453,32 @@ double minDiff = tree->min->F64[index][i] - coords->data.F64[i]; if (minDiff > 0) { - distance += PS_SQR(minDiff); + switch (tree->type) { + case PS_TREE_EUCLIDEAN: + distance += PS_SQR(minDiff); + break; + case PS_TREE_SPHERICAL: { + double sinDiff = sin(minDiff / 2.0); + distance += PS_SQR(sinDiff); + break; + } + default: + psAbort("Unrecognised type: %x", tree->type); + } continue; } double maxDiff = coords->data.F64[i] - tree->max->F64[index][i]; if (maxDiff > 0) { - distance += PS_SQR(maxDiff); + switch (tree->type) { + case PS_TREE_EUCLIDEAN: + distance += PS_SQR(maxDiff); + break; + case PS_TREE_SPHERICAL: { + double sinDiff = sin(maxDiff / 2.0); + distance += PS_SQR(sinDiff); + break; + } + default: + psAbort("Unrecognised type: %x", tree->type); + } continue; } @@ -479,12 +524,12 @@ } -// Return the index of the nearest neighbour to given coordinates, within some radius +// Return the index of the nearest neighbour to given coordinates, within some distance measure // This is the engine for psTreeNearest() and psTreeNearestWithin() static inline long treeNearestWithin(const psTree *tree, // Tree const psVector *coordinates, // Coordinates of interest - double bestDistance // Distance (radius-squared) to best point + double bestDistance // Distance measure to best point ) { -#if 1 // Might be in a tight loop +#ifdef INPUT_CHECK // Might be in a tight loop PS_ASSERT_TREE_NON_NULL(tree, -1); PS_ASSERT_VECTOR_NON_NULL(coordinates, -1); @@ -545,12 +590,31 @@ +// Convert a radius to our internal "distance measure" +// Often, we're given a search radius, but for efficiency reasons, we don't use that internally. +static double treeRadiusToDistance(const psTree *tree, double radius) +{ + switch (tree->type) { + case PS_TREE_EUCLIDEAN: + // Using the square of the distance as the distance measure + return PS_SQR(radius); + case PS_TREE_SPHERICAL: { + // Using a rearrangement of the Haversine formula + double sindist = sin(radius / 2.0); + return PS_SQR(sindist); + } + default: + psAbort("Unrecognised type: %x", tree->type); + } +} + + long psTreeNearestWithin(const psTree *tree, const psVector *coords, double radius) { - return treeNearestWithin(tree, coords, PS_SQR(radius)); -} - - -// Search a leaf node for points within radius squared -static inline long treeLeafSearchWithin(double radius2, // Radius squared to search + return treeNearestWithin(tree, coords, treeRadiusToDistance(tree, radius)); +} + + +// Search a leaf node for points within distance +static inline long treeLeafSearchWithin(double distance, // Distance to search const psTree *tree, // Tree of interest const psTreeNode *leaf, // Leaf to search @@ -561,6 +625,5 @@ for (int i = 0; i < leaf->num; i++) { long index = leaf->contents[i]; // Index of point - double distance = treeContentDistance(tree, index, coords); // Distance to point - if (distance < radius2) { + if (treeContentDistance(tree, index, coords) < distance) { num++; } @@ -572,5 +635,5 @@ long psTreeWithin(const psTree *tree, const psVector *coordinates, double radius) { -#if 1 // Might be in a tight loop +#ifdef INPUT_CHECK // Might be in a tight loop PS_ASSERT_TREE_NON_NULL(tree, -1); PS_ASSERT_VECTOR_NON_NULL(coordinates, -1); @@ -581,5 +644,5 @@ psVectorCopy(NULL, coordinates, PS_TYPE_F64)); // F64 version of coordinates - radius *= radius; // We work with the radius-squared + double distance = treeRadiusToDistance(tree, radius); // Distance measure long num = 0; // Number of points in circle @@ -588,5 +651,5 @@ // Find the closest point in the leaf that contains the point of interest psTreeNode *leaf = psTreeLeaf(tree, coords); // Leaf containing the point of interest - num += treeLeafSearchWithin(radius, tree, leaf, coords); + num += treeLeafSearchWithin(distance, tree, leaf, coords); psArray *work = psArrayAlloc(tree->numNodes); // Work queue @@ -605,5 +668,5 @@ } // Leaf node - num += treeLeafSearchWithin(radius, tree, node, coords); + num += treeLeafSearchWithin(distance, tree, node, coords); work->data[workIndex] = NULL; workIndex--; @@ -618,28 +681,28 @@ } -// Search a leaf node for any points within radius squared -static inline bool treeLeafSearchWithinAny(double radius2, // Radius squared to search - const psTree *tree, // Tree of interest - const psTreeNode *leaf, // Leaf to search - const psVector *coords // Coordinates of interest +// Search a leaf node for points within distance +static inline void treeLeafSearchAllWithin(psVector *result, // Result vector + double distance, // Distance to search + const psTree *tree, // Tree of interest + const psTreeNode *leaf, // Leaf to search + const psVector *coords // Coordinates of interest ) { for (int i = 0; i < leaf->num; i++) { long index = leaf->contents[i]; // Index of point - double distance = treeContentDistance(tree, index, coords); // Distance to point - if (distance < radius2) { - return true; - } - } - return false; -} - -// Given an arbitrary point and a matching radius, return whether there are any points within that radius -bool psTreeWithinAny(const psTree *tree, const psVector *coordinates, double radius) -{ -#if 1 // Might be in a tight loop - PS_ASSERT_TREE_NON_NULL(tree, false); - PS_ASSERT_VECTOR_NON_NULL(coordinates, false); - PS_ASSERT_VECTOR_SIZE(coordinates, (long)tree->dim, false); + if (treeContentDistance(tree, index, coords) < distance) { + psVectorAppend(result, index); + } + } + return; +} + +// Given an arbitrary point and a matching radius, return the index of all points within that radius +psVector *psTreeAllWithin(const psTree *tree, const psVector *coordinates, double radius) +{ +#ifdef INPUT_CHECK // Might be in a tight loop + PS_ASSERT_TREE_NON_NULL(tree, NULL); + PS_ASSERT_VECTOR_NON_NULL(coordinates, NULL); + PS_ASSERT_VECTOR_SIZE(coordinates, (long)tree->dim, NULL); #endif @@ -647,14 +710,13 @@ psVectorCopy(NULL, coordinates, PS_TYPE_F64)); // F64 version of coordinates - radius *= radius; // We work with the radius-squared - - // This is essentially the same as psTreeWithin, except we can bail as soon as we find something + double distance = treeRadiusToDistance(tree, radius); // Distance measure + + psVector *result = psVectorAllocEmpty(4, PS_TYPE_S64); // Indices of points within match radius + + // This is essentially the same as psTreeNearest, except we're not allowed to prune // Find the closest point in the leaf that contains the point of interest psTreeNode *leaf = psTreeLeaf(tree, coords); // Leaf containing the point of interest - if (treeLeafSearchWithinAny(radius, tree, leaf, coords)) { - psFree(coords); - return true; - } + treeLeafSearchAllWithin(result, distance, tree, leaf, coords); psArray *work = psArrayAlloc(tree->numNodes); // Work queue @@ -673,5 +735,72 @@ } // Leaf node - if (treeLeafSearchWithinAny(radius, tree, node, coords)) { + treeLeafSearchAllWithin(result, distance, tree, node, coords); + work->data[workIndex] = NULL; + workIndex--; + } + + leaf = up; + } + psFree(work); + psFree(coords); + + return result; +} + +// Search a leaf node for any points within distance measure +static inline bool treeLeafSearchWithinAny(double distance, // Distance to search + const psTree *tree, // Tree of interest + const psTreeNode *leaf, // Leaf to search + const psVector *coords // Coordinates of interest + ) +{ + for (int i = 0; i < leaf->num; i++) { + long index = leaf->contents[i]; // Index of point + if (treeContentDistance(tree, index, coords) < distance) { + return true; + } + } + return false; +} + +// Given an arbitrary point and a matching radius, return whether there are any points within that radius +bool psTreeWithinAny(const psTree *tree, const psVector *coordinates, double radius) +{ +#ifdef INPUT_CHECK // Might be in a tight loop + PS_ASSERT_TREE_NON_NULL(tree, false); + PS_ASSERT_VECTOR_NON_NULL(coordinates, false); + PS_ASSERT_VECTOR_SIZE(coordinates, (long)tree->dim, false); +#endif + + psVector *coords = (coordinates->type.type == PS_TYPE_F64 ? psMemIncrRefCounter((psVector*)coordinates) : + psVectorCopy(NULL, coordinates, PS_TYPE_F64)); // F64 version of coordinates + + double distance = treeRadiusToDistance(tree, radius); // Distance measure + + // This is essentially the same as psTreeWithin, except we can bail as soon as we find something + + // Find the closest point in the leaf that contains the point of interest + psTreeNode *leaf = psTreeLeaf(tree, coords); // Leaf containing the point of interest + if (treeLeafSearchWithinAny(distance, tree, leaf, coords)) { + psFree(coords); + return true; + } + + psArray *work = psArrayAlloc(tree->numNodes); // Work queue + while (leaf->up) { + psTreeNode *up = leaf->up; // Parent node + + long workIndex = 0; + work->data[workIndex] = (leaf == up->left ? up->right : up->left); // The other node + while (workIndex >= 0) { + psTreeNode *node = work->data[workIndex]; + if (node->left) { + // Branch node + work->data[workIndex] = node->right; + work->data[++workIndex] = node->left; + continue; + } + // Leaf node + if (treeLeafSearchWithinAny(distance, tree, node, coords)) { // Clear out the work queue memset(work->data, 0, (workIndex + 1) * sizeof(void)); @@ -695,5 +824,5 @@ psVector *psTreeCoords(psVector *out, const psTree *tree, long index) { -#if 1 // Might be in a tight loop +#ifdef INPUT_CHECK // Might be in a tight loop PS_ASSERT_TREE_NON_NULL(tree, NULL); PS_ASSERT_INT_NONNEGATIVE(index, NULL); Index: /branches/pap_mops/psLib/src/types/psTree.h =================================================================== --- /branches/pap_mops/psLib/src/types/psTree.h (revision 25179) +++ /branches/pap_mops/psLib/src/types/psTree.h (revision 25180) @@ -16,4 +16,12 @@ } psTreeCoordArray; +/// Type of tree +/// +/// Specifies how distances are measured +typedef enum { + PS_TREE_EUCLIDEAN, // d^2 = dx^2 + dy^2 + ... + PS_TREE_SPHERICAL, // sin(dist/2)^2 = sin(dphi/2)^2 + cos(phi1)cos(phi2)sin(dlambda/2)^2 +} psTreeType; + /// A simple kd-tree implementation /// @@ -23,4 +31,5 @@ int dim; ///< Dimensionality int maxLeafContents; ///< Maximum number of points on a leaf + psTreeType type; ///< Type of tree long numNodes; ///< Number of nodes long numData; ///< Number of data points @@ -67,4 +76,5 @@ psTree *psTreeAlloc(int dim, ///< Dimensionality int maxLeafContents,///< Maximum number of points on a leaf + psTreeType type, ///< Type of tree long numNodes ///< Number of nodes in tree ); @@ -75,4 +85,5 @@ psTree *psTreePlant(int dim, ///< Dimensionality int maxLeafContents,///< Maximum number of points on a leaf + psTreeType type, ///< Type of tree ... ///< psVector for each coordinate ); @@ -108,4 +119,10 @@ ); +/// Return the index of all points within some radius of given coordinates +psVector *psTreeAllWithin(const psTree *tree, ///< Tree + const psVector *coordinates, ///< Coordinates of interest + double radius ///< Radius of interest + ); + /// Return the coordinates of a point in the tree, specified by its index psVector *psTreeCoords(psVector *out, ///< Output vector, or NULL