Changeset 25180
- Timestamp:
- Aug 24, 2009, 5:38:47 PM (17 years ago)
- Location:
- branches/pap_mops/psLib/src/types
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
branches/pap_mops/psLib/src/types/psTree.c
r18344 r25180 14 14 #include "psTree.h" 15 15 16 //#define INPUT_CHECK // Check inputs for functions that may be in a tight loop? 17 16 18 17 19 // XXX Upgrades: … … 84 86 } 85 87 86 psTree *psTreeAlloc(int dim, int maxLeafContents, long numNodes)88 psTree *psTreeAlloc(int dim, int maxLeafContents, psTreeType type, long numNodes) 87 89 { 88 90 psAssert(dim > 0, "Dimensionality (%d) must be positive", dim); … … 95 97 tree->dim = dim; 96 98 tree->maxLeafContents = maxLeafContents; 99 tree->type = type; 97 100 98 101 tree->numNodes = numNodes; … … 115 118 116 119 117 psTree *psTreePlant(int dim, int maxLeafContents, ...)120 psTree *psTreePlant(int dim, int maxLeafContents, psTreeType type, ...) 118 121 { 119 122 PS_ASSERT_INT_POSITIVE(dim, NULL); … … 122 125 // Parse coordinate list 123 126 va_list args; // Variable argument list 124 va_start(args, maxLeafContents);127 va_start(args, type); 125 128 psArray *coords = psArrayAlloc(dim); // Array of coordinates 126 129 long numData = 0; // Number of data points … … 148 151 } 149 152 150 psTree *tree = psTreeAlloc(dim, maxLeafContents, numNodes);153 psTree *tree = psTreeAlloc(dim, maxLeafContents, type, numNodes); 151 154 tree->data = psTreeCoordArrayAlloc(numData, dim); 152 155 tree->numData = numData; … … 365 368 psTreeNode *psTreeLeaf(const psTree *tree, const psVector *coords) 366 369 { 367 #if 1// Might be in a tight loop370 #ifdef INPUT_CHECK // Might be in a tight loop 368 371 PS_ASSERT_TREE_NON_NULL(tree, NULL); 369 372 PS_ASSERT_VECTOR_NON_NULL(coords, NULL); … … 403 406 { 404 407 int dim = tree->dim; // Dimensionality 405 switch (dim) { 406 case 2: 407 return PS_SQR(coords->data.F64[0] - tree->data->F64[index][0]) + 408 PS_SQR(coords->data.F64[1] - tree->data->F64[index][1]); 409 case 3: 410 return PS_SQR(coords->data.F64[0] - tree->data->F64[index][0]) + 411 PS_SQR(coords->data.F64[1] - tree->data->F64[index][1]) + 412 PS_SQR(coords->data.F64[2] - tree->data->F64[index][2]); 413 default: { 414 double distance2 = 0.0; // Distance of interest 415 for (int i = 0; i < dim; i++) { 416 distance2 += PS_SQR(coords->data.F64[i] - tree->data->F64[index][i]); 408 switch (tree->type) { 409 case PS_TREE_EUCLIDEAN: 410 switch (dim) { 411 case 2: 412 return PS_SQR(coords->data.F64[0] - tree->data->F64[index][0]) + 413 PS_SQR(coords->data.F64[1] - tree->data->F64[index][1]); 414 case 3: 415 return PS_SQR(coords->data.F64[0] - tree->data->F64[index][0]) + 416 PS_SQR(coords->data.F64[1] - tree->data->F64[index][1]) + 417 PS_SQR(coords->data.F64[2] - tree->data->F64[index][2]); 418 default: { 419 double distance2 = 0.0; // Distance of interest 420 for (int i = 0; i < dim; i++) { 421 distance2 += PS_SQR(coords->data.F64[i] - tree->data->F64[index][i]); 422 } 423 return distance2; 417 424 } 418 return distance2; 419 } 420 } 425 } 426 break; 427 case PS_TREE_SPHERICAL: 428 switch (dim) { 429 case 2: { 430 double dphi = coords->data.F64[0] - tree->data->F64[index][0]; 431 double dlambda = coords->data.F64[1] - tree->data->F64[index][1]; 432 double sindphi = sin(dphi / 2.0); 433 double sindlambda = sin(dlambda / 2.0); 434 return PS_SQR(sindphi) + 435 cos(coords->data.F64[0]) * cos(coords->data.F64[0]) * PS_SQR(sindlambda); 436 } 437 default: 438 psAbort("Spherical distances not supported for more than 2 dimensions"); 439 } 440 default: 441 psAbort("Unrecognised type: %x", tree->type); 442 } 443 421 444 return NAN; 422 445 } … … 430 453 double minDiff = tree->min->F64[index][i] - coords->data.F64[i]; 431 454 if (minDiff > 0) { 432 distance += PS_SQR(minDiff); 455 switch (tree->type) { 456 case PS_TREE_EUCLIDEAN: 457 distance += PS_SQR(minDiff); 458 break; 459 case PS_TREE_SPHERICAL: { 460 double sinDiff = sin(minDiff / 2.0); 461 distance += PS_SQR(sinDiff); 462 break; 463 } 464 default: 465 psAbort("Unrecognised type: %x", tree->type); 466 } 433 467 continue; 434 468 } 435 469 double maxDiff = coords->data.F64[i] - tree->max->F64[index][i]; 436 470 if (maxDiff > 0) { 437 distance += PS_SQR(maxDiff); 471 switch (tree->type) { 472 case PS_TREE_EUCLIDEAN: 473 distance += PS_SQR(maxDiff); 474 break; 475 case PS_TREE_SPHERICAL: { 476 double sinDiff = sin(maxDiff / 2.0); 477 distance += PS_SQR(sinDiff); 478 break; 479 } 480 default: 481 psAbort("Unrecognised type: %x", tree->type); 482 } 438 483 continue; 439 484 } … … 479 524 } 480 525 481 // Return the index of the nearest neighbour to given coordinates, within some radius526 // Return the index of the nearest neighbour to given coordinates, within some distance measure 482 527 // This is the engine for psTreeNearest() and psTreeNearestWithin() 483 528 static inline long treeNearestWithin(const psTree *tree, // Tree 484 529 const psVector *coordinates, // Coordinates of interest 485 double bestDistance // Distance (radius-squared)to best point530 double bestDistance // Distance measure to best point 486 531 ) 487 532 { 488 #if 1// Might be in a tight loop533 #ifdef INPUT_CHECK // Might be in a tight loop 489 534 PS_ASSERT_TREE_NON_NULL(tree, -1); 490 535 PS_ASSERT_VECTOR_NON_NULL(coordinates, -1); … … 545 590 546 591 592 // Convert a radius to our internal "distance measure" 593 // Often, we're given a search radius, but for efficiency reasons, we don't use that internally. 594 static double treeRadiusToDistance(const psTree *tree, double radius) 595 { 596 switch (tree->type) { 597 case PS_TREE_EUCLIDEAN: 598 // Using the square of the distance as the distance measure 599 return PS_SQR(radius); 600 case PS_TREE_SPHERICAL: { 601 // Using a rearrangement of the Haversine formula 602 double sindist = sin(radius / 2.0); 603 return PS_SQR(sindist); 604 } 605 default: 606 psAbort("Unrecognised type: %x", tree->type); 607 } 608 } 609 610 547 611 long psTreeNearestWithin(const psTree *tree, const psVector *coords, double radius) 548 612 { 549 return treeNearestWithin(tree, coords, PS_SQR(radius));550 } 551 552 553 // Search a leaf node for points within radius squared554 static inline long treeLeafSearchWithin(double radius2, // Radius squaredto search613 return treeNearestWithin(tree, coords, treeRadiusToDistance(tree, radius)); 614 } 615 616 617 // Search a leaf node for points within distance 618 static inline long treeLeafSearchWithin(double distance, // Distance to search 555 619 const psTree *tree, // Tree of interest 556 620 const psTreeNode *leaf, // Leaf to search … … 561 625 for (int i = 0; i < leaf->num; i++) { 562 626 long index = leaf->contents[i]; // Index of point 563 double distance = treeContentDistance(tree, index, coords); // Distance to point 564 if (distance < radius2) { 627 if (treeContentDistance(tree, index, coords) < distance) { 565 628 num++; 566 629 } … … 572 635 long psTreeWithin(const psTree *tree, const psVector *coordinates, double radius) 573 636 { 574 #if 1// Might be in a tight loop637 #ifdef INPUT_CHECK // Might be in a tight loop 575 638 PS_ASSERT_TREE_NON_NULL(tree, -1); 576 639 PS_ASSERT_VECTOR_NON_NULL(coordinates, -1); … … 581 644 psVectorCopy(NULL, coordinates, PS_TYPE_F64)); // F64 version of coordinates 582 645 583 radius *= radius; // We work with the radius-squared646 double distance = treeRadiusToDistance(tree, radius); // Distance measure 584 647 long num = 0; // Number of points in circle 585 648 … … 588 651 // Find the closest point in the leaf that contains the point of interest 589 652 psTreeNode *leaf = psTreeLeaf(tree, coords); // Leaf containing the point of interest 590 num += treeLeafSearchWithin( radius, tree, leaf, coords);653 num += treeLeafSearchWithin(distance, tree, leaf, coords); 591 654 592 655 psArray *work = psArrayAlloc(tree->numNodes); // Work queue … … 605 668 } 606 669 // Leaf node 607 num += treeLeafSearchWithin( radius, tree, node, coords);670 num += treeLeafSearchWithin(distance, tree, node, coords); 608 671 work->data[workIndex] = NULL; 609 672 workIndex--; … … 618 681 } 619 682 620 // Search a leaf node for any points within radius squared 621 static inline bool treeLeafSearchWithinAny(double radius2, // Radius squared to search 622 const psTree *tree, // Tree of interest 623 const psTreeNode *leaf, // Leaf to search 624 const psVector *coords // Coordinates of interest 683 // Search a leaf node for points within distance 684 static inline void treeLeafSearchAllWithin(psVector *result, // Result vector 685 double distance, // Distance to search 686 const psTree *tree, // Tree of interest 687 const psTreeNode *leaf, // Leaf to search 688 const psVector *coords // Coordinates of interest 625 689 ) 626 690 { 627 691 for (int i = 0; i < leaf->num; i++) { 628 692 long index = leaf->contents[i]; // Index of point 629 double distance = treeContentDistance(tree, index, coords); // Distance to point 630 if (distance < radius2) { 631 return true; 632 } 633 } 634 return false; 635 } 636 637 // Given an arbitrary point and a matching radius, return whether there are any points within that radius 638 bool psTreeWithinAny(const psTree *tree, const psVector *coordinates, double radius) 639 { 640 #if 1 // Might be in a tight loop 641 PS_ASSERT_TREE_NON_NULL(tree, false); 642 PS_ASSERT_VECTOR_NON_NULL(coordinates, false); 643 PS_ASSERT_VECTOR_SIZE(coordinates, (long)tree->dim, false); 693 if (treeContentDistance(tree, index, coords) < distance) { 694 psVectorAppend(result, index); 695 } 696 } 697 return; 698 } 699 700 // Given an arbitrary point and a matching radius, return the index of all points within that radius 701 psVector *psTreeAllWithin(const psTree *tree, const psVector *coordinates, double radius) 702 { 703 #ifdef INPUT_CHECK // Might be in a tight loop 704 PS_ASSERT_TREE_NON_NULL(tree, NULL); 705 PS_ASSERT_VECTOR_NON_NULL(coordinates, NULL); 706 PS_ASSERT_VECTOR_SIZE(coordinates, (long)tree->dim, NULL); 644 707 #endif 645 708 … … 647 710 psVectorCopy(NULL, coordinates, PS_TYPE_F64)); // F64 version of coordinates 648 711 649 radius *= radius; // We work with the radius-squared 650 651 // This is essentially the same as psTreeWithin, except we can bail as soon as we find something 712 double distance = treeRadiusToDistance(tree, radius); // Distance measure 713 714 psVector *result = psVectorAllocEmpty(4, PS_TYPE_S64); // Indices of points within match radius 715 716 // This is essentially the same as psTreeNearest, except we're not allowed to prune 652 717 653 718 // Find the closest point in the leaf that contains the point of interest 654 719 psTreeNode *leaf = psTreeLeaf(tree, coords); // Leaf containing the point of interest 655 if (treeLeafSearchWithinAny(radius, tree, leaf, coords)) { 656 psFree(coords); 657 return true; 658 } 720 treeLeafSearchAllWithin(result, distance, tree, leaf, coords); 659 721 660 722 psArray *work = psArrayAlloc(tree->numNodes); // Work queue … … 673 735 } 674 736 // Leaf node 675 if (treeLeafSearchWithinAny(radius, tree, node, coords)) { 737 treeLeafSearchAllWithin(result, distance, tree, node, coords); 738 work->data[workIndex] = NULL; 739 workIndex--; 740 } 741 742 leaf = up; 743 } 744 psFree(work); 745 psFree(coords); 746 747 return result; 748 } 749 750 // Search a leaf node for any points within distance measure 751 static inline bool treeLeafSearchWithinAny(double distance, // Distance to search 752 const psTree *tree, // Tree of interest 753 const psTreeNode *leaf, // Leaf to search 754 const psVector *coords // Coordinates of interest 755 ) 756 { 757 for (int i = 0; i < leaf->num; i++) { 758 long index = leaf->contents[i]; // Index of point 759 if (treeContentDistance(tree, index, coords) < distance) { 760 return true; 761 } 762 } 763 return false; 764 } 765 766 // Given an arbitrary point and a matching radius, return whether there are any points within that radius 767 bool psTreeWithinAny(const psTree *tree, const psVector *coordinates, double radius) 768 { 769 #ifdef INPUT_CHECK // Might be in a tight loop 770 PS_ASSERT_TREE_NON_NULL(tree, false); 771 PS_ASSERT_VECTOR_NON_NULL(coordinates, false); 772 PS_ASSERT_VECTOR_SIZE(coordinates, (long)tree->dim, false); 773 #endif 774 775 psVector *coords = (coordinates->type.type == PS_TYPE_F64 ? psMemIncrRefCounter((psVector*)coordinates) : 776 psVectorCopy(NULL, coordinates, PS_TYPE_F64)); // F64 version of coordinates 777 778 double distance = treeRadiusToDistance(tree, radius); // Distance measure 779 780 // This is essentially the same as psTreeWithin, except we can bail as soon as we find something 781 782 // Find the closest point in the leaf that contains the point of interest 783 psTreeNode *leaf = psTreeLeaf(tree, coords); // Leaf containing the point of interest 784 if (treeLeafSearchWithinAny(distance, tree, leaf, coords)) { 785 psFree(coords); 786 return true; 787 } 788 789 psArray *work = psArrayAlloc(tree->numNodes); // Work queue 790 while (leaf->up) { 791 psTreeNode *up = leaf->up; // Parent node 792 793 long workIndex = 0; 794 work->data[workIndex] = (leaf == up->left ? up->right : up->left); // The other node 795 while (workIndex >= 0) { 796 psTreeNode *node = work->data[workIndex]; 797 if (node->left) { 798 // Branch node 799 work->data[workIndex] = node->right; 800 work->data[++workIndex] = node->left; 801 continue; 802 } 803 // Leaf node 804 if (treeLeafSearchWithinAny(distance, tree, node, coords)) { 676 805 // Clear out the work queue 677 806 memset(work->data, 0, (workIndex + 1) * sizeof(void)); … … 695 824 psVector *psTreeCoords(psVector *out, const psTree *tree, long index) 696 825 { 697 #if 1// Might be in a tight loop826 #ifdef INPUT_CHECK // Might be in a tight loop 698 827 PS_ASSERT_TREE_NON_NULL(tree, NULL); 699 828 PS_ASSERT_INT_NONNEGATIVE(index, NULL); -
branches/pap_mops/psLib/src/types/psTree.h
r19539 r25180 16 16 } psTreeCoordArray; 17 17 18 /// Type of tree 19 /// 20 /// Specifies how distances are measured 21 typedef enum { 22 PS_TREE_EUCLIDEAN, // d^2 = dx^2 + dy^2 + ... 23 PS_TREE_SPHERICAL, // sin(dist/2)^2 = sin(dphi/2)^2 + cos(phi1)cos(phi2)sin(dlambda/2)^2 24 } psTreeType; 25 18 26 /// A simple kd-tree implementation 19 27 /// … … 23 31 int dim; ///< Dimensionality 24 32 int maxLeafContents; ///< Maximum number of points on a leaf 33 psTreeType type; ///< Type of tree 25 34 long numNodes; ///< Number of nodes 26 35 long numData; ///< Number of data points … … 67 76 psTree *psTreeAlloc(int dim, ///< Dimensionality 68 77 int maxLeafContents,///< Maximum number of points on a leaf 78 psTreeType type, ///< Type of tree 69 79 long numNodes ///< Number of nodes in tree 70 80 ); … … 75 85 psTree *psTreePlant(int dim, ///< Dimensionality 76 86 int maxLeafContents,///< Maximum number of points on a leaf 87 psTreeType type, ///< Type of tree 77 88 ... ///< psVector for each coordinate 78 89 ); … … 108 119 ); 109 120 121 /// Return the index of all points within some radius of given coordinates 122 psVector *psTreeAllWithin(const psTree *tree, ///< Tree 123 const psVector *coordinates, ///< Coordinates of interest 124 double radius ///< Radius of interest 125 ); 126 110 127 /// Return the coordinates of a point in the tree, specified by its index 111 128 psVector *psTreeCoords(psVector *out, ///< Output vector, or NULL
Note:
See TracChangeset
for help on using the changeset viewer.
