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 Branch::Branch(const SGBoxd& aBox, int64_t aIdent) :
 118   Node(aBox, aIdent),
 119   childrenLoaded(false)
 120 {
 121   memset(children, 0, sizeof(Node*) * 8);
 122 }
 123
 124 void Branch::visit(const SGVec3d& aPos, double aCutoff,
 125                    FGPositioned::Filter*,
 126                    FindNearestResults&, FindNearestPQueue& aQ)
 127 {
 128   loadChildren();
 129   for (unsigned int i=0; i<8; ++i) {
 130     if (!children[i]) {
 131       continue;
 132     }
 133
 134     double d = children[i]->distToNearest(aPos);
 135     if (d > aCutoff) {
 136       continue; // exceeded cutoff
 137     }
 138
 139     aQ.push(Ordered<Node*>(children[i], d));
 140   } // of child iteration
 141 }
 142
 143 Node* Branch::childForPos(const SGVec3d& aCart) const
 144 {
 145   assert(contains(aCart));
 146   int childIndex = 0;
 147
 148   SGVec3d center(_box.getCenter());
 149 // tests must match indices in SGbox::getCorner
 150   if (aCart.x() < center.x()) {
 151     childIndex += 1;
 152   }
 153
 154   if (aCart.y() < center.y()) {
 155     childIndex += 2;
 156   }
 157
 158   if (aCart.z() < center.z()) {
 159     childIndex += 4;
 160   }
 161
 162   return childAtIndex(childIndex);
 163 }
 164
 165 Node* Branch::childAtIndex(int childIndex) const
 166 {
 167   Node* child = children[childIndex];
 168   if (!child) { // lazy building of children
 169     SGBoxd cb(boxForChild(childIndex));
 170     double d2 = dot(cb.getSize(), cb.getSize());
 171
 172     assert(((_ident << 3) >> 3) == _ident);
 173
 174     // child index is 0..7, so 3-bits is sufficient, and hence we can
 175     // pack 20 levels of octree into a int64, which is plenty
 176     int64_t childIdent = (_ident << 3) | childIndex;
 177
 178     if (d2 < LEAF_SIZE_SQR) {
 179       child = new Leaf(cb, childIdent);
 180     } else {
 181       child = new Branch(cb, childIdent);
 182     }
 183
 184     children[childIndex] = child;
 185
 186     if (childrenLoaded) {
 187     // childrenLoad is done, so we're defining a new node - add it to the
 188     // cache too.
 189       NavDataCache::instance()->defineOctreeNode(const_cast<Branch*>(this), child);
 190     }
 191   }
 192
 193   return children[childIndex];
 194 }
 195
 196 void Branch::loadChildren() const
 197 {
 198   if (childrenLoaded) {
 199     return;
 200   }
 201
 202   int childrenMask = NavDataCache::instance()->getOctreeBranchChildren(guid());
 203   for (int i=0; i<8; ++i) {
 204     if ((1 << i) & childrenMask) {
 205       childAtIndex(i); // accessing will create!
 206     }
 207   } // of child index iteration
 208
 209 // set this after creating the child nodes, so the cache update logic
 210 // in childAtIndex knows any future created children need to be added.
 211   childrenLoaded = true;
 212 }
 213
 214 int Branch::childMask() const
 215 {
 216   int result = 0;
 217   for (int i=0; i<8; ++i) {
 218     if (children[i]) {
 219       result |= 1 << i;
 220     }
 221   }
 222
 223   return result;
 224 }
 225
 226 void findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
 227 {
 228   aResults.clear();
 229   FindNearestPQueue pq;
 230   FindNearestResults results;
 231   pq.push(Ordered<Node*>(global_spatialOctree, 0));
 232   double cut = aCutoffM;
 233
 234   while (!pq.empty()) {
 235     if (!results.empty()) {
 236       // terminate the search if we have sufficent results, and we are
 237       // sure no node still on the queue contains a closer match
 238       double furthestResultOrder = results.back().order();
 239     //  std::cout << "furthest result:" << furthestResultOrder << ", top node order:" << pq.top().order() << std::endl;
 240       if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
 241         break;
 242       }
 243     }
 244
 245     Node* nd = pq.top().get();
 246     pq.pop();
 247
 248 //    std::cout << "visiting:" << std::oct << nd->guid() << "(" << std::dec << nd->guid() << ")" << std::endl;
 249     nd->visit(aPos, cut, aFilter, results, pq);
 250   } // of queue iteration
 251
 252   // depending on leaf population, we may have (slighty) more results
 253   // than requested
 254   unsigned int numResults = std::min((unsigned int) results.size(), aN);
 255   // copy results out
 256   aResults.resize(numResults);
 257   for (unsigned int r=0; r<numResults; ++r) {
 258     aResults[r] = results[r].get();
 259   }
 260 }
 261
 262 bool findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults, int aCutoffMsec)
 263 {
 264   aResults.clear();
 265   FindNearestPQueue pq;
 266   FindNearestResults results;
 267   pq.push(Ordered<Node*>(global_spatialOctree, 0));
 268   double rng = aRangeM;
 269
 270   SGTimeStamp tm;
 271   tm.stamp();
 272
 273   while (!pq.empty() && (tm.elapsedMSec() < aCutoffMsec)) {
 274     Node* nd = pq.top().get();
 275     pq.pop();
 276
 277     nd->visit(aPos, rng, aFilter, results, pq);
 278   } // of queue iteration
 279
 280   unsigned int numResults = results.size();
 281   // copy results out
 282   aResults.resize(numResults);
 283   for (unsigned int r=0; r<numResults; ++r) {
 284     aResults[r] = results[r].get();
 285   }
 286
 287   return !pq.empty();
 288 }
 289
 290 } // of namespace Octree
 291
 292 } // of namespace flightgear