bool test_task_or(TaskManager& task_manager,
const Waypoints &waypoints)
{
const Waypoint *wp;
task_manager.SetFactory(TaskFactoryType::FAI_OR);
AbstractTaskFactory &fact = task_manager.GetFactory();
task_report(task_manager, "# adding start\n");
wp = waypoints.LookupId(1);
if (wp) {
OrderedTaskPoint *tp = fact.CreateStart(*wp);
if (!fact.Append(*tp)) {
return false;
}
delete tp;
}
task_manager.SetActiveTaskPoint(0);
task_manager.Resume();
task_report(task_manager, "# adding intermediate\n");
wp = waypoints.LookupId(2);
if (wp) {
OrderedTaskPoint *tp = fact.CreateIntermediate(*wp);
if (!fact.Append(*tp)) {
return false;
}
delete tp;
}
task_report(task_manager, "# adding finish\n");
wp = waypoints.LookupId(1);
if (wp) {
OrderedTaskPoint *tp = fact.CreateFinish(*wp);
if (!fact.Append(*tp)) {
return false;
}
delete tp;
}
fact.UpdateStatsGeometry();
task_report(task_manager, "# checking task..\n");
if (!fact.Validate()) {
return false;
}
if (!task_manager.CheckOrderedTask()) {
return false;
}
return true;
}
static bool
TestErase(Waypoints& waypoints, unsigned id)
{
waypoints.Optimise();
auto wp = waypoints.LookupId(id);
if (wp == NULL)
return false;
waypoints.Erase(std::move(wp));
waypoints.Optimise();
wp = waypoints.LookupId(id);
return wp == NULL;
}
static bool
test_goto(int n_wind, unsigned id, bool auto_mc)
{
GlidePolar glide_polar(fixed(2));
Waypoints waypoints;
SetupWaypoints(waypoints);
if (verbose)
PrintDistanceCounts();
TaskBehaviour task_behaviour;
task_behaviour.SetDefaults();
task_behaviour.DisableAll();
task_behaviour.auto_mc = auto_mc;
task_behaviour.enable_trace = false;
TaskManager task_manager(task_behaviour, waypoints);
TaskEventsPrint default_events(verbose);
task_manager.SetTaskEvents(default_events);
task_manager.SetGlidePolar(glide_polar);
test_task(task_manager, waypoints, 1);
task_manager.DoGoto(*waypoints.LookupId(id));
task_report(task_manager, "goto");
waypoints.Clear(); // clear waypoints so abort wont do anything
autopilot_parms.goto_target = true;
return run_flight(task_manager, autopilot_parms, n_wind);
}
static bool
TestReplace(Waypoints& waypoints, unsigned id)
{
auto wp = waypoints.LookupId(id);
if (wp == NULL)
return false;
tstring oldName = wp->name;
Waypoint copy = *wp;
copy.name = _T("Fred");
waypoints.Replace(wp, std::move(copy));
waypoints.Optimise();
wp = waypoints.LookupId(id);
return wp != NULL && wp->name != oldName && wp->name == _T("Fred");
}
bool test_task_dash(TaskManager& task_manager,
const Waypoints &waypoints)
{
WaypointPtr wp;
task_manager.SetFactory(TaskFactoryType::TOURING);
AbstractTaskFactory &fact = task_manager.GetFactory();
task_report(task_manager, "# adding start\n");
wp = waypoints.LookupId(1);
if (wp) {
OrderedTaskPoint *tp = fact.CreateStart(std::move(wp));
if (!fact.Append(*tp)) {
return false;
}
delete tp;
}
task_manager.SetActiveTaskPoint(0);
task_manager.Resume();
task_report(task_manager, "# adding finish\n");
wp = waypoints.LookupId(3);
if (wp) {
OrderedTaskPoint *tp = fact.CreateFinish(std::move(wp));
if (!fact.Append(*tp)) {
return false;
}
delete tp;
}
fact.UpdateStatsGeometry();
task_report(task_manager, "# checking task..\n");
if (!fact.Validate()) {
return false;
}
if (!task_manager.CheckOrderedTask()) {
return false;
}
return true;
}
WaypointPtr
random_waypoint(const Waypoints &waypoints)
{
static unsigned id_last = 0;
unsigned id = 0;
do {
id = rand() % waypoints.size()+1;
} while (id==id_last);
id_last = id;
return waypoints.LookupId(id);
}
static void
TestLookups(const Waypoints &waypoints, const GeoPoint ¢er)
{
WaypointPtr waypoint;
ok1((waypoint = waypoints.LookupId(0)) == NULL);
ok1((waypoint = waypoints.LookupId(1)) != NULL);
ok1(waypoint->original_id == 0);
ok1((waypoint = waypoints.LookupId(151)) != NULL);
ok1(waypoint->original_id == 150);
ok1((waypoint = waypoints.LookupId(152)) == NULL);
ok1((waypoint = waypoints.LookupId(160)) == NULL);
ok1((waypoint = waypoints.LookupLocation(center, fixed(0))) != NULL);
ok1(waypoint->original_id == 0);
ok1((waypoint = waypoints.LookupName(_T("Waypoint #5"))) != NULL);
ok1(waypoint->original_id == 4);
ok1((waypoint = waypoints.LookupLocation(waypoint->location, fixed(10000))) != NULL);
ok1(waypoint->original_id == 4);
}
static void
FillLastUsedList(WaypointList &list,
const WaypointIDList &last_used_ids,
const Waypoints &waypoints)
{
for (auto it = last_used_ids.rbegin(); it != last_used_ids.rend(); it++) {
const Waypoint* waypoint = waypoints.LookupId(*it);
if (waypoint == NULL)
continue;
list.push_back(WaypointListItem(*waypoint));
}
}
static void
FillLastUsedList(WaypointList &list,
const WaypointIDList &last_used_ids,
const Waypoints &waypoints)
{
for (auto it = last_used_ids.rbegin(); it != last_used_ids.rend(); it++) {
const Waypoint* waypoint = waypoints.LookupId(*it);
if (waypoint == nullptr)
continue;
list.emplace_back(*waypoint);
}
}
static unsigned
TestCopy(Waypoints& waypoints)
{
const WaypointPtr wp = waypoints.LookupId(5);
if (!wp)
return false;
unsigned size_old = waypoints.size();
Waypoint wp_copy = *wp;
wp_copy.id = waypoints.size() + 1;
waypoints.Append(std::move(wp_copy));
waypoints.Optimise();
unsigned size_new = waypoints.size();
return (size_new == size_old + 1);
}
static void
FillLastUsedList(WaypointSelectInfoVector &list,
const std::list<unsigned int> &last_used_ids,
const Waypoints &waypoints, const GeoPoint location)
{
list.clear();
if (last_used_ids.empty())
return;
for (auto it = last_used_ids.rbegin(); it != last_used_ids.rend(); it++) {
const Waypoint* waypoint = waypoints.LookupId(*it);
if (waypoint == NULL)
continue;
list.push_back(*waypoint, location);
}
}
void
WaypointGlue::SetHome(Waypoints &way_points, const RasterTerrain *terrain,
ComputerSettings &settings,
const bool reset)
{
LogStartUp(_T("SetHome"));
if (reset)
settings.poi.home_waypoint = -1;
// check invalid home waypoint or forced reset due to file change
const Waypoint *wp = FindHomeId(way_points, settings.poi);
if (wp == NULL) {
/* fall back to HomeLocation, try to find it in the waypoint
database */
wp = FindHomeLocation(way_points, settings.poi);
if (wp == NULL)
// search for home in waypoint list, if we don't have a home
wp = FindFlaggedHome(way_points, settings.poi);
}
// check invalid task ref waypoint or forced reset due to file change
if (reset || way_points.IsEmpty() ||
!way_points.LookupId(settings.team_code.team_code_reference_waypoint))
// set team code reference waypoint if we don't have one
settings.team_code.team_code_reference_waypoint = settings.poi.home_waypoint;
if (device_blackboard != NULL) {
if (wp != NULL) {
// OK, passed all checks now
LogStartUp(_T("Start at home waypoint"));
device_blackboard->SetStartupLocation(wp->location, wp->altitude);
} else if (terrain != NULL) {
// no home at all, so set it from center of terrain if available
GeoPoint loc = terrain->GetTerrainCenter();
LogStartUp(_T("Start at terrain center"));
device_blackboard->SetStartupLocation(loc,
fixed(terrain->GetTerrainHeight(loc)));
}
}
}
/**
* Initialises waypoints with random and non-random waypoints
* for testing
*
* @param waypoints waypoints class to add waypoints to
*/
bool setup_waypoints(Waypoints &waypoints, const unsigned n)
{
Waypoint wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_zero),
Angle::Degrees(fixed_zero)));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_zero),
Angle::Degrees(fixed_one)));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_one),
Angle::Degrees(fixed_one)));
wp.name = _T("Hello");
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed_half;
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed(0.8)),
Angle::Degrees(fixed(0.5))));
wp.name = _T("Unk");
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_one),
Angle::Degrees(fixed_zero)));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed_zero),
Angle::Degrees(fixed(0.23))));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(wp);
for (unsigned i=0; i<(unsigned)std::max((int)n-6,0); i++) {
int x = rand()%1200-100;
int y = rand()%1200-100;
double z = rand()% std::max(terrain_height,1);
wp = waypoints.Create(GeoPoint(Angle::Degrees(fixed(x/1000.0)),
Angle::Degrees(fixed(y/1000.0))));
wp.type = Waypoint::Type::NORMAL;
wp.elevation = fixed(z);
waypoints.Append(wp);
}
waypoints.Optimise();
if (verbose) {
std::ofstream fin("results/res-wp-in.txt");
for (unsigned i=1; i<=waypoints.size(); i++) {
const Waypoint *wp = waypoints.LookupId(i);
if (wp != NULL)
fin << *wp;
}
}
return true;
}
bool test_task_aat(TaskManager& task_manager,
const Waypoints &waypoints)
{
task_manager.SetFactory(TaskFactoryType::AAT);
AbstractTaskFactory &fact = task_manager.GetFactory();
const Waypoint *wp;
task_report(task_manager, "# adding start\n");
wp = waypoints.LookupId(1);
if (wp) {
OrderedTaskPoint *tp = fact.CreateStart(*wp);
if (!fact.Append(*tp,false)) {
return false;
}
delete tp;
}
task_manager.SetActiveTaskPoint(0);
task_manager.Resume();
task_report(task_manager, "# adding intermediate\n");
wp = waypoints.LookupId(2);
if (wp) {
OrderedTaskPoint* tp = fact.CreateIntermediate(TaskPointFactoryType::AAT_CYLINDER,*wp);
if (tp->GetObservationZone().GetShape() == ObservationZone::Shape::CYLINDER) {
CylinderZone &cz = (CylinderZone &)tp->GetObservationZone();
cz.SetRadius(fixed(30000.0));
}
if (!fact.Append(*tp,false)) {
return false;
}
delete tp;
}
task_report(task_manager, "# adding intermediate\n");
wp = waypoints.LookupId(3);
if (wp) {
OrderedTaskPoint* tp = fact.CreateIntermediate(TaskPointFactoryType::AAT_CYLINDER,*wp);
if (tp->GetObservationZone().GetShape() == ObservationZone::Shape::CYLINDER) {
CylinderZone &cz = (CylinderZone &)tp->GetObservationZone();
cz.SetRadius(fixed(40000.0));
}
if (!fact.Append(*tp,false)) {
return false;
}
delete tp;
}
task_report(task_manager, "# adding finish\n");
wp = waypoints.LookupId(1);
if (wp) {
OrderedTaskPoint *tp = fact.CreateFinish(*wp);
if (!fact.Append(*tp,false)) {
return false;
}
delete tp;
}
fact.UpdateStatsGeometry();
task_report(task_manager, "# checking task..\n");
if (!fact.Validate()) {
return false;
}
if (!task_manager.CheckOrderedTask()) {
return false;
}
return true;
}
bool test_task_manip(TaskManager& task_manager,
const Waypoints &waypoints)
{
if (!test_task_mixed(task_manager, waypoints)) {
return false;
}
AbstractTaskFactory &fact = task_manager.GetFactory();
task_report(task_manager, "# removing tp 2\n");
if (!fact.Remove(2)) {
return false;
}
task_report(task_manager, "# removing tp 0\n");
if (!fact.Remove(0)) {
return false;
}
task_report(task_manager, "# removing tp -1 (illegal)\n");
if (fact.Remove(0-1)) {
return false;
}
task_report(task_manager, "# removing tp 50 (illegal)\n");
if (fact.Remove(50)) {
return false;
}
OrderedTaskPoint *tp;
const Waypoint *wp;
task_report(task_manager, "# inserting at 3\n");
wp = waypoints.LookupId(3);
if (wp) {
tp = fact.CreateIntermediate(TaskPointFactoryType::AST_CYLINDER,*wp);
if (!fact.Insert(*tp,3)) return false;
delete tp;
}
task_report(task_manager, "# auto-replacing at 2 (no morph)\n");
wp = waypoints.LookupId(9);
if (wp) {
tp = fact.CreateIntermediate(TaskPointFactoryType::AST_CYLINDER,*wp);
if (!fact.Replace(*tp,2)) return false;
delete tp;
}
task_report(task_manager, "# auto-replacing at 2 (morph)\n");
wp = waypoints.LookupId(9);
if (wp) {
tp = fact.CreateStart(*wp);
if (!fact.Replace(*tp,2)) return false;
delete tp;
}
task_report(task_manager, "# auto-replacing at 0 (morph this)\n");
wp = waypoints.LookupId(12);
if (wp) {
tp = fact.CreateIntermediate(TaskPointFactoryType::AST_CYLINDER,*wp);
if (!fact.Replace(*tp,0)) return false;
delete tp;
}
task_report(task_manager, "# auto-replacing at end (morph this)\n");
wp = waypoints.LookupId(14);
if (wp) {
tp = fact.CreateIntermediate(TaskPointFactoryType::AST_CYLINDER,*wp);
if (!fact.Replace(*tp,task_manager.TaskSize()-1)) return false;
delete tp;
}
task_report(task_manager, "# removing finish point\n");
if (!fact.Remove(task_manager.TaskSize()-1)) {
return false;
}
task_report(task_manager, "# inserting at 50 (equivalent to append)\n");
wp = waypoints.LookupId(8);
if (wp) {
tp = fact.CreateFinish(*wp);
if (!fact.Insert(*tp,50)) return false;
delete tp;
}
task_report(task_manager, "# inserting at 0 (morph this)\n");
wp = waypoints.LookupId(3);
if (wp) {
tp = fact.CreateFinish(*wp);
if (!fact.Insert(*tp,0)) return false;
delete tp;
}
task_report(task_manager, "# inserting at 2 (morph this)\n");
wp = waypoints.LookupId(4);
if (wp) {
tp = fact.CreateStart(*wp);
if (!fact.Insert(*tp,2)) return false;
delete tp;
}
task_report(task_manager, "# inserting at 2 (direct)\n");
wp = waypoints.LookupId(6);
if (wp) {
tp = fact.CreateIntermediate(*wp);
if (!fact.Insert(*tp,2,false)) return false;
delete tp;
}
task_report(task_manager, "# checking task\n");
fact.UpdateStatsGeometry();
if (task_manager.CheckOrderedTask()) {
task_manager.Reset();
task_manager.SetActiveTaskPoint(0);
task_manager.Resume();
} else {
return false;
}
return true;
}
/**
* Initialises waypoints with random and non-random waypoints
* for testing
*
* @param waypoints waypoints class to add waypoints to
*/
bool SetupWaypoints(Waypoints &waypoints, const unsigned n)
{
Waypoint wp = waypoints.Create(GeoPoint(Angle::Zero(),
Angle::Zero()));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(std::move(wp));
wp = waypoints.Create(GeoPoint(Angle::Zero(),
Angle::Degrees(1)));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(std::move(wp));
wp = waypoints.Create(GeoPoint(Angle::Degrees(1),
Angle::Degrees(1)));
wp.name = _T("Hello");
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.5);
waypoints.Append(std::move(wp));
wp = waypoints.Create(GeoPoint(Angle::Degrees(0.8),
Angle::Degrees(0.5)));
wp.name = _T("Unk");
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(std::move(wp));
wp = waypoints.Create(GeoPoint(Angle::Degrees(1),
Angle::Zero()));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(std::move(wp));
wp = waypoints.Create(GeoPoint(Angle::Zero(),
Angle::Degrees(0.23)));
wp.type = Waypoint::Type::AIRFIELD;
wp.elevation = fixed(0.25);
waypoints.Append(std::move(wp));
for (unsigned i=0; i<(unsigned)std::max((int)n-6,0); i++) {
int x = rand()%1200-100;
int y = rand()%1200-100;
double z = rand()% std::max(terrain_height,1);
wp = waypoints.Create(GeoPoint(Angle::Degrees(x / 1000.0),
Angle::Degrees(y / 1000.0)));
wp.type = Waypoint::Type::NORMAL;
wp.elevation = fixed(z);
waypoints.Append(std::move(wp));
}
waypoints.Optimise();
if (verbose) {
Directory::Create(Path(_T("output/results")));
std::ofstream fin("output/results/res-wp-in.txt");
for (unsigned i=1; i<=waypoints.size(); i++) {
const Waypoint *wpt = waypoints.LookupId(i);
if (wpt != NULL)
fin << *wpt;
}
}
return true;
}
