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