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