AStar寻路算法源代码
余年寄山水
565次浏览
2020年08月01日 06:02
最佳经验
本文由作者推荐
倒口-譬如
#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->straightCost=10;
this->diagonalCost=14;
}
AStar::~AStar(){
//cout<<"destory aStar"<<"
";
}
int AStar::judgeCanAccess(int row1,int col1,int row2,int col2){//cout<<"22222"<
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->(row1);
this->(col1);
Link ptr_temp=(Link)malloc(sizeof(LinkNode));
ptr_temp->node=this->startNode;ptr_temp->parent=NULL;
this->_back(ptr_temp);//把开始节点放进开启列表
this->(row2);
this->(col2);
this->searchRoad();//寻找路径
if(this->()==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;
_back(ptr_temp);//把第一个节点放入
for(int i=1;i
temp_minus=atan2(this->roadList[i+1]->()-ptr_temp->(),this->roadList[i+1]->()-ptr_temp->())-
atan2(this->roadList[i]->()-ptr_temp->(),this->roadList[i]->()-ptr_temp->());
if(temp_minus>0.00001||temp_minus<-0.00001){
ptr_temp=this->roadList[i];
_back(ptr_temp);
}
}
_back(this->roadList[this->()-1]);//加入最后一个节点
return(roadNodeList);
}
void AStar::searchRoad(){//cout<<"33333"<
iterator p;
Link ptr=(Link)malloc(sizeof(LinkNode));ptr->parent=NULL;
while(this->()!=0){
ptr=openList[0];
if((ptr->()==this->())&&(ptr->()==this->())){
isFind=1;
break;
}
if(ptr->()>=1&&ptr->()<=9){
if(ptr->()-1>=0){//node上边的节点
findParent(ptr->()-1,ptr->(),ptr,this->straightCost);
}
if(ptr->()+1
}
if(ptr->()-1>=0){//node左边的节点
findParent(ptr->(),ptr->()-1,ptr,this->straightCost);
}
if(ptr->()+1
}
}
else{
if(ptr->()-1>=0){//node左边的节点
find
Parent(ptr->(),ptr->()-1,ptr,this->straightCost);
}
if(ptr->()+1
if(ptr->()-1>=0){//node上边的节点
findParent(ptr->()-1,ptr->(),ptr,this->straightCost);
}
if(ptr->()+1
}
}
}
p=this->();//迭代器p指向开启列表的头一个元素
this->(p);//删除头元素
this->_back(ptr);//放入关闭列表
this->sortOpenList();//将最小F值的元素放到开启列表的第一个位置
p=this->();
}
if(isFind==0){
return;
}
this->getRoad(ptr);
for(int i=0;i
this->m->map[this->roadList[i]->()][this->roadList[i]->()]=2;
}
}
void AStar::findParent(int row,int col,Link ptr,int cost){//cout<<"44444"<
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->()+cost
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->_back(ptr_temp);
}
}
int AStar::judgeContain(vector list,int row,int col){//cout<<"55555"<
return(-1);
}
iterator p;int index=-1;
for(p=();p!=();p++){
index++;
if((list[index]->()==row)&&(list[index]->()==col)){
return(index);
}
}
return(-1);
}
void AStar::countG(Link ptr,int cost){
if(ptr->parent==NULL){
ptr->(cost);
}
else{
ptr->(ptr->parent->()+cost);
}
}
void AStar::countH(Link ptr){
ptr->(abs(10*(this->()-ptr->()))+abs(10*(this->()-ptr->())));
}
void AStar::countF(Link ptr){
ptr->(ptr->()+ptr->());
}
void AStar::getRoad(Link ptr){
if(ptr->parent!=NULL){
getRoad(ptr->parent);
}
this->_back(ptr);
}
void AStar::sortOpenList(){//cout<<"66666"<
Link ptr_temp=openList[0];
for(int i=1;i<();i++){
if(openList[i]->()
()){
ptr_temp=openList[i];
index=i;
}
}
if(index!=0){
openList[index]=openList[0];
openList[0]=ptr_temp;
}
}