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