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 Procedure::Procedure(const string& aIdent) :
  35   _ident(aIdent)
  36 {
  37 }
  38
  39 Approach::Approach(const string& aIdent) :
  40   Procedure(aIdent)
  41 {
  42
  43 }
  44
  45 void Approach::setRunway(FGRunwayRef aRwy)
  46 {
  47   _runway = aRwy;
  48 }
  49
  50 void Approach::setPrimaryAndMissed(const WayptVec& aPrimary, const WayptVec& aMissed)
  51 {
  52   _primary = aPrimary;
  53   _primary[0]->setFlag(WPT_IAF, true);
  54   _primary[_primary.size()-1]->setFlag(WPT_FAF, true);
  55
  56   _missed = aMissed;
  57
  58   if (!_missed.empty()) {
  59     // mark the first point as the published missed-approach point
  60     _missed[0]->setFlag(WPT_MAP, true);
  61
  62     // mark all the points as being on the missed approach route
  63     for (unsigned int i=0; i<_missed.size(); ++i) {
  64       _missed[i]->setFlag(WPT_MISS, true);
  65     }
  66   }
  67 }
  68
  69 void Approach::addTransition(Transition* aTrans)
  70 {
  71   WayptRef entry = aTrans->enroute();
  72   _transitions[entry] = aTrans;
  73 }
  74
  75 bool Approach::route(WayptRef aIAF, WayptVec& aWps)
  76 {
  77   WptTransitionMap::iterator it;
  78   bool haveTrans = false;
  79   for (it = _transitions.begin(); it != _transitions.end(); ++it) {
  80     Transition* t= it->second;
  81     if (t->route(aIAF, aWps)) {
  82     haveTrans = true;
  83       break;
  84     }
  85   } // of transitions iteration
  86
  87   if (!haveTrans) {
  88     SG_LOG(SG_GENERAL, SG_INFO, "approach " << ident() << " has no transition " <<
  89       "for IAF: " << aIAF->ident());
  90     return false;
  91   }
  92
  93   aWps.insert(aWps.end(), _primary.begin(), _primary.end());
  94   aWps.push_back(new RunwayWaypt(_runway, NULL));
  95   aWps.insert(aWps.end(), _missed.begin(), _missed.end());
  96   return true;
  97 }
  98
  99 bool Approach::routeFromVectors(WayptVec& aWps)
 100 {
 101   aWps.insert(aWps.end(), _primary.begin(), _primary.end());
 102   aWps.push_back(new RunwayWaypt(_runway, NULL));
 103   aWps.insert(aWps.end(), _missed.begin(), _missed.end());
 104   return true;
 105 }
 106
 107 //////////////////////////////////////////////////////////////////////////////
 108
 109 ArrivalDeparture::ArrivalDeparture(const string& aIdent) :
 110     Procedure(aIdent)
 111 {
 112 }
 113
 114 void ArrivalDeparture::addRunway(FGRunwayRef aWay)
 115 {
 116   _runways[aWay] = NULL;
 117 }
 118
 119 bool ArrivalDeparture::isForRunway(FGRunwayRef aWay) const
 120 {
 121   // null runway always passes
 122   if (!aWay) {
 123     return true;
 124   }
 125
 126   return (_runways.count(aWay));
 127 }
 128
 129 void ArrivalDeparture::addTransition(Transition* aTrans)
 130 {
 131   WayptRef entry = aTrans->enroute();
 132   _enrouteTransitions[entry] = aTrans;
 133 }
 134
 135 void ArrivalDeparture::addRunwayTransition(FGRunwayRef aWay, Transition* aTrans)
 136 {
 137   assert(aWay->ident() == aTrans->ident());
 138   if (!isForRunway(aWay)) {
 139     throw sg_io_exception("adding transition for unspecified runway:" + aWay->ident(), ident());
 140   }
 141
 142   _runways[aWay] = aTrans;
 143 }
 144
 145 void ArrivalDeparture::setCommon(const WayptVec& aWps)
 146 {
 147   _common = aWps;
 148 }
 149
 150 bool ArrivalDeparture::commonRoute(Waypt* aEnroute, WayptVec& aPath, FGRunwayRef aRwy)
 151 {
 152   // assume we're routing from enroute, to the runway.
 153   // for departures, we'll flip the result points
 154
 155   Transition* t = findTransitionByEnroute(aEnroute);
 156   WayptVec::iterator firstCommon = _common.begin();
 157   if (t) {
 158     t->route(aEnroute, aPath);
 159
 160     Waypt* transEnd = t->procedureEnd();
 161     for (; firstCommon != _common.end(); ++firstCommon) {
 162       if ((*firstCommon)->matches(transEnd)) {
 163         // found transition->common point, stop search
 164         break;
 165       }
 166     } // of common points
 167
 168     // if we hit this point, the transition doesn't end (start, for a SID) on
 169     // a common point. We assume this means we should just append the entire
 170     // common section after the transition.
 171     firstCommon = _common.begin();
 172   } else {
 173     if (aEnroute && !(*firstCommon)->matches(aEnroute)) {
 174       return false;
 175     }
 176   } // of not using a transition
 177
 178   // append (some) common points
 179   aPath.insert(aPath.end(), firstCommon, _common.end());
 180
 181   if (!aRwy) {
 182     // no runway specified, we're done
 183     return true;
 184   }
 185
 186   RunwayTransitionMap::iterator r = _runways.find(aRwy);
 187   assert(r != _runways.end());
 188   if (!r->second) {
 189     // no transitions specified. Not great, but not
 190     // much we can do about it. Calling code will insert VECTORS to the approach
 191     // if required, or maybe there's an approach transition defined.
 192     return true;
 193   }
 194
 195   SG_LOG(SG_GENERAL, SG_INFO, ident() << " using runway transition for " << r->first->ident());
 196   r->second->route(NULL, aPath);
 197   return true;
 198 }
 199
 200 Transition* ArrivalDeparture::findTransitionByEnroute(Waypt* aEnroute) const
 201 {
 202   if (!aEnroute) {
 203     return NULL;
 204   }
 205
 206   WptTransitionMap::const_iterator eit;
 207   for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
 208     if (eit->second->enroute()->matches(aEnroute)) {
 209       SG_LOG(SG_GENERAL, SG_INFO, ident() << " using enroute transition " << eit->second->ident());
 210       return eit->second;
 211     }
 212   } // of enroute transition iteration
 213
 214   return NULL;
 215 }
 216
 217 WayptRef ArrivalDeparture::findBestTransition(const SGGeod& aPos) const
 218 {
 219   // no transitions, that's easy
 220   if (_enrouteTransitions.empty()) {
 221     SG_LOG(SG_GENERAL, SG_INFO, "no enroute transitions for " << ident());
 222     return _common.front();
 223   }
 224
 225   double d = 1e9;
 226   WayptRef w;
 227   WptTransitionMap::const_iterator eit;
 228   for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
 229     WayptRef c = eit->second->enroute();
 230     SG_LOG(SG_GENERAL, SG_INFO, "findBestTransition for " << ident() << ", looking at " << c->ident());
 231     // assert(c->hasFixedPosition());
 232     double cd = SGGeodesy::distanceM(aPos, c->position());
 233
 234     if (cd < d) { // distance to 'c' is less, new best match
 235       d = cd;
 236       w = c;
 237     }
 238   } // of transitions iteration
 239
 240   assert(w);
 241   return w;
 242 }
 243
 244 WayptRef ArrivalDeparture::findTransitionByName(const string& aIdent) const
 245 {
 246   WptTransitionMap::const_iterator eit;
 247   for (eit = _enrouteTransitions.begin(); eit != _enrouteTransitions.end(); ++eit) {
 248     WayptRef c = eit->second->enroute();
 249     if (c->ident() == aIdent) {
 250       return c;
 251     }
 252   }
 253
 254   return NULL;
 255 }
 256
 257 ////////////////////////////////////////////////////////////////////////////
 258
 259 SID::SID(const string& aIdent) :
 260     ArrivalDeparture(aIdent)
 261 {
 262 }
 263
 264 bool SID::route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath)
 265 {
 266   if (!isForRunway(aWay)) {
 267     SG_LOG(SG_GENERAL, SG_WARN, "SID " << ident() << " not for runway " << aWay->ident());
 268     return false;
 269   }
 270
 271   WayptVec path;
 272   if (!commonRoute(aEnroute, path, aWay)) {
 273     return false;
 274   }
 275
 276   // SID waypoints (including transitions) are stored reversed, so we can
 277   // re-use the routing code. This is where we fix the ordering for client code
 278   std::back_insert_iterator<WayptVec> bi(aPath);
 279   std::reverse_copy(path.begin(), path.end(), bi);
 280
 281   return true;
 282 }
 283
 284 ////////////////////////////////////////////////////////////////////////////
 285
 286 STAR::STAR(const string& aIdent) :
 287     ArrivalDeparture(aIdent)
 288 {
 289 }
 290
 291 bool STAR::route(FGRunwayRef aWay, Waypt* aEnroute, WayptVec& aPath)
 292 {
 293   if (aWay && !isForRunway(aWay)) {
 294     return false;
 295   }
 296
 297   return commonRoute(aEnroute, aPath, aWay);
 298 }
 299
 300 /////////////////////////////////////////////////////////////////////////////
 301
 302 Transition::Transition(const std::string& aIdent, Procedure* aPr,
 303     const WayptVec& aWps) :
 304   _ident(aIdent),
 305   _parent(aPr),
 306   _primary(aWps)
 307 {
 308   assert(aPr);
 309   assert(!_primary.empty());
 310
 311   _primary[0]->setFlag(WPT_TRANSITION, true);
 312 }
 313
 314 WayptRef Transition::enroute() const
 315 {
 316   assert(!_primary.empty());
 317   return _primary[0];
 318 }
 319
 320 WayptRef Transition::procedureEnd() const
 321 {
 322   assert(!_primary.empty());
 323   return _primary[_primary.size() - 1];
 324 }
 325
 326 bool Transition::route(Waypt* aEnroute, WayptVec& aPath)
 327 {
 328   if (aEnroute && !enroute()->matches(aEnroute)) {
 329     return false;
 330   }
 331
 332   aPath.insert(aPath.end(), _primary.begin(), _primary.end());
 333   return true;
 334 }
 335
 336 } // of namespace