//==========================
// 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;
}
/**
* @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;
}
}
