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
  28 #include <simgear/math/sg_geodesy.hxx>
  29
  30 #include "positioned.hxx"
  31
  32 typedef std::multimap<std::string, FGPositioned*> NamedPositionedIndex;
  33 typedef std::pair<NamedPositionedIndex::const_iterator, NamedPositionedIndex::const_iterator> NamedIndexRange;
  34
  35 /**
  36  * Order positioned elements by type, then pointer address. This allows us to
  37  * use range searches (lower_ and upper_bound) to grab items of a particular
  38  * type out of bucket efficently.
  39  */
  40 class OrderByType
  41 {
  42 public:
  43   bool operator()(const FGPositioned* a, const FGPositioned* b) const
  44   {
  45     if (a->type() == b->type()) return a < b;
  46     return a->type() < b->type();
  47   }
  48 };
  49
  50 typedef std::set<FGPositioned*, OrderByType> BucketEntry;
  51 typedef std::map<long int, BucketEntry> SpatialPositionedIndex;
  52
  53 static NamedPositionedIndex global_namedIndex;
  54 static SpatialPositionedIndex global_spatialIndex;
  55
  56 SpatialPositionedIndex::iterator
  57 bucketEntryForPositioned(FGPositioned* aPos)
  58 {
  59   int bucketIndex = aPos->bucket().gen_index();
  60   SpatialPositionedIndex::iterator it = global_spatialIndex.find(bucketIndex);
  61   if (it != global_spatialIndex.end()) {
  62     return it;
  63   }
  64
  65   // create a new BucketEntry
  66   return global_spatialIndex.insert(it, std::make_pair(bucketIndex, BucketEntry()));
  67 }
  68
  69 static void
  70 addToIndices(FGPositioned* aPos)
  71 {
  72   assert(aPos);
  73   global_namedIndex.insert(global_namedIndex.begin(),
  74     std::make_pair(aPos->ident(), aPos));
  75
  76   SpatialPositionedIndex::iterator it = bucketEntryForPositioned(aPos);
  77   it->second.insert(aPos);
  78 }
  79
  80 static void
  81 removeFromIndices(FGPositioned* aPos)
  82 {
  83   assert(aPos);
  84
  85   NamedPositionedIndex::iterator it = global_namedIndex.find(aPos->ident());
  86   while (it != global_namedIndex.end() && (it->first == aPos->ident())) {
  87     if (it->second == aPos) {
  88       global_namedIndex.erase(it);
  89       break;
  90     }
  91
  92     ++it;
  93   }
  94
  95   SpatialPositionedIndex::iterator sit = bucketEntryForPositioned(aPos);
  96   sit->second.erase(aPos);
  97 }
  98
  99 static void
 100 spatialFilterInBucket(const SGBucket& aBucket, const FGPositioned::Filter& aFilter, FGPositioned::List& aResult)
 101 {
 102   SpatialPositionedIndex::const_iterator it;
 103   it = global_spatialIndex.find(aBucket.gen_index());
 104   if (it == global_spatialIndex.end()) {
 105     return;
 106   }
 107
 108   BucketEntry::const_iterator l = it->second.begin();
 109   BucketEntry::const_iterator u = it->second.end();
 110
 111   for ( ; l != u; ++l) {
 112     if (aFilter(*l)) {
 113       aResult.push_back(*l);
 114     }
 115   }
 116 }
 117
 118 static void
 119 spatialFind(const SGGeod& aPos, double aRange,
 120   const FGPositioned::Filter& aFilter, FGPositioned::List& aResult)
 121 {
 122   SGBucket buck(aPos);
 123   double lat = aPos.getLatitudeDeg(),
 124     lon = aPos.getLongitudeDeg();
 125
 126   int bx = (int)( aRange*SG_NM_TO_METER / buck.get_width_m() / 2);
 127   int by = (int)( aRange*SG_NM_TO_METER / buck.get_height_m() / 2 );
 128
 129   // loop over bucket range
 130   for ( int i=-bx; i<=bx; i++) {
 131     for ( int j=-by; j<=by; j++) {
 132       spatialFilterInBucket(sgBucketOffset(lon, lat, i, j), aFilter, aResult);
 133     } // of j-iteration
 134   } // of i-iteration
 135 }
 136
 137 class LowerLimitOfType
 138 {
 139 public:
 140   bool operator()(const FGPositioned* a, const FGPositioned::Type b) const
 141   {
 142     return a->type() < b;
 143   }
 144
 145   bool operator()(const FGPositioned::Type a, const FGPositioned* b) const
 146   {
 147     return a < b->type();
 148   }
 149 };
 150
 151 static void
 152 spatialFindTyped(const SGGeod& aPos, double aRange, FGPositioned::Type aLower, FGPositioned::Type aUpper, FGPositioned::List& aResult)
 153 {
 154   SGBucket buck(aPos);
 155   double lat = aPos.getLatitudeDeg(),
 156     lon = aPos.getLongitudeDeg();
 157
 158   int bx = (int)( aRange*SG_NM_TO_METER / buck.get_width_m() / 2);
 159   int by = (int)( aRange*SG_NM_TO_METER / buck.get_height_m() / 2 );
 160
 161   // loop over bucket range
 162   for ( int i=-bx; i<=bx; i++) {
 163     for ( int j=-by; j<=by; j++) {
 164       buck = sgBucketOffset(lon, lat, i, j);
 165
 166       SpatialPositionedIndex::const_iterator it;
 167       it = global_spatialIndex.find(buck.gen_index());
 168       if (it == global_spatialIndex.end()) {
 169         continue;
 170       }
 171
 172       BucketEntry::const_iterator l = std::lower_bound(it->second.begin(), it->second.end(), aLower, LowerLimitOfType());
 173       BucketEntry::const_iterator u = std::upper_bound(l, it->second.end(), aUpper, LowerLimitOfType());
 174
 175       for ( ; l != u; ++l) {
 176         aResult.push_back(*l);
 177       }
 178
 179     } // of j-iteration
 180   } // of i-iteration
 181 }
 182
 183 /**
 184  * Cartesian range predicate. Note that for really long ranges, might need to
 185  * to use geodetic / geocentric distance instead
 186  */
 187 class RangePredictate
 188 {
 189 public:
 190   RangePredictate(const Point3D& aOrigin, double aRange) :
 191     mOrigin(aOrigin),
 192     mRangeSquared(aRange * aRange)
 193   { ; }
 194
 195   bool operator()(const FGPositionedRef& aPos)
 196   {
 197     Point3D p(Point3D::fromSGGeod(aPos->geod()));
 198     bool ok = (mOrigin.distance3Dsquared(p) > mRangeSquared);
 199     if (ok) {
 200       double x = sqrt(mOrigin.distance3Dsquared(p) - mRangeSquared);
 201       x *= SG_METER_TO_NM;
 202       //std::cout << "pos:" << aPos->ident() << " failed range check by " << x << std::endl;
 203     }
 204     return ok;
 205   }
 206
 207 private:
 208   Point3D mOrigin;
 209   double mRangeSquared;
 210 };
 211
 212 static void
 213 filterListByRange(const SGGeod& aPos, double aRange, FGPositioned::List& aResult)
 214 {
 215   RangePredictate pred(Point3D::fromSGGeod(aPos), aRange * SG_NM_TO_METER);
 216   FGPositioned::List::iterator newEnd;
 217   newEnd = std::remove_if(aResult.begin(), aResult.end(), pred);
 218   aResult.erase(newEnd, aResult.end());
 219 }
 220
 221 class DistanceOrdering
 222 {
 223 public:
 224   DistanceOrdering(const SGGeod& aPos) :
 225     mPos(Point3D::fromSGGeod(aPos))
 226   { }
 227
 228   bool operator()(const FGPositionedRef& a, const FGPositionedRef& b) const
 229   {
 230     return mPos.distance3Dsquared(Point3D::fromSGGeod(a->geod())) <
 231       mPos.distance3Dsquared(Point3D::fromSGGeod(b->geod()));
 232   }
 233
 234 private:
 235   Point3D mPos;
 236 };
 237
 238 static void
 239 sortByDistance(const SGGeod& aPos, FGPositioned::List& aResult)
 240 {
 241   std::sort(aResult.begin(), aResult.end(), DistanceOrdering(aPos));
 242 }
 243
 244 static FGPositionedRef
 245 namedFindClosestTyped(const std::string& aIdent, const SGGeod& aOrigin,
 246   FGPositioned::Type aLower, FGPositioned::Type aUpper)
 247 {
 248   NamedIndexRange range = global_namedIndex.equal_range(aIdent);
 249   if (range.first == range.second) return NULL;
 250
 251 // common case, only one result. looks a bit ugly because these are
 252 // sequential iterators, not random-access ones
 253   NamedPositionedIndex::const_iterator check = range.first;
 254   if (++check == range.second) {
 255     // excellent, only one match in the range - all we care about is the type
 256     FGPositioned::Type ty = range.first->second->type();
 257     if ((ty < aLower) || (ty > aUpper)) {
 258       return NULL; // type check failed
 259     }
 260
 261     return range.first->second;
 262   } // of short-circuit logic for single-element range
 263
 264 // multiple matches, we need to actually check the distance to each one
 265   double minDist = HUGE_VAL;
 266   FGPositionedRef result;
 267   Point3D origin(Point3D::fromSGGeod(aOrigin));
 268
 269   for (; range.first != range.second; ++range.first) {
 270   // filter by type
 271     FGPositioned::Type ty = range.first->second->type();
 272     if ((ty < aLower) || (ty > aUpper)) {
 273       continue;
 274     }
 275
 276   // find distance
 277     Point3D p(Point3D::fromSGGeod(range.first->second->geod()));
 278     double ds = origin.distance3Dsquared(p);
 279     if (ds < minDist) {
 280       minDist = ds;
 281       result = range.first->second;
 282     }
 283   }
 284
 285   return result;
 286 }
 287
 288 static FGPositioned::List
 289 spatialGetClosest(const SGGeod& aPos, unsigned int aN, double aCutoffNm, const FGPositioned::Filter& aFilter)
 290 {
 291   FGPositioned::List result;
 292   int radius = 1; // start at 1, radius 0 is handled explicitly
 293   SGBucket buck;
 294   double lat = aPos.getLatitudeDeg(),
 295     lon = aPos.getLongitudeDeg();
 296   // final cutoff is in metres, and scaled to account for testing the corners
 297   // of the 'box' instead of the centre of each edge
 298   double cutoffM = aCutoffNm * SG_NM_TO_METER * 1.5;
 299
 300   // base case, simplifes loop to do it seperately here
 301   spatialFilterInBucket(sgBucketOffset(lon, lat, 0, 0), aFilter, result);
 302
 303   for (;result.size() < aN; ++radius) {
 304     // cutoff check
 305     double az1, az2, d1, d2;
 306     SGGeodesy::inverse(aPos, sgBucketOffset(lon, lat, -radius, -radius).get_center(), az1, az2, d1);
 307     SGGeodesy::inverse(aPos, sgBucketOffset(lon, lat, radius, radius).get_center(), az1, az2, d2);
 308
 309     if ((d1 > cutoffM) && (d2 > cutoffM)) {
 310       //std::cerr << "spatialGetClosest terminating due to range cutoff" << std::endl;
 311       break;
 312     }
 313
 314     FGPositioned::List hits;
 315     for ( int i=-radius; i<=radius; i++) {
 316       spatialFilterInBucket(sgBucketOffset(lon, lat, i, -radius), aFilter, hits);
 317       spatialFilterInBucket(sgBucketOffset(lon, lat, -radius, i), aFilter, hits);
 318       spatialFilterInBucket(sgBucketOffset(lon, lat, i, radius), aFilter, hits);
 319       spatialFilterInBucket(sgBucketOffset(lon, lat, radius, i), aFilter, hits);
 320     }
 321
 322     result.insert(result.end(), hits.begin(), hits.end()); // append
 323   } // of outer loop
 324
 325   if (result.size() > aN) {
 326     result.resize(aN); // truncate at requested number of matches
 327   }
 328
 329   sortByDistance(aPos, result);
 330   return result;
 331 }
 332
 333 ///////////////////////////////////////////////////////////////////////////////
 334
 335 FGPositioned::FGPositioned() :
 336   mType(INVALID)
 337 {
 338 }
 339
 340 FGPositioned::FGPositioned(Type ty, const std::string& aIdent, double aLat, double aLon, double aElev) :
 341   mType(ty),
 342   mIdent(aIdent),
 343   mPosition(SGGeod::fromDegFt(aLon, aLat, aElev))
 344 {
 345   //addToIndices(this);
 346   //SGReferenced::get(this); // hold an owning ref, for the moment
 347 }
 348
 349 FGPositioned::FGPositioned(Type ty, const std::string& aIdent, const SGGeod& aPos) :
 350   mType(ty),
 351   mIdent(aIdent),
 352   mPosition(aPos)
 353 {
 354   //addToIndices(this);
 355   //SGReferenced::get(this); // hold an owning ref, for the moment
 356 }
 357
 358 FGPositioned::~FGPositioned()
 359 {
 360   //std::cout << "~FGPositioned:" << mIdent << std::endl;
 361   //removeFromIndices(this);
 362 }
 363
 364 SGBucket
 365 FGPositioned::bucket() const
 366 {
 367   return SGBucket(mPosition);
 368 }
 369
 370 const char* FGPositioned::nameForType(Type aTy)
 371 {
 372  switch (aTy) {
 373  case FIX: return "fix";
 374  case VOR: return "VOR";
 375  case NDB: return "NDB";
 376  case OM: return "outer-marker";
 377  case MM: return "middle-marker";
 378  case IM: return "inner-marker";
 379  case AIRPORT: return "airport";
 380  case HELIPORT: return "heliport";
 381  case SEAPORT: return "seaport";
 382  case WAYPOINT: return "waypoint";
 383  default:
 384   return "unknown";
 385  }
 386 }
 387
 388 ///////////////////////////////////////////////////////////////////////////////
 389 // search / query functions
 390
 391 FGPositionedRef
 392 FGPositioned::findClosestWithIdent(const std::string& aIdent, double aLat, double aLon)
 393 {
 394   return findClosestWithIdent(aIdent, SGGeod::fromDeg(aLon, aLat));
 395 }
 396
 397 FGPositionedRef
 398 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos)
 399 {
 400   return namedFindClosestTyped(aIdent, aPos, INVALID, LAST_TYPE);
 401 }
 402
 403 FGPositioned::List
 404 FGPositioned::findWithinRangeByType(const SGGeod& aPos, double aRangeNm, Type aTy)
 405 {
 406   List result;
 407   spatialFindTyped(aPos, aRangeNm, aTy, aTy, result);
 408   filterListByRange(aPos, aRangeNm, result);
 409   return result;
 410 }
 411
 412 FGPositioned::List
 413 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, const Filter& aFilter)
 414 {
 415   List result;
 416   spatialFind(aPos, aRangeNm, aFilter, result);
 417   filterListByRange(aPos, aRangeNm, result);
 418   return result;
 419 }
 420
 421 FGPositioned::List
 422 FGPositioned::findAllWithIdent(const std::string& aIdent)
 423 {
 424   List result;
 425   NamedIndexRange range = global_namedIndex.equal_range(aIdent);
 426   for (; range.first != range.second; ++range.first) {
 427     result.push_back(range.first->second);
 428   }
 429
 430   return result;
 431 }
 432
 433 FGPositioned::List
 434 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, const Filter& aFilter)
 435 {
 436   return spatialGetClosest(aPos, aN, aCutoffNm, aFilter);
 437 }
 438