src/Navaids/PositionedOctree.cxx

   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 #ifdef HAVE_CONFIG_H
  25 # include "config.h"
  26 #endif
  27
  28 #include "PositionedOctree.hxx"
  29 #include "positioned.hxx"
  30
  31 #include <cassert>
  32 #include <algorithm> // for sort
  33 #include <cstring> // for memset
  34 #include <iostream>
  35
  36 #include <boost/foreach.hpp>
  37
  38 #include <simgear/debug/logstream.hxx>
  39 #include <simgear/structure/exception.hxx>
  40 #include <simgear/timing/timestamp.hxx>
  41
  42 namespace flightgear
  43 {
  44
  45 namespace Octree
  46 {
  47
  48 Node* global_spatialOctree = NULL;
  49
  50 Leaf::Leaf(const SGBoxd& aBox, int64_t aIdent) :
  51   Node(aBox, aIdent),
  52   childrenLoaded(false)
  53 {
  54 }
  55
  56 void Leaf::visit(const SGVec3d& aPos, double aCutoff,
  57                    FGPositioned::Filter* aFilter,
  58                    FindNearestResults& aResults, FindNearestPQueue&)
  59 {
  60   int previousResultsSize = aResults.size();
  61   int addedCount = 0;
  62   NavDataCache* cache = NavDataCache::instance();
  63
  64   loadChildren();
  65
  66   ChildMap::const_iterator it = children.lower_bound(aFilter->minType());
  67   ChildMap::const_iterator end = children.upper_bound(aFilter->maxType());
  68
  69   for (; it != end; ++it) {
  70     FGPositioned* p = cache->loadById(it->second);
  71     double d = dist(aPos, p->cart());
  72     if (d > aCutoff) {
  73       continue;
  74     }
  75
  76     if (aFilter && !aFilter->pass(p)) {
  77       continue;
  78     }
  79
  80     ++addedCount;
  81     aResults.push_back(OrderedPositioned(p, d));
  82   }
  83
  84   if (addedCount == 0) {
  85     return;
  86   }
  87
  88   // keep aResults sorted
  89   // sort the new items, usually just one or two items
  90   std::sort(aResults.begin() + previousResultsSize, aResults.end());
  91
  92   // merge the two sorted ranges together - in linear time
  93   std::inplace_merge(aResults.begin(),
  94                      aResults.begin() + previousResultsSize, aResults.end());
  95 }
  96
  97 void Leaf::insertChild(FGPositioned::Type ty, PositionedID id)
  98 {
  99   assert(childrenLoaded);
 100   children.insert(children.end(), TypedPositioned(ty, id));
 101 }
 102
 103 void Leaf::loadChildren()
 104 {
 105   if (childrenLoaded) {
 106     return;
 107   }
 108
 109   NavDataCache* cache = NavDataCache::instance();
 110   BOOST_FOREACH(TypedPositioned tp, cache->getOctreeLeafChildren(guid())) {
 111     children.insert(children.end(), tp);
 112   } // of leaf members iteration
 113
 114   childrenLoaded = true;
 115 }
 116
 117 void Leaf::addPolyLine(PolyLineRef aLine)
 118 {
 119     lines.push_back(aLine);
 120 }
 121
 122 void Leaf::visitForLines(const SGVec3d& aPos, double aCutoff,
 123                            PolyLineList& aLines,
 124                            FindLinesDeque& aQ) const
 125 {
 126     aLines.insert(aLines.end(), lines.begin(), lines.end());
 127 }
 128
 129 ///////////////////////////////////////////////////////////////////////////////
 130
 131 Branch::Branch(const SGBoxd& aBox, int64_t aIdent) :
 132   Node(aBox, aIdent),
 133   childrenLoaded(false)
 134 {
 135   memset(children, 0, sizeof(Node*) * 8);
 136 }
 137
 138 void Branch::visit(const SGVec3d& aPos, double aCutoff,
 139                    FGPositioned::Filter*,
 140                    FindNearestResults&, FindNearestPQueue& aQ)
 141 {
 142   loadChildren();
 143   for (unsigned int i=0; i<8; ++i) {
 144     if (!children[i]) {
 145       continue;
 146     }
 147
 148     double d = children[i]->distToNearest(aPos);
 149     if (d > aCutoff) {
 150       continue; // exceeded cutoff
 151     }
 152
 153     aQ.push(Ordered<Node*>(children[i], d));
 154   } // of child iteration
 155 }
 156
 157 void Branch::visitForLines(const SGVec3d& aPos, double aCutoff,
 158                            PolyLineList& aLines,
 159                            FindLinesDeque& aQ) const
 160 {
 161     for (unsigned int i=0; i<8; ++i) {
 162         if (!children[i]) {
 163             continue;
 164         }
 165
 166         double d = children[i]->distToNearest(aPos);
 167         if (d > aCutoff) {
 168             continue; // exceeded cutoff
 169         }
 170
 171         aQ.push_back(children[i]);
 172     } // of child iteration
 173 }
 174
 175 Node* Branch::childForPos(const SGVec3d& aCart) const
 176 {
 177   assert(contains(aCart));
 178   int childIndex = 0;
 179
 180   SGVec3d center(_box.getCenter());
 181 // tests must match indices in SGbox::getCorner
 182   if (aCart.x() < center.x()) {
 183     childIndex += 1;
 184   }
 185
 186   if (aCart.y() < center.y()) {
 187     childIndex += 2;
 188   }
 189
 190   if (aCart.z() < center.z()) {
 191     childIndex += 4;
 192   }
 193
 194   return childAtIndex(childIndex);
 195 }
 196
 197 Node* Branch::childAtIndex(int childIndex) const
 198 {
 199   Node* child = children[childIndex];
 200   if (!child) { // lazy building of children
 201     SGBoxd cb(boxForChild(childIndex));
 202     double d2 = dot(cb.getSize(), cb.getSize());
 203
 204     assert(((_ident << 3) >> 3) == _ident);
 205
 206     // child index is 0..7, so 3-bits is sufficient, and hence we can
 207     // pack 20 levels of octree into a int64, which is plenty
 208     int64_t childIdent = (_ident << 3) | childIndex;
 209
 210     if (d2 < LEAF_SIZE_SQR) {
 211       child = new Leaf(cb, childIdent);
 212     } else {
 213       child = new Branch(cb, childIdent);
 214     }
 215
 216     children[childIndex] = child;
 217
 218     if (childrenLoaded) {
 219     // childrenLoad is done, so we're defining a new node - add it to the
 220     // cache too.
 221       NavDataCache::instance()->defineOctreeNode(const_cast<Branch*>(this), child);
 222     }
 223   }
 224
 225   return children[childIndex];
 226 }
 227
 228 void Branch::loadChildren() const
 229 {
 230   if (childrenLoaded) {
 231     return;
 232   }
 233
 234   int childrenMask = NavDataCache::instance()->getOctreeBranchChildren(guid());
 235   for (int i=0; i<8; ++i) {
 236     if ((1 << i) & childrenMask) {
 237       childAtIndex(i); // accessing will create!
 238     }
 239   } // of child index iteration
 240
 241 // set this after creating the child nodes, so the cache update logic
 242 // in childAtIndex knows any future created children need to be added.
 243   childrenLoaded = true;
 244 }
 245
 246 int Branch::childMask() const
 247 {
 248   int result = 0;
 249   for (int i=0; i<8; ++i) {
 250     if (children[i]) {
 251       result |= 1 << i;
 252     }
 253   }
 254
 255   return result;
 256 }
 257
 258 bool findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositionedList& aResults, int aCutoffMsec)
 259 {
 260   aResults.clear();
 261   FindNearestPQueue pq;
 262   FindNearestResults results;
 263   pq.push(Ordered<Node*>(global_spatialOctree, 0));
 264   double cut = aCutoffM;
 265
 266   SGTimeStamp tm;
 267   tm.stamp();
 268
 269   while (!pq.empty() && (tm.elapsedMSec() < aCutoffMsec)) {
 270     if (!results.empty()) {
 271       // terminate the search if we have sufficent results, and we are
 272       // sure no node still on the queue contains a closer match
 273       double furthestResultOrder = results.back().order();
 274       if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
 275     // clear the PQ to mark this has 'full results' instead of partial
 276         pq = FindNearestPQueue();
 277         break;
 278       }
 279     }
 280
 281     Node* nd = pq.top().get();
 282     pq.pop();
 283
 284     nd->visit(aPos, cut, aFilter, results, pq);
 285   } // of queue iteration
 286
 287   // depending on leaf population, we may have (slighty) more results
 288   // than requested
 289   unsigned int numResults = std::min((unsigned int) results.size(), aN);
 290   // copy results out
 291   aResults.resize(numResults);
 292   for (unsigned int r=0; r<numResults; ++r) {
 293     aResults[r] = results[r].get();
 294   }
 295
 296   return !pq.empty();
 297 }
 298
 299 bool findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositionedList& aResults, int aCutoffMsec)
 300 {
 301   aResults.clear();
 302   FindNearestPQueue pq;
 303   FindNearestResults results;
 304   pq.push(Ordered<Node*>(global_spatialOctree, 0));
 305   double rng = aRangeM;
 306
 307   SGTimeStamp tm;
 308   tm.stamp();
 309
 310   while (!pq.empty() && (tm.elapsedMSec() < aCutoffMsec)) {
 311     Node* nd = pq.top().get();
 312     pq.pop();
 313
 314     nd->visit(aPos, rng, aFilter, results, pq);
 315   } // of queue iteration
 316
 317   unsigned int numResults = results.size();
 318   // copy results out
 319   aResults.resize(numResults);
 320   for (unsigned int r=0; r<numResults; ++r) {
 321     aResults[r] = results[r].get();
 322   }
 323
 324   return !pq.empty();
 325 }
 326
 327 } // of namespace Octree
 328
 329 } // of namespace flightgear