//Navigation Mesh Pathfinding with A* in C++ //Project: Navmesh + Dx11 Graphics Techdemo //http://robert-lindner.com/img/projects/navmesh/ai_paper.pdf //http://robert-lindner.com/blog/navmesh/ //Author: Robert Lindenr //This sample contains code for building the navmesh neightbor structure from a mesh, A* and finding the current element in the mesh using point in triangle tests //******************** //A star algotithm for any time of navigation structure, be it grids or meshes //******************** const vector& PathFinder::AStar(NavElement* startElement, NavElement* endElement) { //Clear containers ClearContainers(); if (startElement == endElement) { m_Path.push_back(startElement); return m_Path; } //Temp containers deque openList; deque closedList; //Add StartElement to OpenList NavElement* pCurentElement = nullptr; openList.push_back(startElement); //While our open list is not empty while (openList.size() != 0) { //1. Get the node with the lowest F from the openList float lowestFScore = std::numeric_limits::max(); for (auto el : openList) { if (el->m_FScore < lowestFScore) { pCurentElement = el; lowestFScore = el->m_FScore; } } //2. Pop current off the open list and push it to the closed openList.erase(std::remove_if(openList.begin(), openList.end(), [pCurentElement](const NavElement* e) {return e == pCurentElement; })); closedList.push_back(pCurentElement); //3. Retrieve the chosen elements' adjacent elements auto vAdj = pCurentElement->GetWalkableNeighbors(); //If any of the successors is the goal, call it a day (set it's parent, break the loop by clearing open and breaking if (std::find(vAdj.begin(), vAdj.end(), endElement) != vAdj.end()) { endElement->m_pParent = pCurentElement; openList.clear(); break; } //Else go over all the elements for (auto pAdj : vAdj) { //If node is in closed, ignore it if (std::find(closedList.begin(), closedList.end(), pAdj) != closedList.end()) { } else { //if node not in open list, compute score and add it if (std::find(openList.begin(), openList.end(), pAdj) == openList.end()) { //3.1 Set the parent to the current pAdj->m_pParent = pCurentElement; //Calculate the g, h and f //g = current.g + cost(dist current and this) ---> OR ues GetTileCost() if using hard value (needs extra support for diagonal movement) XMFLOAT2 tempV(abs(pCurentElement->GetCenter().x - pAdj->GetCenter().x), abs(pCurentElement->GetCenter().y - pAdj->GetCenter().y)); float cost = Heuristic(tempV.x, tempV.y); pAdj->m_GScore = cost + pCurentElement->m_GScore; //h = dist this and goal tempV = XMFLOAT2(abs(pAdj->GetCenter().x - endElement->GetCenter().x), abs(pAdj->GetCenter().y - endElement->GetCenter().y)); pAdj->m_HScore = Heuristic(tempV.x, tempV.y); //f = g + h pAdj->m_FScore = pAdj->m_GScore + pAdj->m_HScore; //Add it openList.push_back(pAdj); //Store for showcasing nodes m_pCheckedElements.push_back(pAdj); } } } } //Reconstruct path m_Path.push_back(endElement); m_Path.push_back(endElement->m_pParent); NavElement* pNextElement = pCurentElement->m_pParent; while (pNextElement != nullptr) { if (std::find(m_Path.begin(), m_Path.end(), pNextElement) == m_Path.end()) { m_Path.push_back(pNextElement); pNextElement = pNextElement->m_pParent; } else break; } return m_Path; } void NavMesh::Initialize(const GameContext& gameContext) { //[...] auto mesh = ContentManager::Load(m_AssetFile); for (size_t i = 0; i < mesh->m_Indices.size(); i += 3) { DWORD indices[3] = { mesh->m_Indices[i + 0], mesh->m_Indices[i + 1], mesh->m_Indices[i + 2] }; XMFLOAT3 positions[3] = { mesh->m_Positions[indices[0]], mesh->m_Positions[indices[1]], mesh->m_Positions[indices[2]] }; auto e = new NavMeshElement(positions, indices, m_MatID, m_pMat); AddChild(e); m_Elements.push_back(e); } //Build neightbor structure for (auto el : m_Elements) { for (auto n : m_Elements)if (!(el == n))el->PotentialNeighbor(n); } Logger::LogInfo(L"fknbreak"); } vector > NavMeshElement::GetEdges() { vector > ret; ret.push_back(pair(m_Indices[0], m_Indices[1])); ret.push_back(pair(m_Indices[1], m_Indices[2])); ret.push_back(pair(m_Indices[0], m_Indices[2])); return ret; } vector > NavMeshElement::GetEdgePositions() { vector > ret; ret.push_back(pair(m_Vertices[0], m_Vertices[1])); ret.push_back(pair(m_Vertices[1], m_Vertices[2])); ret.push_back(pair(m_Vertices[0], m_Vertices[2])); return ret; } void NavMeshElement::PotentialNeighbor(NavMeshElement* el) { { auto edges = GetEdges(); auto nEdges = el->GetEdges(); for (auto a : edges) { for (auto b : nEdges) { if ((a.first == b.first && a.second == b.second) || (a.first == b.second && a.second == b.first)) { m_Neighbors.push_back(el); return; } } } } //Also check with positions in case of split edges, //might fail though due to floating point accuracy { auto edges = GetEdgePositions(); auto nEdges = el->GetEdgePositions(); for (auto a : edges) { for (auto b : nEdges) { if ((XMFloat3Equals(a.first, b.first) && XMFloat3Equals(a.second, b.second)) || (XMFloat3Equals(a.first, b.second) && XMFloat3Equals(a.second, b.first))) { m_Neighbors.push_back(el); return; } } } } } //************************* //Determines the element an object is likeley to be standing on to calculate the path from //************************* NavElement* NavMesh::GetElement(XMFLOAT3 pos) { //sort elements by proximity first //Should be more efficient if the search position is inside a triangle because the // check would only be performed a few times, possibly cutting time in half //if search position has no matching triangle its worse because it was uselessly sorted // , but it helps returning the closest triangle in that case for (auto e : m_Elements) { e->StoreProximity(pos); } sort(m_Elements.begin(), m_Elements.end(), [](const NavMeshElement* a, const NavMeshElement* b) -> bool { return a->m_Proximity < b->m_Proximity; }); //Using barycentric coordinate test //Its the same method used for ray triangle intersection tests in my pathtracer lightwhat XMVECTOR P = XMLoadFloat3(&pos); for (auto e : m_Elements) { //Get triangle positions XMVECTOR A = XMLoadFloat3(&e->m_Vertices[0]); XMVECTOR B = XMLoadFloat3(&e->m_Vertices[1]); XMVECTOR C = XMLoadFloat3(&e->m_Vertices[2]); //Some data could be precalculated at the cost of memory bloating (marked with //p) //thats a total of 13 floats //Calculate triangle vectors XMVECTOR v0 = C - A;//p XMVECTOR v1 = B - A;//p XMVECTOR v2 = P - A; //Calculate dot products XMVECTOR dotVec; float dot00, dot01, dot02, dot11, dot12; dotVec = XMVector3Dot(v0, v0); XMStoreFloat(&dot00, dotVec);//p dotVec = XMVector3Dot(v0, v1); XMStoreFloat(&dot01, dotVec);//p dotVec = XMVector3Dot(v0, v2); XMStoreFloat(&dot02, dotVec); dotVec = XMVector3Dot(v1, v1); XMStoreFloat(&dot11, dotVec);//p dotVec = XMVector3Dot(v1, v2); XMStoreFloat(&dot12, dotVec); //Calculate barycentric coordinates float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);//p float u = (dot11*dot02 - dot01*dot12)*invDenom; if (u >= 0) { float v = (dot00*dot12 - dot01*dot02)*invDenom; if ((v >= 0) && (u + v < 1)) { //Point is in triangle return e; } } } //If there is no triangle intersection (efficiency worstcase) //return closest triangle #if defined(DEBUG) | defined(_DEBUG) Logger::LogInfo(L"NavMesh>GetElement no matching triangle found, returning closest"); #endif return m_Elements[0]; } void NavMeshElement::StoreProximity(XMFLOAT3 pos) { auto p = XMLoadFloat3(&pos); auto c = XMLoadFloat3(&GetCenter3()); auto lengthV = XMVector3Length(p - c); XMStoreFloat(&m_Proximity, lengthV); }