+ bool reposition = false;
+ FGAIWaypoint *wpt;
+ double vDescent = ac->getPerformance()->vDescent();
+ double vApproach = ac->getPerformance()->vApproach();
+ double vTouchdown = ac->getPerformance()->vTouchdown();
+
+
+ //Beginning of Descent
+ string rwyClass = getRunwayClassFromTrafficType(fltType);
+ double heading = ac->getTrafficRef()->getCourse();
+ apt->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway,
+ heading);
+ FGRunway * rwy = apt->getRunwayByIdent(activeRunway);
+ assert( rwy != NULL );
+
+ // Create a slow descent path that ends 250 lateral to the runway.
+ double initialTurnRadius = getTurnRadius(vDescent, true);
+ //double finalTurnRadius = getTurnRadius(vApproach, true);
+
+// get length of the downwind leg for the intended runway
+ double distanceOut = apt->getDynamics()->getApproachController()->getRunway(rwy->name())->getApproachDistance(); //12 * SG_NM_TO_METER;
+ //time_t previousArrivalTime= apt->getDynamics()->getApproachController()->getRunway(rwy->name())->getEstApproachTime();
+
+
+ SGGeod current = SGGeod::fromDegM(longitude, latitude, 0);
+ SGGeod initialTarget = rwy->pointOnCenterline(-distanceOut);
+ SGGeod refPoint = rwy->pointOnCenterline(0);
+ double distance = SGGeodesy::distanceM(current, initialTarget);
+ double azimuth = SGGeodesy::courseDeg(current, initialTarget);
+ double dummyAz2;
+
+ // To prevent absurdly steep approaches, compute the origin from where the approach should have started
+ SGGeod origin;
+
+ if (ac->getTrafficRef()->getCallSign() ==
+ fgGetString("/ai/track-callsign")) {
+ //cerr << "Reposition information: Actual distance " << distance << ". required distance " << requiredDistance << endl;
+ //exit(1);
+ }
+
+ if (distance < requiredDistance * 0.8) {
+ reposition = true;
+ SGGeodesy::direct(initialTarget, azimuth,
+ -requiredDistance, origin, dummyAz2);
+
+ distance = SGGeodesy::distanceM(current, initialTarget);
+ azimuth = SGGeodesy::courseDeg(current, initialTarget);
+ } else {
+ origin = current;
+ }
+
+
+ double dAlt = 0; // = alt - (apt->getElevation() + 2000);
+ FGTaxiNode * tn = 0;
+ if (apt->getDynamics()->getGroundNetwork()) {
+ int node = apt->getDynamics()->getGroundNetwork()->findNearestNode(refPoint);
+ tn = apt->getDynamics()->getGroundNetwork()->findNode(node);
+ }
+ if (tn) {
+ dAlt = alt - ((tn->getElevationFt(apt->getElevation())) + 2000);
+ } else {
+ dAlt = alt - (apt->getElevation() + 2000);
+ }
+
+ double nPoints = 100;
+
+ char buffer[16];
+
+ // The descent path contains the following phases:
+ // 1) a linear glide path from the initial position to
+ // 2) a semi circle turn to final
+ // 3) approach
+
+ //cerr << "Phase 1: Linear Descent path to runway" << rwy->name() << endl;
+ // Create an initial destination point on a semicircle
+ //cerr << "lateral offset : " << lateralOffset << endl;
+ //cerr << "Distance : " << distance << endl;
+ //cerr << "Azimuth : " << azimuth << endl;
+ //cerr << "Initial Lateral point: " << lateralOffset << endl;
+// double lat = refPoint.getLatitudeDeg();
+// double lon = refPoint.getLongitudeDeg();
+ //cerr << "Reference point (" << lat << ", " << lon << ")." << endl;
+// lat = initialTarget.getLatitudeDeg();
+// lon = initialTarget.getLongitudeDeg();
+ //cerr << "Initial Target point (" << lat << ", " << lon << ")." << endl;
+
+ double ratio = initialTurnRadius / distance;
+ if (ratio > 1.0)
+ ratio = 1.0;
+ if (ratio < -1.0)
+ ratio = -1.0;
+
+ double newHeading = asin(ratio) * SG_RADIANS_TO_DEGREES;
+ double newDistance =
+ cos(newHeading * SG_DEGREES_TO_RADIANS) * distance;
+ //cerr << "new distance " << newDistance << ". additional Heading " << newHeading << endl;
+ double side = azimuth - rwy->headingDeg();
+ double lateralOffset = initialTurnRadius;
+ if (side < 0)
+ side += 360;
+ if (side < 180) {
+ lateralOffset *= -1;
+ }
+ // Calculate the ETA at final, based on remaining distance, and approach speed.
+ // distance should really consist of flying time to terniary target, plus circle
+ // but the distance to secondary target should work as a reasonable approximation
+ // aditionally add the amount of distance covered by making a turn of "side"
+ double turnDistance = (2 * M_PI * initialTurnRadius) * (side / 360.0);
+ time_t remaining =
+ (turnDistance + distance) / ((vDescent * SG_NM_TO_METER) / 3600.0);
+ time_t now = time(NULL) + fgGetLong("/sim/time/warp");
+ //if (ac->getTrafficRef()->getCallSign() == fgGetString("/ai/track-callsign")) {
+ // cerr << " Arrival time estimation: turn angle " << side << ". Turn distance " << turnDistance << ". Linear distance " << distance << ". Time to go " << remaining << endl;
+ // //exit(1);
+ //}
+
+ time_t eta = now + remaining;
+ //choose a distance to the runway such that it will take at least 60 seconds more
+ // time to get there than the previous aircraft.
+ // Don't bother when aircraft need to be repositioned, because that marks the initialization phased...
+
+ time_t newEta;
+
+ if (reposition == false) {
+ newEta =
+ apt->getDynamics()->getApproachController()->getRunway(rwy->
+ name
+ ())->
+ requestTimeSlot(eta);
+ } else {
+ newEta = eta;
+ }
+ //if ((eta < (previousArrivalTime+60)) && (reposition == false)) {
+ arrivalTime = newEta;
+ time_t additionalTimeNeeded = newEta - eta;
+ double distanceCovered =
+ ((vApproach * SG_NM_TO_METER) / 3600.0) * additionalTimeNeeded;
+ distanceOut += distanceCovered;
+ //apt->getDynamics()->getApproachController()->getRunway(rwy->name())->setEstApproachTime(eta+additionalTimeNeeded);
+ //cerr << "Adding additional distance: " << distanceCovered << " to allow " << additionalTimeNeeded << " seconds of flying time" << endl << endl;
+ //} else {
+ //apt->getDynamics()->getApproachController()->getRunway(rwy->name())->setEstApproachTime(eta);
+ //}
+ //cerr << "Timing information : Previous eta: " << previousArrivalTime << ". Current ETA : " << eta << endl;
+
+ SGGeod secondaryTarget =
+ rwy->pointOffCenterline(-distanceOut, lateralOffset);
+ initialTarget = rwy->pointOnCenterline(-distanceOut);
+ distance = SGGeodesy::distanceM(origin, secondaryTarget);
+ azimuth = SGGeodesy::courseDeg(origin, secondaryTarget);
+
+
+// lat = secondaryTarget.getLatitudeDeg();
+// lon = secondaryTarget.getLongitudeDeg();
+ //cerr << "Secondary Target point (" << lat << ", " << lon << ")." << endl;
+ //cerr << "Distance : " << distance << endl;
+ //cerr << "Azimuth : " << azimuth << endl;
+
+
+ ratio = initialTurnRadius / distance;
+ if (ratio > 1.0)
+ ratio = 1.0;
+ if (ratio < -1.0)
+ ratio = -1.0;
+ newHeading = asin(ratio) * SG_RADIANS_TO_DEGREES;
+ newDistance = cos(newHeading * SG_DEGREES_TO_RADIANS) * distance;
+ //cerr << "new distance realative to secondary target: " << newDistance << ". additional Heading " << newHeading << endl;
+ if (side < 180) {
+ azimuth += newHeading;
+ } else {
+ azimuth -= newHeading;
+ }
+
+ SGGeod tertiaryTarget;
+ SGGeodesy::direct(origin, azimuth,
+ newDistance, tertiaryTarget, dummyAz2);
+
+// lat = tertiaryTarget.getLatitudeDeg();
+// lon = tertiaryTarget.getLongitudeDeg();
+ //cerr << "tertiary Target point (" << lat << ", " << lon << ")." << endl;
+
+
+ for (int i = 1; i < nPoints; i++) {
+ SGGeod result;
+ double currentDist = i * (newDistance / nPoints);
+ double currentAltitude = alt - (i * (dAlt / nPoints));
+ SGGeodesy::direct(origin, azimuth, currentDist, result, dummyAz2);
+ snprintf(buffer, 16, "descent%03d", i);
+ wpt = createInAir(ac, buffer, result, currentAltitude, vDescent);
+ wpt->setCrossat(currentAltitude);
+ wpt->setTrackLength((newDistance / nPoints));
+ pushBackWaypoint(wpt);
+ //cerr << "Track Length : " << wpt->trackLength;
+ //cerr << " Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl;
+ }
+
+ //cerr << "Phase 2: Circle " << endl;
+ double initialAzimuth =
+ SGGeodesy::courseDeg(secondaryTarget, tertiaryTarget);
+ double finalAzimuth =
+ SGGeodesy::courseDeg(secondaryTarget, initialTarget);
+
+ //cerr << "Angles from secondary target: " << initialAzimuth << " " << finalAzimuth << endl;
+ int increment, startval, endval;
+ // circle right around secondary target if orig of position is to the right of the runway
+ // i.e. use negative angles; else circle leftward and use postivi
+ if (side < 180) {
+ increment = -1;
+ startval = floor(initialAzimuth);
+ endval = ceil(finalAzimuth);
+ if (endval > startval) {
+ endval -= 360;
+ }
+ } else {
+ increment = 1;
+ startval = ceil(initialAzimuth);
+ endval = floor(finalAzimuth);
+ if (endval < startval) {
+ endval += 360;
+ }
+
+ }
+
+ //cerr << "creating circle between " << startval << " and " << endval << " using " << increment << endl;
+ //FGTaxiNode * tn = apt->getDynamics()->getGroundNetwork()->findNearestNode(initialTarget);
+ double currentAltitude = 0;
+ if (tn) {
+ currentAltitude = (tn->getElevationFt(apt->getElevation())) + 2000;
+ } else {
+ currentAltitude = apt->getElevation() + 2000;
+ }
+
+ double trackLength = (2 * M_PI * initialTurnRadius) / 360.0;
+ for (int i = startval; i != endval; i += increment) {
+ SGGeod result;
+ //double currentAltitude = apt->getElevation() + 2000;
+
+ SGGeodesy::direct(secondaryTarget, i,
+ initialTurnRadius, result, dummyAz2);
+ snprintf(buffer, 16, "turn%03d", i);
+ wpt = createInAir(ac, buffer, result, currentAltitude, vDescent);
+ wpt->setCrossat(currentAltitude);
+ wpt->setTrackLength(trackLength);
+ //cerr << "Track Length : " << wpt->trackLength;
+ pushBackWaypoint(wpt);
+ //cerr << " Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << endl;
+ }
+
+
+ // The approach leg should bring the aircraft to approximately 4-6 nm out, after which the landing phase should take over.
+ //cerr << "Phase 3: Approach" << endl;
+ double tgt_speed = vApproach;
+ distanceOut -= distanceCovered;
+ double touchDownPoint = 0; //(rwy->lengthM() * 0.1);
+ for (int i = 1; i < nPoints; i++) {
+ SGGeod result;
+ double currentDist = i * (distanceOut / nPoints);
+ //double currentAltitude =
+ // apt->getElevation() + 2000 - (i * 2000 / (nPoints-1));
+ double alt = currentAltitude - (i * 2000 / (nPoints - 1));
+ snprintf(buffer, 16, "final%03d", i);
+ result = rwy->pointOnCenterline((-distanceOut) + currentDist + touchDownPoint);
+ if (i == nPoints - 30) {
+ tgt_speed = vTouchdown;
+ }
+ wpt = createInAir(ac, buffer, result, alt, tgt_speed);
+ wpt->setCrossat(alt);
+ wpt->setTrackLength((distanceOut / nPoints));
+ // account for the extra distance due to an extended downwind leg
+ if (i == 1) {
+ wpt->setTrackLength(wpt->getTrackLength() + distanceCovered);
+ }
+ //cerr << "Track Length : " << wpt->trackLength;
+ pushBackWaypoint(wpt);
+ //if (apt->ident() == fgGetString("/sim/presets/airport-id")) {
+ // cerr << " Position : " << result.getLatitudeDeg() << " " << result.getLongitudeDeg() << " " << currentAltitude << " " << apt->getElevation() << " " << distanceOut << endl;
+ //}
+ }
+
+ //cerr << "Done" << endl;
+
+ // Erase the two bogus BOD points: Note check for conflicts with scripted AI flightPlans
+ IncrementWaypoint(true);
+ IncrementWaypoint(true);
+
+ if (reposition) {
+ double tempDistance;
+ //double minDistance = HUGE_VAL;
+ string wptName;
+ tempDistance = SGGeodesy::distanceM(current, initialTarget);
+ time_t eta =
+ tempDistance / ((vDescent * SG_NM_TO_METER) / 3600.0) + now;
+ time_t newEta =
+ apt->getDynamics()->getApproachController()->getRunway(rwy->
+ name
+ ())->
+ requestTimeSlot(eta);
+ arrivalTime = newEta;
+ double newDistance =
+ ((vDescent * SG_NM_TO_METER) / 3600.0) * (newEta - now);
+ //cerr << "Repositioning information : eta" << eta << ". New ETA " << newEta << ". Diff = " << (newEta - eta) << ". Distance = " << tempDistance << ". New distance = " << newDistance << endl;
+ IncrementWaypoint(true); // remove waypoint BOD2
+ while (checkTrackLength("final001") > newDistance) {
+ IncrementWaypoint(true);
+ }
+ //cerr << "Repositioning to waypoint " << (*waypoints.begin())->name << endl;
+ ac->resetPositionFromFlightPlan();
+ }
+ waypoints[1]->setName( (waypoints[1]->getName() + string("legend")));
+ waypoints.back()->setName(waypoints.back()->getName() + "LandingThreshold");
+ return true;