void
Waypoints::Replace(const Waypoint &orig, const Waypoint &replacement)
{
assert(!waypoint_tree.IsEmpty());
name_tree.Remove(orig);
Waypoint new_waypoint(replacement);
new_waypoint.id = orig.id;
if (waypoint_tree.HaveBounds()) {
new_waypoint.Project(task_projection);
if (!waypoint_tree.IsWithinBounds(new_waypoint)) {
/* schedule an optimise() call */
waypoint_tree.Flatten();
waypoint_tree.ClearBounds();
}
}
const auto it = waypoint_tree.FindPointer(&orig);
assert(it != waypoint_tree.end());
waypoint_tree.Replace(it, new_waypoint);
name_tree.Add(orig);
++serial;
}
void waypoint_add_list(const char *name, SCP_vector<vec3d> &vec_list)
{
Assert(name != NULL);
if (find_matching_waypoint_list(name) != NULL)
{
Warning(LOCATION, "Waypoint list '%s' already exists in this mission! Not adding the new list...", name);
return;
}
waypoint_list new_list(name);
Waypoint_lists.push_back(new_list);
waypoint_list *wp_list = &Waypoint_lists.back();
wp_list->get_waypoints().reserve(vec_list.size());
SCP_vector<vec3d>::iterator ii;
for (ii = vec_list.begin(); ii != vec_list.end(); ++ii)
{
waypoint new_waypoint(&(*ii), wp_list);
wp_list->get_waypoints().push_back(new_waypoint);
}
// so that masking in the other function works
// though if you actually hit this Assert, you have other problems
Assert(wp_list->get_waypoints().size() <= 0xffff);
}
bool
WaypointReaderFS::ParseLine(const TCHAR* line, const unsigned linenum,
Waypoints &way_points)
{
//$FormatGEO
//ACONCAGU S 32 39 12.00 W 070 00 42.00 6962 Aconcagua
//BERGNEUS N 51 03 07.02 E 007 42 22.02 488 Bergneustadt [A]
//GOLDENGA N 37 49 03.00 W 122 28 42.00 227 Golden Gate Bridge
//REDSQUAR N 55 45 15.00 E 037 37 12.00 123 Red Square
//SYDNEYOP S 33 51 25.02 E 151 12 54.96 5 Sydney Opera
//$FormatUTM
//Aconcagu 19H 0405124 6386692 6962 Aconcagua
//Bergneus 32U 0409312 5656398 488 Bergneustadt [A]
//Golden G 10S 0545914 4185695 227 Golden Gate Bridge
//Red Squa 37U 0413390 6179582 123 Red Square
//Sydney O 56H 0334898 6252272 5 Sydney Opera
if (line[0] == '\0')
return true;
if (linenum == 0 &&
_tcsstr(line, _T("$FormatUTM")) == line) {
is_utm = true;
return true;
}
if (line[0] == _T('$'))
return true;
// Determine the length of the line
size_t len = _tcslen(line);
// If less then 27 characters -> something is wrong -> cancel
if (len < (is_utm ? 39 : 47))
return false;
GeoPoint location;
if ((!is_utm && !ParseLocation(line + 10, location)) ||
(is_utm && !ParseLocationUTM(line + 9, location)))
return false;
Waypoint new_waypoint(location);
new_waypoint.file_num = file_num;
new_waypoint.original_id = 0;
if (!ParseString(line, new_waypoint.name, 8))
return false;
if (!ParseAltitude(line + (is_utm ? 32 : 41), new_waypoint.elevation) &&
!CheckAltitude(new_waypoint))
return false;
// Description (Characters 35-44)
if (len > (is_utm ? 38 : 47))
ParseString(line + (is_utm ? 38 : 47), new_waypoint.comment);
way_points.Append(new_waypoint);
return true;
}
bool
WaypointReaderOzi::ParseLine(const TCHAR* line, const unsigned linenum,
Waypoints &way_points)
{
if (line[0] == '\0')
return true;
// Ignore first four header lines
if (linenum < 4)
return true;
TCHAR ctemp[255];
const TCHAR *params[20];
static constexpr unsigned int max_params = ARRAY_SIZE(params);
size_t n_params;
if (_tcslen(line) >= ARRAY_SIZE(ctemp))
/* line too long for buffer */
return false;
// Get fields
n_params = ExtractParameters(line, ctemp, params, max_params, true, _T('"'));
// Check if the basic fields are provided
if (n_params < 15)
return false;
GeoPoint location;
// Latitude (e.g. 5115.900N)
if (!ParseAngle(params[2], location.latitude))
return false;
// Longitude (e.g. 00715.900W)
if (!ParseAngle(params[3], location.longitude))
return false;
location.Normalize(); // ensure longitude is within -180:180
Waypoint new_waypoint(location);
new_waypoint.file_num = file_num;
long value;
new_waypoint.original_id = (ParseNumber(params[0], value) ? value : 0);
if (!ParseString(params[1], new_waypoint.name))
return false;
if (ParseNumber(params[14], value) && value != -777)
new_waypoint.elevation = Units::ToSysUnit(fixed(value), Unit::FEET);
else if (!CheckAltitude(new_waypoint))
return false;
// Description (Characters 35-44)
ParseString(params[11], new_waypoint.comment);
way_points.Append(std::move(new_waypoint));
return true;
}
bool
WayPointFileWinPilot::parseLine(const TCHAR* line, const unsigned linenum,
Waypoints &way_points,
const RasterTerrain *terrain)
{
TCHAR ctemp[255];
const TCHAR *params[20];
size_t n_params;
// If (end-of-file or comment)
if (line[0] == '\0' || line[0] == 0x1a ||
_tcsstr(line, _T("**")) == line ||
_tcsstr(line, _T("*")) == line)
// -> return without error condition
return true;
if (_tcslen(line) >= sizeof(ctemp) / sizeof(ctemp[0]))
/* line too long for buffer */
return false;
GeoPoint location;
// Get fields
n_params = extractParameters(line, ctemp, params, 20);
if (n_params < 6)
return false;
// Latitude (e.g. 51:15.900N)
if (!parseAngle(params[1], location.Latitude, true))
return false;
// Longitude (e.g. 00715.900W)
if (!parseAngle(params[2], location.Longitude, false))
return false;
Waypoint new_waypoint(location);
new_waypoint.FileNum = file_num;
// Name (e.g. KAMPLI)
if (!parseString(params[5], new_waypoint.Name))
return false;
// Altitude (e.g. 458M)
/// @todo configurable behaviour
bool alt_ok = parseAltitude(params[3], new_waypoint.Altitude);
check_altitude(new_waypoint, terrain, alt_ok);
if (n_params > 6) {
// Description (e.g. 119.750 Airport)
parseString(params[6], new_waypoint.Comment);
}
// Waypoint Flags (e.g. AT)
parseFlags(params[4], new_waypoint.Flags);
add_waypoint(way_points, new_waypoint);
return true;
}
bool
WaypointReaderZander::ParseLine(const TCHAR* line, const unsigned linenum,
Waypoints &way_points)
{
// If (end-of-file or comment)
if (line[0] == '\0' || line[0] == 0x1a ||
_tcsstr(line, _T("**")) == line ||
_tcsstr(line, _T("*")) == line)
// -> return without error condition
return true;
// Determine the length of the line
size_t len = _tcslen(line);
// If less then 34 characters -> something is wrong -> cancel
if (len < 34)
return false;
GeoPoint location;
// Latitude (Characters 13-20 // DDMMSS(N/S))
if (!ParseAngle(line + 13, location.latitude, true))
return false;
// Longitude (Characters 21-29 // DDDMMSS(E/W))
if (!ParseAngle(line + 21, location.longitude, false))
return false;
location.Normalize(); // ensure longitude is within -180:180
Waypoint new_waypoint(location);
new_waypoint.file_num = file_num;
new_waypoint.original_id = 0;
// Name (Characters 0-12)
if (!ParseString(line, new_waypoint.name, 12))
return false;
// Altitude (Characters 30-34 // e.g. 1561 (in meters))
/// @todo configurable behaviour
if (!ParseAltitude(line + 30, new_waypoint.elevation) &&
!CheckAltitude(new_waypoint))
return false;
// Description (Characters 35-44)
if (len > 35)
ParseString(line + 35, new_waypoint.comment, 9);
// Flags (Characters 45-49)
if (len < 46 || !ParseFlags(line + 45, new_waypoint))
if (len < 36 || !ParseFlagsFromDescription(line + 35, new_waypoint))
new_waypoint.flags.turn_point = true;
way_points.Append(new_waypoint);
return true;
}
bool
WayPointFileZander::parseLine(const TCHAR* line, const unsigned linenum,
Waypoints &way_points,
const RasterTerrain *terrain)
{
// If (end-of-file or comment)
if (line[0] == '\0' || line[0] == 0x1a ||
_tcsstr(line, _T("**")) == line ||
_tcsstr(line, _T("*")) == line)
// -> return without error condition
return true;
// Determine the length of the line
size_t len = _tcslen(line);
// If less then 34 characters -> something is wrong -> cancel
if (len < 34)
return false;
GeoPoint location;
// Latitude (Characters 13-20 // DDMMSS(N/S))
if (!parseAngle(line + 13, location.Latitude, true))
return false;
// Longitude (Characters 21-29 // DDDMMSS(E/W))
if (!parseAngle(line + 21, location.Longitude, false))
return false;
Waypoint new_waypoint(location);
new_waypoint.FileNum = file_num;
// Name (Characters 0-12)
if (!parseString(line, new_waypoint.Name))
return false;
// Altitude (Characters 30-34 // e.g. 1561 (in meters))
/// @todo configurable behaviour
bool alt_ok = parseAltitude(line + 30, new_waypoint.Altitude);
check_altitude(new_waypoint, terrain, alt_ok);
// Description (Characters 35-44)
if (len > 35)
parseString(line + 35, new_waypoint.Comment);
// Flags (Characters 45-49)
if (len > 45)
parseFlags(line + 45, new_waypoint.Flags);
add_waypoint(way_points, new_waypoint);
return true;
}
bool
WaypointReaderWinPilot::ParseLine(const TCHAR* line, const unsigned linenum,
Waypoints &waypoints)
{
TCHAR ctemp[4096];
const TCHAR *params[20];
static const unsigned int max_params = ARRAY_SIZE(params);
static bool welt2000_format = false;
size_t n_params;
if (linenum == 0)
welt2000_format = false;
// If (end-of-file)
if (line[0] == '\0' || line[0] == 0x1a)
// -> return without error condition
return true;
// If comment
if (line[0] == _T('*')) {
if (linenum == 0)
welt2000_format = (_tcsstr(line, _T("WRITTEN BY WELT2000")) != NULL);
// -> return without error condition
return true;
}
if (_tcslen(line) >= ARRAY_SIZE(ctemp))
/* line too long for buffer */
return false;
GeoPoint location;
// Get fields
n_params = ExtractParameters(line, ctemp, params, max_params, true);
if (n_params < 6)
return false;
// Latitude (e.g. 51:15.900N)
if (!ParseAngle(params[1], location.latitude, true))
return false;
// Longitude (e.g. 00715.900W)
if (!ParseAngle(params[2], location.longitude, false))
return false;
location.Normalize(); // ensure longitude is within -180:180
Waypoint new_waypoint(location);
new_waypoint.file_num = file_num;
new_waypoint.original_id = _tcstoul(params[0], NULL, 0);
// Name (e.g. KAMPLI)
if (*params[5] == _T('\0'))
return false;
new_waypoint.name=params[5];
// Altitude (e.g. 458M)
/// @todo configurable behaviour
if (!ParseAltitude(params[3], new_waypoint.altitude) &&
!CheckAltitude(new_waypoint))
return false;
if (n_params > 6) {
// Description (e.g. 119.750 Airport)
new_waypoint.comment=params[6];
if (welt2000_format)
ParseRunwayDirection(params[6], new_waypoint.runway);
}
// Waypoint Flags (e.g. AT)
ParseFlags(params[4], new_waypoint);
waypoints.Append(new_waypoint);
return true;
}
int waypoint_add(vec3d *pos, int waypoint_instance)
{
Assert(pos != NULL);
waypoint_list *wp_list;
waypoint *wpt;
int i, wp_list_index, wp_index;
// find a new list to start
if (waypoint_instance < 0)
{
// get a name for it
char buf[NAME_LENGTH];
waypoint_find_unique_name(buf, Waypoint_lists.size() + 1);
// add new list with that name
waypoint_list new_list(buf);
Waypoint_lists.push_back(new_list);
wp_list = &Waypoint_lists.back();
// set up references
wp_list_index = Waypoint_lists.size() - 1;
wp_index = wp_list->get_waypoints().size();
}
// create the waypoint on the same list as, and immediately after, waypoint_instance
else
{
calc_waypoint_indexes(waypoint_instance, wp_list_index, wp_index);
wp_list = find_waypoint_list_at_index(wp_list_index);
// theoretically waypoint_instance points to a current waypoint, so advance past it
wp_index++;
// it has to be on, or at the end of, an existing list
Assert(wp_list != NULL);
Assert((uint) wp_index <= wp_list->get_waypoints().size());
// iterate through all waypoints that are at this index or later,
// and edit their instances so that they point to a waypoint one place higher
for (object *objp = GET_FIRST(&obj_used_list); objp != END_OF_LIST(&obj_used_list); objp = GET_NEXT(objp))
{
if ((objp->type == OBJ_WAYPOINT) && (calc_waypoint_list_index(objp->instance) == wp_list_index) && (calc_waypoint_index(objp->instance) >= wp_index))
objp->instance++;
}
}
// so that masking in the other function works
// (though if you actually hit this Assert, you have other problems)
Assert(wp_index < 0x10000);
// create the waypoint object
waypoint new_waypoint(pos, wp_list);
// add it at its appropriate spot, which may be the end of the list
SCP_vector<waypoint>::iterator ii = wp_list->get_waypoints().begin();
for (i = 0; i < wp_index; i++)
++ii;
wp_list->get_waypoints().insert(ii, new_waypoint);
wpt = find_waypoint_at_index(wp_list, wp_index);
// apparently we create it in-game too; this is called by both scripting and FRED
waypoint_create_game_object(wpt, wp_list_index, wp_index);
return wpt->get_objnum();
}
bool
WaypointReaderSeeYou::ParseLine(const TCHAR* line, const unsigned linenum,
Waypoints &waypoints)
{
TCHAR ctemp[4096];
const TCHAR *params[20];
static const unsigned int max_params = ARRAY_SIZE(params);
size_t n_params;
const unsigned iName = 0;
const unsigned iLatitude = 3, iLongitude = 4, iElevation = 5;
const unsigned iStyle = 6, iRWDir = 7, iRWLen = 8;
const unsigned iFrequency = 9, iDescription = 10;
static bool ignore_following;
if (linenum == 0)
ignore_following = false;
// If (end-of-file or comment)
if (line[0] == '\0' || line[0] == 0x1a ||
_tcsstr(line, _T("**")) == line ||
_tcsstr(line, _T("*")) == line)
// -> return without error condition
return true;
if (_tcslen(line) >= ARRAY_SIZE(ctemp))
/* line too long for buffer */
return false;
// Skip first line if it doesn't begin with a quotation character
// (usually the field order line)
if (linenum == 0 && line[0] != _T('\"'))
return true;
// If task marker is reached ignore all following lines
if (_tcsstr(line, _T("-----Related Tasks-----")) == line)
ignore_following = true;
if (ignore_following)
return true;
// Get fields
n_params = ExtractParameters(line, ctemp, params, max_params, true, _T('"'));
// Check if the basic fields are provided
if (iName >= n_params)
return false;
if (iLatitude >= n_params)
return false;
if (iLongitude >= n_params)
return false;
GeoPoint location;
// Latitude (e.g. 5115.900N)
if (!ParseAngle(params[iLatitude], location.latitude, true))
return false;
// Longitude (e.g. 00715.900W)
if (!ParseAngle(params[iLongitude], location.longitude, false))
return false;
location.Normalize(); // ensure longitude is within -180:180
Waypoint new_waypoint(location);
new_waypoint.file_num = file_num;
new_waypoint.original_id = 0;
// Name (e.g. "Some Turnpoint")
if (*params[iName] == _T('\0'))
return false;
new_waypoint.name = params[iName];
// Elevation (e.g. 458.0m)
/// @todo configurable behaviour
if ((iElevation >= n_params ||
!ParseAltitude(params[iElevation], new_waypoint.elevation)) &&
!CheckAltitude(new_waypoint))
return false;
// Style (e.g. 5)
/// @todo include peaks with peak symbols etc.
if (iStyle < n_params)
ParseStyle(params[iStyle], new_waypoint);
// Frequency & runway direction/length (for airports and landables)
// and description (e.g. "Some Description")
if (new_waypoint.IsLandable()) {
if (iFrequency < n_params)
new_waypoint.radio_frequency = RadioFrequency::Parse(params[iFrequency]);
// Runway length (e.g. 546.0m)
fixed rwlen = fixed_minus_one;
if (iRWLen < n_params && ParseDistance(params[iRWLen], rwlen) &&
positive(rwlen))
new_waypoint.runway.SetLength(uround(rwlen));
if (iRWDir < n_params && *params[iRWDir]) {
TCHAR *end;
int direction =_tcstol(params[iRWDir], &end, 10);
if (end == params[iRWDir] || direction < 0 || direction > 360 ||
(direction == 0 && !positive(rwlen)))
direction = -1;
else if (direction == 360)
direction = 0;
if (direction >= 0)
new_waypoint.runway.SetDirectionDegrees(direction);
}
}
if (iDescription < n_params)
new_waypoint.comment = params[iDescription];
waypoints.Append(new_waypoint);
return true;
}
bool
WayPointFileSeeYou::parseLine(const TCHAR* line, const unsigned linenum,
Waypoints &way_points,
const RasterTerrain *terrain)
{
TCHAR ctemp[255];
const TCHAR *params[20];
size_t n_params;
static unsigned iName = 0, iCode = 1, iCountry = 2;
static unsigned iLatitude = 3, iLongitude = 4, iElevation = 5;
static unsigned iStyle = 6, iRWDir = 7, iRWLen = 8;
static unsigned iFrequency = 9, iDescription = 10;
static bool ignore_following = false;
// If (end-of-file or comment)
if (line[0] == '\0' || line[0] == 0x1a ||
_tcsstr(line, _T("**")) == line ||
_tcsstr(line, _T("*")) == line)
// -> return without error condition
return true;
if (_tcslen(line) >= sizeof(ctemp) / sizeof(ctemp[0]))
/* line too long for buffer */
return false;
// Parse first line holding field order
/// @todo linenum == 0 should be the first
/// (not ignored) line, not just line 0
if (linenum == 0) {
// Get fields
n_params = extractParameters(line, ctemp, params, 20);
// Iterate through fields and save the field order
for (unsigned i = 0; i < n_params; i++) {
const TCHAR* value = params[i];
if (!_tcscmp(value, _T("name")))
iName = i;
else if (!_tcscmp(value, _T("code")))
iCode = i;
else if (!_tcscmp(value, _T("country")))
iCountry = i;
else if (!_tcscmp(value, _T("lat")))
iLatitude = i;
else if (!_tcscmp(value, _T("lon")))
iLongitude = i;
else if (!_tcscmp(value, _T("elev")))
iElevation = i;
else if (!_tcscmp(value, _T("style")))
iStyle = i;
else if (!_tcscmp(value, _T("rwdir")))
iRWDir = i;
else if (!_tcscmp(value, _T("rwlen")))
iRWLen = i;
else if (!_tcscmp(value, _T("freq")))
iFrequency = i;
else if (!_tcscmp(value, _T("desc")))
iDescription = i;
}
ignore_following = false;
return true;
}
// If task marker is reached ignore all following lines
if (_tcsstr(line, _T("-----Related Tasks-----")) == line)
ignore_following = true;
if (ignore_following)
return true;
// Get fields
n_params = extractParameters(line, ctemp, params, 20);
// Check if the basic fields are provided
if (iName >= n_params)
return false;
if (iLatitude >= n_params)
return false;
if (iLongitude >= n_params)
return false;
GeoPoint location;
// Latitude (e.g. 5115.900N)
if (!parseAngle(params[iLatitude], location.Latitude, true))
return false;
// Longitude (e.g. 00715.900W)
if (!parseAngle(params[iLongitude], location.Longitude, false))
return false;
Waypoint new_waypoint(location);
new_waypoint.FileNum = file_num;
// Name (e.g. "Some Turnpoint", with quotes)
if (!parseString(params[iName], new_waypoint.Name))
return false;
// Elevation (e.g. 458.0m)
/// @todo configurable behaviour
bool alt_ok = iElevation < n_params &&
parseAltitude(params[iElevation], new_waypoint.Altitude);
check_altitude(new_waypoint, terrain, alt_ok);
// Description (e.g. "Some Turnpoint", with quotes)
/// @todo include frequency and rwdir/len
if (iDescription < n_params)
parseString(params[iDescription], new_waypoint.Comment);
// Style (e.g. 5)
/// @todo include peaks with peak symbols etc.
if (iStyle < n_params)
parseStyle(params[iStyle], new_waypoint.Flags);
// If the Style attribute did not state that this is an airport
if (!new_waypoint.Flags.Airport) {
// -> parse the runway length
fixed rwlen;
// Runway length (e.g. 546.0m)
if (iRWLen < n_params && parseAltitude(params[iRWLen], rwlen)) {
// If runway length is between 100m and 300m -> landpoint
if (rwlen > fixed(100) && rwlen <= fixed(300))
new_waypoint.Flags.LandPoint = true;
// If runway length is higher then 300m -> airport
if (rwlen > fixed(300))
new_waypoint.Flags.Airport = true;
}
}
add_waypoint(way_points, new_waypoint);
return true;
}
void Controller_StateMachine_v2::isStraightFinished() {
if ( rs_act > rs_end) { // straight phase finished
pr_checkpoint = checkpoint; // previous checkpoint
trajectory.achievedCheckpoint(checkpoint);
checkpoint = trajectory.incrementCheckpoint(checkpoint);
if ( r_nextState == SM_stateNames::HOVER ) { // in this case the trajectory should be non-periodic and the end of the trajectory has been reached
if ( !(trajectory.getIsPeriodic()) ) { // Hover to trajectory end
// h_checkpoint = trajectory[checkpoint];
trajectory[checkpoint].convert2Vector(h_checkpoint); // should be (trajectory.getLength()-1)
current_state = r_nextState; // HOVER
h_stay_in_last_checkpoint = true;
justChangedState = true;
return;
} else {
// TODO: add trajEvent, a JP_TRAJ_ERROR, the impossible happened: reached end of periodic trajectory
}
} else {
if ( r_nextState == SM_stateNames::TURN ) { // perform turn
current_state = r_nextState; // TURN
justChangedState = true;
return;
} else { // r_nextState == SM_stateNames::STRAIGHT
if ( trajectory.isAchievedTrueCheckpoint( pr_checkpoint ) ) { // Proceed to next checkpoint
current_state = r_nextState; // STRAIGHT
derivBlock_vxfo.reset(); // to avoid a tiltfi pick
derivBlock_vyfo.reset(); // to avoid a tiltfi pick
justChangedState = true;
return;
} else { // hovert to pr_checkpoint
// h_checkpoint = trajectory[pr_checkpoint]
trajectory[pr_checkpoint].convert2Vector(h_checkpoint);
current_state = SM_stateNames::HOVER; // HOVER
h_stay_in_last_checkpoint = false;
justChangedState = true;
return;
}
}
}
} else { // safety zone STRAIGHT mode
cvg_double distance2straightLine = sqrt( pow( xrefo-xei,2) + pow( yrefo-yei,2) + pow( zrefo-zei,2) );
if ( distance2straightLine > trajectory.traj_config.straighmode_safetyzone_radius_m ) {
// hover to perpendicular point (assign to h_checkpoint) in the straight line
Controller_SM_TrajectoryWaypoint new_waypoint( xrefo, yrefo, zrefo);
new_waypoint.convert2Vector(h_checkpoint);
current_state = SM_stateNames::HOVER; // HOVER
h_stay_in_last_checkpoint = false;
justChangedState = true;
return;
} else {
if ( rs_act < -trajectory.traj_config.straighmode_safetyzone_radius_m ) {
// hover to r_p0
h_checkpoint = r_p0;
current_state = SM_stateNames::HOVER; // HOVER
h_stay_in_last_checkpoint = false;
justChangedState = true;
return;
} else { // Continue STRAIGHT, nothing to be done here
}
}
}
}
