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