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