src/Navaids/PositionedOctree.hxx

   1 /**
   2  * PositionedOctree - define a spatial octree containing Positioned items
   3  * arranged by their global cartesian position.
   4  */
   5
   6 // Written by James Turner, started 2012.
   7 //
   8 // Copyright (C) 2012 James Turner
   9 //
  10 // This program is free software; you can redistribute it and/or
  11 // modify it under the terms of the GNU General Public License as
  12 // published by the Free Software Foundation; either version 2 of the
  13 // License, or (at your option) any later version.
  14 //
  15 // This program is distributed in the hope that it will be useful, but
  16 // WITHOUT ANY WARRANTY; without even the implied warranty of
  17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18 // General Public License for more details.
  19 //
  20 // You should have received a copy of the GNU General Public License
  21 // along with this program; if not, write to the Free Software
  22 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  23
  24 #ifndef FG_POSITIONED_OCTREE_HXX
  25 #define FG_POSITIONED_OCTREE_HXX
  26
  27 // std
  28 #include <vector>
  29 #include <set>
  30 #include <queue>
  31 #include <cassert>
  32 #include <map>
  33
  34 // SimGear
  35 #include <simgear/math/SGGeometry.hxx>
  36
  37 #include <Navaids/positioned.hxx>
  38 #include <Navaids/NavDataCache.hxx>
  39
  40 namespace flightgear
  41 {
  42
  43 namespace Octree
  44 {
  45
  46   const double LEAF_SIZE = SG_NM_TO_METER * 8.0;
  47   const double LEAF_SIZE_SQR = LEAF_SIZE * LEAF_SIZE;
  48
  49   /**
  50    * Decorate an object with a double value, and use that value to order
  51    * items, for the purpoises of the STL algorithms
  52    */
  53   template <class T>
  54   class Ordered
  55   {
  56   public:
  57     Ordered(const T& v, double x) :
  58     _order(x),
  59     _inner(v)
  60     {
  61       assert(!isnan(x));
  62     }
  63
  64     Ordered(const Ordered<T>& a) :
  65     _order(a._order),
  66     _inner(a._inner)
  67     {
  68     }
  69
  70     Ordered<T>& operator=(const Ordered<T>& a)
  71     {
  72       _order = a._order;
  73       _inner = a._inner;
  74       return *this;
  75     }
  76
  77     bool operator<(const Ordered<T>& other) const
  78     {
  79       return _order < other._order;
  80     }
  81
  82     bool operator>(const Ordered<T>& other) const
  83     {
  84       return _order > other._order;
  85     }
  86
  87     const T& get() const
  88     { return _inner; }
  89
  90     double order() const
  91     { return _order; }
  92
  93   private:
  94     double _order;
  95     T _inner;
  96   };
  97
  98   class Node;
  99   typedef Ordered<Node*> OrderedNode;
 100   typedef std::greater<OrderedNode> FNPQCompare;
 101
 102   /**
 103    * the priority queue is fundamental to our search algorithm. When searching,
 104    * we know the front of the queue is the nearest unexpanded node (to the search
 105    * location). The default STL pqueue returns the 'largest' item from top(), so
 106    * to get the smallest, we need to replace the default Compare functor (less<>)
 107    * with greater<>.
 108    */
 109   typedef std::priority_queue<OrderedNode, std::vector<OrderedNode>, FNPQCompare> FindNearestPQueue;
 110
 111   typedef Ordered<FGPositioned*> OrderedPositioned;
 112   typedef std::vector<OrderedPositioned> FindNearestResults;
 113
 114   extern Node* global_spatialOctree;
 115
 116   class Leaf;
 117
 118   /**
 119    * Octree node base class, tracks its bounding box and provides various
 120    * queries relating to it
 121    */
 122   class Node
 123   {
 124   public:
 125     int64_t guid() const
 126     { return _ident; }
 127
 128     const SGBoxd& bbox() const
 129     { return _box; }
 130
 131     bool contains(const SGVec3d& aPos) const
 132     {
 133       return intersects(aPos, _box);
 134     }
 135
 136     double distToNearest(const SGVec3d& aPos) const
 137     {
 138       return dist(aPos, _box.getClosestPoint(aPos));
 139     }
 140
 141     virtual void visit(const SGVec3d& aPos, double aCutoff,
 142                        FGPositioned::Filter* aFilter,
 143                        FindNearestResults& aResults, FindNearestPQueue&) = 0;
 144
 145     virtual Leaf* findLeafForPos(const SGVec3d& aPos) const = 0;
 146   protected:
 147     Node(const SGBoxd &aBox, int64_t aIdent) :
 148     _ident(aIdent),
 149     _box(aBox)
 150     {
 151     }
 152
 153     const int64_t _ident;
 154     const SGBoxd _box;
 155   };
 156
 157   class Leaf : public Node
 158   {
 159   public:
 160     Leaf(const SGBoxd& aBox, int64_t aIdent);
 161
 162     virtual void visit(const SGVec3d& aPos, double aCutoff,
 163                        FGPositioned::Filter* aFilter,
 164                        FindNearestResults& aResults, FindNearestPQueue&);
 165
 166     virtual Leaf* findLeafForPos(const SGVec3d&) const
 167     {
 168       return const_cast<Leaf*>(this);
 169     }
 170
 171     void insertChild(FGPositioned::Type ty, PositionedID id);
 172   private:
 173     bool childrenLoaded;
 174
 175     typedef std::multimap<FGPositioned::Type, PositionedID> ChildMap;
 176     ChildMap children;
 177
 178     void loadChildren();
 179   };
 180
 181   class Branch : public Node
 182   {
 183   public:
 184     Branch(const SGBoxd& aBox, int64_t aIdent);
 185
 186     virtual void visit(const SGVec3d& aPos, double aCutoff,
 187                        FGPositioned::Filter*,
 188                        FindNearestResults&, FindNearestPQueue& aQ);
 189
 190     virtual Leaf* findLeafForPos(const SGVec3d& aPos) const
 191     {
 192       loadChildren();
 193       return childForPos(aPos)->findLeafForPos(aPos);
 194     }
 195
 196     int childMask() const;
 197   private:
 198     Node* childForPos(const SGVec3d& aCart) const;
 199     Node* childAtIndex(int childIndex) const;
 200
 201     /**
 202      * Return the box for a child touching the specified corner
 203      */
 204     SGBoxd boxForChild(unsigned int aCorner) const
 205     {
 206       SGBoxd r(_box.getCenter());
 207       r.expandBy(_box.getCorner(aCorner));
 208       return r;
 209     }
 210
 211     void loadChildren() const;
 212
 213     mutable Node* children[8];
 214     mutable bool childrenLoaded;
 215   };
 216
 217   void findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults);
 218   void findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults);
 219 } // of namespace Octree
 220
 221
 222 } // of namespace flightgear
 223
 224 #endif // of FG_POSITIONED_OCTREE_HXX