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