Пример #1
0
//==========================
//	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;	
}
Пример #2
0
/**
 * @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;
	}
}