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));
}
}
}
}
}
