my_star.c

A*算法

#include<stdio.h>
#include<math.h>
#define N 400
struct point{
    int i;
    int j;
};
struct tree
{
	struct point p;
	int father;
};
struct link
{
	struct tree node;
	int f;
	int last;
};
struct link open[N];
struct link close[N];
int Mg[20][20];
int Si,Sj,Ei,Ej;

void mymove_astar();
struct point get_ward(struct point sp,int ward);
void Ini_Data(int n);
int Get_Data();
int insert_open(int opentop,int lastfirst);
int get_f(int opentop);
void print_way(int closetop,int opentop);
void print_map(int n);

void main()
{
    int n;
    while(1)
    {
    n=Get_Data();
    Ini_Data(n);
    mymove_astar();
    print_map(n);
    }
}

int Get_Data()
{
    int n;
    printf("Please input the Size[1-20]:");
    scanf("%d",&n);
    printf("Please input the Start Location:");
    scanf("%d,%d",&Si,&Sj);
    printf("Please input the End Location:");
    scanf("%d,%d",&Ei,&Ej);
    return n;
}

void Ini_Data(int n)
{
    int i,j;
    for(i=0;i<n;i++)
        for(j=0;j<n;j++)
            Mg[i][j]=0;
    for(i=0;i<n;i++)
    {
        if(i==0||i==n-1)
        {
            for(j=0;j<n;j++)
                Mg[i][j]=1;
        }
        else
        {
            for(j=0;j<n;j=j+n-1)
                Mg[i][j]=1;
        }
    }

    Mg[2][3]=1;
    Mg[2][4]=1;
    Mg[4][4]=1;
    Mg[4][5]=1;
    Mg[4][6]=1;

    for(i=0;i<n;i++)
    {
        for(j=0;j<n;j++)
            printf("%2d",Mg[i][j]);
        printf("\n");
    }
}
void mymove_astar()
{
	int find=0,ward,opentop=0,closetop=0,first=0,i;
	struct point p;
	struct link temp;
	close[0].node.p.i=Si;close[0].node.p.j=Sj;close[0].node.father=-1;
	Mg[Si][Sj]=-3;
    open[0].last=-1;open[0].f=1000;
	while(opentop>-1)
	{
		find=0;
		for(ward=0;ward<4;ward++)
		{
			p=get_ward(close[closetop].node.p,ward);
			if(Mg[p.i][p.j]==0)
			{
				opentop++;
				open[opentop].node.p=p;
				open[opentop].node.father=closetop;
				if(p.i==Ei&&p.j==Ej)
				{
					find=1;
					print_way(closetop,opentop);
					break;
				}
				first=insert_open(opentop,first);
				Mg[p.i][p.j]=-2;
			}
		}
		if(find==1)
        {
            break;
        }
		closetop++;
		close[closetop]=open[first];
		Mg[open[first].node.p.i][open[first].node.p.j]=-3;
		for(i=0;i<opentop;i++)
		  if(open[i].last==opentop)
		  {
		      open[i].last=first;
		      break;
          }
		temp=open[opentop];
		open[opentop]=open[first];
		open[first]=temp;
        first=open[opentop].last;
        opentop--;
	}
	if(opentop==-1) printf("There is no rout!\n");
}

int insert_open(int opentop,int lastfirst)
{
	int first=0,last,i;
	open[opentop].f=get_f(opentop);
	last=lastfirst;
	while(open[opentop].f>open[last].f)
    {
        last=open[last].last;
    }
	open[opentop].last=last;
	for(i=0;i<opentop;i++)
		if(open[i].last==last)
		{	
            open[i].last=opentop;
            first=1;
        }
    if(first)
    {
        first=lastfirst;
    }
    else
    {
        first=opentop;
    }
    return first;
}

int get_f(int opentop)
{
	int g=1,h,f;
	int closetop;
	h=abs(Ei-open[opentop].node.p.i)+abs(Ej-open[opentop].node.p.j);
	closetop=open[opentop].node.father;
	while(close[closetop].node.father>-1)
	{
		closetop=close[closetop].node.father;
		g++;
	}
	f=g+h;
	return f;
}

struct point get_ward(struct point sp,int ward)
{
	struct point p;
	switch(ward)
	{
		case 0:p.i=sp.i-1;p.j=sp.j;break;
		case 1:p.i=sp.i;p.j=sp.j+1;break;
		case 2:p.i=sp.i+1;p.j=sp.j;break;
		case 3:p.i=sp.i;p.j=sp.j-1;break;
	}
	if(ward==0)
	{
	    p.i=sp.i-1;p.j=sp.j;
    }
	return p;
}

void print_way(int closetop,int opentop)
{
	int i;
	closetop++;
	close[closetop].node=open[opentop].node;
	opentop=0;
	while(close[closetop].node.father>-1)
	{
		closetop=close[closetop].node.father;
		open[opentop].node.p=close[closetop].node.p;
		opentop++;
	}
	opentop--;
	for(i=opentop;i>-1;i--)
	{
        Mg[open[i].node.p.i][open[i].node.p.j]=-1;
		printf("%d%d",open[i].node.p.i,open[i].node.p.j);
		printf("  ");
	}
	Mg[Ei][Ej]=-1;
	printf("%d%d",Ei,Ej);
	printf("\n");
}

void print_map(int n)
{
    int i,j;
    for(i=0;i<n;i++)
    {
        for(j=0;j<n;j++)
        {
            if(Mg[i][j]==-3||Mg[i][j]==-2) Mg[i][j]=0;
            printf("%2d",Mg[i][j]);
        }
        printf("\n");
    }
}