00001
00002
#include "a_star.h"
00003
00004
00005
00006 bool cList_A_Star_Nodes::Contain(
cA_Star_Node* node)
00007 {
00008 std::list<cA_Star_Node*>::iterator itr;
00009
for(itr = this->begin() ; itr != this->end() ; itr++)
00010
if( (*itr) == node)
return true;
00011
00012
return false;
00013 }
00014
00015
00016
00017
00018 void cA_Star_Priority_List::Add(
cA_Star_Node* new_node)
00019 {
00020
if( empty() )
00021 {
00022 push_front(new_node);
00023
return;
00024 }
00025
00026 std::list<cA_Star_Node*>::iterator itr;
00027
for( itr = this->begin() ; itr != this->end() ; itr++)
00028 {
00029
00030
if( new_node->
Get_Cost() < (*itr)->Get_Cost() )
00031 {
00032 insert(itr , new_node);
00033
return;
00034 }
00035 }
00036
00037 push_back(new_node);
00038
return;
00039 }
00040
00041
00042 bool cA_Star_Priority_List::Contain(
cA_Star_Node* node)
00043 {
00044 std::list<cA_Star_Node*>::iterator itr;
00045
for(itr = this->begin() ; itr != this->end() ; itr++)
00046
if( (*itr) == node)
00047
return true;
00048
return false;
00049 }
00050
00051
00052 void cA_Star_Search::Construct_Path(
cA_Star_Node* destination ,
cList_A_Star_Nodes & chemin )
00053 {
00054 assert( destination != NULL );
00055
while( destination->
Path_Parent != NULL)
00056 {
00057 chemin.push_front( destination );
00058 destination = destination->
Path_Parent;
00059 }
00060 }
00061
00062
00063 bool cA_Star_Search::Find_Path(
cA_Star_Node* start ,
cA_Star_Node* goal ,
cList_A_Star_Nodes & chemin )
00064 {
00065 assert( start != NULL );
00066 assert( goal != NULL );
00067
00068
cA_Star_Priority_List Open_List;
00069 cA_Star_Priority_List::iterator open_itr;
00070
cList_A_Star_Nodes Closed_List;
00071 cList_A_Star_Nodes::iterator closed_itr;
00072 cList_A_Star_Nodes::iterator voisins_itr;
00073
00074
cA_Star_Node * current_node = NULL;
00075
cList_A_Star_Nodes Voisins_List;
00076
bool bIs_Open;
00077
bool bIs_Closed;
00078
float Cost_From_Start;
00079
int n_voisins;
00080
00081 start->
Cost_From_Start = 0;
00082 start->
Estimated_Cost_To_Goal = start->
Get_Estimated_Cost(goal);
00083 start->
Path_Parent = NULL;
00084
00085 Open_List.
Add(start);
00086
00087
while( ! Open_List.empty() )
00088 {
00089 current_node = Open_List.front();
00090 Open_List.pop_front();
00091
00092
if( current_node == goal)
00093 {
00094
Construct_Path(goal , chemin);
00095
return true;
00096 }
00097
00098 Closed_List.push_front( current_node);
00099 current_node->
Get_List_Neighbors(Voisins_List);
00100 n_voisins = Voisins_List.size();
00101
00102
00103
for( voisins_itr = Voisins_List.begin() ; voisins_itr != Voisins_List.end() ; voisins_itr++)
00104 {
00105 bIs_Open = Open_List.
Contain(*voisins_itr);
00106 bIs_Closed = Closed_List.
Contain(*voisins_itr);
00107 Cost_From_Start = current_node->
Cost_From_Start + current_node->
Get_Cost(*voisins_itr);
00108
00109
00110
if( ( !bIs_Open && !bIs_Closed) || Cost_From_Start < (*voisins_itr)->Cost_From_Start )
00111 {
00112 (*voisins_itr)->Path_Parent = current_node;
00113 (*voisins_itr)->
Cost_From_Start = Cost_From_Start;
00114 (*voisins_itr)->Estimated_Cost_To_Goal = (*voisins_itr)->Get_Estimated_Cost(goal);
00115
00116
if( bIs_Closed)
00117 Closed_List.remove((*voisins_itr));
00118
00119
if (!bIs_Open )
00120 Open_List.
Add((*voisins_itr));
00121 }
00122 }
00123 }
00124
return false;
00125 };