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