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;
}
}
}
void minheap_insert(minheap_t m,struct heapele *e)
{
if(e->index)
return minheap_change(m,e);
if(m->size >= m->max_size)
return;
++m->size;
m->data[m->size] = e;
e->index = m->size;
up(m,e->index);
}
void minheap_insert(minheap_t m,struct heapele *e)
{
if(e->index)
return minheap_change(m,e);
if(m->size >= m->max_size-1)
{
//expand the heap
uint32_t new_size = m->max_size*2;
struct heapele** tmp = (struct heapele**)calloc(new_size,sizeof(struct heapele*));
if(!tmp)
return;
memcpy(tmp,m->data,m->max_size*sizeof(struct heapele*));
free(m->data);
m->data = tmp;
m->max_size = new_size;
}
++m->size;
m->data[m->size] = e;
e->index = m->size;
up(m,e->index);
}
void minheap_push(struct minheap * mh,struct element * elt)
{
if (elt->index)
{
minheap_change(mh,elt);
return;
}
if (mh->size >= mh->cap - 1)
{
int nsize = mh->cap * 2;
struct element ** elts = (struct element **)malloc(nsize * sizeof(struct element*));
memcpy(elts,mh->elts,mh->cap * sizeof(struct element*));
free(mh->elts);
mh->elts = elts;
mh->cap = nsize;
}
++mh->size;
mh->elts[mh->size] = elt;
elt->index = mh->size;
up(mh,elt->index);
}