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 #include <functional>
  34
  35 // SimGear
  36 #include <simgear/math/SGGeometry.hxx>
  37
  38 #include <Navaids/positioned.hxx>
  39 #include <Navaids/NavDataCache.hxx>
  40 #include <Navaids/PolyLine.hxx>
  41
  42 namespace flightgear
  43 {
  44
  45 namespace Octree
  46 {
  47
  48   const double LEAF_SIZE = SG_NM_TO_METER * 8.0;
  49   const double LEAF_SIZE_SQR = LEAF_SIZE * LEAF_SIZE;
  50
  51   /**
  52    * Decorate an object with a double value, and use that value to order
  53    * items, for the purpoises of the STL algorithms
  54    */
  55   template <class T>
  56   class Ordered
  57   {
  58   public:
  59     Ordered(const T& v, double x) :
  60     _order(x),
  61     _inner(v)
  62     {
  63       assert(!isnan(x));
  64     }
  65
  66     Ordered(const Ordered<T>& a) :
  67     _order(a._order),
  68     _inner(a._inner)
  69     {
  70     }
  71
  72     Ordered<T>& operator=(const Ordered<T>& a)
  73     {
  74       _order = a._order;
  75       _inner = a._inner;
  76       return *this;
  77     }
  78
  79     bool operator<(const Ordered<T>& other) const
  80     {
  81       return _order < other._order;
  82     }
  83
  84     bool operator>(const Ordered<T>& other) const
  85     {
  86       return _order > other._order;
  87     }
  88
  89     const T& get() const
  90     { return _inner; }
  91
  92     double order() const
  93     { return _order; }
  94
  95   private:
  96     double _order;
  97     T _inner;
  98   };
  99
 100   class Node;
 101   typedef Ordered<Node*> OrderedNode;
 102   typedef std::greater<OrderedNode> FNPQCompare;
 103
 104   /**
 105    * the priority queue is fundamental to our search algorithm. When searching,
 106    * we know the front of the queue is the nearest unexpanded node (to the search
 107    * location). The default STL pqueue returns the 'largest' item from top(), so
 108    * to get the smallest, we need to replace the default Compare functor (less<>)
 109    * with greater<>.
 110    */
 111   typedef std::priority_queue<OrderedNode, std::vector<OrderedNode>, FNPQCompare> FindNearestPQueue;
 112
 113   typedef Ordered<FGPositioned*> OrderedPositioned;
 114   typedef std::vector<OrderedPositioned> FindNearestResults;
 115
 116   // for extracting lines, we don't care about distance ordering, since
 117   // we're always grabbing all the lines in an area
 118   typedef std::deque<Node*> FindLinesDeque;
 119
 120   extern Node* global_spatialOctree;
 121
 122   class Leaf;
 123
 124   /**
 125    * Octree node base class, tracks its bounding box and provides various
 126    * queries relating to it
 127    */
 128   class Node
 129   {
 130   public:
 131     int64_t guid() const
 132     { return _ident; }
 133
 134     const SGBoxd& bbox() const
 135     { return _box; }
 136
 137     bool contains(const SGVec3d& aPos) const
 138     {
 139       return intersects(aPos, _box);
 140     }
 141
 142     double distToNearest(const SGVec3d& aPos) const
 143     {
 144       return dist(aPos, _box.getClosestPoint(aPos));
 145     }
 146
 147     virtual void visit(const SGVec3d& aPos, double aCutoff,
 148                        FGPositioned::Filter* aFilter,
 149                        FindNearestResults& aResults, FindNearestPQueue&) = 0;
 150
 151     virtual Leaf* findLeafForPos(const SGVec3d& aPos) const = 0;
 152
 153     virtual void visitForLines(const SGVec3d& aPos, double aCutoff,
 154                          PolyLineList& aLines,
 155                          FindLinesDeque& aQ) const = 0;
 156   protected:
 157     Node(const SGBoxd &aBox, int64_t aIdent) :
 158     _ident(aIdent),
 159     _box(aBox)
 160     {
 161     }
 162
 163     const int64_t _ident;
 164     const SGBoxd _box;
 165   };
 166
 167   class Leaf : public Node
 168   {
 169   public:
 170     Leaf(const SGBoxd& aBox, int64_t aIdent);
 171
 172     virtual void visit(const SGVec3d& aPos, double aCutoff,
 173                        FGPositioned::Filter* aFilter,
 174                        FindNearestResults& aResults, FindNearestPQueue&);
 175
 176     virtual Leaf* findLeafForPos(const SGVec3d&) const
 177     {
 178       return const_cast<Leaf*>(this);
 179     }
 180
 181     void insertChild(FGPositioned::Type ty, PositionedID id);
 182
 183     void addPolyLine(PolyLineRef);
 184
 185     virtual void visitForLines(const SGVec3d& aPos, double aCutoff,
 186                                PolyLineList& aLines,
 187                                FindLinesDeque& aQ) const;
 188   private:
 189     bool childrenLoaded;
 190
 191     typedef std::multimap<FGPositioned::Type, PositionedID> ChildMap;
 192     ChildMap children;
 193
 194     PolyLineList lines;
 195
 196     void loadChildren();
 197   };
 198
 199   class Branch : public Node
 200   {
 201   public:
 202     Branch(const SGBoxd& aBox, int64_t aIdent);
 203
 204     virtual void visit(const SGVec3d& aPos, double aCutoff,
 205                        FGPositioned::Filter*,
 206                        FindNearestResults&, FindNearestPQueue& aQ);
 207
 208     virtual Leaf* findLeafForPos(const SGVec3d& aPos) const
 209     {
 210       loadChildren();
 211       return childForPos(aPos)->findLeafForPos(aPos);
 212     }
 213
 214     int childMask() const;
 215
 216     virtual void visitForLines(const SGVec3d& aPos, double aCutoff,
 217                                PolyLineList& aLines,
 218                                FindLinesDeque& aQ) const;
 219   private:
 220     Node* childForPos(const SGVec3d& aCart) const;
 221     Node* childAtIndex(int childIndex) const;
 222
 223     /**
 224      * Return the box for a child touching the specified corner
 225      */
 226     SGBoxd boxForChild(unsigned int aCorner) const
 227     {
 228       SGBoxd r(_box.getCenter());
 229       r.expandBy(_box.getCorner(aCorner));
 230       return r;
 231     }
 232
 233     void loadChildren() const;
 234
 235     mutable Node* children[8];
 236     mutable bool childrenLoaded;
 237   };
 238
 239   bool findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositionedList& aResults, int aCutoffMsec);
 240   bool findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositionedList& aResults, int aCutoffMsec);
 241 } // of namespace Octree
 242
 243
 244 } // of namespace flightgear
 245
 246 #endif // of FG_POSITIONED_OCTREE_HXX