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 <iostream>
  30
  31 #include <simgear/math/sg_geodesy.hxx>
  32
  33 #include "positioned.hxx"
  34
  35 typedef std::multimap<std::string, FGPositioned*> NamedPositionedIndex;
  36 typedef std::pair<NamedPositionedIndex::const_iterator, NamedPositionedIndex::const_iterator> NamedIndexRange;
  37
  38 /**
  39  * Order positioned elements by type, then pointer address. This allows us to
  40  * use range searches (lower_ and upper_bound) to grab items of a particular
  41  * type out of bucket efficently.
  42  */
  43 class OrderByType
  44 {
  45 public:
  46   bool operator()(const FGPositioned* a, const FGPositioned* b) const
  47   {
  48     if (a->type() == b->type()) return a < b;
  49     return a->type() < b->type();
  50   }
  51 };
  52
  53 typedef std::set<FGPositioned*, OrderByType> BucketEntry;
  54 typedef std::map<long int, BucketEntry> SpatialPositionedIndex;
  55
  56 static NamedPositionedIndex global_namedIndex;
  57 static SpatialPositionedIndex global_spatialIndex;
  58
  59 SpatialPositionedIndex::iterator
  60 bucketEntryForPositioned(FGPositioned* aPos)
  61 {
  62   int bucketIndex = aPos->bucket().gen_index();
  63   SpatialPositionedIndex::iterator it = global_spatialIndex.find(bucketIndex);
  64   if (it != global_spatialIndex.end()) {
  65     return it;
  66   }
  67
  68   // create a new BucketEntry
  69   return global_spatialIndex.insert(it, std::make_pair(bucketIndex, BucketEntry()));
  70 }
  71
  72 static void
  73 addToIndices(FGPositioned* aPos)
  74 {
  75   assert(aPos);
  76   if (!aPos->ident().empty()) {
  77     global_namedIndex.insert(global_namedIndex.begin(),
  78       std::make_pair(aPos->ident(), aPos));
  79   }
  80
  81   SpatialPositionedIndex::iterator it = bucketEntryForPositioned(aPos);
  82   it->second.insert(aPos);
  83 }
  84
  85 static void
  86 removeFromIndices(FGPositioned* aPos)
  87 {
  88   assert(aPos);
  89
  90   if (!aPos->ident().empty()) {
  91     NamedPositionedIndex::iterator it = global_namedIndex.find(aPos->ident());
  92     while (it != global_namedIndex.end() && (it->first == aPos->ident())) {
  93       if (it->second == aPos) {
  94         global_namedIndex.erase(it);
  95         break;
  96       }
  97
  98       ++it;
  99     } // of multimap walk
 100   }
 101
 102   SpatialPositionedIndex::iterator sit = bucketEntryForPositioned(aPos);
 103   sit->second.erase(aPos);
 104 }
 105
 106 static void
 107 spatialFilterInBucket(const SGBucket& aBucket, FGPositioned::Filter* aFilter, FGPositioned::List& aResult)
 108 {
 109   SpatialPositionedIndex::const_iterator it;
 110   it = global_spatialIndex.find(aBucket.gen_index());
 111   if (it == global_spatialIndex.end()) {
 112     return;
 113   }
 114
 115   BucketEntry::const_iterator l = it->second.begin();
 116   BucketEntry::const_iterator u = it->second.end();
 117
 118   if (!aFilter) { // pass everything
 119     aResult.insert(aResult.end(), l, u);
 120     return;
 121   }
 122
 123   for ( ; l != u; ++l) {
 124     if ((*aFilter)(*l)) {
 125       aResult.push_back(*l);
 126     }
 127   }
 128 }
 129
 130 static void
 131 spatialFind(const SGGeod& aPos, double aRange,
 132   FGPositioned::Filter* aFilter, FGPositioned::List& aResult)
 133 {
 134   SGBucket buck(aPos);
 135   double lat = aPos.getLatitudeDeg(),
 136     lon = aPos.getLongitudeDeg();
 137
 138   int bx = (int)( aRange*SG_NM_TO_METER / buck.get_width_m() / 2);
 139   int by = (int)( aRange*SG_NM_TO_METER / buck.get_height_m() / 2 );
 140
 141   // loop over bucket range
 142   for ( int i=-bx; i<=bx; i++) {
 143     for ( int j=-by; j<=by; j++) {
 144       spatialFilterInBucket(sgBucketOffset(lon, lat, i, j), aFilter, aResult);
 145     } // of j-iteration
 146   } // of i-iteration
 147 }
 148
 149 /*
 150 class LowerLimitOfType
 151 {
 152 public:
 153   bool operator()(const FGPositioned* a, const FGPositioned::Type b) const
 154   {
 155     return a->type() < b;
 156   }
 157
 158   bool operator()(const FGPositioned::Type a, const FGPositioned* b) const
 159   {
 160     return a < b->type();
 161   }
 162 };
 163
 164
 165 static void
 166 spatialFindTyped(const SGGeod& aPos, double aRange, FGPositioned::Type aLower, FGPositioned::Type aUpper, FGPositioned::List& aResult)
 167 {
 168   SGBucket buck(aPos);
 169   double lat = aPos.getLatitudeDeg(),
 170     lon = aPos.getLongitudeDeg();
 171
 172   int bx = (int)( aRange*SG_NM_TO_METER / buck.get_width_m() / 2);
 173   int by = (int)( aRange*SG_NM_TO_METER / buck.get_height_m() / 2 );
 174
 175   // loop over bucket range
 176   for ( int i=-bx; i<=bx; i++) {
 177     for ( int j=-by; j<=by; j++) {
 178       buck = sgBucketOffset(lon, lat, i, j);
 179
 180       SpatialPositionedIndex::const_iterator it;
 181       it = global_spatialIndex.find(buck.gen_index());
 182       if (it == global_spatialIndex.end()) {
 183         continue;
 184       }
 185
 186       BucketEntry::const_iterator l = std::lower_bound(it->second.begin(), it->second.end(), aLower, LowerLimitOfType());
 187       BucketEntry::const_iterator u = std::upper_bound(l, it->second.end(), aUpper, LowerLimitOfType());
 188
 189       for ( ; l != u; ++l) {
 190         aResult.push_back(*l);
 191       }
 192
 193     } // of j-iteration
 194   } // of i-iteration
 195 }
 196 */
 197
 198 /**
 199  */
 200 class RangePredictate
 201 {
 202 public:
 203   RangePredictate(const SGGeod& aOrigin, double aRange) :
 204     mOrigin(SGVec3d::fromGeod(aOrigin)),
 205     mRangeSqr(aRange * aRange)
 206   { ; }
 207
 208   bool operator()(const FGPositionedRef& aPos)
 209   {
 210     double dSqr = distSqr(aPos->cart(), mOrigin);
 211     return (dSqr > mRangeSqr);
 212   }
 213
 214 private:
 215   SGVec3d mOrigin;
 216   double mRangeSqr;
 217 };
 218
 219 static void
 220 filterListByRange(const SGGeod& aPos, double aRange, FGPositioned::List& aResult)
 221 {
 222   RangePredictate pred(aPos, aRange * SG_NM_TO_METER);
 223   FGPositioned::List::iterator newEnd;
 224   newEnd = std::remove_if(aResult.begin(), aResult.end(), pred);
 225   aResult.erase(newEnd, aResult.end());
 226 }
 227
 228 class DistanceOrdering
 229 {
 230 public:
 231   DistanceOrdering(const SGGeod& aPos) :
 232     mPos(SGVec3d::fromGeod(aPos))
 233   { }
 234
 235   bool operator()(const FGPositionedRef& a, const FGPositionedRef& b) const
 236   {
 237     double dA = distSqr(a->cart(), mPos),
 238       dB = distSqr(b->cart(), mPos);
 239     return dA < dB;
 240   }
 241
 242 private:
 243   SGVec3d 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   SGVec3d cartOrigin(SGVec3d::fromGeod(aOrigin));
 277
 278   for (; it != range.second; ++it) {
 279     if (aFilter && !aFilter->pass(range.first->second)) {
 280       continue;
 281     }
 282
 283   // find distance
 284     double d2 = distSqr(cartOrigin, it->second->cart());
 285     if (d2 < minDist) {
 286       minDist = d2;
 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   sortByDistance(aPos, result);
 332   if (result.size() > aN) {
 333     result.resize(aN); // truncate at requested number of matches
 334   }
 335
 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   //std::cout << "destroying:" << mIdent << "/" << nameForType(mType) << std::endl;
 357   removeFromIndices(this);
 358 }
 359
 360 SGBucket
 361 FGPositioned::bucket() const
 362 {
 363   return SGBucket(mPosition);
 364 }
 365
 366 SGVec3d
 367 FGPositioned::cart() const
 368 {
 369   return SGVec3d::fromGeod(mPosition);
 370 }
 371
 372 const char* FGPositioned::nameForType(Type aTy)
 373 {
 374  switch (aTy) {
 375  case RUNWAY: return "runway";
 376  case TAXIWAY: return "taxiway";
 377  case PARK_STAND: return "parking stand";
 378  case FIX: return "fix";
 379  case VOR: return "VOR";
 380  case NDB: return "NDB";
 381  case ILS: return "ILS";
 382  case LOC: return "localiser";
 383  case GS: return "glideslope";
 384  case OM: return "outer-marker";
 385  case MM: return "middle-marker";
 386  case IM: return "inner-marker";
 387  case AIRPORT: return "airport";
 388  case HELIPORT: return "heliport";
 389  case SEAPORT: return "seaport";
 390  case WAYPOINT: return "waypoint";
 391  case DME: return "dme";
 392  case TACAN: return "tacan";
 393  default:
 394   return "unknown";
 395  }
 396 }
 397
 398 ///////////////////////////////////////////////////////////////////////////////
 399 // search / query functions
 400
 401 FGPositionedRef
 402 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
 403 {
 404   return namedFindClosest(aIdent, aPos, aFilter);
 405 }
 406
 407 FGPositioned::List
 408 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter)
 409 {
 410   List result;
 411   spatialFind(aPos, aRangeNm, aFilter, result);
 412   filterListByRange(aPos, aRangeNm, result);
 413   return result;
 414 }
 415
 416 FGPositioned::List
 417 FGPositioned::findAllWithIdentSortedByRange(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
 418 {
 419   List result;
 420   NamedIndexRange range = global_namedIndex.equal_range(aIdent);
 421   for (; range.first != range.second; ++range.first) {
 422     if (aFilter && !aFilter->pass(range.first->second)) {
 423       continue;
 424     }
 425
 426     result.push_back(range.first->second);
 427   }
 428
 429   sortByDistance(aPos, result);
 430   return result;
 431 }
 432
 433 FGPositionedRef
 434 FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
 435 {
 436    FGPositioned::List l(spatialGetClosest(aPos, 1, aCutoffNm, aFilter));
 437    if (l.empty()) {
 438       return NULL;
 439    }
 440
 441    assert(l.size() == 1);
 442    return l.front();
 443 }
 444
 445 FGPositioned::List
 446 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter)
 447 {
 448   return spatialGetClosest(aPos, aN, aCutoffNm, aFilter);
 449 }
 450
 451 FGPositionedRef
 452 FGPositioned::findNextWithPartialId(FGPositionedRef aCur, const std::string& aId, Filter* aFilter)
 453 {
 454   NamedIndexRange range = global_namedIndex.equal_range(aId);
 455   for (; range.first != range.second; ++range.first) {
 456     FGPositionedRef candidate = range.first->second;
 457     if (aCur == candidate) {
 458       aCur = NULL; // found our start point, next match will pass
 459       continue;
 460     }
 461
 462     if (aFilter && !aFilter->pass(candidate)) {
 463       continue;
 464     }
 465
 466     if (!aCur) {
 467       return candidate;
 468     }
 469   }
 470
 471   return NULL; // fell out, no match in range
 472 }
 473