src/Navaids/positioned.cxx

   1 // positioned.cxx - base class for objects which are positioned
   2 //
   3 // Copyright (C) 2008 James Turner
   4 //
   5 // This program is free software; you can redistribute it and/or
   6 // modify it under the terms of the GNU General Public License as
   7 // published by the Free Software Foundation; either version 2 of the
   8 // License, or (at your option) any later version.
   9 //
  10 // This program is distributed in the hope that it will be useful, but
  11 // WITHOUT ANY WARRANTY; without even the implied warranty of
  12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13 // General Public License for more details.
  14 //
  15 // You should have received a copy of the GNU General Public License
  16 // along with this program; if not, write to the Free Software
  17 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  18 //
  19 // $Id$
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include "config.h"
  23 #endif
  24
  25 #include <map>
  26 #include <set>
  27 #include <algorithm>
  28
  29 #include <simgear/math/sg_geodesy.hxx>
  30
  31 #include "positioned.hxx"
  32
  33 typedef std::multimap<std::string, FGPositioned*> NamedPositionedIndex;
  34 typedef std::pair<NamedPositionedIndex::const_iterator, NamedPositionedIndex::const_iterator> NamedIndexRange;
  35
  36 /**
  37  * Order positioned elements by type, then pointer address. This allows us to
  38  * use range searches (lower_ and upper_bound) to grab items of a particular
  39  * type out of bucket efficently.
  40  */
  41 class OrderByType
  42 {
  43 public:
  44   bool operator()(const FGPositioned* a, const FGPositioned* b) const
  45   {
  46     if (a->type() == b->type()) return a < b;
  47     return a->type() < b->type();
  48   }
  49 };
  50
  51 typedef std::set<FGPositioned*, OrderByType> BucketEntry;
  52 typedef std::map<long int, BucketEntry> SpatialPositionedIndex;
  53
  54 static NamedPositionedIndex global_namedIndex;
  55 static SpatialPositionedIndex global_spatialIndex;
  56
  57 SpatialPositionedIndex::iterator
  58 bucketEntryForPositioned(FGPositioned* aPos)
  59 {
  60   int bucketIndex = aPos->bucket().gen_index();
  61   SpatialPositionedIndex::iterator it = global_spatialIndex.find(bucketIndex);
  62   if (it != global_spatialIndex.end()) {
  63     return it;
  64   }
  65
  66   // create a new BucketEntry
  67   return global_spatialIndex.insert(it, std::make_pair(bucketIndex, BucketEntry()));
  68 }
  69
  70 static void
  71 addToIndices(FGPositioned* aPos)
  72 {
  73   assert(aPos);
  74   if (!aPos->ident().empty()) {
  75     global_namedIndex.insert(global_namedIndex.begin(),
  76       std::make_pair(aPos->ident(), aPos));
  77   }
  78
  79   SpatialPositionedIndex::iterator it = bucketEntryForPositioned(aPos);
  80   it->second.insert(aPos);
  81 }
  82
  83 static void
  84 removeFromIndices(FGPositioned* aPos)
  85 {
  86   assert(aPos);
  87
  88   if (!aPos->ident().empty()) {
  89     NamedPositionedIndex::iterator it = global_namedIndex.find(aPos->ident());
  90     while (it != global_namedIndex.end() && (it->first == aPos->ident())) {
  91       if (it->second == aPos) {
  92         global_namedIndex.erase(it);
  93         break;
  94       }
  95
  96       ++it;
  97     } // of multimap walk
  98   }
  99
 100   SpatialPositionedIndex::iterator sit = bucketEntryForPositioned(aPos);
 101   sit->second.erase(aPos);
 102 }
 103
 104 static void
 105 spatialFilterInBucket(const SGBucket& aBucket, FGPositioned::Filter* aFilter, FGPositioned::List& aResult)
 106 {
 107   SpatialPositionedIndex::const_iterator it;
 108   it = global_spatialIndex.find(aBucket.gen_index());
 109   if (it == global_spatialIndex.end()) {
 110     return;
 111   }
 112
 113   BucketEntry::const_iterator l = it->second.begin();
 114   BucketEntry::const_iterator u = it->second.end();
 115
 116   if (!aFilter) { // pass everything
 117     aResult.insert(aResult.end(), l, u);
 118     return;
 119   }
 120
 121   for ( ; l != u; ++l) {
 122     if ((*aFilter)(*l)) {
 123       aResult.push_back(*l);
 124     }
 125   }
 126 }
 127
 128 static void
 129 spatialFind(const SGGeod& aPos, double aRange,
 130   FGPositioned::Filter* aFilter, FGPositioned::List& aResult)
 131 {
 132   SGBucket buck(aPos);
 133   double lat = aPos.getLatitudeDeg(),
 134     lon = aPos.getLongitudeDeg();
 135
 136   int bx = (int)( aRange*SG_NM_TO_METER / buck.get_width_m() / 2);
 137   int by = (int)( aRange*SG_NM_TO_METER / buck.get_height_m() / 2 );
 138
 139   // loop over bucket range
 140   for ( int i=-bx; i<=bx; i++) {
 141     for ( int j=-by; j<=by; j++) {
 142       spatialFilterInBucket(sgBucketOffset(lon, lat, i, j), aFilter, aResult);
 143     } // of j-iteration
 144   } // of i-iteration
 145 }
 146
 147 /*
 148 class LowerLimitOfType
 149 {
 150 public:
 151   bool operator()(const FGPositioned* a, const FGPositioned::Type b) const
 152   {
 153     return a->type() < b;
 154   }
 155
 156   bool operator()(const FGPositioned::Type a, const FGPositioned* b) const
 157   {
 158     return a < b->type();
 159   }
 160 };
 161
 162
 163 static void
 164 spatialFindTyped(const SGGeod& aPos, double aRange, FGPositioned::Type aLower, FGPositioned::Type aUpper, FGPositioned::List& aResult)
 165 {
 166   SGBucket buck(aPos);
 167   double lat = aPos.getLatitudeDeg(),
 168     lon = aPos.getLongitudeDeg();
 169
 170   int bx = (int)( aRange*SG_NM_TO_METER / buck.get_width_m() / 2);
 171   int by = (int)( aRange*SG_NM_TO_METER / buck.get_height_m() / 2 );
 172
 173   // loop over bucket range
 174   for ( int i=-bx; i<=bx; i++) {
 175     for ( int j=-by; j<=by; j++) {
 176       buck = sgBucketOffset(lon, lat, i, j);
 177
 178       SpatialPositionedIndex::const_iterator it;
 179       it = global_spatialIndex.find(buck.gen_index());
 180       if (it == global_spatialIndex.end()) {
 181         continue;
 182       }
 183
 184       BucketEntry::const_iterator l = std::lower_bound(it->second.begin(), it->second.end(), aLower, LowerLimitOfType());
 185       BucketEntry::const_iterator u = std::upper_bound(l, it->second.end(), aUpper, LowerLimitOfType());
 186
 187       for ( ; l != u; ++l) {
 188         aResult.push_back(*l);
 189       }
 190
 191     } // of j-iteration
 192   } // of i-iteration
 193 }
 194 */
 195
 196 /**
 197  */
 198 class RangePredictate
 199 {
 200 public:
 201   RangePredictate(const SGGeod& aOrigin, double aRange) :
 202     mOrigin(aOrigin),
 203     mRange(aRange)
 204   { ; }
 205
 206   bool operator()(const FGPositionedRef& aPos)
 207   {
 208     double d, az1, az2;
 209     SGGeodesy::inverse(aPos->geod(), mOrigin, az1, az2, d);
 210     return (d > mRange);
 211   }
 212
 213 private:
 214   SGGeod mOrigin;
 215   double mRange;
 216 };
 217
 218 static void
 219 filterListByRange(const SGGeod& aPos, double aRange, FGPositioned::List& aResult)
 220 {
 221   RangePredictate pred(aPos, aRange * SG_NM_TO_METER);
 222   FGPositioned::List::iterator newEnd;
 223   newEnd = std::remove_if(aResult.begin(), aResult.end(), pred);
 224   aResult.erase(newEnd, aResult.end());
 225 }
 226
 227 class DistanceOrdering
 228 {
 229 public:
 230   DistanceOrdering(const SGGeod& aPos) :
 231     mPos(aPos)
 232   { }
 233
 234   bool operator()(const FGPositionedRef& a, const FGPositionedRef& b) const
 235   {
 236     double dA, dB, az1, az2;
 237     SGGeodesy::inverse(mPos, a->geod(), az1, az2, dA);
 238     SGGeodesy::inverse(mPos, b->geod(), az1, az2, dB);
 239     return dA < dB;
 240   }
 241
 242 private:
 243   SGGeod mPos;
 244 };
 245
 246 static void
 247 sortByDistance(const SGGeod& aPos, FGPositioned::List& aResult)
 248 {
 249   std::sort(aResult.begin(), aResult.end(), DistanceOrdering(aPos));
 250 }
 251
 252 static FGPositionedRef
 253 namedFindClosest(const std::string& aIdent, const SGGeod& aOrigin, FGPositioned::Filter* aFilter)
 254 {
 255   NamedIndexRange range = global_namedIndex.equal_range(aIdent);
 256   if (range.first == range.second) {
 257     return NULL;
 258   }
 259
 260 // common case, only one result. looks a bit ugly because these are
 261 // sequential iterators, not random-access ones
 262   NamedPositionedIndex::const_iterator check = range.first;
 263   if (++check == range.second) {
 264     // excellent, only one match in the range - all we care about is the type
 265     if (aFilter && !aFilter->pass(range.first->second)) {
 266       return NULL; // type check failed
 267     }
 268
 269     return range.first->second;
 270   } // of short-circuit logic for single-element range
 271
 272 // multiple matches, we need to actually check the distance to each one
 273   double minDist = HUGE_VAL;
 274   FGPositionedRef result;
 275   NamedPositionedIndex::const_iterator it = range.first;
 276
 277   for (; it != range.second; ++it) {
 278     if (aFilter && !aFilter->pass(range.first->second)) {
 279       continue;
 280     }
 281
 282   // find distance
 283     double d, az1, az2;
 284     SGGeodesy::inverse(aOrigin, it->second->geod(), az1, az2, d);
 285     if (d < minDist) {
 286       minDist = d;
 287       result = it->second;
 288     }
 289   }
 290
 291   return result;
 292 }
 293
 294 static FGPositioned::List
 295 spatialGetClosest(const SGGeod& aPos, unsigned int aN, double aCutoffNm, FGPositioned::Filter* aFilter)
 296 {
 297   FGPositioned::List result;
 298   int radius = 1; // start at 1, radius 0 is handled explicitly
 299   SGBucket buck;
 300   double lat = aPos.getLatitudeDeg(),
 301     lon = aPos.getLongitudeDeg();
 302   // final cutoff is in metres, and scaled to account for testing the corners
 303   // of the 'box' instead of the centre of each edge
 304   double cutoffM = aCutoffNm * SG_NM_TO_METER * 1.5;
 305
 306   // base case, simplifes loop to do it seperately here
 307   spatialFilterInBucket(sgBucketOffset(lon, lat, 0, 0), aFilter, result);
 308
 309   for (;result.size() < aN; ++radius) {
 310     // cutoff check
 311     double az1, az2, d1, d2;
 312     SGGeodesy::inverse(aPos, sgBucketOffset(lon, lat, -radius, -radius).get_center(), az1, az2, d1);
 313     SGGeodesy::inverse(aPos, sgBucketOffset(lon, lat, radius, radius).get_center(), az1, az2, d2);
 314
 315     if ((d1 > cutoffM) && (d2 > cutoffM)) {
 316       //std::cerr << "spatialGetClosest terminating due to range cutoff" << std::endl;
 317       break;
 318     }
 319
 320     FGPositioned::List hits;
 321     for ( int i=-radius; i<=radius; i++) {
 322       spatialFilterInBucket(sgBucketOffset(lon, lat, i, -radius), aFilter, hits);
 323       spatialFilterInBucket(sgBucketOffset(lon, lat, -radius, i), aFilter, hits);
 324       spatialFilterInBucket(sgBucketOffset(lon, lat, i, radius), aFilter, hits);
 325       spatialFilterInBucket(sgBucketOffset(lon, lat, radius, i), aFilter, hits);
 326     }
 327
 328     result.insert(result.end(), hits.begin(), hits.end()); // append
 329   } // of outer loop
 330
 331   if (result.size() > aN) {
 332     result.resize(aN); // truncate at requested number of matches
 333   }
 334
 335   sortByDistance(aPos, result);
 336   return result;
 337 }
 338
 339 ///////////////////////////////////////////////////////////////////////////////
 340
 341 FGPositioned::FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos, bool aIndexed) :
 342   mType(ty),
 343   mPosition(aPos),
 344   mIdent(aIdent)
 345 {
 346   SGReferenced::get(this); // hold an owning ref, for the moment
 347
 348   if (aIndexed) {
 349     assert(ty != TAXIWAY);
 350     addToIndices(this);
 351   }
 352 }
 353
 354 FGPositioned::~FGPositioned()
 355 {
 356   removeFromIndices(this);
 357 }
 358
 359 SGBucket
 360 FGPositioned::bucket() const
 361 {
 362   return SGBucket(mPosition);
 363 }
 364
 365 const char* FGPositioned::nameForType(Type aTy)
 366 {
 367  switch (aTy) {
 368  case FIX: return "fix";
 369  case VOR: return "VOR";
 370  case NDB: return "NDB";
 371  case OM: return "outer-marker";
 372  case MM: return "middle-marker";
 373  case IM: return "inner-marker";
 374  case AIRPORT: return "airport";
 375  case HELIPORT: return "heliport";
 376  case SEAPORT: return "seaport";
 377  case WAYPOINT: return "waypoint";
 378  default:
 379   return "unknown";
 380  }
 381 }
 382
 383 ///////////////////////////////////////////////////////////////////////////////
 384 // search / query functions
 385
 386 FGPositionedRef
 387 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
 388 {
 389   return namedFindClosest(aIdent, aPos, aFilter);
 390 }
 391
 392 FGPositioned::List
 393 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter)
 394 {
 395   List result;
 396   spatialFind(aPos, aRangeNm, aFilter, result);
 397   filterListByRange(aPos, aRangeNm, result);
 398   return result;
 399 }
 400
 401 FGPositioned::List
 402 FGPositioned::findAllWithIdentSortedByRange(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
 403 {
 404   List result;
 405   NamedIndexRange range = global_namedIndex.equal_range(aIdent);
 406   for (; range.first != range.second; ++range.first) {
 407     if (aFilter && !aFilter->pass(range.first->second)) {
 408       continue;
 409     }
 410
 411     result.push_back(range.first->second);
 412   }
 413
 414   sortByDistance(aPos, result);
 415   return result;
 416 }
 417
 418 FGPositionedRef
 419 FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
 420 {
 421    FGPositioned::List l(spatialGetClosest(aPos, 1, aCutoffNm, aFilter));
 422    if (l.empty()) {
 423       return NULL;
 424    }
 425
 426    assert(l.size() == 1);
 427    return l.front();
 428 }
 429
 430 FGPositioned::List
 431 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter)
 432 {
 433   return spatialGetClosest(aPos, aN, aCutoffNm, aFilter);
 434 }
 435
 436 FGPositionedRef
 437 FGPositioned::findNextWithPartialId(FGPositionedRef aCur, const std::string& aId, Filter* aFilter)
 438 {
 439   NamedIndexRange range = global_namedIndex.equal_range(aId);
 440   for (; range.first != range.second; ++range.first) {
 441     FGPositionedRef candidate = range.first->second;
 442     if (aCur == candidate) {
 443       aCur = NULL; // found our start point, next match will pass
 444       continue;
 445     }
 446
 447     if (aFilter && !aFilter->pass(candidate)) {
 448       continue;
 449     }
 450
 451     if (!aCur) {
 452       return candidate;
 453     }
 454   }
 455
 456   return NULL; // fell out, no match in range
 457 }
 458