bool hit_test(PathType & path, double x, double y, double tol)
{
bool inside=false;
double x0 = 0;
double y0 = 0;
double x1 = 0;
double y1 = 0;
path.rewind(0);
unsigned command = path.vertex(&x0, &y0);
if (command == SEG_END) return false;
unsigned count = 0;
while (SEG_END != (command = path.vertex(&x1, &y1)))
{
++count;
if (command == SEG_MOVETO)
{
x0 = x1;
y0 = y1;
continue;
}
if ((((y1 <= y) && (y < y0)) ||
((y0 <= y) && (y < y1))) &&
(x < (x0 - x1) * (y - y1)/ (y0 - y1) + x1))
inside=!inside;
x0 = x1;
y0 = y1;
}
if (count == 0) // one vertex
{
return distance(x, y, x0, y0) <= std::fabs(tol);
}
return inside;
}
bool middle_point(PathType & path, double & x, double & y)
{
double x0 = 0;
double y0 = 0;
double x1 = 0;
double y1 = 0;
double mid_length = 0.5 * path_length(path);
path.rewind(0);
unsigned command = path.vertex(&x0,&y0);
if (command == SEG_END) return false;
double dist = 0.0;
while (SEG_END != (command = path.vertex(&x1, &y1)))
{
double seg_length = distance(x0, y0, x1, y1);
if ( dist + seg_length >= mid_length)
{
double r = (mid_length - dist)/seg_length;
x = x0 + (x1 - x0) * r;
y = y0 + (y1 - y0) * r;
break;
}
dist += seg_length;
x0 = x1;
y0 = y1;
}
return true;
}
bool hit_test(PathType & path, double x, double y, double tol)
{
bool inside=false;
double x0 = 0;
double y0 = 0;
double x1 = 0;
double y1 = 0;
path.rewind(0);
unsigned command = path.vertex(&x0, &y0);
if (command == SEG_END)
{
return false;
}
unsigned count = 0;
mapnik::geometry_type::types geom_type = static_cast<mapnik::geometry_type::types>(path.type());
while (SEG_END != (command = path.vertex(&x1, &y1)))
{
if (command == SEG_CLOSE)
{
continue;
}
++count;
if (command == SEG_MOVETO)
{
x0 = x1;
y0 = y1;
continue;
}
switch(geom_type)
{
case mapnik::geometry_type::types::Polygon:
{
if ((((y1 <= y) && (y < y0)) ||
((y0 <= y) && (y < y1))) &&
(x < (x0 - x1) * (y - y1)/ (y0 - y1) + x1))
inside=!inside;
break;
}
case mapnik::geometry_type::types::LineString:
{
double distance = point_to_segment_distance(x,y,x0,y0,x1,y1);
if (distance < tol)
return true;
break;
}
default:
break;
}
x0 = x1;
y0 = y1;
}
// TODO - handle multi-point?
if (count == 0) // one vertex
{
return distance(x, y, x0, y0) <= tol;
}
return inside;
}
bool centroid(PathType & path, double & x, double & y)
{
double x0 = 0.0;
double y0 = 0.0;
double x1 = 0.0;
double y1 = 0.0;
double start_x;
double start_y;
path.rewind(0);
unsigned command = path.vertex(&x0, &y0);
if (command == SEG_END) return false;
start_x = x0;
start_y = y0;
double atmp = 0.0;
double xtmp = 0.0;
double ytmp = 0.0;
unsigned count = 1;
while (SEG_END != (command = path.vertex(&x1, &y1)))
{
if (command == SEG_CLOSE) continue;
double dx0 = x0 - start_x;
double dy0 = y0 - start_y;
double dx1 = x1 - start_x;
double dy1 = y1 - start_y;
double ai = dx0 * dy1 - dx1 * dy0;
atmp += ai;
xtmp += (dx1 + dx0) * ai;
ytmp += (dy1 + dy0) * ai;
x0 = x1;
y0 = y1;
++count;
}
if (count <= 2) {
x = (start_x + x0) * 0.5;
y = (start_y + y0) * 0.5;
return true;
}
if (atmp != 0)
{
x = (xtmp/(3*atmp)) + start_x;
y = (ytmp/(3*atmp)) + start_y;
}
else
{
x = x0;
y = y0;
}
return true;
}
void set_join_caps(Stroke const& stroke_, PathType & stroke)
{
line_join_e join=stroke_.get_line_join();
switch (join)
{
case MITER_JOIN:
stroke.generator().line_join(agg::miter_join);
break;
case MITER_REVERT_JOIN:
stroke.generator().line_join(agg::miter_join);
break;
case ROUND_JOIN:
stroke.generator().line_join(agg::round_join);
break;
default:
stroke.generator().line_join(agg::bevel_join);
}
line_cap_e cap=stroke_.get_line_cap();
switch (cap)
{
case BUTT_CAP:
stroke.generator().line_cap(agg::butt_cap);
break;
case SQUARE_CAP:
stroke.generator().line_cap(agg::square_cap);
break;
default:
stroke.generator().line_cap(agg::round_cap);
}
}
//==========================
// NW N N NE
// \ | | /
// a-----------b
// / | | \
// SW S S SE
//==========================
int Stick::extrudeLines(const PathType& path, float width)
{
if (path.size() < 2)
{
return EXTRUDE_FAIL;
}
int pointSize = path.size();
int lineSize = pointSize - 1;
_indexList.resize( lineSize*IDEX_FACTOR );
for (int idx=0; idx < lineSize; idx++)
{
const VecType& a = path[idx];
const VecType& b = path[idx+1];
VecType e = (b-a);
e.normalize();
e *= width;
VecType N = VecType(-e.y(), e.x(), 0);
VecType S = -N;
VecType NE = N + e;
VecType NW = N - e;
VecType SW = -NE;
VecType SE = -NW;
_vertexList.push_back( Vertex(a + SW) );
_vertexList.push_back( Vertex(a + NW) );
_vertexList.push_back( Vertex(a + S) );
_vertexList.push_back( Vertex(a + N) );
_vertexList.push_back( Vertex(b + S) );
_vertexList.push_back( Vertex(b + N) );
_vertexList.push_back( Vertex(b + SE) );
_vertexList.push_back( Vertex(b + NE) );
}
_generateTriangleTexCoord();
_generateTriangesIndices();
return EXTRUDE_SUCCESS;
}
void set_join_caps(Symbolizer const& sym, PathType & stroke, Feature const& feature, attributes const& vars)
{
line_join_enum join = get<line_join_enum>(sym, keys::stroke_linejoin, feature, vars, MITER_JOIN);
switch (join)
{
case MITER_JOIN:
stroke.generator().line_join(agg::miter_join);
break;
case MITER_REVERT_JOIN:
stroke.generator().line_join(agg::miter_join);
break;
case ROUND_JOIN:
stroke.generator().line_join(agg::round_join);
break;
default:
stroke.generator().line_join(agg::bevel_join);
}
line_cap_enum cap = get<line_cap_enum>(sym, keys::stroke_linecap, feature, vars, BUTT_CAP);
switch (cap)
{
case BUTT_CAP:
stroke.generator().line_cap(agg::butt_cap);
break;
case SQUARE_CAP:
stroke.generator().line_cap(agg::square_cap);
break;
default:
stroke.generator().line_cap(agg::round_cap);
}
}
double path_length(PathType & path)
{
double x0 = 0;
double y0 = 0;
double x1 = 0;
double y1 = 0;
path.rewind(0);
unsigned command = path.vertex(&x0,&y0);
if (command == SEG_END) return 0;
double length = 0;
while (SEG_END != (command = path.vertex(&x1, &y1)))
{
length += distance(x0,y0,x1,y1);
x0 = x1;
y0 = y1;
}
return length;
}
bool hit_test_first(PathType & path, double x, double y)
{
bool inside=false;
double x0 = 0;
double y0 = 0;
double x1 = 0;
double y1 = 0;
path.rewind(0);
unsigned command = path.vertex(&x0, &y0);
if (command == SEG_END)
{
return false;
}
unsigned count = 0;
while (SEG_END != (command = path.vertex(&x1, &y1)))
{
if (command == SEG_CLOSE)
{
break;
}
++count;
if (command == SEG_MOVETO)
{
x0 = x1;
y0 = y1;
continue;
}
if ((((y1 <= y) && (y < y0)) ||
((y0 <= y) && (y < y1))) &&
(x < (x0 - x1) * (y - y1)/ (y0 - y1) + x1))
inside=!inside;
x0 = x1;
y0 = y1;
}
return inside;
}
bool interior_position(PathType & path, double & x, double & y)
{
// start with the centroid
if (!label::centroid(path, x,y))
return false;
// if we are not a polygon, or the default is within the polygon we are done
if (hit_test(path,x,y,0.001))
return true;
// otherwise we find a horizontal line across the polygon and then return the
// center of the widest intersection between the polygon and the line.
std::vector<double> intersections; // only need to store the X as we know the y
double x0 = 0;
double y0 = 0;
path.rewind(0);
unsigned command = path.vertex(&x0, &y0);
double x1 = 0;
double y1 = 0;
while (SEG_END != (command = path.vertex(&x1, &y1)))
{
if (command != SEG_MOVETO)
{
// if the segments overlap
if (y0==y1)
{
if (y0==y)
{
double xi = (x0+x1)/2.0;
intersections.push_back(xi);
}
}
// if the path segment crosses the bisector
else if ((y0 <= y && y1 >= y) ||
(y0 >= y && y1 <= y))
{
// then calculate the intersection
double xi = x0;
if (x0 != x1)
{
double m = (y1-y0)/(x1-x0);
double c = y0 - m*x0;
xi = (y-c)/m;
}
intersections.push_back(xi);
}
}
x0 = x1;
y0 = y1;
}
// no intersections we just return the default
if (intersections.empty())
return true;
x0=intersections[0];
double max_width = 0;
for (unsigned ii = 1; ii < intersections.size(); ++ii)
{
double xi=intersections[ii];
double xc=(x0+xi)/2.0;
double width = std::fabs(xi-x0);
if (width > max_width && hit_test(path,xc,y,0))
{
x=xc;
max_width = width;
break;
}
}
return true;
}
/**
* @brief Search the graph for a minimun path between originVertex and targetVertex. Returns the path
*
* @param originVertex ...
* @param targetVertex ...
* @param vertexPath std::vector of Vertex with the path
* @return bool
*/
bool PlannerPRM::searchGraph(const Vertex &originVertex, const Vertex &targetVertex, std::vector<Vertex> &vertexPath)
{
qDebug() << __FUNCTION__ << "Searching the graph between " << graph[originVertex].pose << "and " << graph[targetVertex].pose;
// Create things for Dijkstra
std::vector<Vertex> predecessors(boost::num_vertices(graph)); // To store parents
std::vector<float> distances(boost::num_vertices(graph)); // To store distances
//Create a vertex_index property map, since VertexList is listS
typedef std::map<Vertex, size_t>IndexMap;
IndexMap indexMap;
boost::associative_property_map<IndexMap> propmapIndex(indexMap);
//indexing the vertices
int i=0;
BGL_FORALL_VERTICES(v, graph, Graph)
boost::put(propmapIndex, v, i++);
auto predecessorMap = boost::make_iterator_property_map(&predecessors[0], propmapIndex);
auto distanceMap = boost::make_iterator_property_map(&distances[0], propmapIndex);
boost::dijkstra_shortest_paths(graph, originVertex, boost::weight_map(boost::get(&EdgePayload::dist, graph))
.vertex_index_map(propmapIndex)
.predecessor_map(predecessorMap)
.distance_map(distanceMap));
//////////////////////////
// Extract a shortest path
//////////////////////////
PathType path;
Vertex v = targetVertex;
//////////////////////////
// Start by setting 'u' to the destintaion node's predecessor |||// Keep tracking the path until we get to the source
/////////////////////////
for( Vertex u = predecessorMap[v]; u != v; v = u, u = predecessorMap[v]) // Set the current vertex to the current predecessor, and the predecessor to one level up
{
std::pair<Graph::edge_descriptor, bool> edgePair = boost::edge(u, v, graph);
Graph::edge_descriptor edge = edgePair.first;
path.push_back( edge );
}
qDebug() << __FUNCTION__ << " Path found with length: " << path.size() << "steps and length " << distanceMap[targetVertex];
;
Vertex lastVertex;
if(path.size() > 0)
{
vertexPath.clear();
for(PathType::reverse_iterator pathIterator = path.rbegin(); pathIterator != path.rend(); ++pathIterator)
{
vertexPath.push_back(boost::source(*pathIterator, graph));
lastVertex = boost::target(*pathIterator, graph);
}
vertexPath.push_back(lastVertex);
return true;
}
else
{
qDebug() << "Path no found between nodes";
return false;
}
}
bool interior_position(PathType & path, double & x, double & y)
{
// start with the centroid
if (!label::centroid(path, x,y))
return false;
// otherwise we find a horizontal line across the polygon and then return the
// center of the widest intersection between the polygon and the line.
std::vector<double> intersections; // only need to store the X as we know the y
double x0 = 0;
double y0 = 0;
path.rewind(0);
unsigned command = path.vertex(&x0, &y0);
double x1 = 0;
double y1 = 0;
while (SEG_END != (command = path.vertex(&x1, &y1)))
{
if (command == SEG_CLOSE)
continue;
if (command != SEG_MOVETO)
{
// if the segments overlap
if (y0==y1)
{
if (y0==y)
{
double xi = (x0+x1)/2.0;
intersections.push_back(xi);
}
}
// if the path segment crosses the bisector
else if ((y0 <= y && y1 >= y) ||
(y0 >= y && y1 <= y))
{
// then calculate the intersection
double xi = x0;
if (x0 != x1)
{
double m = (y1-y0)/(x1-x0);
double c = y0 - m*x0;
xi = (y-c)/m;
}
intersections.push_back(xi);
}
}
x0 = x1;
y0 = y1;
}
// no intersections we just return the default
if (intersections.empty())
return true;
std::sort(intersections.begin(), intersections.end());
double max_width = 0;
for (unsigned ii = 1; ii < intersections.size(); ii += 2)
{
double xlow = intersections[ii-1];
double xhigh = intersections[ii];
double width = xhigh - xlow;
if (width > max_width)
{
x = (xlow + xhigh) / 2.0;
max_width = width;
}
}
return true;
}
PathType getPath(const GraphAdapterType& graphAdapter, const NodeAdapterType& start, const std::function<bool(const NodeAdapterType&)>& endCondition) const {
PathType resultPath;
struct NodePositionComparator {
bool operator() (const NodeAdapterType& lhs, const NodeAdapterType& rhs) {
return lhs.position < rhs.position;
}
};
std::set<NodeAdapterType> open = { start };
std::set<NodeAdapterType, NodePositionComparator> closed;
std::map<NodeAdapterType, NodeAdapterType, NodePositionComparator> came_from;
typename std::set<NodeAdapterType>::iterator current;
while(open.empty() == false) {
current = open.begin();
if(endCondition(*current) == true) {
// Goal found, recreating path from 'goal' node to 'start' node
resultPath.push_front(current->position);
auto pathCurrent = came_from.find(*current);
if(pathCurrent != came_from.end()) {
while(pathCurrent->second.position != start.position) {
resultPath.push_front(pathCurrent->second.position);
pathCurrent = came_from.find(pathCurrent->second);
}
}
return resultPath;
}
closed.insert(*current);
std::vector<NodeAdapterType> neighbours = graphAdapter.getNeighboursOf(*current);
for(NodeAdapterType& neighbour : neighbours) {
if(graphAdapter.isAvailable(neighbour.position)) {
typename std::set<NodeAdapterType>::iterator cIter, oIter;
cIter = closed.find(neighbour);
if(cIter != closed.end()) {
continue;
}
neighbour.g(current->g() + 1);
neighbour.h(graphAdapter.getHeuristicCostLeft(neighbour, neighbour));
oIter = open.find(neighbour);
if(oIter == open.end() || neighbour.g() < oIter->g()) {
if(oIter != open.end()) {
open.erase(oIter);
}
auto cameFromIter = came_from.find(neighbour);
if(cameFromIter != came_from.end()) {
if(cameFromIter->second.g() > neighbour.g()) {
came_from.erase(cameFromIter);
}
}
came_from.emplace(std::pair<NodeAdapterType, NodeAdapterType>(neighbour, *current));
open.insert(neighbour);
}
}
}
open.erase(current);
}
// If algorithm comes here, no path was found, and return value of this method will be empty vector
return resultPath;
}
