cgma
WeightedOctree.cpp
Go to the documentation of this file.
00001 //-------------------------------------------------------------------------
00002 // Filename      : WOctree.cpp
00003 //
00004 // Purpose       : Class for O(ln n) search for nodes within a specified
00005 //                 tolerance of a specified position.
00006 //
00007 // Creator       : Corey McBride 
00008 //
00009 // Creation Date : 02/18/10
00010 //-------------------------------------------------------------------------
00011 
00012 #include "WeightedOctree.hpp"
00013 #include "CubitDefines.h"
00014 #include "GeometryDefines.h"
00015 #include <vector>
00016 #include <algorithm>
00017 
00018 const int DEFAULT_MAX_WEIGHT_PER_BOX = 100;
00019 // The default value to use for the maximum weight of each box
00020 // , if none is specified in the constructr.
00021 
00022 //-------------------------------------------------------------------------
00023 // Purpose       : Constructor
00024 //
00025 // Creator       : Corey McBride 
00026 //
00027 // Creation Date : 02/18/10
00028 //-------------------------------------------------------------------------
00029 template<class X, class C, class E>
00030 WeightedOctree<X,C,E>::WeightedOctree(int max_weight_per_box,double tolerance)
00031 {
00032 
00033   this->Tolerance = tolerance < GEOMETRY_RESABS ? GEOMETRY_RESABS : tolerance;
00034   this->ToleranceSquared=this->Tolerance*this->Tolerance;
00035 
00036   this->MaxBoxWeight= max_weight_per_box < 0 ? DEFAULT_MAX_WEIGHT_PER_BOX : max_weight_per_box;
00037  
00038   this->Root = new Cell();
00039 
00040 }
00041 
00042 //-------------------------------------------------------------------------
00043 // Purpose       : Destructor
00044 //
00045 // Creator       : Corey McBride 
00046 //
00047 // Creation Date : 02/18/10
00048 //-------------------------------------------------------------------------
00049 template<class X, class C, class E>
00050 WeightedOctree<X,C,E>::~WeightedOctree()
00051 {
00052   // Release all memory
00053   delete this->Root;
00054 
00055 
00056   this->Root = 0;
00057 }
00058 template<class X, class C, class E>
00059 bool WeightedOctree<X,C,E>::addNode(const X& node,
00060                                     std::vector<Cell*>& oldCells,
00061                                     std::vector<Cell*>& newCells,
00062                                     std::vector<Cell*>& modifiedCells)
00063 {
00064   newCells.clear();
00065   oldCells.clear();
00066   modifiedCells.clear();
00067 
00068   Cell* currentCell=this->Root;
00069 
00070   CubitVector coords = E::coordinates(node);
00071   int weight=E::weight(node);
00072   while(!currentCell->leaf())
00073   {
00074     int x = coords.x() < currentCell->center().x() ? Cell::X_MIN : Cell::X_MAX;
00075     int y = coords.y() < currentCell->center().y() ? Cell::Y_MIN : Cell::Y_MAX;
00076     int z = coords.z() < currentCell->center().z() ? Cell::Z_MIN : Cell::Z_MAX;
00077 
00078     currentCell=currentCell->child(x|y|z);
00079   }
00080 
00081 
00082   currentCell->addNode(node);
00083   currentCell->addWeight(weight);
00084 
00085   this->NodeToCellMap[node]=currentCell;
00086 
00087   Cell* currentParent=currentCell->parent();
00088   while(currentParent)
00089   {
00090     currentParent->addWeight(weight);
00091     currentParent=currentParent->parent();
00092   }
00093 
00094 
00095   typename std::map<X,Cell*>::iterator nodeIter;
00096   if(currentCell->weight()>this->MaxBoxWeight && currentCell->nodeCount()>=2)
00097   {
00098     //Try to rebalance first.
00099     if(currentCell->parent())
00100     {
00101       if(this->rebalanceBranch(currentCell->parent(),oldCells,newCells))
00102       {
00103         for(unsigned int i=0;i<newCells.size();i++)
00104         {
00105           Cell* cell=newCells[i];
00106           std::vector<X> cellNodes;
00107           cell->appendNodes(cellNodes);
00108 
00109           for(unsigned int j=0;j<cellNodes.size();j++)
00110           {
00111             X& localNode=cellNodes[j];
00112 
00113             nodeIter=this->NodeToCellMap.find(localNode);
00114             if(nodeIter!=this->NodeToCellMap.end())
00115             {
00116               E::setLastCell(localNode,nodeIter->second);
00117             }
00118             this->NodeToCellMap[localNode]=cell;
00119           }
00120         }
00121         return true;
00122       }
00123     }
00124 
00125     //didn't rebalance so
00126     //Try to split
00127     if(this->splitCell(currentCell,newCells))
00128     {
00129       //The cell was split. We need to update the node map.
00130       for(unsigned int i=0;i<newCells.size();i++)
00131       {
00132         Cell* cell=newCells[i];
00133         std::vector<X> cellNodes;
00134         cell->appendNodes(cellNodes);
00135 
00136         for(unsigned int j=0;j<cellNodes.size();j++)
00137         {
00138           X& localNode=cellNodes[j];
00139           nodeIter=this->NodeToCellMap.find(localNode);
00140           if(nodeIter!=this->NodeToCellMap.end())
00141           {
00142             E::setLastCell(localNode,nodeIter->second);
00143           }
00144           this->NodeToCellMap[localNode]=cell;
00145         }
00146       }
00147       oldCells.push_back(currentCell);
00148     }
00149     else
00150     {
00151       //the cells wasn't split
00152       if(currentCell->nodeCount()==1)
00153       {
00154         newCells.push_back(currentCell);
00155       }
00156       else
00157       {
00158 
00159         modifiedCells.push_back(currentCell);
00160       }
00161     }
00162   }
00163   else
00164   {
00165     if(currentCell->nodeCount()==1)
00166     {
00167       newCells.push_back(currentCell);
00168     }
00169     else
00170     {
00171       modifiedCells.push_back(currentCell);
00172     }
00173   }
00174   return true;
00175 }
00176 template<class X, class C, class E>
00177 bool WeightedOctree<X,C,E>::removeNode(X& node,
00178                                        std::vector<Cell*>& oldCells,
00179                                        std::vector<Cell*>& newCells,
00180                                        std::vector<Cell*>& modifiedCells)
00181 {
00182   typename std::map<X,Cell*>::iterator iter;
00183   iter=this->NodeToCellMap.find(node);
00184   if(iter==this->NodeToCellMap.end())
00185   {
00186     return false;
00187   }
00188   Cell* currentCell=iter->second;
00189 
00190 
00191   E::setLastCell(node,iter->second);
00192 
00193   //remove node from cell.
00194   if(currentCell->removeNode(node))
00195   {
00196     this->NodeToCellMap.erase(node);
00197     int weight=E::weight(node);
00198     currentCell->addWeight(-weight);
00199 
00200     //update weight of parents.
00201     Cell* currentParent=currentCell->parent();
00202     while(currentParent)
00203     {
00204       currentParent->addWeight(-weight);
00205       currentParent=currentParent->parent();
00206     }
00207 
00208     //Determine if the parent can be collapsed.
00209     currentParent=currentCell->parent();
00210     Cell* lastParent=NULL;
00211     while(currentParent && currentParent->weight()<this->MaxBoxWeight)
00212     {
00213       //Traverse up the tree until the total weight is less that the max weight
00214       lastParent=currentParent;
00215       currentParent=currentParent->parent();
00216     }
00217 
00218 
00219     if(lastParent)
00220     {
00221       //Parent can be collapsed
00222       this->mergeCellChildren(lastParent,oldCells);
00223       newCells.push_back(lastParent);
00224 
00225       std::vector<X> cellNodes;
00226       lastParent->appendNodes(cellNodes);
00227 
00228       for(unsigned int j=0;j<cellNodes.size();j++)
00229       {
00230         X& localNode=cellNodes[j];
00231         iter=this->NodeToCellMap.find(localNode);
00232         if(iter!=this->NodeToCellMap.end())
00233         {
00234           E::setLastCell(localNode,iter->second);
00235         }
00236 
00237         this->NodeToCellMap[localNode]=lastParent;
00238       }
00239     }
00240     else
00241     {
00242       //Parent cannot be collapsed.
00243       if(currentCell->nodeCount()==0)
00244       {
00245         oldCells.push_back(currentCell);
00246 
00247       }
00248       else
00249       {
00250         modifiedCells.push_back(currentCell);
00251       }
00252     }
00253 
00254 
00255     return true;
00256   }
00257   return false;
00258 }
00259 template <class X, class C, class E>
00260 bool WeightedOctree<X,C,E>::calculateCellCenter(Cell* cell)
00261 {
00262   if(cell->nodeCount()==0)
00263   {
00264     return false;
00265   }
00266   std::vector<X> nodes;
00267   cell->appendNodes(nodes);
00268 
00269   //Calculate Center of cell.
00270   double wx=0.0;
00271   double wy=0.0;
00272   double wz=0.0;
00273   for(unsigned int i=0;i<nodes.size();i++)
00274   {
00275     const X&  node = nodes[i];
00276     CubitVector coords=E::coordinates(node);
00277     int weight=E::weight(node);
00278 
00279     wx+=coords.x()*weight;
00280     wy+=coords.y()*weight;
00281     wz+=coords.z()*weight;
00282   }
00283   wx/=cell->weight();
00284   wy/=cell->weight();
00285   wz/=cell->weight();
00286 
00287 
00288   cell->setCenter(wx,wy,wz);
00289   return true;
00290 }
00291 template <class X, class C, class E>
00292 bool WeightedOctree<X,C,E>::splitCell(Cell* cell,std::vector<Cell*>& newLeafs)
00293 {
00294   if(!cell->leaf())
00295   {
00296     return false;
00297   }
00298 
00299   if(cell->nodeCount()>=2 && cell->weight()>this->MaxBoxWeight)
00300   {
00301     for(int q=0;q<8;q++)
00302     {
00303       Cell* newCell= new Cell();
00304       newCell->setParent(cell);
00305       cell->setChild(q,newCell);
00306     }
00307     this->calculateCellCenter(cell);
00308 
00309     std::vector<X> nodes;
00310     cell->appendNodes(nodes);
00311     //Assign nodes to children based on center.
00312     int lastQuadrant=0;
00313     for(unsigned int i=0;i<nodes.size();i++)
00314     {
00315       const X&  node= nodes[i];
00316       CubitVector coords = E::coordinates(node);
00317       CubitVector center=cell->center();
00318       int quadrant=0;
00319       double distance=center.distance_between_squared(coords);
00320       if(distance<this->ToleranceSquared)
00321       {
00322         lastQuadrant++;
00323         if(lastQuadrant>=8)
00324         {
00325           lastQuadrant=0;
00326         }
00327         quadrant=lastQuadrant;
00328       }
00329       else
00330       {
00331         int x = coords.x() < center.x() ? Cell::X_MIN : Cell::X_MAX;
00332         int y = coords.y() < center.y() ? Cell::Y_MIN : Cell::Y_MAX;
00333         int z = coords.z() < center.z() ? Cell::Z_MIN : Cell::Z_MAX;
00334         quadrant=x|y|z;
00335       }
00336 
00337       cell->child(quadrant)->addNode(node);
00338       int weight=E::weight(node);
00339       cell->child(quadrant)->addWeight(weight);
00340     }
00341     int weight=cell->weight();
00342     cell->removeAllNodes();
00343     cell->setWeight(weight);
00344 
00345     for(unsigned int c=0;c<8;c++)
00346     {
00347       if(cell->child(c)->nodeCount()>0)
00348       {
00349         newLeafs.push_back(cell->child(c));
00350       }
00351     }
00352 
00353 
00354     return true;
00355 
00356   }
00357 
00358   return false;
00359 }  
00360 
00361 
00362 template<class X, class C, class E>
00363 void WeightedOctree<X,C,E>::mergeCellChildren(Cell* cell,std::vector<Cell*>& oldLeafs)
00364 {
00365   if(!cell->leaf())
00366   {
00367     std::vector<X> nodes;
00368     for(unsigned int i=0;i<8;i++)
00369     {
00370       this->mergeCellChildren(cell->child(i),oldLeafs);
00371       cell->child(i)->appendNodes(nodes);
00372       delete cell->child(i);
00373       cell->setChild(i,NULL);
00374     }
00375     for(unsigned int j=0;j<nodes.size();j++)
00376     {
00377       int nodeWeight=E::weight(nodes[j]);
00378       cell->addNode(nodes[j]);
00379       cell->addWeight(nodeWeight);
00380     }
00381   }
00382   else if(cell->nodeCount()!=0)
00383   {
00384     oldLeafs.push_back(cell);
00385   }
00386 }
00387 
00388 
00389 
00390 template <class X, class C, class E>
00391 bool WeightedOctree<X,C,E>::rebalanceBranch(Cell* cell,
00392                                             std::vector<Cell*>& oldLeafs,
00393                                             std::vector<Cell*>& newLeafs)
00394 {
00395   if(cell->leaf())
00396     return false;
00397 
00398   for(unsigned int i=0;i<8;i++)
00399   {
00400     if(!cell->child(i)->leaf())
00401     {
00402       //Found a child that is not a leaf;
00403       //Currently we only do 1 level rebalancing.
00404       return false;
00405     }
00406   }
00407   //All children are leafs;
00408 
00409   std::vector<X> nodes;
00410   cell->appendAllNodes(nodes);
00411 
00412   cell->setWeight(0);
00413 
00414   for(unsigned int i=0;i<nodes.size();i++)
00415   {
00416     const X& node = nodes[i];
00417     cell->addNode(node);
00418 
00419     int weight=E::weight(node);
00420     cell->addWeight(weight);
00421   }
00422 
00423   //First find all cells with nodes;
00424   if(cell->nodeCount()>=2 && cell->weight()>this->MaxBoxWeight)
00425   {
00426     // cell can be split. 
00427     //So we don't delete the children so they can be reused.
00428     for(unsigned int i=0;i<8;i++)
00429     {
00430       cell->child(i)->removeAllNodes();
00431     }
00432     this->recursiveSplitCell(cell,newLeafs);
00433   }
00434   else
00435   {
00436     //cell won't be split so we delete all of the children.
00437     for(unsigned int i=0;i<8;i++)
00438     {
00439       if(cell->child(i)->nodeCount()>0)
00440       {
00441         oldLeafs.push_back(cell->child(i));
00442       }
00443       delete cell->child(i);
00444       cell->setChild(i,NULL);
00445     }
00446 
00447     newLeafs.push_back(cell);
00448   }
00449 
00450   return true;
00451 }
00452 
00453 
00454 template <class X, class C, class E>
00455 bool WeightedOctree<X,C,E>::recursiveSplitCell(Cell* cell,std::vector<Cell*>& newLeafs)
00456 {
00457   if(!cell->leaf())
00458   {
00459     return false;
00460   }
00461   if(cell->nodeCount()>=2 && cell->weight()>this->MaxBoxWeight)
00462   {
00463     if(!cell->child(0))
00464     {
00465       for(int q=0;q<8;q++)
00466       {
00467         Cell* newCell= new Cell();
00468         newCell->setParent(cell);
00469         cell->setChild(q,newCell);
00470       }
00471     }
00472     this->calculateCellCenter(cell);
00473 
00474     std::vector<X> nodes;
00475     cell->appendNodes(nodes);
00476     //Assign nodes to children based on center.
00477     int lastQuadrant=0;
00478     for(unsigned int i=0;i<nodes.size();i++)
00479     {
00480       const X&  node= nodes[i];
00481       CubitVector coords = E::coordinates(node);
00482       CubitVector center=cell->center();
00483       int quadrant=0;
00484       double distance=center.distance_between_squared(coords);
00485       if(distance<this->ToleranceSquared)
00486       {
00487         lastQuadrant++;
00488         if(lastQuadrant>=8)
00489         {
00490           lastQuadrant=0;
00491         }
00492         quadrant=lastQuadrant;
00493       }
00494       else
00495       {
00496         int x = coords.x() < center.x() ? Cell::X_MIN : Cell::X_MAX;
00497         int y = coords.y() < center.y() ? Cell::Y_MIN : Cell::Y_MAX;
00498         int z = coords.z() < center.z() ? Cell::Z_MIN : Cell::Z_MAX;
00499         quadrant=x|y|z;
00500       }
00501 
00502       cell->child(quadrant)->addNode(node);
00503       int weight=E::weight(node);
00504       cell->child(quadrant)->addWeight(weight);
00505    }
00506 
00507     int weight=cell->weight();
00508     cell->removeAllNodes();
00509     cell->setWeight(weight);
00510 
00511     for(unsigned int c=0;c<8;c++)
00512     {
00513       if(cell->child(c)->nodeCount()>0)
00514       {
00515         if(!this->recursiveSplitCell(cell->child(c),newLeafs))
00516         {
00517           newLeafs.push_back(cell->child(c));
00518         }
00519       }
00520     }
00521 
00522     return true;
00523 
00524   }
00525 
00526   return false;
00527 
00528 
00529 }
00530 
00531 template <class X, class C, class E>
00532 void WeightedOctree<X,C,E>::clear()
00533 {
00534   delete this->Root;
00535   this->Root = new Cell;
00536 }
00537 
00538 template <class X, class C, class E>
00539 bool WeightedOctree<X,C,E>::initilizeTree(std::vector<X>& nodes,std::vector<Cell*>& newCells)
00540 {
00541   if(this->Root->leaf() && this->Root->nodeCount()==0)
00542   {
00543     for(unsigned int i=0;i<nodes.size();i++)
00544     {
00545       const X&  node= nodes[i];
00546       this->Root->addNode(node);
00547       int weight=E::weight(node);
00548       this->Root->addWeight(weight);
00549     }
00550 
00551     if(!this->recursiveSplitCell(this->Root,newCells))
00552     {
00553       //this cell wasn't split;
00554       newCells.push_back(this->Root);
00555     } 
00556     return true;
00557   }
00558 
00559   return false;
00560 }
00561 template <class X, class C, class E>
00562 WeightedOctree<X,C,E>::Cell::Cell()
00563 {
00564   this->TotalWeight=0;
00565   this->IsLeaf=true;
00566 
00567 
00568   this->Parent=NULL;
00569   for(int i=0;i<8;i++)
00570   {
00571     this->Children[i]=NULL;
00572   }
00573 }
00574 
00575 template <class X, class C, class E>
00576 void WeightedOctree<X,C,E>::Cell::setParent(Cell* parent)
00577 {
00578   this->Parent=parent;
00579 }
00580 
00581 
00582 template <class X, class C, class E>
00583 void WeightedOctree<X,C,E>::Cell::setChild(int quadrant,Cell* c)
00584 {
00585   assert( quadrant >= 0 && quadrant < 8);
00586   this->Children[quadrant]=c;
00587 }
00588 
00589 template <class X, class C, class E>
00590 void WeightedOctree<X,C,E>::Cell::appendAllNodes( std::vector<X>& list )
00591 {
00592   if(this->Nodes.size())
00593   {
00594     appendNodes( list );
00595   }
00596  
00597   for( int i = 0; i < 8; i++ )
00598   {
00599     if(this->Children[i])
00600       this->Children[i]->appendAllNodes(list);
00601   }
00602 
00603 }
00604 
00605 template <class X, class C, class E>
00606 bool WeightedOctree<X,C,E>::Cell::leaf()
00607 {
00608  return !this->Children[0];
00609 }
00610 template <class X, class C, class E>
00611 void WeightedOctree<X,C,E>::Cell::appendNodes( std::vector<X>& list )
00612 {
00613   list.insert(list.end(),this->Nodes.begin(),this->Nodes.end());
00614 }
00615 
00616 template <class X, class C, class E>
00617     bool WeightedOctree<X,C,E>::Cell::removeNode(const X&  node)
00618 {
00619   typename std::vector<X>::iterator iter;
00620   iter=std::remove(this->Nodes.begin(),this->Nodes.end(),node);
00621   if(iter!=this->Nodes.end())
00622   {
00623     //The nodes was found;
00624     this->Nodes.erase(iter,this->Nodes.end());
00625     return true;
00626   }
00627 
00628   return false;
00629 }
00630 template <class X, class C, class E>
00631 void WeightedOctree<X,C,E>::Cell::addNode(const X&  node )
00632 {
00633   
00634   this->Nodes.push_back(node);
00635 }
00636 template <class X, class C, class E>
00637 int WeightedOctree<X,C,E>::Cell::weight()
00638   {
00639   return this->TotalWeight;
00640   }
00641 
00642 template <class X, class C, class E>
00643 void WeightedOctree<X,C,E>::Cell::setWeight(int w)
00644 {
00645    this->TotalWeight=w;
00646 }
00647 template <class X, class C, class E>
00648 void WeightedOctree<X,C,E>::Cell::addWeight(int w)
00649 {
00650    this->TotalWeight+=w;
00651 }
00652 
00653 template <class X, class C, class E>
00654     void WeightedOctree<X,C,E>::Cell::setCenter(CubitVector& center)
00655 {
00656   this->Center=center;
00657 }
00658 template <class X, class C, class E>
00659     void WeightedOctree<X,C,E>::Cell::removeAllNodes()
00660 {
00661   this->Nodes.clear();
00662   this->TotalWeight=0;
00663   
00664 }
00665 
00666 template <class X, class C, class E>
00667     void WeightedOctree<X,C,E>::Cell::setCenter(double& x,double& y,double& z)
00668 {
00669   this->Center.set(x,y,z);
00670 }
00671 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines