能力值:
( LV4,RANK:40 )
4 楼
A星算法寻路 其实难得不是算法 是你怎么获取当前地图的原始图
////A性算法寻路///
/*******************************************************************************
* CopyRight (c) HYTC Ltd. All rights reserved.
* Filename: main.c
* Creator: GaoLei
* Version: 0.0
* Date: 2011-06-15
* QQ: 38929568
* Description: A*寻路算法 测试类
*******************************************************************************/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "CONIO.H"
#define FALSE 0
#define TRUE 1
#define NULL 0
#define MAXROW 9
typedef int BOOL;
int map[MAXROW][MAXROW]={
{1,1,1,1,1,1,1,1,1},
{1,1,1,1,0,1,1,1,1},
{1,1,1,0,0,0,1,1,1},
{1,1,1,0,1,0,1,1,1},
{1,1,2,1,1,0,3,1,1},
{1,1,1,1,0,0,0,1,1},
{1,1,0,0,0,1,1,1,1},
{1,1,1,1,0,1,1,1,1},
{1,1,1,1,1,1,1,1,1}
};
typedef struct direction
{
int i,j;
}Direction,Ldirection;
typedef struct Node
{//节点结构体
int f,g,h;
int row; //该节点所在行
int col; //该节点所在列
Direction direct;//parent节点要移动的方向就能到达本节点
struct Node * parent;
}Node, *Lnode;
typedef struct Stack
{//OPEN CLOSED 表结构体
Node * npoint;
struct Stack * next;
}Stack, *Lstack;
int rows = 9; //地图行数
int cols = 9; //地图列数
int G_OFFSET1 = 10; //每个图块G值的增加值
int G_OFFSET2 = 14;
int destinationRow; //目标所在行
int destinationCol; //目标所在列
int canMoveIndex =0; //可以通行的地图图块索引
int tileSize = 1; //图块大小
Lstack Open = NULL;
Lstack Closed = NULL;
void PrintMap()
{
int i,j;
for (i=0;i<MAXROW;i++)
{
for (j=0;j<MAXROW;j++)
{
if (map[i][j]==1)
printf("□");
else if (map[i][j]==0)
printf("■");
else if (map[i][j]==2)
printf("☆");
else if (map[i][j]==3)
printf("★");
else if (map[i][j]==4)
printf("●");
}
printf("\n");
}
// getch();
}
Node * getNodeFromOpen()
{//选取OPEN表上f值最小的节点,返回该节点地址
Lstack temp = Open->next,min = Open->next,minp = Open;
Node * minx;
if( temp == NULL )
return NULL;
while(temp->next != NULL)
{
if( (temp->next ->npoint->f) < (min->npoint->f))
{
min = temp->next;//min表示f最小的链表节
minp = temp;//指向min的指针,为的是在后面的释放中用到
}
temp = temp->next;//temp指向下一个链表节
}
minx = min->npoint;
temp = minp->next;
minp->next = minp->next->next;
free(temp);
return minx;
}
BOOL Equal(Node * suc,Node * goal)
{//判断节点是否相等,相等,不相等
if ( (suc->row == goal->row) && (suc->col == goal->col) )
{
return TRUE;
}
else
{
return FALSE;
}
}
Node * BelongInOpen( Node * suc )
{//判断节点是否属于OPEN表或CLOSED表,是则返回节点地址,否则返回空地址
Lstack temp = Open -> next ;
if(temp == NULL)
return NULL;
while( temp != NULL )
{
if( Equal(suc,temp->npoint) )
{
return temp -> npoint;
}
else
{
temp = temp->next;
}
}
return NULL;
}
Node * BelongInClosed( Node * suc )
{//判断节点是否属于OPEN表或CLOSED表,是则返回节点地址,否则返回空地址
Lstack temp = Closed -> next ;
if(temp == NULL)
return NULL;
while(temp != NULL)
{
if( Equal(suc,temp->npoint) )
{
return temp -> npoint;
}
else
{
temp = temp->next;
}
}
return NULL;
}
void PutintoOpen(Node * suc )
{//把节点放入OPEN 或CLOSED 表中
Stack * temp;
temp =(Stack *) malloc(sizeof(Stack));
temp->npoint = suc;
temp->next = Open->next;
Open->next = temp;
}
void PutintoClosed(Node * suc )
{//把节点放入OPEN 或CLOSED 表中
Stack * temp;
temp =(Stack *) malloc(sizeof(Stack));
temp->npoint = suc;
temp->next = Closed->next;
Closed->next = temp;
}
//得到该图块的H值
int getH(int row, int col)
{
int x;
x=abs(destinationRow - row) + abs(destinationCol - col)+1;
return x*10;
}
//得到该位置所在地图行
int getRowPosition(int y)
{
return (y/tileSize);
}
//得到该位置所在地图列
int getColPosition(int x)
{
return (x / tileSize);
}
//检测该图块是否可通行
BOOL isCanMove(int col, int row)
{
if(col < 0 || col >= cols)
return FALSE;
if(row < 0 || row >= rows)
return FALSE;
if(map[row][col]==canMoveIndex)
return FALSE;
}
Node* checkOpen(int row, int col)
{
Lstack temp = Open -> next;
if ( temp == NULL )
return NULL;
while (temp != NULL)
{
if ( (temp->npoint->row==row) && (temp->npoint->col == col) )
{
return temp -> npoint;
}
else
{
temp = temp->next;
}
}
return NULL;
}
BOOL isInClose(int row, int col)
{
Lstack temp = Closed -> next;
if ( temp == NULL )
return FALSE;
while (temp != NULL)
{
if ( (temp->npoint->row==row) && (temp->npoint->col == col) )
{
return TRUE;
}
else
{
temp = temp->next;
}
}
return FALSE;
}
int directionIndex =0;
int direction[256];
void creatSeccessionNode(Node *bestNode, int row, int col,int G_OFFSET)
{
int g = bestNode->g + G_OFFSET;
if(!isInClose(row, col))
{
Node *oldNode = NULL;
if((oldNode = checkOpen(row, col)) != NULL)
{
if(oldNode->g < g)
{
oldNode->direct.i = oldNode->parent->col;
oldNode->direct.j = oldNode->parent->row;
oldNode->parent = bestNode;
oldNode->g = g;
oldNode->f = g + oldNode->h;
}
}
else
{
Node *node = (Node *) malloc(sizeof(Node));
node->parent = bestNode;
node->g = g;
node->h = getH(row,col);
node->f = node->g + node->h;
node->row = row;
node->col = col;
// directionIndex++;
node->direct.i = bestNode->col;
node->direct.j = bestNode->row;
// openNode.addElement(node);
// printf("row:%d col:%d\n",node->row,node->col);
PutintoOpen( node );
}
}
}
/**
* 根据传入的节点生成子节点
* @param bestNode
* @param destinationRow
* @param destinationCol
*/
void seachSeccessionNode(Node *bestNode)
{
int row, col;
// Node *bestNodeInOpen = NULL;
//上部节点
if(isCanMove(row = bestNode->row - 1, col = bestNode->col))
{
creatSeccessionNode(bestNode, row, col,G_OFFSET1);
}
//下部节点
if(isCanMove(row = bestNode->row + 1, col = bestNode->col))
{
creatSeccessionNode(bestNode, row, col,G_OFFSET1);
}
//左部节点
if(isCanMove(row = bestNode->row, col = bestNode->col-1))
{
creatSeccessionNode(bestNode, row, col,G_OFFSET1);
}
//右部节点
if(isCanMove(row = bestNode->row, col = bestNode->col + 1))
{
creatSeccessionNode(bestNode, row, col,G_OFFSET1);
}
//左上角
if(isCanMove(row = bestNode->row-1, col = bestNode->col - 1))
{
creatSeccessionNode(bestNode, row, col,G_OFFSET2);
}
//右上角
if(isCanMove(row = bestNode->row+1, col = bestNode->col - 1))
{
creatSeccessionNode(bestNode, row, col,G_OFFSET2);
}
//左下角
if(isCanMove(row = bestNode->row-1, col = bestNode->col + 1))
{
creatSeccessionNode(bestNode, row, col,G_OFFSET2);
}
//右下角
if(isCanMove(row = bestNode->row+1, col = bestNode->col + 1))
{
creatSeccessionNode(bestNode, row, col,G_OFFSET2);
}
PutintoClosed( bestNode );
}
//正序遍历链表
void ClearOpenData()
{//判断节点是否属于OPEN表或CLOSED表,是则返回节点地址,否则返回空地址
Lstack temp = Open -> next;
Node *p_node;
if(temp == NULL)
return;
while(temp != NULL)
{
Lstack head = temp;
temp = temp->next;
p_node = head->npoint;
// printf("Open库数据![%d,%d]\n", p_node->col, p_node->row );
free(p_node);
free( head );
Open->next = temp;
}
printf("\n Open库数据 数据全部清楚 \n");
return;
}
void ClearClosedData()
{//判断节点是否属于OPEN表或CLOSED表,是则返回节点地址,否则返回空地址
Lstack temp = Closed -> next ;
Node *p_node;
if(temp == NULL)
return;
while(temp != NULL)
{
Lstack head = temp;
temp = temp->next;
p_node = head->npoint;
// printf("Closed库数据![%d,%d]\n", p_node->col, p_node->row );
free(p_node);
free( head );
Closed -> next = temp;
}
printf("\n Closed库数据 数据全部清楚 \n");
/*
temp = Closed -> next;
while(temp != NULL)
{
printf("Closed库数据!节点");
temp = temp->next;
}*/
return;
}
void getPath(int startX, int StartY, int destinationX, int destinationY)
{
Node *startNode = (Node *) malloc(sizeof(Node));
Node *bestNode = NULL;
int index = 0;
destinationRow = getRowPosition(destinationY);
destinationCol = getColPosition(destinationX);
startNode->parent= NULL;
startNode->row = getRowPosition(StartY);
startNode->col = getColPosition(startX);
startNode->g = 0;
startNode->h = getH( startNode->row, startNode->col );
startNode->f = startNode->g + startNode->h;
startNode->direct.i = startNode->col;
startNode->direct.j = startNode->row;
PutintoOpen( startNode );// openNode.add(startNode);
while(TRUE)
{
bestNode = getNodeFromOpen(); //从OPEN表中取出f值最小的节点
if(bestNode == NULL)//未找到路径
{
printf("未找到路径\n");
return;
}
else if(bestNode->row == destinationRow&& bestNode->col == destinationCol)
{
// Lnode _Node = bestNode;
int nodeSum = 0;
int nodeIndex =0;
printf("程序运行次数=%d\n",index);
map[bestNode->col][bestNode->row]=4;
while(!(bestNode->direct.i==startX&&bestNode->direct.j==StartY))
{
// printf("x:%d y:%d direction = %d \n", _Node->col, _Node->row, _Node->direction );
map[bestNode->direct.i][bestNode->direct.j]=4;
// PrintMap();
bestNode = bestNode->parent;
nodeSum += 1;
}
/*
printf("节点数量=%d\n",nodeSum);
_Node = bestNode;
nodeIndex = nodeSum-1;
while( _Node->parent != NULL && nodeIndex>=0)
{
Node *_NodeParent = _Node->parent;
printf("x:%d y:%d direction = %d \n", _Node->col, _Node->row, _Node->direction );
if( _NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == +1 )
{//从父节点到本节点的操作是 上
direction[nodeIndex] = 1;
}
else if( _NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == -1 )
{//从父节点到本节点的操作是 下
direction[nodeIndex] = 2;
}
else if( _NodeParent->col - _Node->col == +1 && _NodeParent->row - _Node->row == 0 )
{//从父节点到本节点的操作是 左
direction[nodeIndex] = 3;
}
else if( _NodeParent->col - _Node->col == -1 && _NodeParent->row - _Node->row == 0 )
{//从父节点到本节点的操作是 右
direction[nodeIndex] = 4;
}
else
{
direction[nodeIndex] = 0;
}
nodeIndex -= 1;
_Node = _Node->parent;
}
for( nodeIndex=0; nodeIndex<nodeSum; nodeIndex++ )
{
printf("direction[%d]=%d\n",nodeIndex,direction[nodeIndex]);
}
*/
return ;
}
index++;
// map[bestNode->col][bestNode->row]=4;
// PrintMap();
seachSeccessionNode(bestNode);
}
} void main()
{//主函数
printf("说明:1.☆为开始地址 2.★为目标地址 3.●为路径 4.■为障碍物\n");
printf("寻路地图:\n");
PrintMap();
//初始操作,建立open和closed表
Open = (Stack *) malloc(sizeof(Stack));
Open->next = NULL;
Closed = (Stack *) malloc(sizeof(Stack));
Closed->next = NULL;
//-----------------------------------
getPath(4,2,4,6);
// printf("程序认定该起始状态无法道达目标状态!\n");
printf("运行结果:\n");
PrintMap();
ClearOpenData();
ClearClosedData();
getch();
free(Open);
free(Closed);
//--------------------------------------
}