5b2e16ca73
maintain a resonable distance from each other while taxiing on the same route. The current code does not yet take crossing routes or aircraft taxiing into opposite directions into account.
379 lines
11 KiB
C++
Executable file
379 lines
11 KiB
C++
Executable file
/******************************************************************************
|
|
* AIFlightPlanCreateCruise.cxx
|
|
* Written by Durk Talsma, started February, 2006.
|
|
*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License as
|
|
* published by the Free Software Foundation; either version 2 of the
|
|
* License, or (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, write to the Free Software
|
|
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
*
|
|
*
|
|
**************************************************************************/
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include "AIFlightPlan.hxx"
|
|
#include <simgear/math/polar3d.hxx>
|
|
#include <simgear/math/sg_geodesy.hxx>
|
|
#include <simgear/route/waypoint.hxx>
|
|
|
|
#include <Navaids/awynet.hxx>
|
|
#include <Airports/runways.hxx>
|
|
|
|
#include <Environment/environment_mgr.hxx>
|
|
#include <Environment/environment.hxx>
|
|
|
|
SG_USING_STD(iostream);
|
|
|
|
void FGAIFlightPlan::evaluateRoutePart(double deplat,
|
|
double deplon,
|
|
double arrlat,
|
|
double arrlon)
|
|
{
|
|
// First do a prescan of all the waypoints that are within a reasonable distance of the
|
|
// ideal route.
|
|
intVec nodes;
|
|
int tmpNode, prevNode;
|
|
|
|
|
|
SGWayPoint first (deplon,
|
|
deplat,
|
|
100);
|
|
SGWayPoint sec (arrlon,
|
|
arrlat,
|
|
100);
|
|
first.CourseAndDistance(sec, &course, &distance);
|
|
distance *= SG_METER_TO_NM;
|
|
temp = sgPolarToCart3d(Point3D(deplon *
|
|
SG_DEGREES_TO_RADIANS,
|
|
deplat *
|
|
SG_DEGREES_TO_RADIANS,
|
|
1.0));
|
|
a[0] = temp.x();
|
|
a[1] = temp.y();
|
|
a[2] = temp.z();
|
|
|
|
temp = sgPolarToCart3d(Point3D(arrlon *
|
|
SG_DEGREES_TO_RADIANS,
|
|
arrlat *
|
|
SG_DEGREES_TO_RADIANS,
|
|
1.0));
|
|
b[0] = temp.x();
|
|
b[1] = temp.y();
|
|
b[2] = temp.z();
|
|
sgdNormaliseVec3(a);
|
|
sgdNormaliseVec3(b);
|
|
sgdVectorProductVec3(cross,b,a);
|
|
|
|
angle = sgACos(sgdScalarProductVec3(a,b));
|
|
tmpNode = 0;
|
|
for (double ang = 0.0; ang < angle; ang += 0.05)
|
|
{
|
|
//cerr << "Angle = " << ang << endl;
|
|
sgdMakeRotMat4(matrix, ang, cross);
|
|
for(int j = 0; j < 3; j++)
|
|
{
|
|
newPos[j] =0.0;
|
|
for (int k = 0; k<3; k++)
|
|
{
|
|
newPos[j] += matrix[j][k]*a[k];
|
|
}
|
|
}
|
|
//cerr << "1"<< endl;
|
|
temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
|
|
midlat = temp.lat() * SG_RADIANS_TO_DEGREES;
|
|
midlon = temp.lon() * SG_RADIANS_TO_DEGREES;
|
|
|
|
prevNode = tmpNode;
|
|
tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
|
|
|
|
double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude ();
|
|
double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
|
|
SGWayPoint curr(midlat,
|
|
midlon,
|
|
100);
|
|
SGWayPoint node(nodelat,
|
|
nodelon,
|
|
100);
|
|
curr.CourseAndDistance(node, &course, &distance);
|
|
if ((distance < 25000) && (tmpNode != prevNode))
|
|
nodes.push_back(tmpNode);
|
|
}
|
|
|
|
intVecIterator i = nodes.begin();
|
|
intVecIterator j = nodes.end();
|
|
while (i != nodes.end())
|
|
{
|
|
j = nodes.end();
|
|
while (j != i)
|
|
{
|
|
j--;
|
|
FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
|
|
if (!(routePart.empty()))
|
|
{
|
|
airRoute.add(routePart);
|
|
i = j;
|
|
break;
|
|
}
|
|
}
|
|
i++;
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
|
|
FGAirport *arr, double latitude,
|
|
double longitude, double speed, double alt)
|
|
{
|
|
bool useInitialWayPoint = true;
|
|
bool useCurrentWayPoint = false;
|
|
double heading;
|
|
double lat, lon, az;
|
|
double lat2, lon2, az2;
|
|
double azimuth;
|
|
waypoint *wpt;
|
|
waypoint *init_waypoint;
|
|
intVec ids;
|
|
char buffer[32];
|
|
SGPath routefile = globals->get_fg_root();
|
|
init_waypoint = new waypoint;
|
|
init_waypoint->name = "Initial waypoint";
|
|
init_waypoint->latitude = latitude;
|
|
init_waypoint->longitude = longitude;;
|
|
//wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
|
|
init_waypoint->altitude = alt;
|
|
init_waypoint->speed = 450; //speed;
|
|
init_waypoint->crossat = -10000;
|
|
init_waypoint->gear_down = false;
|
|
init_waypoint->flaps_down= false;
|
|
init_waypoint->finished = false;
|
|
init_waypoint->on_ground = false;
|
|
waypoints.push_back(init_waypoint);
|
|
routefile.append("Data/AI/FlightPlans");
|
|
snprintf(buffer, 32, "%s-%s.txt",
|
|
dep->getId().c_str(),
|
|
arr->getId().c_str());
|
|
routefile.append(buffer);
|
|
cerr << "trying to read " << routefile.c_str()<< endl;
|
|
//exit(1);
|
|
if (routefile.exists())
|
|
{
|
|
sg_gzifstream in( routefile.str() );
|
|
do {
|
|
in >> route;
|
|
} while (!(in.eof()));
|
|
}
|
|
else {
|
|
//int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
|
|
//int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
|
|
//int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
|
|
//FGAirRoute route;
|
|
evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
|
|
arr->getLatitude(), arr->getLongitude());
|
|
//exit(1);
|
|
}
|
|
route.first();
|
|
int node;
|
|
if (route.empty()) {
|
|
// if no route could be found, create a direct gps route...
|
|
cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
|
|
|
|
//exit(1);
|
|
} else {
|
|
while(route.next(&node))
|
|
{
|
|
FGNode *fn = globals->get_airwaynet()->findNode(node);
|
|
//cerr << "Checking status of each waypoint: " << fn->getIdent();
|
|
|
|
SGWayPoint first(init_waypoint->longitude,
|
|
init_waypoint->latitude,
|
|
alt);
|
|
SGWayPoint curr (fn->getLongitude(),
|
|
fn->getLatitude(),
|
|
alt);
|
|
SGWayPoint arr (arr->getLongitude(),
|
|
arr->getLatitude(),
|
|
alt);
|
|
|
|
double crse, crsDiff;
|
|
double dist;
|
|
first.CourseAndDistance(arr, &course, &distance);
|
|
first.CourseAndDistance(curr, &crse, &dist);
|
|
|
|
dist *= SG_METER_TO_NM;
|
|
|
|
// We're only interested in the absolute value of crsDiff
|
|
// wich should fall in the 0-180 deg range.
|
|
crsDiff = fabs(crse-course);
|
|
if (crsDiff > 180)
|
|
crsDiff = 360-crsDiff;
|
|
// These are the three conditions that we consider including
|
|
// in our flight plan:
|
|
// 1) current waypoint is less then 100 miles away OR
|
|
// 2) curren waypoint is ahead of us, at any distance
|
|
//cerr << " Distance : " << dist << " : Course diff " << crsDiff
|
|
// << " crs to dest : " << course
|
|
// << " crs to wpt : " << crse;
|
|
if ((dist > 20.0) && (crsDiff > 90.0))
|
|
{
|
|
//useWpt = false;
|
|
// Once we start including waypoints, we have to continue, even though
|
|
// one of the following way point would suffice.
|
|
// so once is the useWpt flag is set to true, we cannot reset it to false.
|
|
//cerr << " discarding " << endl;
|
|
// << ": Course difference = " << crsDiff
|
|
// << "Course = " << course
|
|
// << "crse = " << crse << endl;
|
|
}
|
|
else {
|
|
//i = ids.end()-1;
|
|
//cerr << " accepting " << endl;
|
|
|
|
//ids.pop_back();
|
|
wpt = new waypoint;
|
|
wpt->name = "Airway"; // fixme: should be the name of the waypoint
|
|
wpt->latitude = fn->getLatitude();
|
|
wpt->longitude = fn->getLongitude();
|
|
//wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
|
|
wpt->altitude = alt;
|
|
wpt->speed = 450; //speed;
|
|
wpt->crossat = -10000;
|
|
wpt->gear_down = false;
|
|
wpt->flaps_down= false;
|
|
wpt->finished = false;
|
|
wpt->on_ground = false;
|
|
waypoints.push_back(wpt);
|
|
}
|
|
|
|
if (!(routefile.exists()))
|
|
{
|
|
route.first();
|
|
fstream outf( routefile.c_str(), fstream::out );
|
|
while (route.next(&node))
|
|
outf << node << endl;
|
|
}
|
|
}
|
|
}
|
|
arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
|
|
if (!(globals->get_runways()->search(arr->getId(),
|
|
activeRunway,
|
|
&rwy)))
|
|
{
|
|
cout << "Failed to find runway for " << arr->getId() << endl;
|
|
// Hmm, how do we handle a potential error like this?
|
|
exit(1);
|
|
}
|
|
//string test;
|
|
//arr->getActiveRunway(string("com"), 1, test);
|
|
//exit(1);
|
|
|
|
//cerr << "Altitude = " << alt << endl;
|
|
//cerr << "Done" << endl;
|
|
//if (arr->getId() == "EHAM")
|
|
// {
|
|
// cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
|
|
// }
|
|
heading = rwy._heading;
|
|
azimuth = heading + 180.0;
|
|
while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
|
|
|
|
|
|
// Note: This places us at the location of the active
|
|
// runway during initial cruise. This needs to be
|
|
// fixed later.
|
|
geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
|
|
110000,
|
|
&lat2, &lon2, &az2 );
|
|
wpt = new waypoint;
|
|
wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
|
|
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);
|
|
}
|
|
*/
|
|
|
|
|
|
/*******************************************************************
|
|
* CreateCruise
|
|
* initialize the Aircraft at the parking location
|
|
*
|
|
* Note that this is the original version that does not
|
|
* do any dynamic route computation.
|
|
******************************************************************/
|
|
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;
|
|
wpt->routeIndex = 0;
|
|
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._heading;
|
|
azimuth = heading + 180.0;
|
|
while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
|
|
|
|
|
|
geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, 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;
|
|
waypoints.push_back(wpt);
|
|
}
|