AStar寻路算法源代码

#include
#include
#include "Node.h"
#include "AStar.h"
#define pi 3.141592

using std::cout;
using std::endl;
using std::vector;
using std::vector::iterator;

AStar::AStar(Map m){//cout<<"11111"<this->m=m;
this->straightCost=10;
this->diagonalCost=14;
}

AStar::~AStar(){
//cout<<"destory aStar"<<"\n";
}

int AStar::judgeCanAccess(int row1,int col1,int row2,int col2){//cout<<"22222"<if(row1<0||row1>=ROW||col1<0||col1>=COL){//位置不合法
return(-1);
}
if(row2<0||row2>=ROW||col2<0||col2>=COL){//位置不合法
return(-1);
}
if(this->m->map[row1][col1]==0||this->m->map[row2][col2]==0){//位置不合法
return(-1);
}
this->startNode.setRow(row1);
this->startNode.setCol(col1);
Link ptr_temp=(Link)malloc(sizeof(LinkNode));
ptr_temp->node=this->startNode;ptr_temp->parent=NULL;
this->openList.push_back(ptr_temp);//把开始节点放进开启列表
this->endNode.setRow(row2);
this->endNode.setCol(col2);
this->searchRoad();//寻找路径
if(this->roadList.size()==0){//没有路
return(0);
}
return(1);
}

vector AStar::returnRoadList(){
return(this->roadList);
}

vector AStar::returnRoadNodeList(){
float temp_minus=0.0;
Link ptr_temp=(Link)malloc(sizeof(LinkNode));
ptr_temp=this->roadList[0];
vector roadNodeList;
roadNodeList.push_back(ptr_temp);//把第一个节点放入
for(int i=1;iroadList.size()-1;i++){
temp_minus=atan2(this->roadList[i+1]->node.getCol()-ptr_temp->node.getCol(),this->roadList[i+1]->node.getRow()-ptr_temp->node.getRow())-
atan2(this->roadList[i]->node.getCol()-ptr_temp->node.getCol(),this->roadList[i]->node.getRow()-ptr_temp->node.getRow());
if(temp_minus>0.00001||temp_minus<-0.00001){
ptr_temp=this->roadList[i];
roadNodeList.push_back(ptr_temp);
}
}
roadNodeList.push_back(this->roadList[this->roadList.size()-1]);//加入最后一个节点
return(roadNodeList);
}

void AStar::searchRoad(){//cout<<"33333"<int isFind=0;
iterator p;
Link ptr=(Link)malloc(sizeof(LinkNode));ptr->parent=NULL;
while(this->openList.size()!=0){
ptr=openList[0];
if((ptr->node.getRow()==this->endNode.getRow())&&(ptr->node.getCol()==this->endNode.getCol())){
isFind=1;
break;
}
if(ptr->node.getRow()>=1&&ptr->node.getCol()<=9){
if(ptr->node.getRow()-1>=0){//node上边的节点
findParent(ptr->node.getRow()-1,ptr->node.getCol(),ptr,this->straightCost);
}
if(ptr->node.getRow()+1findParent(ptr->node.getRow()+1,ptr->node.getCol(),ptr,this->straightCost);
}
if(ptr->node.getCol()-1>=0){//node左边的节点
findParent(ptr->node.getRow(),ptr->node.getCol()-1,ptr,this->straightCost);
}
if(ptr->node.getCol()+1findParent(ptr->node.getRow(),ptr->node.getCol()+1,ptr,this->straightCost);
}
}
else{
if(ptr->node.getCol()-1>=0){//node左边的节点
find

Parent(ptr->node.getRow(),ptr->node.getCol()-1,ptr,this->straightCost);
}
if(ptr->node.getCol()+1findParent(ptr->node.getRow(),ptr->node.getCol()+1,ptr,this->straightCost);
if(ptr->node.getRow()-1>=0){//node上边的节点
findParent(ptr->node.getRow()-1,ptr->node.getCol(),ptr,this->straightCost);
}
if(ptr->node.getRow()+1findParent(ptr->node.getRow()+1,ptr->node.getCol(),ptr,this->straightCost);
}
}
}
p=this->openList.begin();//迭代器p指向开启列表的头一个元素
this->openList.erase(p);//删除头元素
this->closeList.push_back(ptr);//放入关闭列表
this->sortOpenList();//将最小F值的元素放到开启列表的第一个位置
p=this->openList.begin();
}
if(isFind==0){
return;
}
this->getRoad(ptr);
for(int i=0;iroadList.size();i++){
this->m->map[this->roadList[i]->node.getRow()][this->roadList[i]->node.getCol()]=2;
}
}

void AStar::findParent(int row,int col,Link ptr,int cost){//cout<<"44444"<if(this->m->map[row][col]==0){//此节点不可达,忽略之
return;
}
int index=-1;
if((index=this->judgeContain(this->closeList,row,col))!=-1){//此节点在closeArray中则忽略之
return;
}
if((index=this->judgeContain(this->openList,row,col))!=-1){//此节点在openArray中
if(ptr->node.getG()+costopenList[index]->node.getG()){//发现这条新路更短
this->openList[index]->parent=ptr;//更改父节点
this->countG(this->openList[index],cost);
this->countH(this->openList[index]);
this->countF(this->openList[index]);//重新计算G,H,F值
}
}
else{//此节点不在openArray中
Node node(row,col);
Link ptr_temp=(Link)malloc(sizeof(LinkNode));
ptr_temp->node=node;ptr_temp->parent=ptr;
this->countG(ptr_temp,cost);
this->countH(ptr_temp);
this->countF(ptr_temp);
this->openList.push_back(ptr_temp);
}
}

int AStar::judgeContain(vector list,int row,int col){//cout<<"55555"<if(list.size()==0){
return(-1);
}
iterator p;int index=-1;
for(p=list.begin();p!=list.end();p++){
index++;
if((list[index]->node.getRow()==row)&&(list[index]->node.getCol()==col)){
return(index);
}
}
return(-1);
}

void AStar::countG(Link ptr,int cost){
if(ptr->parent==NULL){
ptr->node.setG(cost);
}
else{
ptr->node.setG(ptr->parent->node.getG()+cost);
}
}

void AStar::countH(Link ptr){
ptr->node.setH(abs(10*(this->endNode.getRow()-ptr->node.getRow()))+abs(10*(this->endNode.getCol()-ptr->node.getCol())));
}

void AStar::countF(Link ptr){
ptr->node.setF(ptr->node.getG()+ptr->node.getH());
}

void AStar::getRoad(Link ptr){
if(ptr->parent!=NULL){
getRoad(ptr->parent);
}
this->roadList.push_back(ptr);
}

void AStar::sortOpenList(){//cout<<"66666"<int index=0;
Link ptr_temp=openList[0];
for(int i=1;iif(openList[i]->node.getF()nod

e.getF()){
ptr_temp=openList[i];
index=i;
}
}
if(index!=0){
openList[index]=openList[0];
openList[0]=ptr_temp;
}
}