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