00001 #ifndef STAR_STBITREE
00002 #define STAR_STBITREE
00003
00004 #include <vector>
00005 #include <iterator>
00006 #include <cassert>
00007 #include <math.h>
00008 #include <iostream>
00009 #include <iomanip>
00010
00011 #include "StBiTreeIter.h"
00012
00013 namespace std {
00014
00015 template <class Key>
00016 class StBiTree
00017 {
00018 public:
00019 enum EBranch {kLeft, kRight, kLeaf, kEmpty};
00020 private:
00021 StBiTree *mParent;
00022 std::vector<StBiTree *>mBranches;
00023 Key mParameters;
00024 bool mIsLeaf;
00025 protected:
00026 void Insert(StBiTree *node);
00027 void Insert(const Key &data,bool leaf=false);
00028 int Reparent(StBiTree *newParent);
00029 void SetParent(StBiTree *node);
00030 void SetLeaf(bool leaf=true) { mIsLeaf = leaf; }
00031 public:
00032
00041
00042 StBiTree() :mParent(), mBranches(kLeaf),mParameters(), mIsLeaf(false)
00043 { mBranches[kLeft] =0; mBranches[kRight]=0; }
00044
00053
00054 StBiTree(const Key &mParameters, bool isLeaf=true)
00055 :mParent(), mBranches(kLeaf),mParameters(mParameters) , mIsLeaf(isLeaf)
00056 { mBranches[kLeft] =0; mBranches[kRight]=0; }
00057
00058 StBiTree(StBiTree *parent, const Key &mParameters, bool isLeaf=false)
00059 :mParent(), mBranches(kLeaf), mParameters(mParameters) , mIsLeaf(isLeaf)
00060 {
00061 mBranches[kLeft] =0; mBranches[kRight]=0;
00062 if (mParent) {Reparent(mParent);}
00063 }
00064 StBiTree(const StBiTree &src);
00065 virtual ~StBiTree();
00066 StBiTree *Parent() const;
00067 void Print(int shift=0) const;
00068 void PrintData(int shift=0) const;
00069
00070 StBiTree *Left() const;
00071 StBiTree *Left();
00072 void SetLeft(StBiTree *node);
00073
00074 StBiTree *Right() const;
00075 StBiTree *Right();
00076 void SetRight(StBiTree *node);
00077
00078 unsigned int Depth() const;
00079 const Key &Data() const;
00080 Key &Data();
00081
00082 static int Where( const Key &plane, const Key &data);
00083 static Key Plane( const Key &left, const Key &right);
00084 bool IsLeaf() const { return mIsLeaf; }
00085 bool IsEmpty() const { return !mIsLeaf && Left()==0 && Right() ==0; }
00086 EBranch Where( const Key &data) const
00087 {
00088 return Where(Data(),data) ? kLeft : kRight;
00089 }
00090 EBranch WhereAmI() const
00091 {
00092 EBranch iAmHere = kLeaf;
00093 if ( Parent() ) {
00094 if ( Parent()->Left() == this) iAmHere = kLeft;
00095 else if ( Parent()->Right() == this) iAmHere = kRight;
00096 else assert(0 && "__FUNCTION__ wrong parent");
00097 }
00098 return iAmHere;
00099 }
00100 const StBiTree *push_back( const Key &data);
00101 void SetBranch(StBiTree *node,EBranch branch);
00102 StBiTree *Branch(EBranch branch) const;
00103 StBiTree *Branch(EBranch branch);
00104 iterator<input_iterator_tag, class StBiTree<Key> > begin()
00105 {
00106 return iterator<input_iterator_tag, class StBiTree<Key> >(this);
00107 }
00108 iterator<input_iterator_tag, class StBiTree<Key> > *CreateIterator()
00109 {
00110 return new iterator<input_iterator_tag, class StBiTree<Key> >(this);
00111 }
00112
00113 static iterator<input_iterator_tag, class StBiTree<Key> > end()
00114 {
00115 return iterator<input_iterator_tag, class StBiTree<Key> >();
00116 }
00117 bool empty() const { return IsEmpty(); }
00118 iterator<input_iterator_tag, class StBiTree<Key> > find(const Key &data);
00119 iterator<input_iterator_tag, class StBiTree<Key> > find(const StBiTree<Key> &node);
00120 };
00121
00122
00123 template <class Key>
00124 StBiTree<Key>::~StBiTree()
00125 {
00126
00127
00128 if (mParent) Reparent(0);
00129
00130 for (unsigned int i=0;i<mBranches.size();++i) {
00131 if (mBranches[i]) {
00132
00133 mBranches[i]->SetParent(0);
00134 delete mBranches[i];
00135 mBranches[i] = 0;
00136 }
00137 }
00138 }
00139
00140
00141 template <class Key>
00142 void StBiTree<Key>::Insert(const Key &data, bool leaf)
00143 {
00144 Insert(new StBiTree(this,data,leaf));
00145 }
00146
00147
00148 template <class Key>
00149 void StBiTree<Key>::Insert(StBiTree<Key> *node)
00150 {
00151
00152 }
00153
00154
00155 template <class Key>
00156 StBiTree<Key> *StBiTree<Key>::Branch(StBiTree<Key>::EBranch branch) const
00157 {
00158 return mBranches[branch];
00159 }
00160
00161
00162 template <class Key>
00163 StBiTree<Key> *StBiTree<Key>::Branch(StBiTree<Key>::EBranch branch)
00164 {
00165 return mBranches[branch];
00166 }
00167
00168
00169 template <class Key>
00170 StBiTree<Key> *StBiTree<Key>::Left() const
00171 {
00172 return Branch(kLeft);
00173 }
00174
00175
00176 template <class Key>
00177 StBiTree<Key> *StBiTree<Key>::Left()
00178 {
00179 return Branch(kLeft);
00180 }
00181
00182
00183
00184 template <class Key>
00185 void StBiTree<Key>::SetBranch(StBiTree<Key> *node,StBiTree<Key>::EBranch branch)
00186 {
00187 if (branch!= kLeaf) mBranches[branch]=node;
00188 else Reparent(0);
00189 }
00190
00191
00192 template <class Key>
00193 void StBiTree<Key>::SetLeft(StBiTree *left)
00194 {
00195 SetBranch(left,kLeft);
00196 }
00197
00198
00199 template <class Key>
00200 StBiTree<Key> *StBiTree<Key>::Right()
00201 {
00202 return Branch(kRight);
00203 }
00204
00205
00206 template <class Key>
00207 StBiTree<Key> *StBiTree<Key>::Right() const
00208 {
00209 return Branch(kRight);
00210 }
00211
00212
00213 template <class Key>
00214 void StBiTree<Key>::SetRight(StBiTree<Key> *right)
00215 {
00216 SetBranch(right,kRight);
00217 }
00218
00219
00220
00221 template <class Key>
00222 int StBiTree<Key>::Reparent(StBiTree<Key> *newParent)
00223 {
00224 EBranch branch = kLeft;
00225 StBiTree *oldParent = Parent();
00226 if (newParent != oldParent) {
00227 if (oldParent)
00228 oldParent->SetBranch(newParent,WhereAmI());
00229 SetParent(newParent);
00230 if (newParent) {
00231 newParent->SetParent(oldParent);
00232 switch (branch = newParent->Where(Data())) {
00233 case kLeft:
00234 newParent->SetLeft(this);
00235 break;
00236 case kRight:
00237 newParent->SetRight(this);
00238 break;
00239 case kLeaf: case kEmpty: assert(0 && " Wrong position"); break;
00240 }
00241 }
00242 }
00243 return branch;
00244 }
00245
00246
00247 template <class Key>
00248 void StBiTree<Key>::SetParent(StBiTree<Key> *newParent)
00249 { mParent=newParent; }
00250
00251
00252 template <class Key>
00253 StBiTree<Key> *StBiTree<Key>::Parent() const
00254 { return mParent; }
00255
00256
00257 template <class Key>
00258 const Key &StBiTree<Key>::Data() const
00259 {
00260 return mParameters;
00261 }
00262
00263
00264 template <class Key>
00265 Key &StBiTree<Key>::Data()
00266 {
00267 return mParameters;
00268 }
00269
00270
00271 template <class Key>
00272 const StBiTree<Key> *StBiTree<Key>::push_back( const Key &data)
00273 {
00274 const StBiTree<Key> *leafBranch = 0;
00275 StBiTree<Key> &branch = *this;
00276 if (branch.IsEmpty() ) {
00277 mParameters = data;
00278 mIsLeaf=true;
00279 leafBranch = this;
00280 } else if ( branch.IsLeaf() ) {
00281
00282 StBiTree<Key> *rightLeaf = new StBiTree<Key>(data,true);
00283 StBiTree<Key> *parent = branch.Parent();
00284 if (parent) {
00285 StBiTree<Key> *newParentPlane = new StBiTree<Key>(Plane(branch.Data(),data),false);
00286
00287 if (newParentPlane->Where(data) == kRight) {
00288 newParentPlane->SetRight(rightLeaf);
00289 newParentPlane->SetLeft(&branch);
00290 } else {
00291 newParentPlane->SetLeft(rightLeaf);
00292 newParentPlane->SetRight(&branch);
00293 }
00294 newParentPlane->SetParent(parent);
00295
00296 parent->SetBranch(newParentPlane,branch.WhereAmI());
00297 branch.SetParent(newParentPlane);
00298 rightLeaf->SetParent(newParentPlane);
00299 } else {
00300 StBiTree<Key> *leftLeaf = new StBiTree<Key>(branch.Data(),true);
00301 branch.Data() = Plane(branch.Data(),data);
00302 if ( branch.Where(data) == kRight) {
00303 branch.SetLeft(leftLeaf);
00304 branch.SetRight(rightLeaf);
00305 } else {
00306 branch.SetLeft(rightLeaf);
00307 branch.SetRight(leftLeaf);
00308 }
00309 rightLeaf->SetParent(&branch);
00310 leftLeaf->SetParent(&branch);
00311 branch.SetLeaf(false);
00312 }
00313 leafBranch = rightLeaf;
00314 } else {
00315
00316 EBranch loc = Where(data);
00317 StBiTree<Key> *next = Branch( loc );
00318 if(next) {
00319 leafBranch = next->push_back(data);
00320 } else {
00321 assert(0);
00322 SetBranch(new StBiTree<Key>(branch.Data(),true),loc);
00323 }
00324 }
00325 return leafBranch;
00326 }
00327
00328
00329 template <class Key>
00330 iterator<input_iterator_tag, class StBiTree<Key> > StBiTree<Key>::find(const StBiTree<Key> &node)
00331 {
00332 iterator<input_iterator_tag, class StBiTree<Key> > it = find(node.Data());
00333 if ( it == node.begin()) return it;
00334 return end();
00335 }
00336
00337
00338 template <class Key>
00339 iterator<input_iterator_tag, class StBiTree<Key> > StBiTree<Key>::find(const Key &data)
00340 {
00341 if (empty() || IsLeaf() ) {
00342 return begin();
00343 } else {
00344 EBranch loc = Where(data);
00345 StBiTree<Key> *next = Branch( loc );
00346 if(next) {
00347 return next->find(data);
00348 } else {
00349 return begin();
00350 }
00351 }
00352 }
00353
00354
00355 template<>
00356 inline int StBiTree< vector<float> >::Where( const vector<float> &plane, const vector<float> &data)
00357 {
00358 unsigned int nDim=data.size();
00359 assert(plane.size() == nDim+1);
00360 float dist = plane[nDim];
00361 for (unsigned int i=0;i<nDim;++i) dist += plane[i]*data[i];
00362 return dist > 0;
00363 }
00364
00365
00366 template<>
00367 inline vector<float> StBiTree< vector<float> >::Plane( const vector<float> &left, const vector<float> &right)
00368 {
00369
00370
00371
00372
00373
00374
00375 unsigned int nDim = left.size();
00376 vector<float> plane(nDim+1);
00377 unsigned int indx = 0;
00378 float length = 0;
00379 plane[nDim] = 0;
00380 for (;indx<nDim;++indx) {
00381 plane[indx] = right[indx]-left[indx];
00382 plane[nDim] += plane[indx]*(right[indx]+left[indx]);
00383 length += plane[indx]*plane[indx];
00384 }
00385 for (;indx>nDim+1;++indx) plane[indx] /= sqrt(length);
00386 plane[nDim] = -0.5*plane[nDim];
00387 cout << endl;
00388 cout << __FUNCTION__ << "::Left: " << "x=" << left[0]
00389 << "; y=" << left[1];
00390 indx = 2;
00391 if (nDim >= 3) cout << "; z=" << left[2];
00392 cout << endl;
00393
00394 cout << __FUNCTION__ << "::Right: "<< "x=" << right[0]
00395 << "; y=" << right[1];
00396 indx = 2;
00397 if (nDim >= 3) cout << "; z=" << right[2];
00398 cout << endl;
00399
00400 cout << __FUNCTION__ << "::Plane:" << plane[0] << "*x+"
00401 << plane[1] << "*y+";
00402 indx = 2;
00403 if (nDim >= 3) cout << plane[indx++] << "*z+";
00404 cout << plane[indx] << endl;
00405
00406 cout << endl;
00407 return plane;
00408 }
00409
00410 template <class Key>
00411 unsigned int StBiTree<Key>::Depth() const
00412 {
00413 const StBiTree<Key> *currentNode = Parent();
00414 unsigned int depth = 0;
00415 while (currentNode) {
00416 depth++;
00417 currentNode = currentNode->Parent();
00418 }
00419 return depth;
00420 }
00421
00422 template <class Key>
00423 void StBiTree<Key>::Print(int shift) const
00424 {
00425 cout << __FUNCTION__ << setw(shift) << " "<< " me=" << this
00426 << " Parent=" <<Parent()
00427 << " Leaf=" << IsLeaf()
00428 << " Left branch =" << Left()
00429 << " Right branch = " << Right() << endl;
00430 if (shift >= 0) {
00431 shift++;
00432 if (Left()) Left()->Print(2*shift);
00433 if (Right()) Right()->Print(2*shift);
00434 }
00435 }
00436
00437
00438 template<>
00439 inline void StBiTree< vector<float> >::PrintData(int shift) const
00440 {
00441 unsigned int nDim = mParameters.size();
00442 const char *nodeType = nDim <=3 ? "point: " : "plane";
00443 cout << setw(shift)<< nodeType;
00444 for (unsigned int i =0; i<nDim;++i) cout << " : [" << i << "]=" << mParameters[i] ;
00445 cout << endl << " " << " Total: " << nDim << endl;
00446 if (shift >= 0) {
00447 shift++;
00448 if (Left()) Left()->PrintData(2*shift);
00449 if (Right()) Right()->PrintData(2*shift);
00450 }
00451 }
00452 }
00453 #endif
