void CAstar::Astar(CAstarNode*& result) { CAstarNode s0(m_hgevStart,0,CalculateH(m_hgevStart),NULL); //open表中插入起点 m_vOpenList.push_back(s0); make_heap(m_vOpenList.begin(),m_vOpenList.end()); // CAstarNode* min; while(!m_vOpenList.empty()) { //pop出堆顶节点 pop_heap(m_vOpenList.begin(),m_vOpenList.end(),cmpAstarNode); result = new CAstarNode(m_vOpenList.back().m_hgevCoordinate,m_vOpenList.back().m_nG, m_vOpenList.back().m_nH, m_vOpenList.back().m_pParent); //若节点是目标节点,则返回 if(result->m_hgevCoordinate == m_hgevGoal) { // memcpy(result,&min,sizeof(CAstarNode)); return ; } m_vOpenList.pop_back(); // make_heap(m_vOpenList.begin(),m_vOpenList.end()); //扩展该节点 ExtendNode(result); // make_heap(m_vOpenList.begin(),m_vOpenList.end()); m_vClosedList.push_back(*result); } result = NULL; }
//扩展节点 void CAstar::ExtendNode(CAstarNode* s) { vector<Point> neighbor = GetNeighbor(s->m_hgevCoordinate); for(vector<Point>::iterator iter1 = neighbor.begin(); iter1 != neighbor.end(); iter1++) { bool exist = false; vector<CAstarNode>::iterator iter2 = m_vClosedList.begin(); for(; iter2 != m_vClosedList.end(); iter2++) { if((*iter1) == (*iter2).m_hgevCoordinate) { exist = true; break; } } //节点在close表中,跳过该节点 if(exist) continue; //计算该节点的确定耗费 int tentative_g = s->m_nG; if((abs((*iter1).x-s->m_hgevCoordinate.x) + abs((*iter1).y-s->m_hgevCoordinate.y)) == 1) tentative_g += DEMO::MOVE_MIN_STEP; else tentative_g += DEMO::MOVE_MAX_STEP; iter2 = m_vOpenList.begin(); for(; iter2 != m_vOpenList.end(); iter2++) { if((*iter1) == (*iter2).m_hgevCoordinate) { exist = true; break; } } //节点不在open表中,则添加到open表中 if(!exist) { CAstarNode temp(*iter1,tentative_g,CalculateH(*iter1),s); m_vOpenList.push_back(temp); push_heap(m_vOpenList.begin(),m_vOpenList.end(),cmpAstarNode); } //否则若g小于open表中该节点的g值,则更新表中的g else if((*iter2).m_nG > tentative_g) { (*iter2).m_nG = tentative_g; } } }
void GameMapPathFinder::FindPath( ) { // 현재 위치로부터 갈수 있는 노드들 std::multimap<int, Node> open; // 이미 확인하고 더이상 검색할 필요가 없는 노드들 std::multimap<int, Node> close; // 시작 위치를 정한다. // 시작점이므로 시작점과의 거리인 g값을 0으로 넣는다. m_Map[m_StartNode.x][m_StartNode.y].g = 0; // 목적지와의 거리값을 찾는다. CalculateH( m_StartNode ); // 목적지와 선택지의 비용값을 더한다. (사실 시작점이므로 h값과 동일하다.) CalculateF( m_StartNode ); // 경로의 끝을 표시하기 위해 부모를 자신으로 둔다. m_Map[m_StartNode.x][m_StartNode.y].parent = m_StartNode; // 검색을 하도록 open에 시작값을 넣는다. open.insert( std::make_pair( m_Map[m_StartNode.x][m_StartNode.y].f, m_Map[m_StartNode.x][m_StartNode.y] ) ); // 갈수있는곳이나 목적지를 찾을때까지 계속 검색 ( open이 다 비워도 목적지를 찾지 못하면 길이 없다. 종료조건1) while ( !open.empty() ) { // 검색된 주변 위치를 꺼낸다. Node nowNode = open.begin()->second; open.erase( open.begin() ); // 목적지를 찾음 - 종료조건2 if ( nowNode.point.x == m_EndNode.x && nowNode.point.y == m_EndNode.y ) { m_IsFindPath = true; printf_s( "경로를 찾았다!\n" ); break; } // 이미 검색한 위치는 검색하지 않도록 close에 넣는다. close.insert( std::make_pair( m_Map[nowNode.point.x][nowNode.point.y].f, m_Map[nowNode.point.x][nowNode.point.y] ) ); // 현재 위치에서 주변을 탐색한다. for ( int i = -1; i <= 1; ++i ) { for ( int j = -1; j <= 1; ++j ) { int cost; // 대각선방향이면 14로 직선방향이면 10 (루트2 약1.4를 정수로 만들기 위해 14,10을 씀) if ( i && j ) { cost = 14; } else { cost = 10; } // 대각선에 대해서는 그냥 넘겨버리면 대각선 이동을 하지 않음 // 어차피 길이 4방향 게임이면 상관없긴 하지만 직선운동도 해주는것이 인간답게 하는 꼼수라고 함 (근데 이건 너무 단순) if ( i && j ) { continue; } NodePoint nearNodeP; nearNodeP.x = nowNode.point.x + i; nearNodeP.y = nowNode.point.y + j; // 주변의 노드가 벽이거나 맵을 벗어나는것은 넘겨버림 if ( !ValidPoint( nearNodeP ) ) { continue; } // 이미 close된 곳에 노드가 있다! if ( close.find( m_Map[nearNodeP.x][nearNodeP.y].f ) != close.end() ) { continue; } // open되어있는 목록에 이 노드가없다! auto openNode = open.find( m_Map[nearNodeP.x][nearNodeP.y].f ); if ( open.end() == openNode ) { std::multimap<int, Node>::iterator iterTime = open.end(); CalculateG( nowNode.point, nearNodeP, cost ); CalculateH( nearNodeP ); CalculateF( nearNodeP ); // 지금 노드로 가기전의 노드가 누군지 표시 - 이걸 따라가면 경로가 나온다. m_Map[nearNodeP.x][nearNodeP.y].parent = nowNode.point; // 지금 노드를 검색 대상으로 올려놓는다. open.insert( std::make_pair( m_Map[nearNodeP.x][nearNodeP.y].f, m_Map[nearNodeP.x][nearNodeP.y] ) ); } else //만약 이것이 이미 열린목록에 있다면, G비용을 이용하해 어느쪽이 더 나은가 알아보고 //그것의 G비용이 더 작으면 그것이 더 나은 길이라는 것을 의미하므로 //기존에 있던 노드를 비교 대상인 nearNode로 교체 //즉 그 노드로 가는데 더 좋은 방법 찾음 { Node nearNode = openNode->second; if ( nearNode.g < m_Map[nearNodeP.x][nearNodeP.y].g ) { open.erase( m_Map[nearNode.point.x][nearNode.point.y].f ); open.insert( std::make_pair( m_Map[nearNodeP.x][nearNodeP.y].f, m_Map[nearNodeP.x][nearNodeP.y] ) ); } } } } } // 경로를 찾지 못함 if ( !m_IsFindPath ) { printf_s( "경로를 못찾았다...\n" ); } WritePathOnTheMap(); }