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