From 632d89d6e79f6bad6f53145efc431d89a03f5fb7 Mon Sep 17 00:00:00 2001 From: durk Date: Tue, 30 Dec 2008 07:41:15 +0000 Subject: [PATCH] James Turner: Cleanup of AI traffic route generation code. --- src/AIModel/AIFlightPlan.hxx | 10 +- src/AIModel/AIFlightPlanCreate.cxx | 1010 ++++++---------------- src/AIModel/AIFlightPlanCreateCruise.cxx | 46 +- src/Airports/gnnode.hxx | 4 + src/Airports/groundnetwork.cxx | 5 +- src/Airports/groundnetwork.hxx | 3 + 6 files changed, 305 insertions(+), 773 deletions(-) diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index 593a83a5c..0e9e29e40 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -126,7 +126,6 @@ private: void createPushBack(bool, FGAirport*, double, double, double, const string&, const string&, const string&); void createPushBackFallBack(bool, FGAirport*, double, double, double, const string&, const string&, const string&); - void createTaxi(bool, int, FGAirport *, double, double, double, const string&, const string&, const string&); void createTakeOff(bool, FGAirport *, double, const string&); void createClimb(bool, FGAirport *, double, double, const string&); void createCruise(bool, FGAirport*, FGAirport*, double, double, double, double, const string&); @@ -136,6 +135,15 @@ private: void deleteWaypoints(); void resetWaypoints(); + void createLandingTaxi(FGAirport *apt, double radius, const string& fltType, const string& acType, const string& airline); + void createDefaultLandingTaxi(FGAirport* aAirport); + void createDefaultTakeoffTaxi(FGAirport* aAirport, FGRunway* aRunway); + void createTakeoffTaxi(bool firstFlight, FGAirport *apt, double radius, const string& fltType, const string& acType, const string& airline); + + waypoint* createOnGround(const std::string& aName, const SGGeod& aPos, double aElev, double aSpeed); + waypoint* createInAir(const std::string& aName, const SGGeod& aPos, double aElev, double aSpeed); + waypoint* cloneWithPos(waypoint* aWpt, const std::string& aName, const SGGeod& aPos); + string getRunwayClassFromTrafficType(string fltType); //void createCruiseFallback(bool, FGAirport*, FGAirport*, double, double, double, double); diff --git a/src/AIModel/AIFlightPlanCreate.cxx b/src/AIModel/AIFlightPlanCreate.cxx index 7d414bed7..f48110b72 100644 --- a/src/AIModel/AIFlightPlanCreate.cxx +++ b/src/AIModel/AIFlightPlanCreate.cxx @@ -55,8 +55,7 @@ void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, int legNr, radius, fltType, aircraftType, airline); break; case 2: - createTaxi(firstFlight, 1, dep, latitude, longitude, - radius, fltType, aircraftType, airline); + createTakeoffTaxi(firstFlight, dep, radius, fltType, aircraftType, airline); break; case 3: createTakeOff(firstFlight, dep, speed, fltType); @@ -74,8 +73,7 @@ void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, int legNr, createLanding(arr); break; case 8: - createTaxi(false, 2, arr, latitude, longitude, radius, - fltType, aircraftType, airline); + createLandingTaxi(arr, radius, fltType, aircraftType, airline); break; case 9: createParking(arr, radius); @@ -89,397 +87,249 @@ void FGAIFlightPlan::create(FGAirport *dep, FGAirport *arr, int legNr, leg++; } +FGAIFlightPlan::waypoint* +FGAIFlightPlan::createOnGround(const std::string& aName, const SGGeod& aPos, double aElev, double aSpeed) +{ + waypoint* wpt = new waypoint; + wpt->name = aName; + wpt->longitude = aPos.getLongitudeDeg(); + wpt->latitude = aPos.getLatitudeDeg(); + wpt->altitude = aElev; + wpt->speed = aSpeed; + wpt->crossat = -10000; + wpt->gear_down = true; + wpt->flaps_down= true; + wpt->finished = false; + wpt->on_ground = true; + wpt->routeIndex= 0; + return wpt; +} +FGAIFlightPlan::waypoint* +FGAIFlightPlan::createInAir(const std::string& aName, const SGGeod& aPos, double aElev, double aSpeed) +{ + waypoint* wpt = new waypoint; + wpt->name = aName; + wpt->longitude = aPos.getLongitudeDeg(); + wpt->latitude = aPos.getLatitudeDeg(); + wpt->altitude = aElev; + wpt->speed = aSpeed; + wpt->crossat = -10000; + wpt->gear_down = false; + wpt->flaps_down= false; + wpt->finished = false; + wpt->on_ground = false; + wpt->routeIndex= 0; + return wpt; +} +FGAIFlightPlan::waypoint* +FGAIFlightPlan::cloneWithPos(waypoint* aWpt, const std::string& aName, const SGGeod& aPos) +{ + waypoint* wpt = new waypoint; + wpt->name = aName; + wpt->longitude = aPos.getLongitudeDeg(); + wpt->latitude = aPos.getLatitudeDeg(); + + wpt->altitude = aWpt->altitude; + wpt->speed = aWpt->speed; + wpt->crossat = aWpt->crossat; + wpt->gear_down = aWpt->gear_down; + wpt->flaps_down= aWpt->flaps_down; + wpt->finished = aWpt->finished; + wpt->on_ground = aWpt->on_ground; + wpt->routeIndex = 0; + + return wpt; +} +void FGAIFlightPlan::createDefaultTakeoffTaxi(FGAirport* aAirport, FGRunway* aRunway) +{ + SGGeod runwayTakeoff = aRunway->pointOnCenterline(5.0); + double airportElev = aAirport->getElevation(); + + waypoint* wpt; + wpt = createOnGround("Airport Center", aAirport->geod(), airportElev, 15); + waypoints.push_back(wpt); + wpt = createOnGround("Runway Takeoff", runwayTakeoff, airportElev, 15); + waypoints.push_back(wpt); +} - -/******************************************************************* - * createCreate Taxi. - * initialize the Aircraft at the parking location - ******************************************************************/ -void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, - FGAirport *apt, double latitude, double longitude, +void FGAIFlightPlan::createTakeoffTaxi(bool firstFlight, + FGAirport *apt, double radius, const string& fltType, const string& acType, const string& airline) { - double heading; - double lat, lon; - double lat2, lon2, az2; - waypoint *wpt; - - int nrWaypointsToSkip; - - if (direction == 1) + double heading, lat, lon; + + // If this function is called during initialization, + // make sure we obtain a valid gate ID first + // and place the model at the location of the gate. + if (firstFlight) { + if (!(apt->getDynamics()->getAvailableParking(&lat, &lon, + &heading, &gateId, + radius, fltType, + acType, airline))) { - // If this function is called during initialization, - // make sure we obtain a valid gate ID first - // and place the model at the location of the gate. - if (firstFlight) - { - if (!(apt->getDynamics()->getAvailableParking(&lat, &lon, - &heading, &gateId, - radius, fltType, - acType, airline))) - { - SG_LOG(SG_INPUT, SG_WARN, "Could not find parking for a " << - acType << - " of flight type " << fltType << - " of airline " << airline << - " at airport " << apt->getId()); - } - //waypoint *wpt = new waypoint; - //wpt->name = "park"; - //wpt->latitude = lat; - //wpt->longitude = lon; - //wpt->altitude = apt->getElevation(); - //wpt->speed = -10; - //wpt->crossat = -10000; - //wpt->gear_down = true; - //wpt->flaps_down= true; - //wpt->finished = false; - //wpt->on_ground = true; - //waypoints.push_back(wpt); - } - string rwyClass = getRunwayClassFromTrafficType(fltType); - apt->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway); - rwy = apt->getRunwayByIdent(activeRunway); - - // Determine the beginning of he runway - heading = rwy->headingDeg(); - double azimuth = heading + 180.0; - while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), azimuth, - rwy->lengthM() * 0.5 - 5.0, - &lat2, &lon2, &az2 ); - - if (apt->getDynamics()->getGroundNetwork()->exists()) - { - intVec ids; - int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, - lon2); - - - // A negative gateId indicates an overflow parking, use a - // fallback mechanism for this. - // Starting from gate 0 in this case is a bit of a hack - // which requires a more proper solution later on. - //FGTaxiRoute route; - delete taxiRoute; - taxiRoute = new FGTaxiRoute; - - // Determine which node to start from. - int node; - // Find out which node to start from - FGParking *park = apt->getDynamics()->getParking(gateId); - if (park) - node = park->getPushBackPoint(); - else - node = 0; - if (node == -1) - node = gateId; + SG_LOG(SG_INPUT, SG_WARN, "Could not find parking for a " << + acType << + " of flight type " << fltType << + " of airline " << airline << + " at airport " << apt->getId()); + } + } + + string rwyClass = getRunwayClassFromTrafficType(fltType); + apt->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway); + rwy = apt->getRunwayByIdent(activeRunway); + SGGeod runwayTakeoff = rwy->pointOnCenterline(5.0); - // HAndle case where parking doens't have a node - if ((node == 0) && park) { - if (firstFlight) { - node = gateId; - } else { - node = lastNodeVisited; - } - } + FGGroundNetwork* gn = apt->getDynamics()->getGroundNetwork(); + if (!gn->exists()) { + createDefaultTakeoffTaxi(apt, rwy); + return; + } + + intVec ids; + int runwayId = gn->findNearestNode(runwayTakeoff); + + // A negative gateId indicates an overflow parking, use a + // fallback mechanism for this. + // Starting from gate 0 in this case is a bit of a hack + // which requires a more proper solution later on. + delete taxiRoute; + taxiRoute = new FGTaxiRoute; + + // Determine which node to start from. + int node = 0; + // Find out which node to start from + FGParking *park = apt->getDynamics()->getParking(gateId); + if (park) { + node = park->getPushBackPoint(); + } + + if (node == -1) { + node = gateId; + } + + // HAndle case where parking doens't have a node + if ((node == 0) && park) { + if (firstFlight) { + node = gateId; + } else { + node = lastNodeVisited; + } + } - //cerr << "Using node " << node << endl; - *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(node, runwayId); - intVecIterator i; + *taxiRoute = gn->findShortestRoute(node, runwayId); + intVecIterator i; - if (taxiRoute->empty()) { - //Add the runway startpoint; - wpt = new waypoint; - wpt->name = "Airport Center"; - wpt->latitude = latitude; - wpt->longitude = longitude; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - - //Add the runway startpoint; - wpt = new waypoint; - wpt->name = "Runway Takeoff"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - } else { - int node; - taxiRoute->first(); - //bool isPushBackPoint = false; - if (firstFlight) { - - // If this is called during initialization, randomly - // skip a number of waypoints to get a more realistic - // taxi situation. - //isPushBackPoint = true; - int nrWaypoints = taxiRoute->size(); - nrWaypointsToSkip = rand() % nrWaypoints; - // but make sure we always keep two active waypoints - // to prevent a segmentation fault - for (int i = 0; i < nrWaypointsToSkip-2; i++) { - //isPushBackPoint = false; - taxiRoute->next(&node); - } - apt->getDynamics()->releaseParking(gateId); - } else { - if (taxiRoute->size() > 1) { - taxiRoute->next(&node); // chop off the first waypoint, because that is already the last of the pushback route - } - } - int route; - while(taxiRoute->next(&node, &route)) - { - //FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findSegment(node)->getEnd(); - char buffer[10]; - snprintf (buffer, 10, "%d", node); - FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node); - //ids.pop_back(); - wpt = new waypoint; - wpt->name = string(buffer); // fixme: should be the name of the taxiway - wpt->latitude = tn->getLatitude(); - wpt->longitude = tn->getLongitude(); - // Elevation is currently disregarded when on_ground is true - // because the AIModel obtains a periodic ground elevation estimate. - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = route; - waypoints.push_back(wpt); - } - /* - //cerr << endl; - // finally, rewind the taxiRoute object to the point where we started - // generating the Flightplan, for AI use. - // This is a bit tricky, because the - taxiRoute->first(); - if (firstFlight) { - for (int i = 0; i < nrWaypointsToSkip-1; i++) { - taxiRoute->next(&node); - } - } else { - int size = taxiRoute->size(); - if (size > 2) { - //taxiRoute->next(&node); - //taxiRoute->next(&node); - //taxiRoute->next(&node); - } - }*/ - } // taxiRoute not empty - } - else - { - // This is the fallback mechanism, in case no ground network is available - //Add the runway startpoint; - wpt = new waypoint; - wpt->name = "Airport Center"; - wpt->latitude = apt->getLatitude(); - wpt->longitude = apt->getLongitude(); - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - - //Add the runway startpoint; - wpt = new waypoint; - wpt->name = "Runway Takeoff"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - } + if (taxiRoute->empty()) { + createDefaultTakeoffTaxi(apt, rwy); + return; + } + + taxiRoute->first(); + //bool isPushBackPoint = false; + if (firstFlight) { + // If this is called during initialization, randomly + // skip a number of waypoints to get a more realistic + // taxi situation. + int nrWaypointsToSkip = rand() % taxiRoute->size(); + // but make sure we always keep two active waypoints + // to prevent a segmentation fault + for (int i = 0; i < nrWaypointsToSkip-2; i++) { + taxiRoute->next(&node); } - else // Landing taxi - { - apt->getDynamics()->getAvailableParking(&lat, &lon, &heading, - &gateId, radius, fltType, - acType, airline); - - double lat3 = (*(waypoints.end()-1))->latitude; - double lon3 = (*(waypoints.end()-1))->longitude; - //cerr << (*(waypoints.end()-1))->name << endl; - - // Find a route from runway end to parking/gate. - if (apt->getDynamics()->getGroundNetwork()->exists()) - { - intVec ids; - int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat3, - lon3); - // A negative gateId indicates an overflow parking, use a - // fallback mechanism for this. - // Starting from gate 0 is a bit of a hack... - //FGTaxiRoute route; - delete taxiRoute; - taxiRoute = new FGTaxiRoute; - if (gateId >= 0) - *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(runwayId, - gateId); - else - *taxiRoute = apt->getDynamics()->getGroundNetwork()->findShortestRoute(runwayId, 0); - intVecIterator i; - - // No route found: go from gate directly to runway - if (taxiRoute->empty()) { - //Add the runway startpoint; - wpt = new waypoint; - wpt->name = "Airport Center"; - wpt->latitude = latitude; - wpt->longitude = longitude; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - - //Add the runway startpoint; - wpt = new waypoint; - wpt->name = "Runway Takeoff"; - wpt->latitude = lat3; - wpt->longitude = lon3; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - } else { - int node; - taxiRoute->first(); - int size = taxiRoute->size(); - // Omit the last two waypoints, as - // those are created by createParking() - int route; - for (int i = 0; i < size-2; i++) - { - taxiRoute->next(&node, &route); + apt->getDynamics()->releaseParking(gateId); + } else { + if (taxiRoute->size() > 1) { + taxiRoute->next(&node); // chop off the first waypoint, because that is already the last of the pushback route + } + } + + // push each node on the taxi route as a waypoint + int route; + while(taxiRoute->next(&node, &route)) { char buffer[10]; snprintf (buffer, 10, "%d", node); - //FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node); FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node); - wpt = new waypoint; - //wpt->name = "taxiway"; // fixme: should be the name of the taxiway - wpt->name = string(buffer);// fixme: should be the name of the taxiway - wpt->latitude = tn->getLatitude(); - wpt->longitude = tn->getLongitude(); - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = route; + waypoint* wpt = createOnGround(buffer, tn->geod(), apt->getElevation(), 15); + wpt->routeIndex = route; waypoints.push_back(wpt); - } - //taxiRoute->first(); - //taxiRoute->next(&node); - } - } - else - { - // Use a fallback mechanism in case no ground network is available - // obtain the location of the gate entrance point - heading += 180.0; - if (heading > 360) - heading -= 360; - geo_direct_wgs_84 ( 0, lat, lon, heading, - 100, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "Airport Center"; - wpt->latitude = apt->getLatitude(); - wpt->longitude = apt->getLongitude(); - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - - wpt = new waypoint; - wpt->name = "Begin Parking"; //apt->getId(); //wpt_node->getStringValue("name", "END"); - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); + } +} - //waypoint* wpt; - //double lat; - //double lon; - //double heading; - apt->getDynamics()->getParking(gateId, &lat, &lon, &heading); - heading += 180.0; - if (heading > 360) - heading -= 360; - - wpt = new waypoint; - wpt->name = "END"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = lat; - wpt->longitude = lon; - wpt->altitude = 19; - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - } - - } +void FGAIFlightPlan::createDefaultLandingTaxi(FGAirport* aAirport) +{ + SGGeod lastWptPos = + SGGeod::fromDeg(waypoints.back()->longitude, waypoints.back()->latitude); + double airportElev = aAirport->getElevation(); + + waypoint* wpt; + wpt = createOnGround("Runway Exit", lastWptPos, airportElev, 15); + waypoints.push_back(wpt); + wpt = createOnGround("Airport Center", aAirport->geod(), airportElev, 15); + waypoints.push_back(wpt); + + double heading, lat, lon; + aAirport->getDynamics()->getParking(gateId, &lat, &lon, &heading); + wpt = createOnGround("END", SGGeod::fromDeg(lon, lat), airportElev, 15); + waypoints.push_back(wpt); +} + +void FGAIFlightPlan::createLandingTaxi(FGAirport *apt, + double radius, const string& fltType, + const string& acType, const string& airline) +{ + double heading, lat, lon; + apt->getDynamics()->getAvailableParking(&lat, &lon, &heading, + &gateId, radius, fltType, acType, airline); + + SGGeod lastWptPos = + SGGeod::fromDeg(waypoints.back()->longitude, waypoints.back()->latitude); + FGGroundNetwork* gn = apt->getDynamics()->getGroundNetwork(); + + // Find a route from runway end to parking/gate. + if (!gn->exists()) { + createDefaultLandingTaxi(apt); + return; + } + + intVec ids; + int runwayId = gn->findNearestNode(lastWptPos); + // A negative gateId indicates an overflow parking, use a + // fallback mechanism for this. + // Starting from gate 0 is a bit of a hack... + //FGTaxiRoute route; + delete taxiRoute; + taxiRoute = new FGTaxiRoute; + if (gateId >= 0) + *taxiRoute = gn->findShortestRoute(runwayId, gateId); + else + *taxiRoute = gn->findShortestRoute(runwayId, 0); + intVecIterator i; + + if (taxiRoute->empty()) { + createDefaultLandingTaxi(apt); + return; + } + + int node; + taxiRoute->first(); + int size = taxiRoute->size(); + // Omit the last two waypoints, as + // those are created by createParking() + int route; + for (int i = 0; i < size-2; i++) { + taxiRoute->next(&node, &route); + char buffer[10]; + snprintf (buffer, 10, "%d", node); + FGTaxiNode *tn = gn->findNode(node); + waypoint* wpt = createOnGround(buffer, tn->geod(), apt->getElevation(), 15); + wpt->routeIndex = route; + waypoints.push_back(wpt); + } } /******************************************************************* @@ -488,272 +338,77 @@ void FGAIFlightPlan::createTaxi(bool firstFlight, int direction, ******************************************************************/ void FGAIFlightPlan::createTakeOff(bool firstFlight, FGAirport *apt, double speed, const string &fltType) { - double heading; - double lat, lon, az; - double lat2, lon2, az2; waypoint *wpt; // Get the current active runway, based on code from David Luff // This should actually be unified and extended to include // Preferential runway use schema's if (firstFlight) - { - //string name; - string rwyClass = getRunwayClassFromTrafficType(fltType); - apt->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway); - rwy = apt->getRunwayByIdent(activeRunway); - } - // Acceleration point, 105 meters into the runway, - heading = rwy->headingDeg(); - double azimuth = heading + 180.0; - while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), azimuth, - rwy->lengthM() * 0.5 - 105.0, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "accel"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = speed; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - - lat = lat2; - lon = lon2; - az = az2; + { + string rwyClass = getRunwayClassFromTrafficType(fltType); + apt->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway); + rwy = apt->getRunwayByIdent(activeRunway); + } - //Start Climbing to 3000 ft. Let's do this - // at the center of the runway for now: - // - geo_direct_wgs_84 ( 0, lat, lon, heading, - 2560 * SG_FEET_TO_METER, - &lat2, &lon2, &az2 ); - - wpt = new waypoint; - wpt->name = "SOC"; - wpt->latitude = rwy->latitude(); - wpt->longitude = rwy->longitude(); - wpt->altitude = apt->getElevation()+1000; - wpt->speed = speed; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = false; - wpt->routeIndex = 0; - waypoints.push_back(wpt); - - - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), heading, - rwy->lengthM(), - &lat2, &lon2, &az2 ); + double airportElev = apt->getElevation(); + // Acceleration point, 105 meters into the runway, + SGGeod accelPoint = rwy->pointOnCenterline(105.0); + wpt = createOnGround("accel", accelPoint, airportElev, speed); + waypoints.push_back(wpt); + + //Start Climbing to 3000 ft. Let's do this + // at the center of the runway for now: + wpt = cloneWithPos(wpt, "SOC", rwy->geod()); + wpt->altitude = airportElev+1000; + wpt->on_ground = false; + waypoints.push_back(wpt); - wpt = new waypoint; - wpt->name = "3000 ft"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation()+3000; - wpt->speed = speed; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = false; - wpt->routeIndex = 0; - waypoints.push_back(wpt); + wpt = cloneWithPos(wpt, "3000 ft", rwy->reverseThreshold()); + wpt->altitude = airportElev+3000; + waypoints.push_back(wpt); // Finally, add two more waypoints, so that aircraft will remain under - // Tower control until they have reached the 3000 ft climb point - - - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), heading, - 5000, - &lat2, &lon2, &az2 ); - - - wpt = new waypoint; - wpt->name = "5000 ft"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation()+5000; - wpt->speed = speed; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = false; - wpt->routeIndex = 0; + // Tower control until they have reached the 3000 ft climb point + SGGeod pt = rwy->pointOnCenterline(5000 + rwy->lengthM() * 0.5); + wpt = cloneWithPos(wpt, "5000 ft", pt); + wpt->altitude = airportElev+5000; waypoints.push_back(wpt); - //cerr << "Created takeoff plan : " << endl; - //for (wpt_vector_iterator i = waypoints.begin(); i != waypoints.end(); i++) { - // cerr << "Waypoint Name: " << (*i)->name << ". Speed = " << (*i)->speed << endl; - //} - - - // geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), heading, -// 100000, -// &lat2, &lon2, &az2 ); -// wpt = new waypoint; -// wpt->name = "5100 ft"; -// wpt->latitude = lat2; -// wpt->longitude = lon2; -// wpt->altitude = apt->getElevation()+5100; -// wpt->speed = speed; -// wpt->crossat = -10000; -// wpt->gear_down = true; -// wpt->flaps_down= true; -// wpt->finished = false; -// wpt->on_ground = false; -// wpt->routeIndex = 0; -// waypoints.push_back(wpt); } - + /******************************************************************* * CreateClimb * initialize the Aircraft at the parking location ******************************************************************/ void FGAIFlightPlan::createClimb(bool firstFlight, FGAirport *apt, double speed, double alt, const string &fltType) { - double heading; - //FGRunway rwy; - double lat2, lon2, az2; - //int direction; waypoint *wpt; - - if (firstFlight) - { - //string name; - string rwyClass = getRunwayClassFromTrafficType(fltType); - apt->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway); - rwy = apt->getRunwayByIdent(activeRunway); - } - + if (firstFlight) { + string rwyClass = getRunwayClassFromTrafficType(fltType); + apt->getDynamics()->getActiveRunway(rwyClass, 1, activeRunway); + rwy = apt->getRunwayByIdent(activeRunway); + } - heading = rwy->headingDeg(); - double azimuth = heading + 180.0; - while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - //cerr << "Creating climb at : " << rwy._id << " " << rwy._rwy_no << endl; - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), heading, - 10*SG_NM_TO_METER, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "10000ft climb"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = 10000; - wpt->speed = speed; - wpt->crossat = -10000; + SGGeod climb1 = rwy->pointOnCenterline(10*SG_NM_TO_METER); + wpt = createInAir("10000ft climb", climb1, speed, 10000); wpt->gear_down = true; wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = false; - wpt->routeIndex = 0; waypoints.push_back(wpt); - - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), heading, - 20*SG_NM_TO_METER, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "18000ft climb"; - wpt->latitude = lat2; - wpt->longitude = lon2; + SGGeod climb2 = rwy->pointOnCenterline(20*SG_NM_TO_METER); + wpt = cloneWithPos(wpt, "18000ft climb", climb2); wpt->altitude = 18000; - wpt->speed = speed; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = false; - wpt->routeIndex = 0; waypoints.push_back(wpt); } -// /******************************************************************* -// * CreateCruise -// * initialize the Aircraft at the parking location -// ******************************************************************/ -// void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, -// FGAirport *arr, double latitude, -// double longitude, double speed, -// double alt) -// { -// double wind_speed; -// double wind_heading; -// double heading; -// double lat, lon, az; -// double lat2, lon2, az2; -// double azimuth; -// waypoint *wpt; - -// wpt = new waypoint; -// wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END"); -// wpt->latitude = latitude; -// wpt->longitude = longitude; -// wpt->altitude = alt; -// wpt->speed = speed; -// wpt->crossat = -10000; -// wpt->gear_down = false; -// wpt->flaps_down= false; -// wpt->finished = false; -// wpt->on_ground = false; -// waypoints.push_back(wpt); - - -// // should be changed dynamically to allow "gen" and "mil" -// arr->getDynamics()->getActiveRunway("com", 2, activeRunway); -// if (!(globals->get_runways()->search(arr->getId(), -// activeRunway, -// &rwy))) -// { -// SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << -// activeRunway << -// " at airport " << arr->getId()); -// exit(1); -// } -// heading = rwy->headingDeg(); -// azimuth = heading + 180.0; -// while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - - -// geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), azimuth, -// 110000, -// &lat2, &lon2, &az2 ); -// wpt = new waypoint; -// wpt->name = "BOD"; -// wpt->latitude = lat2; -// wpt->longitude = lon2; -// wpt->altitude = alt; -// wpt->speed = speed; -// wpt->crossat = alt; -// wpt->gear_down = false; -// wpt->flaps_down= false; -// wpt->finished = false; -// wpt->on_ground = false; -// waypoints.push_back(wpt); -// } - /******************************************************************* * CreateDecent * initialize the Aircraft at the parking location ******************************************************************/ void FGAIFlightPlan::createDecent(FGAirport *apt, const string &fltType) { - // Ten thousand ft. Slowing down to 240 kts - double heading; - //FGRunway rwy; - double lat2, lon2, az2; - double azimuth; - //int direction; waypoint *wpt; //Beginning of Decent @@ -763,43 +418,17 @@ void FGAIFlightPlan::createDecent(FGAirport *apt, const string &fltType) apt->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway); rwy = apt->getRunwayByIdent(activeRunway); - heading = rwy->headingDeg(); - azimuth = heading + 180.0; - while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), azimuth, - 100000, - &lat2, &lon2, &az2 ); - - wpt = new waypoint; - wpt->name = "Dec 10000ft"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 240; + SGGeod descent1 = rwy->pointOnCenterline(-100000); // 100km out + wpt = createInAir("Dec 10000ft", descent1, apt->getElevation(), 240); wpt->crossat = 10000; - wpt->gear_down = false; - wpt->flaps_down= false; - wpt->finished = false; - wpt->on_ground = false; - wpt->routeIndex = 0; waypoints.push_back(wpt); // Three thousand ft. Slowing down to 160 kts - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), azimuth, - 8*SG_NM_TO_METER, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "DEC 3000ft"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 160; + SGGeod descent2 = rwy->pointOnCenterline(-8*SG_NM_TO_METER); // 8nm out + wpt = createInAir("DEC 3000ft", descent2, apt->getElevation(), 160); wpt->crossat = 3000; wpt->gear_down = true; wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = false; - wpt->routeIndex = 0; waypoints.push_back(wpt); } /******************************************************************* @@ -809,69 +438,20 @@ void FGAIFlightPlan::createDecent(FGAirport *apt, const string &fltType) void FGAIFlightPlan::createLanding(FGAirport *apt) { // Ten thousand ft. Slowing down to 150 kts - double heading; - //FGRunway rwy; - double lat2, lon2, az2; - double azimuth; - //int direction; waypoint *wpt; - - - heading = rwy->headingDeg(); - azimuth = heading + 180.0; - while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - + double aptElev = apt->getElevation(); //Runway Threshold - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), azimuth, - rwy->lengthM() *0.45, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "Threshold"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 150; - wpt->crossat = apt->getElevation(); - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; + wpt = createOnGround("Threshold", rwy->displacedThreshold(), aptElev, 150); + wpt->crossat = apt->getElevation(); waypoints.push_back(wpt); - //Full stop at the runway centerpoint - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), azimuth, - rwy->lengthFt() *0.45, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "Center"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = rwy->latitude(); - wpt->longitude = rwy->longitude(); - wpt->altitude = apt->getElevation(); - wpt->speed = 30; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; + // Roll-out + wpt = createOnGround("Center", rwy->geod(), aptElev, 30); waypoints.push_back(wpt); - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), heading, - rwy->lengthM() *0.45, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "Threshold"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; + SGGeod rollOut = rwy->pointOnCenterline(rwy->lengthM() * 0.9); + wpt = createOnGround("Roll Out", rollOut, aptElev, 15); wpt->crossat = apt->getElevation(); - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; waypoints.push_back(wpt); } @@ -882,6 +462,7 @@ void FGAIFlightPlan::createLanding(FGAirport *apt) void FGAIFlightPlan::createParking(FGAirport *apt, double radius) { waypoint* wpt; + double aptElev = apt->getElevation(); double lat, lat2; double lon, lon2; double az2; @@ -893,48 +474,17 @@ void FGAIFlightPlan::createParking(FGAirport *apt, double radius) geo_direct_wgs_84 ( 0, lat, lon, heading, 2.2*radius, &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "taxiStart"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 10; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; - waypoints.push_back(wpt); + wpt = createOnGround("taxiStart", SGGeod::fromDeg(lon2, lat2), aptElev, 10); + waypoints.push_back(wpt); + geo_direct_wgs_84 ( 0, lat, lon, heading, 0.1 *radius, &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "taxiStart"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = apt->getElevation(); - wpt->speed = 10; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; + + wpt = createOnGround("taxiStart2", SGGeod::fromDeg(lon2, lat2), aptElev, 10); waypoints.push_back(wpt); - wpt = new waypoint; - wpt->name = "END"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = lat; - wpt->longitude = lon; - wpt->altitude = apt->getElevation(); - wpt->speed = 15; - wpt->crossat = -10000; - wpt->gear_down = true; - wpt->flaps_down= true; - wpt->finished = false; - wpt->on_ground = true; - wpt->routeIndex = 0; + wpt = createOnGround("END", SGGeod::fromDeg(lon, lat), aptElev, 10); waypoints.push_back(wpt); } diff --git a/src/AIModel/AIFlightPlanCreateCruise.cxx b/src/AIModel/AIFlightPlanCreateCruise.cxx index d38896b7c..d2fc62473 100755 --- a/src/AIModel/AIFlightPlanCreateCruise.cxx +++ b/src/AIModel/AIFlightPlanCreateCruise.cxx @@ -276,7 +276,7 @@ void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, // { // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl; // } - heading = rwy->headingDeg(); + heading = rwy._heading; azimuth = heading + 180.0; while ( azimuth >= 360.0 ) { azimuth -= 360.0; } @@ -315,52 +315,16 @@ void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, double longitude, double speed, double alt, const string& fltType) { - double wind_speed; - double wind_heading; - double heading; - double lat, lon, az; - double lat2, lon2, az2; - double azimuth; waypoint *wpt; - - wpt = new waypoint; - wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END"); - wpt->latitude = latitude; - wpt->longitude = longitude; - wpt->altitude = alt; - wpt->speed = speed; - wpt->crossat = -10000; - wpt->gear_down = false; - wpt->flaps_down= false; - wpt->finished = false; - wpt->on_ground = false; - wpt->routeIndex = 0; + wpt = createInAir("Cruise", SGGeod::fromDeg(longitude, latitude), alt, speed); waypoints.push_back(wpt); - string rwyClass = getRunwayClassFromTrafficType(fltType); arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway); rwy = arr->getRunwayByIdent(activeRunway); + // begin descent 110km out + SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000); - heading = rwy->headingDeg(); - azimuth = heading + 180.0; - while ( azimuth >= 360.0 ) { azimuth -= 360.0; } - - - geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), azimuth, - 110000, - &lat2, &lon2, &az2 ); - wpt = new waypoint; - wpt->name = "BOD"; - wpt->latitude = lat2; - wpt->longitude = lon2; - wpt->altitude = alt; - wpt->speed = speed; - wpt->crossat = alt; - wpt->gear_down = false; - wpt->flaps_down= false; - wpt->finished = false; - wpt->on_ground = false; - wpt->routeIndex = 0; + wpt = createInAir("BOD", beginDescentPoint, alt, speed); waypoints.push_back(wpt); } diff --git a/src/Airports/gnnode.hxx b/src/Airports/gnnode.hxx index aee5d6a81..d265ff5ad 100644 --- a/src/Airports/gnnode.hxx +++ b/src/Airports/gnnode.hxx @@ -18,7 +18,9 @@ #include #include + #include +#include using std::string; using std::vector; @@ -107,6 +109,8 @@ FGTaxiNode &operator =(const FGTaxiNode &other) double getLatitude() { return lat;}; double getLongitude(){ return lon;}; + SGGeod geod() const { return SGGeod::fromDeg(lon, lat); } + int getIndex() { return index; }; int getHoldPointType() { return holdType; }; bool getIsOnRunway() { return isOnRunway; }; diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx index b4f48caee..9df9b5625 100644 --- a/src/Airports/groundnetwork.cxx +++ b/src/Airports/groundnetwork.cxx @@ -312,7 +312,10 @@ void FGGroundNetwork::init() //exit(1); } - +int FGGroundNetwork::findNearestNode(const SGGeod& aGeod) +{ + return findNearestNode(aGeod.getLatitudeDeg(), aGeod.getLongitudeDeg()); +} int FGGroundNetwork::findNearestNode(double lat, double lon) { diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx index a70fa2c09..e602f9094 100644 --- a/src/Airports/groundnetwork.hxx +++ b/src/Airports/groundnetwork.hxx @@ -253,7 +253,10 @@ public: void init(); bool exists() { return hasNetwork; }; void setTowerController(FGTowerController *twrCtrlr) { towerController = twrCtrlr; }; + int findNearestNode(double lat, double lon); + int findNearestNode(const SGGeod& aGeod); + FGTaxiNode *findNode(int idx); FGTaxiSegment *findSegment(int idx); FGTaxiRoute findShortestRoute(int start, int end, bool fullSearch=true); -- 2.39.5