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