00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101 #ifndef VRS_SPACETREE_H_
00102 #define VRS_SPACETREE_H_
00103
00104 #include <vrs/container/array.h>
00105 #include <vrs/container/iterator.h>
00106 #include <vrs/container/staticarray.h>
00107 #include <vrs/transactionno.h>
00108
00109 namespace VRS {
00110
00111 template<UINT DIM> class Point;
00112 template<UINT DIM> class Cell;
00113 template<typename Content, UINT DIM> class StaticSpaceTree;
00114 template<typename Content, UINT DIM> class DynamicSpaceTree;
00115
00189
00190 template<UINT DIM> class Point {
00191
00192 public:
00193
00194 Point();
00195 Point(double x);
00196 Point(double x, double y);
00197 Point(double x, double y, double z);
00198
00199 double& operator[](UINT index);
00200 const double& operator[](UINT index) const;
00201 const Point& operator/(double d);
00202 Point operator+(const Point p) const;
00203
00204 bool isContainedIn(const Cell<DIM>& cell) const;
00205 bool intersects(const Cell<DIM>& cell) const;
00206
00207 private:
00208
00209 double coords_[DIM];
00210 };
00211
00212 template<UINT DIM> std::ostream& operator<<(std::ostream& s, const Point<DIM>& p);
00213
00214
00218 template<UINT DIM> class Cell {
00219
00220 public:
00221
00222 Cell();
00223 Cell(const Point<DIM>& pmin, const Point<DIM>& pmax);
00224
00225 void setCell(const Point<DIM>& pmin, const Point<DIM>& pmax);
00226
00227 double getExtend(UINT index) const;
00228 Point<DIM> getExtend() const;
00229 const Point<DIM>& getMinPoint() const { return pmin_; }
00230 const Point<DIM>& getMaxPoint() const { return pmax_; }
00231
00232 void setExtend(UINT index, double w);
00233 void setExtend(const Point<DIM>& p);
00234 void setMinPoint(const Point<DIM>& p) { pmin_ = p; }
00235 void setMaxPoint(const Point<DIM>& p) { pmax_ = p; }
00236 void setPosition(const Point<DIM>& p);
00237 Point<DIM> getMidpoint() const;
00238
00239 const Cell& operator=(const Cell& other);
00240 bool isContainedIn(const Cell& cell) const;
00241 bool intersects(const Cell& cell) const;
00242 bool empty() const;
00243
00244 private:
00245
00246 Point<DIM> pmin_;
00247 Point<DIM> pmax_;
00248 };
00249
00250 template<UINT DIM> std::ostream& operator<<(std::ostream& out, const Cell<DIM>& cell);
00251
00252 typedef unsigned char ChildNumber;
00263 template<UINT DIM> class SpaceTreePath {
00264
00265 public:
00266 SpaceTreePath();
00267 SpaceTreePath(const SpaceTreePath<DIM>& other);
00268 SpaceTreePath(Iterator<ChildNumber>* childNumbers);
00269
00270 SpaceTreePath getChild(ChildNumber childNumber) const;
00272 SpaceTreePath getParent() const;
00275 ChildNumber getChildNumber() const;
00279 bool getNeighbor(UINT directionBitCode);
00313 Iterator<SpaceTreePath>* getNeighbors() const;
00318 bool getMainAxisNeighbor(UINT direction);
00327 bool isRoot() const;
00328 UINT getLength() const;
00329 bool valid() const;
00330
00331 Iterator<ChildNumber>* newIterator() const;
00332
00333 const SpaceTreePath& operator=(const SpaceTreePath& other);
00334 bool operator==(const SpaceTreePath& other) const;
00335 bool operator<(const SpaceTreePath& other) const;
00336 bool operator!=(const SpaceTreePath& other) const;
00337
00338 static Iterator<SpaceTreePath>* newLevelIterator(UINT level);
00339
00340 static SO<Iterator<SpaceTreePath> > newSubTreeLevelIterator(SpaceTreePath subTreeRoot, unsigned int level);
00352 static const UINT Left;
00353 static const UINT Right;
00354 static const UINT Down;
00355 static const UINT Up;
00356 static const UINT Front;
00357 static const UINT Back;
00358
00359 static const ChildNumber MaxChilds;
00360 static const ChildNumber InvalidChildNumber;
00361 static const SpaceTreePath InvalidPath;
00362
00363 static UINT MaxPathLength() { return (32/DIM)-1; }
00364
00365
00369 SpaceTreePath(UINT pathCode);
00370 UINT getPathCode() const;
00371 void setPathCode(UINT c);
00372
00373 private:
00374
00375 UINT pathCode_;
00376 };
00377
00378 template<UINT DIM> std::ostream& operator<<(std::ostream& s, const SpaceTreePath<DIM>& path);
00379
00380
00381
00393 template<typename NODE, UINT DIM> class SpaceTree : public SharedObj {
00394
00395 public:
00396
00397 typedef SpaceTreePath<DIM> Path;
00398
00441
00442
00443
00444 virtual Point<DIM> findNearMainCellPoint(const Point<DIM>& p) const;
00445
00446
00447 class NodePointer;
00461 virtual NodePointer getRoot() = 0;
00462 virtual NodePointer getParent(const NodePointer& node) = 0;
00463 virtual NodePointer getChild(const NodePointer& node, ChildNumber childNumber) = 0;
00464 virtual ChildNumber getChildNumber(const NodePointer& node) const = 0;
00465
00469 virtual SpaceTreePath<DIM> getPath(const NodePointer& node) const;
00470 virtual NodePointer getNode(const SpaceTreePath<DIM>& path);
00471 virtual SpaceTreePath<DIM> cropPath(const SpaceTreePath<DIM>& path) const;
00476 virtual Cell<DIM> getCell(const NodePointer& node) const;
00477 virtual Cell<DIM> getCell(const SpaceTreePath<DIM>& path) const;
00478
00481 NodePointer getNeighbor(const NodePointer& node, UINT direction);
00482 Iterator<NodePointer>* getNeighbors(NodePointer node);
00483
00489 const TransactionNo& getTransactionNumber() const;
00490
00491 VRS_TYPEINFO(SpaceTree, SharedObj);
00492 VRS_SERIALIZABLE_ABSTRACT_CLASS(SpaceTree);
00493
00494 protected:
00495 SpaceTree() : m_transactionNo(0) { }
00496
00498 const NodePointer getRoot() const;
00499 const NodePointer getParent(const NodePointer& node) const;
00500 const NodePointer getChild(const NodePointer& node, ChildNumber childNumber) const;
00501
00503 NodePointer createNodePointer(NODE* node);
00504 NODE* getPointer(const NodePointer& pointer);
00505 const NODE* getPointer(const NodePointer& pointer) const;
00506
00507
00508 virtual Cell<DIM> getSubCell(const NodePointer& node, const Cell<DIM>& cell,
00509 ChildNumber childNumber) const;
00516 Cell<DIM> mainCell_;
00517
00518 static const ChildNumber MaxChilds;
00519 static const ChildNumber InvalidChildNumber;
00520
00522 void invalidatePointers();
00523 TransactionNo m_transactionNo;
00524
00525 private:
00526
00527 template<typename POINTSET> class DefaultInclusionTest {
00528 public:
00529 DefaultInclusionTest() {}
00530 bool operator()(const POINTSET& pointSet, const Cell<DIM>& cell) {
00531 return pointSet.isContainedIn(cell);
00532 }
00533 };
00534
00535 template<typename POINTSET> class DefaultIntersectionTest {
00536 public:
00537 DefaultIntersectionTest() {}
00538 bool operator()(const POINTSET& pointSet, const Cell<DIM>& cell) {
00539 return pointSet.intersects(cell);
00540 }
00541 };
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555 public:
00556
00557 class NodePointer {
00558 public:
00559 NodePointer() : m_transactionNo(0), pointer_(0) { }
00560 const NodePointer& operator=(const NodePointer& other) {
00561 m_transactionNo = other.m_transactionNo;
00562 pointer_ = other.pointer_;
00563 tree_ = other.tree_;
00564 return *this;
00565 }
00566
00567 bool operator==(const NodePointer& other) const { return (m_transactionNo==other.m_transactionNo && pointer_==other.pointer_ && tree_==other.tree_); }
00568 bool operator!=(const NodePointer& other) const { return !operator==(other); }
00569 bool operator<(const NodePointer& other) const { return pointer_<other.pointer_; }
00570
00571 NODE& operator*() { VRS_InternalError(valid(), "invalid node pointer dereferenced"); return *pointer_; }
00572 NODE* operator->() { VRS_InternalError(valid(), "invalid node pointer dereferenced"); return pointer_; }
00573 const NODE& operator*() const { VRS_InternalError(valid(), "invalid node pointer dereferenced"); return *pointer_; }
00574 const NODE* operator->() const { VRS_InternalError(valid(), "invalid node pointer dereferenced"); return pointer_; }
00575 bool valid() const { if (tree_==0 || pointer_==0) return false; return (m_transactionNo==tree_->getTransactionNumber()); }
00576
00577 static const NodePointer& InvalidNodePointer() { static const NodePointer np = NodePointer(TransactionNo(0), 0, 0); return np; }
00578
00579 private:
00580 friend class SpaceTree<NODE, DIM>;
00581 NodePointer(const TransactionNo& tn, NODE* p, SpaceTree* tree) : m_transactionNo(tn), pointer_(p), tree_(tree) {}
00582 mutable TransactionNo m_transactionNo;
00583 mutable NODE* pointer_;
00584 mutable SO<SpaceTree> tree_;
00585 };
00586
00587
00588 template<typename POINTSET,typename INCLUSIONTEST> SpaceTreePath<DIM> findSmallestEnclosingNode(
00589 const POINTSET& pointSet,
00590 UINT maxLevel,
00591 bool mustExist,
00592 INCLUSIONTEST test) {
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602 VRS_InternalError(!mainCell_.empty(), "mainCell_ must not be empty");
00603 UINT maxLength = Path::MaxPathLength();
00604 if (maxLevel>maxLength) maxLevel = maxLength;
00605
00606
00607 if (!test.operator()(pointSet, mainCell_)) {
00608 return SpaceTreePath<DIM>::InvalidPath;
00609 }
00610
00611
00612 NodePointer node = (mustExist ? getRoot() : NodePointer::InvalidNodePointer());
00613 if (!node.valid() && mustExist) {
00614 return SpaceTreePath<DIM>::InvalidPath;
00615 }
00616 Cell<DIM> cell = mainCell_;
00617 SpaceTreePath<DIM> path;
00618
00619 for (UINT i=0; i<maxLevel; i++) {
00620 bool enclosingChildFound = false;
00621
00622
00623 for (ChildNumber childNum=0; childNum<MaxChilds; childNum++) {
00624 Cell<DIM> subCell = getSubCell(node, cell, childNum);
00625 if (test.operator()(pointSet, subCell)) {
00626
00627 if (mustExist) {
00628 NodePointer child = getChild(node, childNum);
00629 if (!child.valid()) {
00630 continue;
00631 }
00632 else node = getChild(node, childNum);
00633 }
00634
00635 cell = subCell;
00636 path = path.getChild(childNum);
00637 enclosingChildFound = true;
00638 break;
00639 }
00640 }
00641
00642 if (!enclosingChildFound) return path;
00643 }
00644 return path;
00645 }
00646 template<typename POINTSET> SpaceTreePath<DIM> findSmallestEnclosingNode(
00647 const POINTSET& pointSet,
00648 UINT maxLevel = 32,
00649 bool mustExist = true) {
00650 return findSmallestEnclosingNode(pointSet, maxLevel, mustExist, DefaultInclusionTest<POINTSET>());
00651 }
00652
00653
00654 struct STNODEInfo {
00655 STNODEInfo(const NodePointer& node, const Cell<DIM>& cell, const SpaceTreePath<DIM>& path) :
00656 node_(node), cell_(cell), path_(path) {}
00657 NodePointer node_;
00658 Cell<DIM> cell_;
00659 SpaceTreePath<DIM> path_;
00660 };
00661
00662 Iterator<SpaceTreePath<DIM> >* emptyIterator() {
00663 static SO<NonPersistentStaticArray<SpaceTreePath<DIM> > > empty =
00664 new NonPersistentStaticArray<SpaceTreePath<DIM> >(static_cast<UINT>(0));
00665 return empty->newIterator();
00666 }
00667
00668 template<typename POINTSET, typename INTERSECTIONTEST> Iterator<SpaceTreePath<DIM> >* findIntersectingNodes(
00669 const POINTSET& pointSet,
00670 UINT level,
00671 bool mustExist,
00672 INTERSECTIONTEST test) {
00673 VRS_InternalError(!mainCell_.empty(), "mainCell_ must not be 0");
00674 VRS_CheckArg(level<=Path::MaxPathLength(), "level must not exceed Path::maxPathLength");
00675
00676
00677 if (!test.operator()(pointSet, mainCell_)) return emptyIterator();
00678
00679
00680 NodePointer root = getRoot();
00681 if (!root.valid() && mustExist) return emptyIterator();
00682
00683 SO<NonPersistentArray<STNODEInfo> > stack1 = new NonPersistentArray<STNODEInfo>;
00684 SO<NonPersistentArray<STNODEInfo> > stack2 = new NonPersistentArray<STNODEInfo>;
00685 NonPersistentArray<STNODEInfo>* sp1 = stack1;
00686 NonPersistentArray<STNODEInfo>* sp2 = stack2;
00687 stack1->append(STNODEInfo(root, mainCell_, SpaceTreePath<DIM>()));
00688
00689
00690
00691 for (UINT curLevel=0; curLevel<level; curLevel++) {
00692 sp2->clear();
00693 for(unsigned int i=0; i<sp1->size(); i++) {
00694
00695 STNODEInfo nwc = (*sp1)[i];
00696 NodePointer node = nwc.node_;
00697 const Cell<DIM>& cell = nwc.cell_;
00698 SpaceTreePath<DIM> path = nwc.path_;
00699
00700
00701 for (ChildNumber childNum = 0; childNum<MaxChilds; childNum++) {
00702 Cell<DIM> subCell = getSubCell(node, cell, childNum);
00703
00704 if (test.operator()(pointSet, subCell)) {
00705 NodePointer childNode = (node.valid() ?
00706 getChild(node, childNum) :
00707 NodePointer::InvalidNodePointer());
00708
00709 if (childNode.valid() || !mustExist) {
00710
00711 SpaceTreePath<DIM> childPath = path.getChild(childNum);
00712
00713 sp2->append(STNODEInfo(childNode, subCell, childPath));
00714 }
00715 }
00716 }
00717 }
00718 if (sp2->isEmpty()) return emptyIterator();
00719 std::swap(sp1, sp2);
00720 }
00721
00722
00723
00724 SO<Iterator<STNODEInfo> > infos = sp1->newIterator();
00725 NonPersistentStaticArray<SpaceTreePath<DIM> >* paths = new NonPersistentStaticArray<SpaceTreePath<DIM> >(sp1->size());
00726
00727 for (UINT i=0; i<infos->size(); i++) {
00728 (*paths)[i] = infos->get(i).path_;
00729 }
00730
00731 return paths->newIterator();
00732 }
00733 template<typename POINTSET> Iterator<SpaceTreePath<DIM> >* findIntersectingNodes(
00734 const POINTSET& pointSet,
00735 UINT level,
00736 bool mustExist = true) {
00737 return findIntersectingNodes(pointSet, level, mustExist, DefaultIntersectionTest<POINTSET>());
00738 }
00739
00740
00741
00742
00743
00744 template<typename POINTSET, typename INCLUSIONTEST> Iterator<SpaceTreePath<DIM> >* findEnclosingNodes(
00745 const POINTSET& pointSet,
00746 UINT level,
00747 bool mustExist,
00748 INCLUSIONTEST test) {
00749
00750
00751
00752 VRS_CheckArg(level<=Path::MaxPathLength(), "level must not exceed Path::maxPathLength");
00753 return findIntersectingNodes(pointSet, level, mustExist, test);
00754 }
00755 template<typename POINTSET> Iterator<SpaceTreePath<DIM> >* findEnclosingNodes(
00756 const POINTSET& cell,
00757 UINT level,
00758 bool mustExist = true) {
00759 return findEnclosingNodes(cell, level, mustExist, DefaultInclusionTest<POINTSET>());
00760 }
00761 };
00762
00763 template<typename CONTENT> class StaticNode {
00764 public:
00765 CONTENT content;
00766 VRS_SERIALIZABLE_NO_SO_CLASS(StaticNode);
00767 };
00768
00769 template<typename CONTENT, UINT DIM> class DynamicNode {
00770
00771 public:
00772
00773 CONTENT content;
00774 VRS_SERIALIZABLE_NO_SO_CLASS(DynamicNode);
00775
00776 private:
00777
00778 DynamicNode(const DynamicNode* parent, ChildNumber childNumber);
00779 ~DynamicNode();
00780 DynamicNode* getChild(ChildNumber childNumber) const;
00781 ChildNumber getChildNumber() const;
00782 DynamicNode* getParent() const;
00783 bool createChild(ChildNumber childNumber) const;
00784 bool deleteChild(ChildNumber childNumber) const;
00785 UINT countChilds() const;
00786
00787
00788 mutable DynamicNode* parent_;
00789 mutable DynamicNode* childs_[1<<DIM];
00790 ChildNumber childNumber_;
00791
00792 friend class DynamicSpaceTree<CONTENT, DIM>;
00793 DynamicNode();
00794 };
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804
00805
00806
00807
00808
00813 template<typename CONTENT, UINT DIM> class DynamicSpaceTree :
00814 public SpaceTree<DynamicNode<CONTENT, DIM>, DIM> {
00815
00816 public:
00817 typedef SpaceTree<DynamicNode<CONTENT, DIM>, DIM> BaseClass;
00818 VRS_TYPEINFO(DynamicSpaceTree, BaseClass);
00819 VRS_SERIALIZABLE(DynamicSpaceTree);
00820
00821 typedef typename SpaceTree<DynamicNode<CONTENT, DIM>, DIM>::NodePointer NodePointer;
00822 typedef SpaceTreePath<DIM> Path;
00823
00824 DynamicSpaceTree(const Cell<DIM>& mainCell);
00825 virtual ~DynamicSpaceTree();
00826
00827 bool createChild(const NodePointer& node, ChildNumber childNumber);
00828 NodePointer createNode(const SpaceTreePath<DIM>& path);
00831 void deleteNode(const NodePointer& node);
00832
00833 UINT getSize() const;
00836 int getDepth() const;
00837
00839 virtual NodePointer getRoot();
00840 virtual NodePointer getParent(const NodePointer& node);
00841 virtual NodePointer getChild(const NodePointer& node, ChildNumber childNumber);
00842 virtual ChildNumber getChildNumber(const NodePointer& node) const;
00843
00844 typedef DynamicNode<CONTENT, DIM> Node;
00845 Iterator<NodePointer>* leafIterator(Path path = Path()) const;
00850 protected:
00851 DynamicSpaceTree();
00852
00853 private:
00854 DynamicNode<CONTENT, DIM>* root_;
00855 UINT size_;
00856 };
00857
00864 template<typename CONTENT, UINT DIM> class StaticSpaceTree :
00865 public SpaceTree<StaticNode<CONTENT>, DIM> {
00866
00867 public:
00868 typedef SpaceTree<StaticNode<CONTENT>, DIM> BaseClass;
00869 VRS_TYPEINFO(StaticSpaceTree, BaseClass);
00870 VRS_SERIALIZABLE(StaticSpaceTree);
00871
00872 typedef typename SpaceTree<StaticNode<CONTENT>, DIM>::NodePointer NodePointer;
00873 typedef SpaceTreePath<DIM> Path;
00874
00875 StaticSpaceTree(const Cell<DIM>& mainCell, UINT depth);
00876 virtual ~StaticSpaceTree();
00877
00878 UINT getSize() const;
00879 UINT getDepth() const;
00880
00882 virtual NodePointer getRoot();
00883 virtual NodePointer getParent(const NodePointer& node);
00884 virtual NodePointer getChild(const NodePointer& node, ChildNumber childNumber);
00885 virtual ChildNumber getChildNumber(const NodePointer& node) const;
00886
00888 virtual SpaceTreePath<DIM> getPath(const NodePointer& node) const;
00889 virtual NodePointer getNode(const SpaceTreePath<DIM>& path);
00890
00891 typedef StaticNode<CONTENT> Node;
00892
00893 protected:
00894 StaticSpaceTree(UINT depth);
00895
00896 private:
00897
00898 StaticSpaceTree();
00899
00900 int depth_;
00901 unsigned int size_;
00902
00903 StaticNode<CONTENT>* nodes_;
00904 };
00905
00906 }
00907
00908 #include "spacetree.tli"
00909
00910 #endif // VRS_SPACETREE_H_
00911