int main(void)
{
if( test(-1) != 1 ) fail( __LINE__ );
if( test(0) != 1 ) fail( __LINE__ );
if( test(1) != 1 ) fail( __LINE__ );
if( u_test(-1) != 1 ) fail( __LINE__ );
if( u_test(0) != 1 ) fail( __LINE__ );
if( u_test(1) != 1 ) fail( __LINE__ );
if( a_test(-2) != 1 ) fail( __LINE__ );
if( a_test(-1) != 1 ) fail( __LINE__ );
if( a_test(0) != 1 ) fail( __LINE__ );
if( b_test(INT_MIN+1) != 1 ) fail( __LINE__ );
if( b_test(INT_MIN) != 1 ) fail( __LINE__ );
if( b_test(INT_MAX) != 1 ) fail( __LINE__ );
if( c_test(INT_MAX-1) != 1 ) fail( __LINE__ );
if( c_test(INT_MAX) != 1 ) fail( __LINE__ );
if( c_test(INT_MIN) != 1 ) fail( __LINE__ );
if( d_test(INT_MAX-1) != 1 ) fail( __LINE__ );
if( d_test(INT_MAX) != 1 ) fail( __LINE__ );
if( d_test(2) != 1 ) fail( __LINE__ );
if( e_test(INT_MIN+1) != 1 ) fail( __LINE__ );
if( e_test(INT_MIN) != 1 ) fail( __LINE__ );
if( e_test(INT_MAX) != 1 ) fail( __LINE__ );
if( f_test(INT_MAX-1) != 1 ) fail( __LINE__ );
if( f_test(INT_MAX) != 1 ) fail( __LINE__ );
if( f_test(INT_MIN) != 1 ) fail( __LINE__ );
_PASS;
}
bool
RoutePlanner::solve(const AGeoPoint& origin,
const AGeoPoint& destination,
const RoutePlannerConfig& config,
const short h_ceiling)
{
on_solve(origin, destination);
rpolars_route.set_config(config, std::max(destination.altitude, origin.altitude),
h_ceiling);
rpolars_reach.set_config(config, std::max(destination.altitude, origin.altitude),
h_ceiling);
m_reach_polar_mode = config.reach_polar_mode;
{
const AFlatGeoPoint s_origin(task_projection.project(origin), origin.altitude);
const AFlatGeoPoint s_destination(task_projection.project(destination), destination.altitude);
if (!(s_origin == origin_last) || !(s_destination == destination_last))
dirty = true;
if (is_trivial())
return false;
dirty = false;
origin_last = s_origin;
destination_last = s_destination;
h_min = std::min(s_origin.altitude, s_destination.altitude);
h_max = rpolars_route.cruise_altitude;
}
solution_route.clear();
solution_route.push_back(origin);
solution_route.push_back(destination);
if (!rpolars_route.terrain_enabled() && !rpolars_route.airspace_enabled())
return false; // trivial
m_search_hull.clear();
m_search_hull.push_back(SearchPoint(origin_last, task_projection));
RoutePoint start = origin_last;
m_astar_goal = destination_last;
RouteLink e_test(start, m_astar_goal, task_projection);
if (e_test.is_short())
return false;
if (!rpolars_route.achievable(e_test))
return false;
count_dij=0;
count_airspace=0;
count_terrain=0;
count_supressed=0;
bool retval = false;
m_planner.restart(start);
unsigned best_d = UINT_MAX;
if (verbose) {
printf("# goal node (%d,%d,%d)\n",
m_astar_goal.Longitude, m_astar_goal.Latitude, m_astar_goal.altitude);
printf("# start node (%d,%d,%d)\n",
start.Longitude, start.Latitude, start.altitude);
}
while (!m_planner.empty()) {
const RoutePoint node = m_planner.pop();
if (verbose>1) {
printf("# processing node (%d,%d,%d) %d,%d q size %d\n",
node.Longitude, node.Latitude, node.altitude,
m_planner.get_node_value(node).g,
m_planner.get_node_value(node).h,
m_planner.queue_size());
}
h_min = std::min(h_min, node.altitude);
h_max = std::max(h_max, node.altitude);
bool is_final = (node == m_astar_goal);
if (is_final) {
if (!retval)
best_d = UINT_MAX;
retval = true;
}
if (is_final) // @todo: allow fallback if failed
{ // copy improving solutions
Route this_solution;
unsigned d = find_solution(node, this_solution);
if (d< best_d) {
best_d = d;
solution_route = this_solution;
}
}
if (retval)
break; // want top solution only
// shoot for final
RouteLink e(node, m_astar_goal, task_projection);
if (set_unique(e))
add_edges(e);
while (!m_links.empty()) {
add_edges(m_links.front());
m_links.pop();
}
}
count_unique = m_unique.size();
if (retval && verbose) {
printf("# solved with %d intermediate points\n", (int)(solution_route.size()-2));
}
if (retval) {
// correct solution for rounding
assert(solution_route.size()>=2);
for (unsigned i=0; i< solution_route.size(); ++i) {
FlatGeoPoint p(task_projection.project(solution_route[i]));
if (p== origin_last) {
solution_route[i] = AGeoPoint(origin, solution_route[i].altitude);
} else if (p== destination_last) {
solution_route[i] = AGeoPoint(destination, solution_route[i].altitude);
}
}
} else {
solution_route.clear();
solution_route.push_back(origin);
solution_route.push_back(destination);
}
m_planner.clear();
m_unique.clear();
// m_search_hull.clear();
return retval;
}
