Index: trunk/psLib/src/sys/psType.h =================================================================== --- trunk/psLib/src/sys/psType.h (revision 25087) +++ trunk/psLib/src/sys/psType.h (revision 25256) @@ -141,5 +141,5 @@ } psDataType; -// macros to abstract the generic mask type : these values must be consistent +// macros to abstract the generic mask type : these values must be consistent #define PS_TYPE_MASK PS_TYPE_U8 /**< the psElemType to use for mask image */ #define PS_TYPE_MASK_DATA U8 /**< the data member to use for mask image */ @@ -152,8 +152,8 @@ // alternate versions if needed // #define PS_NOT_MASK(A)(UINT16_MAX-(A)) -// #define PS_NOT_MASK(A)(UINT32_MAX-(A)) +// #define PS_NOT_MASK(A)(UINT32_MAX-(A)) // #define PS_NOT_MASK(A)(UINT64_MAX-(A)) -// macros to abstract the vector mask type : these values must be consistent +// macros to abstract the vector mask type : these values must be consistent #define PS_TYPE_VECTOR_MASK PS_TYPE_U8 /**< the psElemType to use for mask image */ #define PS_TYPE_VECTOR_MASK_DATA U8 /**< the data member to use for mask image */ @@ -161,11 +161,11 @@ #define PS_MIN_VECTOR_MASK_TYPE 0 /**< minimum valid Vector Mask value */ #define PS_MAX_VECTOR_MASK_TYPE UINT8_MAX /**< maximum valid Vector Mask value */ -typedef psU8 psVectorMaskType; ///< the C datatype for a mask image +typedef psU8 psVectorMaskType; ///< the C datatype for a mask image #define PS_NOT_VECTOR_MASK(A)(UINT8_MAX-(A)) -// macros to abstract the image mask type : these values must be consistent -#define PS_TYPE_IMAGE_MASK PS_TYPE_U16 /**< the psElemType to use for mask image */ -#define PS_TYPE_IMAGE_MASK_DATA U16 /**< the data member to use for mask image */ -#define PS_TYPE_IMAGE_MASK_NAME "psU16" /**< the data type for mask as a string */ +// macros to abstract the image mask type : these values must be consistent +#define PS_TYPE_IMAGE_MASK PS_TYPE_U16 /**< the psElemType to use for mask image */ +#define PS_TYPE_IMAGE_MASK_DATA U16 /**< the data member to use for mask image */ +#define PS_TYPE_IMAGE_MASK_NAME "psU16" /**< the data type for mask as a string */ #define PS_MIN_IMAGE_MASK_TYPE 0 /**< minimum valid Image Mask value */ #define PS_MAX_IMAGE_MASK_TYPE UINT16_MAX /**< maximum valid Image Mask value */ @@ -246,16 +246,4 @@ }; -/// Macro to get the bad pixel reason code (stored as part of mask value) -#define PS_BADPIXEL_BITMASK 0x0f -#define PS_GET_BADPIXEL(maskValue) (maskValue & PS_BADPIXEL_BITMASK) - -#define PS_IS_BADPIXEL(maskValue) (PS_GET_BADPIXEL(maskValue) != 0) - -/// Macro to apply a bad pixel reason code to mask image -#define PS_SET_BADPIXEL(maskValue, reasonCode) \ -{ \ - maskValue = (psMaskType)((reasonCode & PS_BADPIXEL_BITMASK) | (maskValue & ~PS_BADPIXEL_BITMASK)); \ -} - /// Macro to determine if the psElemType is an integer. #define PS_IS_PSELEMTYPE_INT(x) ((x & 0x100) == 0x100) Index: trunk/psLib/src/types/psTree.c =================================================================== --- trunk/psLib/src/types/psTree.c (revision 25087) +++ trunk/psLib/src/types/psTree.c (revision 25256) @@ -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 @@ -145,8 +148,8 @@ long pow2; // Smallest power of two >= numData for (pow2 = 1; pow2 < numData; pow2 <<= 1); - numNodes = PS_MIN(pow2 - 1, 2 * numData - pow2 / 2 - 1); - } - - psTree *tree = psTreeAlloc(dim, maxLeafContents, numNodes); + numNodes = PS_MAX(PS_MIN(pow2 - 1, 2 * numData - pow2 / 2 - 1), 1); + } + + psTree *tree = psTreeAlloc(dim, maxLeafContents, type, numNodes); tree->data = psTreeCoordArrayAlloc(numData, dim); tree->numData = numData; @@ -199,4 +202,9 @@ } + if (numData <= maxLeafContents) { + // Don't need to do any more work + return tree; + } + psArray *work = psArrayAlloc(numNodes); // Work queue work->data[0] = root; @@ -365,5 +373,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 +411,41 @@ { 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: { + // Haversine formula + double dphi = coords->data.F64[1] - tree->data->F64[index][1]; + double sindphi = sin(dphi / 2.0); + double dlambda = coords->data.F64[0] - tree->data->F64[index][0]; + double sindlambda = sin(dlambda / 2.0); + return PS_SQR(sindphi) + + cos(coords->data.F64[1]) * cos(tree->data->F64[index][1]) * 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 +459,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 +530,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 +596,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 +631,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 +641,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 +650,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 +657,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 +674,5 @@ } // Leaf node - num += treeLeafSearchWithin(radius, tree, node, coords); + num += treeLeafSearchWithin(distance, tree, node, coords); work->data[workIndex] = NULL; workIndex--; @@ -618,28 +687,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 +716,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 +741,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 +830,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: trunk/psLib/src/types/psTree.h =================================================================== --- trunk/psLib/src/types/psTree.h (revision 25087) +++ trunk/psLib/src/types/psTree.h (revision 25256) @@ -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 Index: trunk/psLib/test/types/tap_psTree.c =================================================================== --- trunk/psLib/test/types/tap_psTree.c (revision 25087) +++ trunk/psLib/test/types/tap_psTree.c (revision 25256) @@ -3,11 +3,13 @@ #include "pstap.h" -#define NUM 10000 // Number of points +#define NUM 1000000 // Number of points +#define SPHERICAL_DISTANCE 3.0 // Distance of interest for spherical test int main(int argc, char *argv[]) { psLibInit(NULL); - plan_tests(6); + plan_tests(13); + // Euclidean geometry: 6 tests { psMemId id = psMemGetId(); @@ -23,5 +25,5 @@ psFree(rng); - psTree *tree = psTreePlant(2, 2, x, y); + psTree *tree = psTreePlant(2, 2, PS_TREE_EUCLIDEAN, x, y); ok(tree, "Tree planted"); @@ -35,5 +37,5 @@ long closeIndex = psTreeNearest(tree, coords); psFree(coords); - ok(closeIndex >= 0 && closeIndex < tree->numNodes, "found point: %ld", closeIndex); + ok(closeIndex >= 0 && closeIndex < tree->numNodes, "found closest point: %ld", closeIndex); long bestIndex = -1; @@ -68,4 +70,89 @@ } + // Spherical geometry: 7 tests + { + psMemId id = psMemGetId(); + + psVector *ra = psVectorAlloc(NUM, PS_TYPE_F64); + psVector *dec = psVectorAlloc(NUM, PS_TYPE_F64); + + psRandom *rng = psRandomAllocSpecific(PS_RANDOM_TAUS, 0); + for (int i = 0; i < NUM; i++) { + // Using http://mathworld.wolfram.com/SpherePointPicking.html + ra->data.F64[i] = psRandomUniform(rng); + dec->data.F64[i] = acos(2.0 * psRandomUniform(rng) - 1.0) - M_PI_2; + } + + psTree *tree = psTreePlant(2, 2, PS_TREE_SPHERICAL, ra, dec); + + ok(tree, "Tree planted"); + skip_start(!tree, 4, "tree died"); + { + // psTreePrint(stderr, tree); + + psVector *coords = psVectorAlloc(2, PS_TYPE_F64); + coords->data.F64[0] = psRandomUniform(rng); + coords->data.F64[1] = acos(2.0 * psRandomUniform(rng) - 1.0) - M_PI_2; + + psVector *indices = psTreeAllWithin(tree, coords, DEG_TO_RAD(SPHERICAL_DISTANCE)); + ok(indices && indices->type.type == PS_TYPE_S64, "got list of indices (%ld points)", indices->n); + long closeIndex = psTreeNearest(tree, coords); + ok(closeIndex >= 0 && closeIndex < tree->numNodes, "found closest point: %ld", closeIndex); + + ok(psVectorSortInPlace(indices), "sorted indices"); + + bool allgood = true; // All points in the appropriate place? + double bestDistance = INFINITY; // Distance to best point + long bestIndex = -1; // Index of best point + for (long i = 0, j = 0; i < NUM; i++) { +#if 1 + // Traditional formula + double dist = acos(sin(coords->data.F64[1]) * sin(dec->data.F64[i]) + + cos(coords->data.F64[1]) * cos(dec->data.F64[i]) * + cos(coords->data.F64[0] - ra->data.F64[i])); +#else + // Haversine formula: used in psTree + double dphi = coords->data.F64[1] - dec->data.F64[i]; + double sindphi = sin(dphi/2.0); + double dlambda = coords->data.F64[0] - ra->data.F64[i]; + double sindlambda = sin(dlambda/2.0); + double dist = 2.0 * asin(sqrt(PS_SQR(sindphi) + + cos(coords->data.F64[1]) * cos(dec->data.F64[i]) * + PS_SQR(sindlambda))); +#endif + if (dist < bestDistance) { + bestDistance = dist; + bestIndex = i; + } + if (i == indices->data.S64[j]) { + j++; + if (dist > DEG_TO_RAD(SPHERICAL_DISTANCE)) { + diag("%ld is in the list, but shouldn't be (%lf vs %lf)", + i, RAD_TO_DEG(dist), SPHERICAL_DISTANCE); + allgood = false; + } + } else if (dist <= DEG_TO_RAD(SPHERICAL_DISTANCE)) { + diag("%ld is not in the list, but should be (%lf vs %lf)", + i, RAD_TO_DEG(dist), SPHERICAL_DISTANCE); + allgood = false; + } + } + ok(allgood, "list is acurate"); + ok(bestIndex == closeIndex, "correct point: %ld vs %ld", closeIndex, bestIndex); + + psFree(coords); + psFree(indices); + + } + skip_end(); + + psFree(rng); + psFree(tree); + psFree(ra); + psFree(dec); + + ok(!psMemCheckLeaks(id, NULL, NULL, false), "no memory leaks"); + } + psLibFinalize(); }