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