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 "positioned.hxx"
  26
  27 #include <map>
  28 #include <set>
  29 #include <algorithm> // for sort
  30 #include <queue>
  31 #include <memory>
  32
  33 #include <boost/foreach.hpp>
  34 #include <boost/algorithm/string/case_conv.hpp>
  35 #include <boost/algorithm/string/predicate.hpp>
  36
  37 #include <simgear/timing/timestamp.hxx>
  38 #include <simgear/debug/logstream.hxx>
  39 #include <simgear/structure/exception.hxx>
  40 #include <simgear/math/SGGeometry.hxx>
  41 #include <simgear/sg_inlines.h>
  42
  43 #include "Navaids/PositionedOctree.hxx"
  44
  45 using std::string;
  46 using namespace flightgear;
  47
  48 static void validateSGGeod(const SGGeod& geod)
  49 {
  50   if (SGMisc<double>::isNaN(geod.getLatitudeDeg()) ||
  51       SGMisc<double>::isNaN(geod.getLongitudeDeg()))
  52   {
  53     throw sg_range_exception("position is invalid, NaNs");
  54   }
  55 }
  56
  57 static bool validateFilter(FGPositioned::Filter* filter)
  58 {
  59     if (filter->maxType() < filter->minType()) {
  60         SG_LOG(SG_GENERAL, SG_WARN, "invalid positioned filter specified");
  61         return false;
  62     }
  63
  64     return true;
  65 }
  66
  67
  68 ///////////////////////////////////////////////////////////////////////////////
  69
  70 FGPositioned::FGPositioned( PositionedID aGuid,
  71                             Type ty,
  72                             const std::string& aIdent,
  73                             const SGGeod& aPos ):
  74   mGuid(aGuid),
  75   mType(ty),
  76   mIdent(aIdent),
  77   mPosition(aPos),
  78   mCart(SGVec3d::fromGeod(mPosition))
  79 {
  80
  81 }
  82
  83 FGPositioned::~FGPositioned()
  84 {
  85 //  std::cout << "destroying:" << mIdent << "/" << nameForType(mType) << std::endl;
  86 }
  87
  88 FGPositioned*
  89 FGPositioned::createUserWaypoint(const std::string& aIdent, const SGGeod& aPos)
  90 {
  91   NavDataCache* cache = NavDataCache::instance();
  92   TypeFilter filter(WAYPOINT);
  93   FGPositionedList existing = cache->findAllWithIdent(aIdent, &filter, true);
  94   if (!existing.empty()) {
  95     SG_LOG(SG_NAVAID, SG_WARN, "attempt to insert duplicate WAYPOINT:" << aIdent);
  96     return existing.front().ptr();
  97   }
  98
  99   PositionedID id = cache->createPOI(WAYPOINT, aIdent, aPos);
 100   return cache->loadById(id);
 101 }
 102
 103 bool FGPositioned::deleteUserWaypoint(const std::string& aIdent)
 104 {
 105   NavDataCache* cache = NavDataCache::instance();
 106   return cache->removePOI(WAYPOINT, aIdent);
 107 }
 108
 109
 110 const SGVec3d&
 111 FGPositioned::cart() const
 112 {
 113   return mCart;
 114 }
 115
 116 FGPositioned::Type FGPositioned::typeFromName(const std::string& aName)
 117 {
 118   if (aName.empty() || (aName == "")) {
 119     return INVALID;
 120   }
 121
 122   typedef struct {
 123     const char* _name;
 124     Type _ty;
 125   } NameTypeEntry;
 126
 127   const NameTypeEntry names[] = {
 128     {"airport", AIRPORT},
 129     {"vor", VOR},
 130     {"loc", LOC},
 131     {"ils", ILS},
 132     {"gs", GS},
 133     {"ndb", NDB},
 134     {"wpt", WAYPOINT},
 135     {"fix", FIX},
 136     {"tacan", TACAN},
 137     {"dme", DME},
 138     {"atis", FREQ_ATIS},
 139     {"awos", FREQ_AWOS},
 140     {"tower", FREQ_TOWER},
 141     {"ground", FREQ_GROUND},
 142     {"approach", FREQ_APP_DEP},
 143     {"departure", FREQ_APP_DEP},
 144     {"runway", RUNWAY},
 145     {"helipad", HELIPAD},
 146     {"country", COUNTRY},
 147     {"city", CITY},
 148     {"town", TOWN},
 149     {"village", VILLAGE},
 150
 151   // aliases
 152     {"localizer", LOC},
 153     {"gnd", FREQ_GROUND},
 154     {"twr", FREQ_TOWER},
 155     {"waypoint", WAYPOINT},
 156     {"apt", AIRPORT},
 157     {"arpt", AIRPORT},
 158     {"rwy", RUNWAY},
 159     {"any", INVALID},
 160     {"all", INVALID},
 161
 162     {NULL, INVALID}
 163   };
 164
 165   std::string lowerName(boost::to_lower_copy(aName));
 166
 167   for (const NameTypeEntry* n = names; (n->_name != NULL); ++n) {
 168     if (::strcmp(n->_name, lowerName.c_str()) == 0) {
 169       return n->_ty;
 170     }
 171   }
 172
 173   SG_LOG(SG_NAVAID, SG_WARN, "FGPositioned::typeFromName: couldn't match:" << aName);
 174   return INVALID;
 175 }
 176
 177 const char* FGPositioned::nameForType(Type aTy)
 178 {
 179  switch (aTy) {
 180  case RUNWAY: return "runway";
 181  case HELIPAD: return "helipad";
 182  case TAXIWAY: return "taxiway";
 183  case PAVEMENT: return "pavement";
 184  case PARKING: return "parking stand";
 185  case FIX: return "fix";
 186  case VOR: return "VOR";
 187  case NDB: return "NDB";
 188  case ILS: return "ILS";
 189  case LOC: return "localizer";
 190  case GS: return "glideslope";
 191  case OM: return "outer-marker";
 192  case MM: return "middle-marker";
 193  case IM: return "inner-marker";
 194  case AIRPORT: return "airport";
 195  case HELIPORT: return "heliport";
 196  case SEAPORT: return "seaport";
 197  case WAYPOINT: return "waypoint";
 198  case DME: return "dme";
 199  case TACAN: return "tacan";
 200  case FREQ_TOWER: return "tower";
 201  case FREQ_ATIS: return "atis";
 202  case FREQ_AWOS: return "awos";
 203  case FREQ_GROUND: return "ground";
 204  case FREQ_CLEARANCE: return "clearance";
 205  case FREQ_UNICOM: return "unicom";
 206  case FREQ_APP_DEP: return "approach-departure";
 207  case TAXI_NODE: return "taxi-node";
 208  case COUNTRY: return "country";
 209  case CITY: return "city";
 210  case TOWN: return "town";
 211  case VILLAGE: return "village";
 212  default:
 213   return "unknown";
 214  }
 215 }
 216
 217 ///////////////////////////////////////////////////////////////////////////////
 218 // search / query functions
 219
 220 FGPositionedRef
 221 FGPositioned::findClosestWithIdent(const std::string& aIdent, const SGGeod& aPos, Filter* aFilter)
 222 {
 223   validateSGGeod(aPos);
 224   return NavDataCache::instance()->findClosestWithIdent(aIdent, aPos, aFilter);
 225 }
 226
 227 FGPositionedRef
 228 FGPositioned::findFirstWithIdent(const std::string& aIdent, Filter* aFilter)
 229 {
 230   if (aIdent.empty()) {
 231     return NULL;
 232   }
 233
 234   FGPositionedList r =
 235     NavDataCache::instance()->findAllWithIdent(aIdent, aFilter, true);
 236   if (r.empty()) {
 237     return NULL;
 238   }
 239
 240   return r.front();
 241 }
 242
 243 FGPositionedList
 244 FGPositioned::findWithinRange(const SGGeod& aPos, double aRangeNm, Filter* aFilter)
 245 {
 246   validateSGGeod(aPos);
 247
 248     if (!validateFilter(aFilter)) {
 249         return FGPositionedList();
 250     }
 251
 252   FGPositionedList result;
 253   Octree::findAllWithinRange(SGVec3d::fromGeod(aPos),
 254     aRangeNm * SG_NM_TO_METER, aFilter, result, 0xffffff);
 255   return result;
 256 }
 257
 258 FGPositionedList
 259 FGPositioned::findWithinRangePartial(const SGGeod& aPos, double aRangeNm, Filter* aFilter, bool& aPartial)
 260 {
 261   validateSGGeod(aPos);
 262
 263     if (!validateFilter(aFilter)) {
 264         return FGPositionedList();
 265     }
 266
 267   int limitMsec = 32;
 268   FGPositionedList result;
 269   aPartial = Octree::findAllWithinRange(SGVec3d::fromGeod(aPos),
 270                              aRangeNm * SG_NM_TO_METER, aFilter, result,
 271                                         limitMsec);
 272   return result;
 273 }
 274
 275 FGPositionedList
 276 FGPositioned::findAllWithIdent(const std::string& aIdent, Filter* aFilter, bool aExact)
 277 {
 278     if (!validateFilter(aFilter)) {
 279         return FGPositionedList();
 280     }
 281
 282   return NavDataCache::instance()->findAllWithIdent(aIdent, aFilter, aExact);
 283 }
 284
 285 FGPositionedList
 286 FGPositioned::findAllWithName(const std::string& aName, Filter* aFilter, bool aExact)
 287 {
 288     if (!validateFilter(aFilter)) {
 289         return FGPositionedList();
 290     }
 291
 292   return NavDataCache::instance()->findAllWithName(aName, aFilter, aExact);
 293 }
 294
 295 FGPositionedRef
 296 FGPositioned::findClosest(const SGGeod& aPos, double aCutoffNm, Filter* aFilter)
 297 {
 298   validateSGGeod(aPos);
 299
 300     if (!validateFilter(aFilter)) {
 301         return NULL;
 302     }
 303
 304   FGPositionedList l(findClosestN(aPos, 1, aCutoffNm, aFilter));
 305   if (l.empty()) {
 306     return NULL;
 307   }
 308
 309   assert(l.size() == 1);
 310   return l.front();
 311 }
 312
 313 FGPositionedList
 314 FGPositioned::findClosestN(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter)
 315 {
 316   validateSGGeod(aPos);
 317
 318   FGPositionedList result;
 319   int limitMsec = 0xffff;
 320   Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result, limitMsec);
 321   return result;
 322 }
 323
 324 FGPositionedList
 325 FGPositioned::findClosestNPartial(const SGGeod& aPos, unsigned int aN, double aCutoffNm, Filter* aFilter, bool &aPartial)
 326 {
 327     validateSGGeod(aPos);
 328
 329     FGPositionedList result;
 330     int limitMsec = 32;
 331     aPartial = Octree::findNearestN(SGVec3d::fromGeod(aPos), aN, aCutoffNm * SG_NM_TO_METER, aFilter, result,
 332                         limitMsec);
 333     return result;
 334 }
 335
 336 void
 337 FGPositioned::sortByRange(FGPositionedList& aResult, const SGGeod& aPos)
 338 {
 339   validateSGGeod(aPos);
 340
 341   SGVec3d cartPos(SGVec3d::fromGeod(aPos));
 342 // computer ordering values
 343   Octree::FindNearestResults r;
 344   FGPositionedList::iterator it = aResult.begin(), lend = aResult.end();
 345   for (; it != lend; ++it) {
 346     double d2 = distSqr((*it)->cart(), cartPos);
 347     r.push_back(Octree::OrderedPositioned(*it, d2));
 348   }
 349
 350 // sort
 351   std::sort(r.begin(), r.end());
 352
 353 // convert to a plain list
 354   unsigned int count = aResult.size();
 355   for (unsigned int i=0; i<count; ++i) {
 356     aResult[i] = r[i].get();
 357   }
 358 }
 359
 360 //------------------------------------------------------------------------------
 361 void FGPositioned::modifyPosition(const SGGeod& newPos)
 362 {
 363   const_cast<SGGeod&>(mPosition) = newPos;
 364   const_cast<SGVec3d&>(mCart) = SGVec3d::fromGeod(newPos);
 365 }
 366
 367 //------------------------------------------------------------------------------
 368 void FGPositioned::invalidatePosition()
 369 {
 370   const_cast<SGGeod&>(mPosition) = SGGeod::fromDeg(999,999);
 371   const_cast<SGVec3d&>(mCart) = SGVec3d::zeros();
 372 }
 373
 374 //------------------------------------------------------------------------------
 375 FGPositionedRef FGPositioned::loadByIdImpl(PositionedID id)
 376 {
 377   return flightgear::NavDataCache::instance()->loadById(id);
 378 }
 379
 380 FGPositioned::TypeFilter::TypeFilter(Type aTy) :
 381   mMinType(LAST_TYPE),
 382   mMaxType(INVALID)
 383 {
 384   addType(aTy);
 385 }
 386
 387 void FGPositioned::TypeFilter::addType(Type aTy)
 388 {
 389   if (aTy == INVALID) {
 390     return;
 391   }
 392
 393   types.push_back(aTy);
 394   mMinType = std::min(mMinType, aTy);
 395   mMaxType = std::max(mMaxType, aTy);
 396 }
 397
 398 FGPositioned::TypeFilter
 399 FGPositioned::TypeFilter::fromString(const std::string& aFilterSpec)
 400 {
 401   if (aFilterSpec.empty()) {
 402     throw sg_format_exception("empty filter spec:", aFilterSpec);
 403   }
 404
 405   string_list parts = simgear::strutils::split(aFilterSpec, ",");
 406   TypeFilter f;
 407
 408   BOOST_FOREACH(std::string token, parts) {
 409     f.addType(typeFromName(token));
 410   }
 411
 412   return f;
 413 }
 414
 415 bool
 416 FGPositioned::TypeFilter::pass(FGPositioned* aPos) const
 417 {
 418   if (types.empty()) {
 419     return true;
 420   }
 421
 422     std::vector<Type>::const_iterator it = types.begin(),
 423         end = types.end();
 424     for (; it != end; ++it) {
 425         if (aPos->type() == *it) {
 426             return true;
 427         }
 428     }
 429
 430     return false;
 431 }
 432