int main()
{
// push waypoints to a routes' map
waypoints.push_back(waypoint(-4.0, -3.0)); // 0
waypoints.push_back(waypoint(-3.0, -1.0)); // 1
waypoints.push_back(waypoint(1.0, -4.0)); // 2
waypoints.push_back(waypoint(3.0, -2.0)); // 3
waypoints.push_back(waypoint(-1.0, 1.0)); // 4
waypoints.push_back(waypoint(4.0, -3.0)); // 5
waypoints.push_back(waypoint(2.0, 1.0)); // 6
waypoints.push_back(waypoint(-2.0, 2.0)); // 7
waypoints.push_back(waypoint(5.0, -1.0)); // 8
waypoints.push_back(waypoint(6.0, 2.0)); // 9
waypoints.push_back(waypoint(-1.0, 4.0)); // 10
waypoints.push_back(waypoint(4.0, 4.0)); // 11
waypoints.push_back(waypoint(8.0, 3.0)); // 12
waypoints.push_back(waypoint(1.0, 6.0)); // 13
waypoints.push_back(waypoint(6.0, 5.0)); // 14
waypoints.push_back(waypoint(2.0, 7.0)); // 15
waypoints.push_back(waypoint(7.0, 7.0)); // 16
waypoints.push_back(waypoint(4.0, 7.0)); // 17
waypoints.push_back(waypoint(6.0, 9.0)); // 18
waypoints.push_back(waypoint(8.0, 9.0)); // 19
obstacles.push_back(obstacle(0.0,5.0,1.0)); // 0
obstacles.push_back(obstacle(3.0,3.0,2.0)); // 1
obstacles.push_back(obstacle(3.5,0.0,1.0)); // 2
obstacles.push_back(obstacle(8.0,7.0,2.5)); // 3
// define exponential traverser type and create an exemplar
typedef ExpTr<node_value,cost_value,expand,less<cost_value> > ExpTr_t;
ExpTr_t j;
cout << "in exp traverser forward search ...\n";
j = ExpTr_t(0);
// set a maximal lag length
j.getexpfunc().setMaxLagLen(5.0); // or 6.0
// add two defined handlers to enable calculation of an expansions number
j.set_handler_on_expand_root(OnExpandRoot);
j.set_handler_on_select_cursor(OnSelectCursor);
// launch A* algorithm (combination of a predefined heuristics with a best - first
// search that is provided by exponential traverser is A*!)
ForwardSearch(j, goal);
ShowPath(j);
// show founded path
cout << "number of expansions = " << num_expansions << '\n';
cout << "...out \n\n";
cout << "Search is completed.\n";
return 0;
}
void Polygon::addAllWaypoints(const waypoints_t& waypoints_)
{
for (auto i = waypoints_.begin(); i != waypoints_.end(); i++)
addWaypoints(*i);
}
// main operator that is used according to GSL specification
// it is used as a callback from a traverser code
// list<node_value> &e - list of child nodes - this list is used as an output - the follwoing code
// pushes node values to this list
// list<cost_value>& ce - output list for costs of child nodes - costs should have the same
// positions at the list as corresponding nodes in a previous list
// node_value n - this is a node to be expanded
// node_value - this is a parent of a previous node
// cost_value c - the cost of a node to be expanded
void operator()(list<node_value> &e, list<cost_value>& ce, node_value n, node_value, cost_value c) {
// x and y coordinates of a lag start waypoint
float xs = waypoints[n.waypointid].x;
float ys = waypoints[n.waypointid].y;
// iterator over all waypoints on a routes' map
waypoints_const_iter_t i = waypoints.begin();
for(;i != waypoints.end();++ i) {
waypoint wp = *i;
// find if a waypoint is already examined in a current route - to avoid cycles
vector<long>::iterator i = find(
n.previds.begin(),n.previds.end(),wp.id);
if( (i == n.previds.end()) && (wp.id != n.waypointid) ) {
// x and y coordinates of a lag end waypoint
float xf = wp.x;
float yf = wp.y;
// lag length
float LagLen = sqrt( SQR(xf - xs) + SQR(yf - ys) );
// waypoint is added if the lag length does not exceed predefined value
if(LagLen < _MaxLagLen) {
bool crosses = false;
// iterating over all obstacles
for(obstacles_const_iter_t j = obstacles.begin(); j != obstacles.end(); ++j) {
obstacle ob = *j;
float xo = ob.x;
float yo = ob.y;
float ro = ob.r;
float a = SQR( (xf - xs)/(yf - ys) ) + 1.0;
float b = 2.0 * (xf - xs)/(yf - ys) * ( (xs - xo) - (xf - xs)/(yf - ys) * ys ) - 2.0 * yo;
float c = SQR( (xs - xo) - (xf - xs)/(yf - ys) * ys ) + SQR(yo) - SQR(ro);
float D = SQR(b) - 4.0 * a * c;
// if a line connecting two waypoints crosses an obstacle area
if(D > 0.0) {
// find whether the area is crossed between waypoints
float y1 = ( - b + sqrt(D) ) / (2.0 * a);
float y2 = ( - b - sqrt(D) ) / (2.0 * a);
float ymin = ys > yf? yf: ys;
float ymax = ys > yf? ys: yf;
if ( (y1 <= ymax) && (y1 >= ymin) || (y2 <= ymax) && (y2 >= ymin) ) {
crosses = true;
break;
}
}
}
if(!crosses) {
// adding node value to a result list
e.push_back(node_value(wp.id, n));
// calculation of heuristics
// x and y coordinates of a final route waypoint
float xg = waypoints[19].x;
float yg = waypoints[19].y;
// euclidean distance for a child node waypoint to final route waypoint
float h = sqrt( SQR(xf - xg) + SQR(yf - yg) );
// adding cost value to a result list
// one should put 0.0 instead of h as a constrcutor parameter here
// if no heuristics is used
ce.push_back(cost_value(c.g + LagLen, h));
}
}
}
}
}
