Esempio n. 1
0
void Pathfinder::BuildPath(PathNode * lastNode) {
	m_finalPath.clear();
	m_edgesPath.clear();
	//iterate over all node parents to get the full path
	PathNode * node = lastNode;
	USVec2D edgeDir;
	USVec2D edgePos;
	float edgeLength;
	while (node->GetParent()) {
		m_finalPath.push_back(node);
		for (std::vector<Polygon::Edge>::iterator itr = node->GetPolygon()->m_edges.begin();
		itr != node->GetPolygon()->m_edges.end(); ++itr) {
			if (itr->m_neighbour == node->GetParent()->GetPolygon()) {
				edgeDir = node->GetPolygon()->m_vertices[itr->m_verticesID[1]]
					- node->GetPolygon()->m_vertices[itr->m_verticesID[0]];
				edgeLength = edgeDir.Length();
				edgePos = node->GetPolygon()->m_vertices[itr->m_verticesID[0]]
					+ (edgeDir.NormVector() * (edgeLength / 2));
				m_edgesPath.push_back(edgePos);
			}
		}
		node = node->GetParent();
	}
	node = node;
}
Esempio n. 2
0
void Pathfinder::UpdatePath() {
	//need to set all parents to null before recalculating the path.
	//if not, there's an infinite loop in BuildPath()
	for (std::vector<PathNode>::iterator itr = m_nodes.begin(); itr != m_nodes.end(); ++itr) {
		itr->SetParent(nullptr);
	}

	//set StartNode and EndNode
	m_startNode = NearestNode(m_StartPosition);
	m_endNode = NearestNode(m_EndPosition);

	//recalculate all estimated costs
	int16_t cost = 0;
	PathNode * node;
	for (std::vector<PathNode>::iterator itr = m_nodes.begin(); itr != m_nodes.end(); ++itr) {
		node = &(*itr);
		if (node->GetCost() != -1) {
			//estimated based on pixels distance
			float cost = node->GetCost();
			float estimated = (sqrt(pow((m_endNode->GetPos().mX - node->GetPos().mX), 2)
				+ pow(m_endNode->GetPos().mY - node->GetPos().mY, 2)));
			node->SetEstimatedCost(estimated);
			node->SetCost(cost + estimated);
			node->SetTotalCost(cost + estimated);
		}
	}

	//A*
	m_openNodes.clear();
	m_closedNodes.clear();
	m_startNode->SetCost(0);
	m_startNode->SetTotalCost(0);
	m_openNodes.push_back(m_startNode);
	bool objectiveFound = false;
	while (!m_openNodes.empty() && !objectiveFound) {
		PathNode * node = *(m_openNodes.begin());
		m_openNodes.erase(m_openNodes.begin());
		if (node->GetPos().mX == m_endNode->GetPos().mX
			&& node->GetPos().mY == m_endNode->GetPos().mY) {
			BuildPath(node);
		} else {
			for (std::vector<Polygon::Edge>::iterator itr = node->GetPolygon()->m_edges.begin();
			itr != node->GetPolygon()->m_edges.end(); ++itr) {
				printf("EDGE: %d - %d\n", node->GetPolygon()->m_id, itr->m_neighbour->m_id);
				PathNode * nextNode = nullptr;
				for (std::vector<PathNode>::iterator pathNodeItr = m_nodes.begin();
				pathNodeItr != m_nodes.end(); ++pathNodeItr) {
					if (pathNodeItr->GetPolygon()->m_id == itr->m_neighbour->m_id) {
						nextNode = &(*pathNodeItr);
					}
				}
				if (nextNode == m_endNode) {
					nextNode->SetParent(node);
					nextNode->SetTotalCost(node->GetTotalCost() + nextNode->GetCost());
					BuildPath(nextNode);
					objectiveFound = true;
					break;
				}
				if (nextNode->GetCost() != -1) {
					if (node == nextNode) {
						continue;
					} else if (find(m_closedNodes.begin(), m_closedNodes.end(), nextNode)
						!= m_closedNodes.end()) {
						continue;
					} else if (find(m_openNodes.begin(), m_openNodes.end(), nextNode)
						!= m_openNodes.end()) {
						if (nextNode->GetTotalCost() > 0
							&& nextNode->GetTotalCost() >
							node->GetTotalCost() + nextNode->GetCost()) {
							nextNode->SetTotalCost(node->GetTotalCost() + nextNode->GetCost());
							nextNode->SetParent(node);
						}
					} else {
						nextNode->SetParent(node);
						m_openNodes.push_back(nextNode);
					}
				}
			}
			std::vector<PathNode *>::iterator el = find(m_openNodes.begin(),
				m_openNodes.end(), node);
			if (el != m_openNodes.end()) {
				m_openNodes.erase(el);
			}
			m_closedNodes.push_back(node);
		}
	}
}