1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143
|
AStar(D3DXVECTOR2 start, D3DXVECTOR2 goal)
{
bool quit=false;
//The path
node *Path = new node();
Path->next=NULL;
//The 2 list of nodes open and closed
node *OpenList=new node();
OpenList->next=NULL;
node *ClosedList = new node();
ClosedList=NULL;
//pointer to the current node
node *Current = new node();
Current=OpenList;
//pointer to the best node
node *Best=new node();
Best=NULL;
//create the start node
node *Start=new node();
Start->Position=start;
Start->g=0;
Start->h=DetermineH(Start->Position,goal);
Start->h=DetermineF(Start->g,Start->h);
Start->next=NULL;
//set the start point to the past
Path=Start;
//add the start node to the open list
OpenList=Start;
while(quit==false)
{
Current=OpenList;
//find the closest node to the goal
//if Best= NULL then define the first node as the best one
if(Best==NULL)
{
Best=Current;
}
else
{
//else loop through all the nodes
while(Current!=NULL)
{
if(Current->h<Best->h)
{
Best=Current;
}
Current=Current->next;
}
}
//find the path
if(Best->Position==goal)
{
quit=true;
return 1;
}
//failed
if(OpenList==NULL)
{
quit=true;
return -1;
}
//find connected node to the best node
int x=1;
int y=0;
for(int i=0;i<3;i++)
{
//pointer to a connected node
node *connected=new node();
connected->Position=D3DXVECTOR2(Best->Position.x+x,Best->Position.y+y);
connected->h=DetermineH(connected->Position,goal);
connected->next=NULL;
Current=OpenList;
//determine if the node is in the openlist
if(Current->next==NULL)
{
//found it in the open list
if(Current->Position==connected->Position)
{
//check if the path is more efficient
if(connected->h<Best->h)
{
//add the new node to the open list
AddTotheEnd(Path,connected);
}
}
else
{
//add the node to the openlist
AddTotheEnd(OpenList,connected);
}
}
else
{
while(Current!=NULL)
{
//found it in the open list
if(Current->Position==connected->Position)
{
//check if the path is more efficient
if(connected->h<Best->h)
{
//add the new node to the open list
AddTotheEnd(Path,connected);
}
}
else
{
//add the node to the openlist
AddTotheEnd(OpenList,connected);
}
Current=Current->next;
}
}
if(x==1 && y==0)
{
x=0;
y=1;
}
else
{
if(x==0 && y==1)
{
x=1;
y=1;
}
}
}
}
return 0;
} |
Partager