bool Tools::CheckInBound(Node checkedNode) { if (checkedNode.GetX() < 0 || checkedNode.GetY() < 0 || checkedNode.GetX() > 63 || checkedNode.GetY() > 39) { return false; } return true; }
int Tools::DistanceManhattan(Node start, Node origin) { int distance = 0; distance = abs(start.GetX() - origin.GetX())+ abs(start.GetY() - origin.GetY()); //std::cout << "distance mana" << distance << "\n"; return distance; }
bool Tools::Passable(Node testedNode, bool **pathingMap) { // std::cout << pathingMap[testedNode.GetX()][testedNode.GetY()]; if(pathingMap[testedNode.GetX()][testedNode.GetY()]) { //std::cout << testedNode.GetX() << "oba" << testedNode.GetY() << "\n"; } return pathingMap[testedNode.GetX()][testedNode.GetY()]; }
void Map::AddToWListM(Node n, Node p) { if (CheckDuplicates(n.GetX(), n.GetY())) { n.SetParent(p); nodes[n.GetX()][n.GetY()].SetParent(p); nodes[n.GetX()][n.GetY()].SetGDistance(p.GetGDistance() + 10); nodes[n.GetX()][n.GetY()].SetHDistanceManhatten(eNode.GetX(), eNode.GetY()); workinglist.push(nodes[n.GetX()][n.GetY()]); duplicates[n.GetX()][n.GetY()] = true; } }
void Map::ReturnPath() { Node temp; while(!path.empty()) { temp = path.top(); pathnodes[temp.GetX()][temp.GetY()] = true; path.pop(); std::cout << "X: " << temp.GetX() << "\tY: " << temp.GetY() << std::endl; } }
bool Tools::VectorContainsNode(Node addedNode, std::vector<Node> nodeVector) { for(std::vector<Node>::iterator it = nodeVector.begin(); it != nodeVector.end(); ++it) { Node currentNode = *it; if ( addedNode.GetX() == currentNode.GetX() && addedNode.GetY() == currentNode.GetY()) { //std::cout << addedNode.GetX() << " : " << addedNode.GetY() << " contains " << currentNode.GetX() << " : " << currentNode.GetY() <<"\n"; return true; } } return false; }
double AStar::GetCost(Node *node, Connector *connector) { double dx, dy; Node *child = connector->GetChild(); dx = fabs(node->GetX() - child->GetX()); dy = fabs(node->GetY() - child->GetY()); return sqrt(dx * dx + dy * dy) + connector->GetCost(); }
ribi::cmap::Edge ribi::cmap::EdgeFactory::Create( const Node& from, const Node& to ) const noexcept { //Nodes may be similar, but not the same assert(&from != &to); const double x{(from.GetX() + to.GetX()) / 2.0}; const double y{(from.GetY() + to.GetY()) / 2.0}; const auto concept = ConceptFactory().Create(); const Node node(concept,x,y); Edge p( node ); return p; }
void Tools::UpdateNodeScoreInVector(Node *currentNode,Node updatedNode,std::vector<Node>& nodeVector, int gAdd, std::map<Node,Node>& parents) { for(std::vector<Node>::iterator it = nodeVector.begin(); it != nodeVector.end(); ++it) { Node itNode = *it; if ( updatedNode.GetX() == itNode.GetX() && updatedNode.GetY() == itNode.GetY()) { int tempScore = currentNode->GetG() + gAdd + itNode.GetH(); //std::cout << "UPDAAAAATTTEEEEEE: " << itNode.GetScore()<< " into :" << tempScore << "\n"; if(tempScore < itNode.GetScore()) { itNode.SetG(currentNode->GetG() + gAdd); parents[itNode] = *currentNode; itNode.SetParent(currentNode); } } } }
void Map::AddToWListD(Node n, Node p) { if (CheckDuplicates(n.GetX(), n.GetY())) { n.SetParent(p); nodes[n.GetX()][n.GetY()].SetParent(p); if (n.GetX() != p.GetX() && n.GetY() != p.GetY()) { nodes[n.GetX()][n.GetY()].SetGDistance(p.GetGDistance() + 14); } else { nodes[n.GetX()][n.GetY()].SetGDistance(p.GetGDistance() + 10); } nodes[n.GetX()][n.GetY()].SetHDistanceDiagonal(eNode.GetX(), eNode.GetY()); workinglist.push(nodes[n.GetX()][n.GetY()]); duplicates[n.GetX()][n.GetY()] = true; } }
void Semester::showLine(wxDC& dc, Node *snode, Node *par){ Node *temp = snode, *temp2 = par; std::vector<int> pos; bool dragtemp; int y; while(temp != NULL){ temp2 = temp; if(dragging && !drageffect){ pos = checkDragEffect(temp); if(pos.size() != 0){ dc.SetBrush(wxBrush(wxColour("#008888"), wxBRUSHSTYLE_TRANSPARENT)); dc.SetPen(wxPen(wxColour("#000000"), 2)); dc.DrawRectangle(pos[0], pos[1], 240, 60); } } switch(temp->GetNodeType()){ case NODE_NORMAL: y = (drageffect)?temp->GetY()+70:temp->GetY(); dc.DrawBitmap(temp->GetCourse()->bitmap, temp->GetX(), y); temp = temp->GetChild(); break; case NODE_SPLIT: dragtemp = drageffect; showLine(dc, temp->GetChild(1), temp); drageffect = dragtemp; temp = temp->GetChild(); break; case NODE_CHOICE: default: break; } } if(dragging && !drageffect){ if(temp2 != NULL && temp2 == par){ pos = checkUnder(temp2->GetX()+125, temp2->GetY() - 70); } else if(temp2 != NULL && temp2->GetNodeType() == NODE_SPLIT){ pos = checkUnder(temp2->GetX()-125, temp2->GetY() - 70); } else pos = checkUnder(temp2); if(pos.size() != 0){ dc.SetBrush(wxBrush(wxColour("#008888"), wxBRUSHSTYLE_TRANSPARENT)); dc.SetPen(wxPen(wxColour("#000000"), 2)); dc.DrawRectangle(pos[0], pos[1] + 70, 240, 60); } } else drageffect = false; }//showLine
int Map::FindPath() { unsigned long t; t = clock(); bool foundEnd = false; Node temp; //temporary value for calculation if (!workinglist.empty()) { // Ensure the workinglist is not empty. workinglist.top().SetHDistanceManhatten(eNode.GetX(), eNode.GetY()); // If it is empty then do not attempt to find } else { // path and return -3; } while(!workinglist.empty() && !foundEnd) //Move through working set checkin each node to see if { //it is the end node, if not then add it's neighbours to temp = workinglist.top(); //the working set. workinglist.pop(); if (temp == eNode) { foundEnd = true; } else { if (temp.GetX() + 1 < mWidth) AddToWListM(nodes[temp.GetX() + 1][temp.GetY()], temp); if (temp.GetX() > 0) AddToWListM(nodes[temp.GetX() - 1][temp.GetY()], temp); if (temp.GetY() + 1 < mHeight) AddToWListM(nodes[temp.GetX()][temp.GetY() + 1], temp); if (temp.GetY() > 0) AddToWListM(nodes[temp.GetX()][temp.GetY() - 1], temp); } } if (foundEnd) //Once a route is found track back through parent references { //and add to stack for retrieval later, return 0 to indicate success. while(temp != sNode) { path.push(temp); temp = nodes[temp.ParentX()][temp.ParentY()]; } seconds = ((double)(clock() - t) * 1000) / CLOCKS_PER_SEC; return 0; } else { //If no route is found return -2. seconds = ((double)(clock() - t) * 1000) / CLOCKS_PER_SEC; return -2; } }
bool Tools::Astar(Node start, Node destination, bool **pathingMap, std::vector<Node>& path) { //initialisation des structure necessaire bool solution = false; std::vector<Node> openNodes; std::vector<Node> closedNodes; std::map<Node,Node> parents; //initialisation du cout du premier node a 0 start.SetG(0); start.SetH(0); //ajout au vecteur ouvert openNodes.push_back(start); Node current; int currentIndex; while (solution == false) { //Si le vecteur ouvert est vide il n'y a pas de solution if(openNodes.empty()) { return false; } //index pour faciliter le retrer de current au vecteur ouvert int index = 0; //choisi le meilleur noeud dans le vecteur ouvert current = BestNodeInVector(openNodes, index); //ajout dans le vecteur fermer closedNodes.push_back(current); currentIndex = closedNodes.size() - 1; //on suprime le node choisi du vecteur ouvert openNodes.erase(openNodes.begin()+index); //verification si on est a destination alors on quitte la boucle if (current.GetX() == destination.GetX() && current.GetY() == destination.GetY()) { solution = true; break; } //Verification de la case en bas a gauche if(CheckInBound(current.GetBottomLeftNode()) && Passable(current.GetBottomLeftNode(),pathingMap) && !VectorContainsNode(current.GetBottomLeftNode(),closedNodes)) { if(!VectorContainsNode(current.GetBottomLeftNode(),openNodes)) { Node nodeToAdd = current.GetBottomLeftNode(); nodeToAdd.SetG(current.GetG()+14); nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10); nodeToAdd.SetParent(&closedNodes[currentIndex]); parents[nodeToAdd] = closedNodes[currentIndex]; openNodes.push_back(nodeToAdd); } else { UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetBottomLeftNode(),openNodes, 14,parents); } } //Verification de la case en bas if(CheckInBound(current.GetBottomNode()) && Passable(current.GetBottomNode(),pathingMap) && !VectorContainsNode(current.GetBottomNode(),closedNodes)) { if(!VectorContainsNode(current.GetBottomNode(),openNodes)) { Node nodeToAdd = current.GetBottomNode(); nodeToAdd.SetG(current.GetG()+10); nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10); nodeToAdd.SetParent(&closedNodes[currentIndex]); parents[nodeToAdd] = closedNodes[currentIndex]; openNodes.push_back(nodeToAdd); } else { UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetBottomNode(),openNodes, 10,parents); } } //Verification de la case a gauche if(CheckInBound(current.GetLeftNode()) && Passable(current.GetLeftNode(),pathingMap) && !VectorContainsNode(current.GetLeftNode(),closedNodes)) { if(!VectorContainsNode(current.GetLeftNode(),openNodes)) { Node nodeToAdd = current.GetLeftNode(); nodeToAdd.SetG(current.GetG()+10); nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10); nodeToAdd.SetParent(&closedNodes[currentIndex]); parents[nodeToAdd] = closedNodes[currentIndex]; openNodes.push_back(nodeToAdd); } else { UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetLeftNode(),openNodes, 10,parents); } } // Verification de la case en haut a gauche if(CheckInBound(current.GetTopLeftNode()) && Passable(current.GetTopLeftNode(),pathingMap) && !VectorContainsNode(current.GetTopLeftNode(),closedNodes)) { if(!VectorContainsNode(current.GetTopLeftNode(),openNodes)) { Node nodeToAdd = current.GetTopLeftNode(); nodeToAdd.SetG(current.GetG()+14); nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10); nodeToAdd.SetParent(&closedNodes[currentIndex]); parents[nodeToAdd] = closedNodes[currentIndex]; openNodes.push_back(nodeToAdd); } else { UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetTopLeftNode(),openNodes, 14,parents); } } //Verification de la case en haut if(CheckInBound(current.GetTopNode()) && Passable(current.GetTopNode(),pathingMap) && !VectorContainsNode(current.GetTopNode(),closedNodes)) { if(!VectorContainsNode(current.GetTopNode(),openNodes)) { Node nodeToAdd = current.GetTopNode(); nodeToAdd.SetG(current.GetG()+10); nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10); nodeToAdd.SetParent(&closedNodes[currentIndex]); parents[nodeToAdd] = closedNodes[currentIndex]; openNodes.push_back(nodeToAdd); } else { UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetTopNode(),openNodes, 10,parents); } } // Verification de la case en haut a droite if(CheckInBound(current.GetTopRightNode()) && Passable(current.GetTopRightNode(),pathingMap) && !VectorContainsNode(current.GetTopRightNode(),closedNodes)) { if(!VectorContainsNode(current.GetTopRightNode(),openNodes)) { Node nodeToAdd = current.GetTopRightNode(); nodeToAdd.SetG(current.GetG()+14); nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10); nodeToAdd.SetParent(&closedNodes[currentIndex]); parents[nodeToAdd] = closedNodes[currentIndex]; openNodes.push_back(nodeToAdd); } else { UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetTopRightNode(),openNodes, 14,parents); } } //Verification de la case a droite if(CheckInBound(current.GetRightNode()) && Passable(current.GetRightNode(),pathingMap) && !VectorContainsNode(current.GetRightNode(),closedNodes)) { if(!VectorContainsNode(current.GetRightNode(),openNodes)) { Node nodeToAdd = current.GetRightNode(); nodeToAdd.SetG(current.GetG()+10); nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10); nodeToAdd.SetParent(&closedNodes[currentIndex]); parents[nodeToAdd] = closedNodes[currentIndex]; openNodes.push_back(nodeToAdd); } else { UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetRightNode(),openNodes, 10,parents); } } // Verification de la case en bas a droite if(CheckInBound(current.GetBottomRightNode()) && Passable(current.GetBottomRightNode(),pathingMap) && !VectorContainsNode(current.GetBottomRightNode(),closedNodes)) { if(!VectorContainsNode(current.GetBottomRightNode(),openNodes)) { Node nodeToAdd = current.GetBottomRightNode(); nodeToAdd.SetG(current.GetG()+14); nodeToAdd.SetH(DistanceManhattan(nodeToAdd,destination)*10); nodeToAdd.SetParent(&closedNodes[currentIndex]); parents[nodeToAdd] = closedNodes[currentIndex]; openNodes.push_back(nodeToAdd); } else { UpdateNodeScoreInVector(&closedNodes[currentIndex],current.GetBottomRightNode(),openNodes, 14,parents); } } } //Si on trouve une solution alors on Backtrack jusqu'au noeux d'origine if(solution == true) { // Implementation avec les pointeur parent directement dans les Node // Node *currentBactrack = &closedNodes[currentIndex]; // path.push_back(*currentBactrack); // while (currentBactrack->HasParent()){ // std::cout << "solution x : " << currentBactrack->GetX()<<" solution y : " << currentBactrack->GetY() << "\n"; // std::cout << "trackback : "<< path.size() << "\n"; // currentBactrack = currentBactrack->GetParent(); // std::cout << "test"; // path.push_back(*currentBactrack); // std::cout << " solution x : " << currentBactrack->GetX()<<" solution y : " << currentBactrack->GetY() << "\n"; // // } //Implementation en utilisant une map entre les Nodes et leur parents Node currentBactrack = closedNodes[currentIndex]; path.push_back(currentBactrack); std::map<Node,Node>::iterator it = parents.find(currentBactrack); while(it != parents.end()) { currentBactrack = it->second; //std::cout << "solution x : " << currentBactrack.GetX()<<" solution y : " << currentBactrack.GetY() << "\n"; //std::cout << "trackback : "<< path.size() << "\n"; path.push_back(currentBactrack); it = parents.find(currentBactrack); } } return true; }
bool Node::operator!=(Node rhs) const { return (x != rhs.GetX() || y != rhs.GetY()); }
void Skein::Layout(CDC& dc, int spacing, bool force) { if (force) m_inst.root->ClearWidths(); if (force || (m_layout == false)) { std::vector<std::vector<Node*> > nodesByDepth; m_inst.root->GetNodesByDepth(0,nodesByDepth); // Visit each level, from the bottom to the top, left to right for (size_t row = nodesByDepth.size(); row > 0; --row) { int x = 0; const std::vector<Node*>& rowNodes = nodesByDepth[row-1]; for (size_t col = 0; col < rowNodes.size(); ++col) { Node* node = rowNodes[col]; int width = node->GetLineWidth(dc); width = max(width,node->GetLabelTextWidth()); int x_next = x + (width/2); // Centre a parent node between its children int numc = node->GetNumChildren(); if (numc > 0) { int x_first_child = node->GetChild(0)->GetX(); int x_last_child = node->GetChild(numc-1)->GetX(); int x_centre = (x_first_child + x_last_child)/2; node->SetX(x_centre); if (x_next > x_centre) { // Move this node and all its children to the right node->ShiftX(x_next - x_centre); // Move all nodes after this node to the right for (size_t col2 = col+1; col2 < rowNodes.size(); ++col2) rowNodes[col2]->ShiftX(x_next - x_centre); } } else node->SetX(x_next); x = node->GetX() + (width/2) + spacing; } } // Shift the entire tree so that is centered horizontally m_inst.root->ShiftX(-m_inst.root->GetX()); int xmin = 0, xmax = 0; bool valid = false; for (size_t row = 0; row < nodesByDepth.size(); ++row) { const std::vector<Node*>& rowNodes = nodesByDepth[row]; if (rowNodes.size() > 0) { Node* node1 = rowNodes[0]; Node* node2 = rowNodes[rowNodes.size()-1]; int x1 = node1->GetX() - (node1->GetLayoutWidth()/2); int x2 = node2->GetX() + (node2->GetLayoutWidth()/2); if (!valid || (x1 < xmin)) xmin = x1; if (!valid || (x2 > xmax)) xmax = x2; valid = true; } } if (valid) m_inst.root->ShiftX((xmin + xmax) / -2); } m_layout = true; }
Node Tools::FindClosestPassable(Node start, Node unpassable,bool **pathingMap) { Node passable; bool found = false; std::vector<Node> unpassableNodes; std::vector<Node> passableNodes; unpassableNodes.push_back(unpassable); std::cout << unpassableNodes.size()<<" unpassable \n"; while(!found && unpassableNodes.end() != unpassableNodes.begin()) { //std::cout << "while \n"; //std::cout << "test \n"; Node currentNode = unpassableNodes[0]; //std::cout << unpassableNodes.size()<<" unpassable \n"; if (CheckInBound(currentNode.GetBottomNode()) && Passable(currentNode.GetBottomNode(),pathingMap)) { passableNodes.push_back(currentNode.GetBottomNode()); } else { unpassableNodes.push_back(currentNode.GetBottomNode()); } if (CheckInBound(currentNode.GetBottomLeftNode()) && Passable(currentNode.GetBottomLeftNode(),pathingMap)) { passableNodes.push_back(currentNode.GetBottomLeftNode()); } else { unpassableNodes.push_back(currentNode.GetBottomLeftNode()); } if (CheckInBound(currentNode.GetLeftNode()) && Passable(currentNode.GetLeftNode(),pathingMap)) { passableNodes.push_back(currentNode.GetLeftNode()); } else { unpassableNodes.push_back(currentNode.GetLeftNode()); } if (CheckInBound(currentNode.GetTopLeftNode()) && Passable(currentNode.GetTopLeftNode(),pathingMap)) { passableNodes.push_back(currentNode.GetTopLeftNode()); } else { unpassableNodes.push_back(currentNode.GetTopLeftNode()); } if (CheckInBound(currentNode.GetTopNode()) && Passable(currentNode.GetTopNode(),pathingMap)) { passableNodes.push_back(currentNode.GetTopNode()); } else { unpassableNodes.push_back(currentNode.GetTopNode()); } if (CheckInBound(currentNode.GetTopRightNode()) && Passable(currentNode.GetTopRightNode(),pathingMap)) { passableNodes.push_back(currentNode.GetTopRightNode()); } else { unpassableNodes.push_back(currentNode.GetTopRightNode()); } if (CheckInBound(currentNode.GetRightNode()) && Passable(currentNode.GetRightNode(),pathingMap)) { passableNodes.push_back(currentNode.GetRightNode()); } else { unpassableNodes.push_back(currentNode.GetRightNode()); } if (CheckInBound(currentNode.GetBottomRightNode()) && Passable(currentNode.GetBottomRightNode(),pathingMap)) { passableNodes.push_back(currentNode.GetBottomRightNode()); } else { unpassableNodes.push_back(currentNode.GetBottomRightNode()); } //std::cout << passableNodes.size()<<"\n passable \n"; //std::cout << passableNodes.size()<<"\n passable \n"; if(!passableNodes.empty()) { found = true; break; } unpassableNodes.erase(unpassableNodes.begin()); } passable = passableNodes[0]; for(std::vector<Node>::iterator it = passableNodes.begin(); it != passableNodes.end(); ++it) { Node currentNode = *it; if (DistanceEuclidienne(start.GetX(),passable.GetX(),start.GetY(),passable.GetY()) > DistanceEuclidienne(start.GetX(),currentNode.GetX(),start.GetY(),currentNode.GetY())) { passable = currentNode; } } return passable; }
void ribi::cmap::QtNodeDialog::OnXchanged(const Node& node) { assert(!"Am I called?"); ui->box_x->setValue(node.GetX()); }
bool operator< (Node lhs, Node rhs) { return (lhs.GetY()*100000000 + lhs.GetX() < rhs.GetY()*100000000 + rhs.GetX()); }
bool Node::operator==(Node rhs) const { return (x == rhs.GetX() && y == rhs.GetY()); }
void UnitCaC::UnitMove() { if (destination->x == x && destination->y == y){ delete destination; destination = NULL; } if (destination != NULL){ Node destinationNode = Tools::GetNodeFromAxis(static_cast <int> (floor(destination->x)),floor(destination->y)); //std::cout << "out of bound X :"<< (int) destinationNode.GetX() << "out of bound Y :"<< (int) destinationNode.GetX() <<"\n" ; if (!Tools::CheckInBound(destinationNode)){ if (destinationNode.GetX()<0){ destinationNode.SetX(0); } if (destinationNode.GetY()<0){ destinationNode.SetY(0); } if (destinationNode.GetX()>33){ destinationNode.SetX(33); } if (destinationNode.GetY()>33){ destinationNode.SetY(33); } delete destination; destination = new Vector2D(destinationNode.GetWorldX(),destinationNode.GetWorldY()); } if ( !Tools::Passable(destinationNode,pathingMap)){ destinationNode = Tools::FindClosestPassable(Tools::GetNodeFromAxis(x,y),destinationNode,pathingMap); //std::cout << "position x : \n"; delete destination; destination = new Vector2D(destinationNode.GetWorldX(),destinationNode.GetWorldY()); } if (path.empty()){ Tools::Astar(Tools::GetNodeFromAxis(x,y),destinationNode,pathingMap, path); } else{ //std::cout << "position x : " << x << "position y : " << y << "\n"; Vector2D targetPosition; int index = 0; index = path.size()-1; speed = 1.5; pathForce = 1; Node currentNode = path[index]; //std::cout << "node x : " << currentNode.GetWorldX() << "node y : " << currentNode.GetWorldY() << "\n"; if (path.size() == 1){ targetPosition = Vector2D(destination->x,destination->y); } else{ targetPosition = Vector2D(currentNode.GetWorldX(),currentNode.GetWorldY()); } //std::cout << "seek x : " << Seek(targetPosition).x << "seek y : " << Seek(targetPosition).y << "\n"; Vector2D steering = velocity + Seek(targetPosition).Normalized() + AvoidUnitCollision(); facing = steering.Normalized(); velocity = steering; velocity.Truncate(speed); //std::cout << "steering x : " << steering.x << "steering y : " << steering.y << "\n"; Move(steering); if( Seek(targetPosition).Length()<= (32/2) && path.size() !=1){ path.erase(path.begin()+index); }else if (Seek(targetPosition).Length() <= 0 && path.size() == 1){ delete destination; destination = NULL; //std::cout << Seek(targetPosition).Length()<< "\n"; path.erase(path.begin()+index); } if (destinationNode.GetX() != path[0].GetX() || destinationNode.GetY()!= path[0].GetY()){ //std::cout << "clear" << "\n"; path.clear(); } //std::cout << Seek(targetPosition).Length()<< "\n"; } } else{ path.clear(); } }
NodeList* Pathfinder::PathBetweenPoints(int x1, int y1, int x2, int y2) { // Set up all the data structures we need, lots o' stuff NodeList Q; PreviousNodeMap prev; PopulateListWithNodes(Q); Node* source = m_nodeMap[x1][y1]; Node* dest = m_nodeMap[x2][y2]; // Make sure source and dest are in Q if(find(Q.begin(), Q.end(), source) == Q.end()) { Q.push_back(source); } if(find(Q.begin(), Q.end(), dest) == Q.end()) { Q.push_back(dest); } ResetNodes(Q, x2, y2); source->SetDistance(0); while(Q.size() > 0) { Q.sort(NodesByScore); Node* u = Q.front(); if(u == dest) { // found our node, break! break; } if(u->GetDistance() == NODE_INFINITY) { // In this case, no valid path from point 1 to point 2 return NULL; } // Remove it from the unvisited queue Q.remove(u); // Update its neighbors int x = u->GetX(); int y = u->GetY(); if(x - 1 >= 0 && m_nodeMap[x-1][y]) { Node* toUpdate = m_nodeMap[x-1][y]; if(u->GetDistance() + 1 < toUpdate->GetDistance()) { prev[toUpdate] = u; toUpdate->SetDistance(u->GetDistance() + 1); } } if(x + 1 < m_currentLevel->GetWidth() && m_nodeMap[x+1][y]) { Node* toUpdate = m_nodeMap[x+1][y]; if(u->GetDistance() + 1 < toUpdate->GetDistance()) { prev[toUpdate] = u; toUpdate->SetDistance(u->GetDistance() + 1); } } if(y - 1 >= 0 && m_nodeMap[x][y-1]) { Node* toUpdate = m_nodeMap[x][y-1]; if(u->GetDistance() + 1 < toUpdate->GetDistance()) { prev[toUpdate] = u; toUpdate->SetDistance(u->GetDistance() + 1); } } if(y + 1 < m_currentLevel->GetHeight() && m_nodeMap[x][y+1]) { Node* toUpdate = m_nodeMap[x][y+1]; if(u->GetDistance() + 1 < toUpdate->GetDistance()) { prev[toUpdate] = u; toUpdate->SetDistance(u->GetDistance() + 1); } } } // Prep the list of path nodes to send back NodeList* toReturn = new NodeList(); Node* next = prev[dest]; toReturn->push_back(next); while(prev.find(next) != prev.end() && prev[next] != source) { next = prev[next]; toReturn->push_back(next); } return toReturn; }