基于二叉堆优化的A星算法

#include"stdafx.h" #include"a_star.h" U8 Astar::m_mtrxMapMatrix[HEIGHT][WIDTH]; U16 BitNum(U8 n) { return 1<<n; } U8 GetBit(U16 n) { assert(n != 0); assert(n%2 == 0 || n == 1); U8 t=0; while(n != 1) { t++; n>>=1; } return t; } Astar::Astar(void):m_pntTerminal(),m_pntStart() { memset(m_mtrxMapMatrix,0,sizeof(m_mtrxMapMatrix)); m_lstPathList.clear(); m_vctOpenList.clear(); m_lstCloseList.clear(); m_wdStep = 0; } Astar::~Astar() { InitOpenList(); InitCloseList(); m_lstPathList.clear(); } MapNode* Astar::CreateNewNode(MapNode* p,const U8 dir) { if(nullptr != p) { //不是开始点 U8 t = GetBit(dir); APoint pos(p->m_pntPoint+DIR_VECTOR[t]); //判断点是否在地图内 if(WithinMap(pos)) { //在地图内 U8 pv = m_mtrxMapMatrix[pos.GetX()][pos.GetY()]; //判断对应位置上是否有障碍物 if(BAR != pv) { //若无障碍物 //依据规则创建新节点，防止出现重复 MapNode* temp = new MapNode; temp->m_btValue = pv; temp->m_bIsVisited = false; temp->m_bIsReachable = true; temp->m_pntPoint = p->m_pntPoint+DIR_VECTOR[t];//计算新节点位置 p->m_btSur += dir; //标记新节点相对于源节点的位置 if(0 == t%2)//非对角线方向 { temp->m_btSur = ~(BitNum((t+7)%8)+BitNum((t+9)%8)+BitNum(t));//依据规则，该点只能探测三个方向 temp->m_pFrom = p; } else//对角线方向 { t = (t+4)%8; temp->m_btSur = (BitNum((t+7)%8)+BitNum((t+9)%8)+BitNum(t));//依据规则，该点有三个方向不用探索 temp->m_pFrom = p; } return temp;//返回新节点 } else { //若有障碍物 //不创建新节点 p->m_btSur = dir;//标记障碍物相对于源节点的位置：注意，障碍物不可跨越 if(0 != t%2)//判断是否是对角线上的障碍物 { //不是则对角线方向不可通过 p->m_btSur += BitNum((t+7)%8); p->m_btSur += BitNum((t+9)%8); } return nullptr; } } else { //不在地图内 //不创建新节点 p->m_btSur += dir;//标记表示已经访问过 return nullptr; } } else { //是开始点 //创建开始点 MapNode* temp = new MapNode; temp->m_btValue = 0; temp->m_bIsVisited = false; temp->m_bIsReachable = true; temp->m_pntPoint = m_pntStart; temp->m_btSur = 0; temp->m_pFrom = nullptr; return temp;//返回新节点 } } void Astar::InitCloseList(void) { if(!m_lstCloseList.empty()) { for(list<MapNode*>::iterator it = m_lstCloseList.begin();it != m_lstCloseList.end();it++) { assert(*it != nullptr); delete *it; } m_lstCloseList.clear(); } } void Astar::InitOpenList(void) { if(!m_vctOpenList.empty()) { for(vector<MapNode*>::iterator it = m_vctOpenList.begin();it != m_vctOpenList.end();it++) { assert(*it != nullptr); delete *it; } m_vctOpenList.clear(); } } void Astar::SetFec(MapNode* node) { SetGac(node); SetHec(node); node->m_fFec = node->m_fGac + node->m_fHec; } void Astar::SetGac(MapNode* node) { float t; if(nullptr != node->m_pFrom) { APoint c,a,b; a = (node->m_pntPoint); b = (node->m_pFrom->m_pntPoint); c = a - b; t = node->m_pFrom->m_fGac; t += VectorLenth(c); } else { assert(node->m_pntPoint == m_pntStart); t = 0; } node->m_fGac = t; } void Astar::SetHec(MapNode* node) { float t,m,d; APoint v = m_pntTerminal - node->m_pntPoint; m = float(abs(v.GetX()) + abs(v.GetY())); d = float((abs(v.GetX()) <= abs(v.GetY()))?abs(v.GetX()):abs(v.GetY())); t = (float)(d*DREADIA + (m-2*d)*DREASTR); node->m_fHec = t; } void Astar::Push(MapNode* node) { SetFec(node); m_vctOpenList.push_back(node); push_heap(m_vctOpenList.begin(),m_vctOpenList.end(),greater_class()); } bool Astar::AstarAlgorithm(void) { m_lstPathList.clear(); InitOpenList();//清空开始列表 InitCloseList();//清空关闭列表 MapNode* start = CreateNewNode(nullptr,0);//创建开始节点 SetFec(start); m_vctOpenList.push_back(start); make_heap(m_vctOpenList.begin(),m_vctOpenList.end(),greater_class());//创建二元堆 while(!m_vctOpenList.empty()) { pop_heap(m_vctOpenList.begin(),m_vctOpenList.end(),greater_class());//弹出最小堆的堆顶 MapNode* mintop = m_vctOpenList.back();//获取最小堆的堆顶 m_lstCloseList.push_back(mintop);//推入关闭列表 m_vctOpenList.pop_back();//删除最小堆的堆顶 if(0 == m_lstCloseList.back()->m_fHec)//判断是否到达终点 return true; InitOpenList();//清空开始列表 if(m_lstCloseList.size()>=MAXSIZE) { list<APoint> lstBuffer; lstBuffer.clear(); MapNode* end = m_lstCloseList.back();//获取当前点 lstBuffer.push_front(end->m_pntPoint);//将当前点坐标推入路径链表 MapNode* temp = end->m_pFrom; //填充路径链表 while(nullptr != temp) { lstBuffer.push_front(temp->m_pntPoint); m_wdStep++; temp = temp->m_pFrom; } m_pntStart = end->m_pntPoint;//将当前点作为新路径的起点(用作计算) lstBuffer.pop_front();//去除重复节点 for(list<APoint>::iterator it = lstBuffer.begin();it!=lstBuffer.end();it++) { m_lstPathList.push_back(*it); }//连接路径链表 m_lstCloseList.pop_back(); InitCloseList();//清空关闭列表，为防止陷入死循环，保存关闭列表尾节点 start = end; start->m_pFrom = nullptr; SetFec(start); m_vctOpenList.push_back(start); make_heap(m_vctOpenList.begin(),m_vctOpenList.end(),greater_class());//创建二元堆 pop_heap(m_vctOpenList.begin(),m_vctOpenList.end(),greater_class());//弹出最小堆的堆顶 mintop = m_vctOpenList.back();//获取最小堆的堆顶 m_lstCloseList.push_back(mintop);//推入关闭列表 m_vctOpenList.pop_back();//删除最小堆的堆顶 } for(int i=0;i<8;i++)//向八个方向搜索点 { if(mintop->m_btSur & BitNum(i))//判断该点是否已经搜索过，或是有障碍，是/有则跳过 continue; MapNode* temp = CreateNewNode(mintop,BitNum(i));//在该方向上创建一个新的节点 if(nullptr != temp)//判断是否创建成功 Push(temp);//成功就把新节点推入开始列表 } } return false; } bool Astar::InsertPath(void) { m_wdStep = 0; if(AstarAlgorithm()) { MapNode* end = m_lstCloseList.back();//获取终点 m_lstPathList.push_front(end->m_pntPoint);//将终点坐标推入路径链表的尾部 MapNode* temp = end->m_pFrom; while(nullptr != temp) { m_lstPathList.push_front(temp->m_pntPoint); m_wdStep++; temp = temp->m_pFrom; } return true; } else { m_wdStep = 0; return false; } }