src/Navaids/procedure.cxx

   1 // procedure.cxx - define route storing an approach, arrival or departure procedure
   2 // Written by James Turner, started 2009.
   3 //
   4 // Copyright (C) 2009  Curtis L. Olson
   5 //
   6 // This program is free software; you can redistribute it and/or
   7 // modify it under the terms of the GNU General Public License as
   8 // published by the Free Software Foundation; either version 2 of the
   9 // License, or (at your option) any later version.
  10 //
  11 // This program is distributed in the hope that it will be useful, but
  12 // WITHOUT ANY WARRANTY; without even the implied warranty of
  13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14 // General Public License for more details.
  15 //
  16 // You should have received a copy of the GNU General Public License
  17 // along with this program; if not, write to the Free Software
  18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  19
  20 #include "procedure.hxx"
  21
  22 #include <cassert>
  23 #include <algorithm> // for reverse_copy
  24
  25 #include <simgear/structure/exception.hxx>
  26
  27 #include <Navaids/waypoint.hxx>
  28
  29 using std::string;
  30
  31 namespace flightgear
  32 {
  33
  34 static void markWaypoints(WayptVec& wps, WayptFlag f)
  35 {
  36   for (unsigned int i=0; i<wps.size(); ++i) {
  37     wps[i]->setFlag(f, true);
  38   }
  39 }
  40
  41 Procedure::Procedure(const string& aIdent) :
  42   _ident(aIdent)
  43 {
  44 }
  45
  46 Approach::Approach(const string& aIdent, ProcedureType ty) :
  47   Procedure(aIdent),
  48   _type(ty)
  49 {
  50
  51 }
  52
  53 Approach* Approach::createTempApproach(const std::string& aIdent, FGRunway* aRunway, const WayptVec& aPath)
  54 {
  55     Approach* app = new Approach(aIdent, PROCEDURE_APPROACH_RNAV);
  56     app->setRunway(aRunway);
  57     app->setPrimaryAndMissed(aPath, WayptVec());
  58     return app;
  59 }
  60
  61 void Approach::setRunway(FGRunwayRef aRwy)
  62 {
  63   _runway = aRwy;
  64 }
  65
  66 FGAirport* Approach::airport() const
  67 {
  68   return _runway->airport();
  69 }
  70
  71 RunwayVec Approach::runways() const
  72 {
  73   RunwayVec r;
  74   r.push_back(_runway);
  75   return r;
  76 }
  77
  78 void Approach::setPrimaryAndMissed(const WayptVec& aPrimary, const WayptVec& aMissed)
  79 {
  80   _primary = aPrimary;
  81   _primary[0]->setFlag(WPT_IAF, true);
  82   _primary[_primary.size()-1]->setFlag(WPT_FAF, true);
  83   markWaypoints(_primary, WPT_APPROACH);
  84
  85   _missed = aMissed;
  86
  87   if (!_missed.empty()) {
  88     // mark the first point as the published missed-approach point
  89     _missed[0]->setFlag(WPT_MAP, true);
  90     markWaypoints(_missed, WPT_MISS);
  91     markWaypoints(_missed, WPT_APPROACH);
  92   }
  93 }
  94
  95 void Approach::addTransition(Transition* aTrans)
  96 {
  97   WayptRef entry = aTrans->enroute();
  98   _transitions[entry] = aTrans;
  99   aTrans->mark(WPT_APPROACH);
 100 }
 101
 102 bool Approach::route(WayptRef aIAF, WayptVec& aWps)
 103 {
 104   if (aIAF.valid()) {
 105     WptTransitionMap::iterator it;
 106     bool haveTrans = false;
 107     for (it = _transitions.begin(); it != _transitions.end(); ++it) {
 108       Transition* t= it->second;
 109       if (t->enroute()->matches(aIAF)) {
 110         t->route(aWps);
 111         haveTrans = true;
 112         break;
 113       }
 114     } // of transitions iteration
 115
 116     if (!haveTrans) {
 117       SG_LOG(SG_NAVAID, SG_INFO, "approach " << ident() << " has no transition " <<
 118         "for IAF: " << aIAF->ident());
 119       return false;
 120     }
 121   }
 122
 123   return routeFromVectors(aWps);
 124 }
 125
 126 bool Approach::routeFromVectors(WayptVec& aWps)
 127 {
 128   aWps.insert(aWps.end(), _primary.begin(), _primary.end());
 129   RunwayWaypt* rwy = new RunwayWaypt(_runway, NULL);
 130   rwy->setFlag(WPT_APPROACH);
 131   aWps.push_back(rwy);
 132   aWps.insert(aWps.end(), _missed.begin(), _missed.end());
 133   return true;
 134 }
 135
 136 bool Approach::isApproach(ProcedureType ty)
 137 {
 138   return (ty >= PROCEDURE_APPROACH_ILS) && (ty <= PROCEDURE_APPROACH_RNAV);
 139 }
 140
 141 //////////////////////////////////////////////////////////////////////////////
 142
 143 ArrivalDeparture::ArrivalDeparture(const string& aIdent, FGAirport* apt) :
 144   Procedure(aIdent),
 145   _airport(apt)
 146 {
 147 }
 148
 149 void ArrivalDeparture::addRunway(FGRunwayRef aWay)
 150 {
 151   assert(aWay->airport() == _airport);
 152   _runways[aWay] = NULL;
 153 }
 154
 155 bool ArrivalDeparture::isForRunway(const FGRunway* aWay) const
 156 {
 157   // null runway always passes
 158   if (!aWay) {
 159     return true;
 160   }
 161
 162   FGRunwayRef r(const_cast<FGRunway*>(aWay));
 163   return (_runways.count(r) > 0);
 164 }
 165
 166 RunwayVec ArrivalDeparture::runways() const
 167 {
 168   RunwayVec r;
 169   RunwayTransitionMap::const_iterator it = _runways.begin();
 170   for (; it != _runways.end(); ++it) {
 171     r.push_back(it->first);
 172   }
 173
 174   return r;
 175 }
 176
 177 void ArrivalDeparture::addTransition(Transition* aTrans)
 178 {
 179   WayptRef entry = aTrans->enroute();
 180   aTrans->mark(flagType());
 181   _enrouteTransitions[entry] = aTrans;
 182 }
 183
 184 string_list ArrivalDeparture::transitionIdents() const
 185 {
 186   string_list r;
 187   WptTransitionMap::const_iterator eit;
 188   for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
 189     r.push_back(eit->second->ident());
 190   }
 191   return r;
 192 }
 193
 194 void ArrivalDeparture::addRunwayTransition(FGRunwayRef aWay, Transition* aTrans)
 195 {
 196   assert(aWay->ident() == aTrans->ident());
 197   if (!isForRunway(aWay)) {
 198     throw sg_io_exception("adding transition for unspecified runway:" + aWay->ident(), ident());
 199   }
 200
 201   aTrans->mark(flagType());
 202   _runways[aWay] = aTrans;
 203 }
 204
 205 void ArrivalDeparture::setCommon(const WayptVec& aWps)
 206 {
 207   _common = aWps;
 208   markWaypoints(_common, flagType());
 209 }
 210
 211 bool ArrivalDeparture::commonRoute(Transition* t, WayptVec& aPath, FGRunwayRef aRwy)
 212 {
 213   // assume we're routing from enroute, to the runway.
 214   // for departures, we'll flip the result points
 215
 216   WayptVec::iterator firstCommon = _common.begin();
 217   if (t) {
 218     t->route(aPath);
 219
 220     Waypt* transEnd = t->procedureEnd();
 221     for (; firstCommon != _common.end(); ++firstCommon) {
 222       if ((*firstCommon)->matches(transEnd)) {
 223         // found transition->common point, stop search
 224         break;
 225       }
 226     } // of common points
 227
 228     // if we hit this point, the transition doesn't end (start, for a SID) on
 229     // a common point. We assume this means we should just append the entire
 230     // common section after the transition.
 231     firstCommon = _common.begin();
 232   } else {
 233     // no tranasition
 234   } // of not using a transition
 235
 236   // append (some) common points
 237   aPath.insert(aPath.end(), firstCommon, _common.end());
 238
 239   if (!aRwy) {
 240     // no runway specified, we're done
 241     return true;
 242   }
 243
 244   RunwayTransitionMap::iterator r = _runways.find(aRwy);
 245   assert(r != _runways.end());
 246   if (!r->second) {
 247     // no transitions specified. Not great, but not
 248     // much we can do about it. Calling code will insert VECTORS to the approach
 249     // if required, or maybe there's an approach transition defined.
 250     return true;
 251   }
 252
 253   SG_LOG(SG_NAVAID, SG_INFO, ident() << " using runway transition for " << r->first->ident());
 254   r->second->route(aPath);
 255   return true;
 256 }
 257
 258 Transition* ArrivalDeparture::findTransitionByEnroute(Waypt* aEnroute) const
 259 {
 260   if (!aEnroute) {
 261     return NULL;
 262   }
 263
 264   WptTransitionMap::const_iterator eit;
 265   for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
 266     if (eit->second->enroute()->matches(aEnroute)) {
 267       SG_LOG(SG_NAVAID, SG_INFO, ident() << " using enroute transition " << eit->second->ident());
 268       return eit->second;
 269     }
 270   } // of enroute transition iteration
 271
 272   return NULL;
 273 }
 274
 275 WayptRef ArrivalDeparture::findBestTransition(const SGGeod& aPos) const
 276 {
 277   // no transitions, that's easy
 278   if (_enrouteTransitions.empty()) {
 279     SG_LOG(SG_NAVAID, SG_INFO, "no enroute transitions for " << ident());
 280     return _common.front();
 281   }
 282
 283   double d = 1e9;
 284   WayptRef w;
 285   WptTransitionMap::const_iterator eit;
 286   for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
 287     WayptRef c = eit->second->enroute();
 288     SG_LOG(SG_NAVAID, SG_INFO, "findBestTransition for " << ident() << ", looking at " << c->ident());
 289     // assert(c->hasFixedPosition());
 290     double cd = SGGeodesy::distanceM(aPos, c->position());
 291
 292     if (cd < d) { // distance to 'c' is less, new best match
 293       d = cd;
 294       w = c;
 295     }
 296   } // of transitions iteration
 297
 298   assert(w);
 299   return w;
 300 }
 301
 302 Transition* ArrivalDeparture::findTransitionByName(const string& aIdent) const
 303 {
 304   WptTransitionMap::const_iterator eit;
 305   for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
 306     if (eit->second->ident() == aIdent) {
 307       return eit->second;
 308     }
 309   }
 310
 311   return NULL;
 312 }
 313
 314 ////////////////////////////////////////////////////////////////////////////
 315
 316 SID::SID(const string& aIdent, FGAirport* apt) :
 317     ArrivalDeparture(aIdent, apt)
 318 {
 319 }
 320
 321 bool SID::route(FGRunwayRef aWay, Transition* trans, WayptVec& aPath)
 322 {
 323   if (!isForRunway(aWay)) {
 324     SG_LOG(SG_NAVAID, SG_WARN, "SID " << ident() << " not for runway " << aWay->ident());
 325     return false;
 326   }
 327
 328   WayptVec path;
 329   if (!commonRoute(trans, path, aWay)) {
 330     return false;
 331   }
 332
 333   // SID waypoints (including transitions) are stored reversed, so we can
 334   // re-use the routing code. This is where we fix the ordering for client code
 335   std::back_insert_iterator<WayptVec> bi(aPath);
 336   std::reverse_copy(path.begin(), path.end(), bi);
 337
 338   return true;
 339 }
 340
 341 SID* SID::createTempSID(const std::string& aIdent, FGRunway* aRunway, const WayptVec& aPath)
 342 {
 343 // flip waypoints since SID stores them reversed
 344     WayptVec path;
 345     std::back_insert_iterator<WayptVec> bi(path);
 346     std::reverse_copy(aPath.begin(), aPath.end(), bi);
 347
 348     SID* sid = new SID(aIdent, aRunway->airport());
 349     sid->setCommon(path);
 350     sid->addRunway(aRunway);
 351     return sid;
 352 }
 353
 354 ////////////////////////////////////////////////////////////////////////////
 355
 356 STAR::STAR(const string& aIdent, FGAirport* apt) :
 357     ArrivalDeparture(aIdent, apt)
 358 {
 359 }
 360
 361 bool STAR::route(FGRunwayRef aWay, Transition* trans, WayptVec& aPath)
 362 {
 363   if (aWay && !isForRunway(aWay)) {
 364     return false;
 365   }
 366
 367   return commonRoute(trans, aPath, aWay);
 368 }
 369
 370 /////////////////////////////////////////////////////////////////////////////
 371
 372 Transition::Transition(const std::string& aIdent, ProcedureType ty, Procedure* aPr) :
 373   Procedure(aIdent),
 374   _type(ty),
 375   _parent(aPr)
 376 {
 377   assert(aPr);
 378 }
 379
 380 void Transition::setPrimary(const WayptVec& aWps)
 381 {
 382   _primary = aWps;
 383   assert(!_primary.empty());
 384   _primary[0]->setFlag(WPT_TRANSITION, true);
 385 }
 386
 387 WayptRef Transition::enroute() const
 388 {
 389   assert(!_primary.empty());
 390   return _primary[0];
 391 }
 392
 393 WayptRef Transition::procedureEnd() const
 394 {
 395   assert(!_primary.empty());
 396   return _primary[_primary.size() - 1];
 397 }
 398
 399 bool Transition::route(WayptVec& aPath)
 400 {
 401   aPath.insert(aPath.end(), _primary.begin(), _primary.end());
 402   return true;
 403 }
 404
 405 FGAirport* Transition::airport() const
 406 {
 407   return _parent->airport();
 408 }
 409
 410 void Transition::mark(WayptFlag f)
 411 {
 412   markWaypoints(_primary, f);
 413 }
 414
 415 } // of namespace