00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <utils/search/astar.h>
00025
00026
00027 namespace fawkes {
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056 AStar::AStar()
00057 {
00058 while ( openList.size() > 0 )
00059 {
00060 openList.pop();
00061 }
00062 closedList.clear();
00063 solution.clear();
00064 }
00065
00066
00067
00068
00069 AStar::~AStar()
00070 {
00071
00072 AStarState * best = 0;
00073 while ( openList.size() > 0 )
00074 {
00075 best = openList.top();
00076 openList.pop();
00077 delete best;
00078 }
00079 closedList.clear();
00080 }
00081
00082
00083
00084
00085
00086
00087 std::vector< AStarState * > AStar::solve( AStarState * initialState )
00088 {
00089 AStarState * best = 0;
00090 while ( openList.size() > 0 )
00091 {
00092 best = openList.top();
00093 openList.pop();
00094 delete best;
00095 }
00096 closedList.clear();
00097
00098 openList.push( initialState );
00099 return getSolutionSequence( search() );
00100 }
00101
00102
00103
00104 AStarState * AStar::search( )
00105 {
00106 AStarState * best = 0;
00107 long key = 0;
00108 std::vector< AStarState * > children;
00109
00110
00111 while ( openList.size() > 0 )
00112 {
00113
00114 do
00115 {
00116 if ( openList.size() > 0 )
00117 {
00118 best = openList.top();
00119 openList.pop( );
00120 }
00121 else
00122 return 0;
00123 key = best->calculateKey( );
00124 }
00125 while ( closedList.find( key ) != closedList.end() );
00126
00127
00128 closedList[key] = best;
00129
00130
00131 if ( best->isGoal( ) )
00132 {
00133 return best;
00134 }
00135
00136 children = best->generateChildren( );
00137 for ( unsigned int i = 0; i < children.size(); i++ )
00138 openList.push( children[i] );
00139 }
00140 return 0;
00141 }
00142
00143
00144
00145
00146
00147
00148
00149 std::vector< AStarState * > AStar::getSolutionSequence( AStarState * node )
00150 {
00151 solution.clear();
00152 AStarState * state = node;
00153
00154 while ( state != 0 )
00155 {
00156 closedList.erase(state->key);
00157 solution.insert( solution.begin(), state );
00158 state = state->father;
00159 }
00160
00161
00162 while ( closedList.size() > 0 )
00163 {
00164 state = closedList.begin()->second;
00165 closedList.erase(state->key);
00166 delete state;
00167 }
00168 return solution;
00169 }
00170
00171
00172 }