int find_path(AStar_t astar,int x,int y,int x1,int y1,kn_dlist *path){
AStarNode *from = get_node(astar,x,y);
AStarNode *to = get_node(astar,x1,y1);
if(!from || !to || from == to || to->block)
return 0;
minheap_insert(astar->open_list,&from->heap);
AStarNode *current_node = NULL;
while(1){
struct heapele *e = minheap_popmin(astar->open_list);
if(!e){
reset(astar);
return 0;
}
current_node = (AStarNode*)((int8_t*)e-sizeof(kn_dlist_node));
if(current_node == to){
while(current_node)
{
kn_dlist_remove((kn_dlist_node*)current_node);//可能在close_list中，需要删除
if(current_node != from)//当前点无需加入到路径点中
kn_dlist_push_front(path,(kn_dlist_node*)current_node);
AStarNode *t = current_node;
current_node = current_node->parent;
t->parent = NULL;
t->F = t->G = t->H = 0;
t->heap.index = 0;
}
reset(astar);
return 1;
}
//current插入到close表
kn_dlist_push(&astar->close_list,(kn_dlist_node*)current_node);
//获取current的相邻节点
kn_dlist *neighbors = get_neighbors(astar,current_node);
if(neighbors)
{
AStarNode *n;
while((n = (AStarNode*)kn_dlist_pop(neighbors))){
if(n->heap.index)//在openlist中
{
float new_G = current_node->G + cost_2_neighbor(current_node,n);
if(new_G < n->G)
{
//经过当前neighbor路径更佳,更新路径
n->G = new_G;
n->F = n->G + n->H;
n->parent = current_node;
minheap_change(astar->open_list,&n->heap);
}
continue;
}
n->parent = current_node;
n->G = current_node->G + cost_2_neighbor(current_node,n);
n->H = cost_2_goal(n,to);
n->F = n->G + n->H;
minheap_insert(astar->open_list,&n->heap);
}
neighbors = NULL;
}
}
}
bool AStar::find_path(int x,int y,int x1,int y1,std::list<mapnode*> &path)
{
mapnode *from = get_mapnode(x,y);
mapnode *to = get_mapnode(x1,y1);
if(from == to || to->value == 0xFFFFFFFF){
path.push_back(from);
return true;
}
open_list.insert(from);
mapnode *current_node = NULL;
while(1){
current_node = (mapnode*)open_list.popmin();
if(!current_node){
reset();
return false;
}
if(current_node == to){
while(current_node)
{
path.push_front(current_node);
mapnode *t = current_node;
current_node = current_node->parent;
t->parent = NULL;
t->F = t->G = t->H = 0;
t->index = 0;
}
reset();
return true;
}
//current插入到close表
close_list.Push(current_node);
//获取current的相邻节点
std::vector<mapnode*> *neighbors = get_neighbors(current_node);
if(neighbors)
{
int size = neighbors->size();
for(int i =0 ; i < size; i++)
{
mapnode *neighbor = (*neighbors)[i];
if(neighbor->pre || neighbor->next){
continue;//在close表中,不做处理
}
if(neighbor->index)//在openlist中
{
double new_G = current_node->G + cost_2_neighbor(current_node,neighbor);
if(new_G < neighbor->G)
{
//经过当前neighbor路径更佳,更新路径
neighbor->G = new_G;
neighbor->F = neighbor->G + neighbor->H;
neighbor->parent = current_node;
open_list.change(neighbor);
}
continue;
}
neighbor->parent = current_node;
neighbor->G = current_node->G + cost_2_neighbor(current_node,neighbor);
neighbor->H = cost_2_goal(neighbor,to);
neighbor->F = neighbor->G + neighbor->H;
open_list.insert(neighbor);
}
neighbors = NULL;
}
}
}
