Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * astar.cpp - Implementation of A* 00004 * 00005 * Generated: Mon Sep 15 18:38:00 2002 00006 * Copyright 2002-2007 Stefan Jacobs, Martin Liebenberg 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 #include <utils/search/astar.h> 00025 00026 00027 namespace fawkes { 00028 00029 /** @class AStar <utils/search/astar.h> 00030 * This is an implementation of the A* search algorithm. 00031 * 00032 * @author Stefan Jacobs, Martin Liebenberg 00033 */ 00034 /** @var AStar::closedList 00035 * This is AStars closedList. 00036 */ 00037 /** @var AStar::solution 00038 * This is the final solution vector. 00039 */ 00040 /** @var AStar::openList 00041 * This is AStars openlist. 00042 */ 00043 /** @struct AStar::Cmp <utils/search/astar.h> 00044 * Comparison structure to be used for the ordering on AStar::openList. 00045 * @fn AStar::Cmp::operator() ( AStarState * a1, AStarState * a2 ) const 00046 * The relation >= of this ordering. 00047 * @param a1 the left operand 00048 * @param a2 the right operand 00049 * @return true, if a1 <= b1, else false 00050 */ 00051 00052 00053 /** Constructor. 00054 * This is the constructor for the AStar Object. 00055 */ 00056 AStar::AStar() 00057 { 00058 while ( openList.size() > 0 ) 00059 { 00060 openList.pop(); 00061 } 00062 closedList.clear(); 00063 solution.clear(); 00064 } 00065 00066 /** Destructor. 00067 * This destructs the AStarObject. 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 /** Solves a situation given by the initial state with AStar, and 00083 * returns a vector of AStarStates that solve the problem. 00084 * @param initialState pointer of AStarState to the initial state 00085 * @return a vector of pointers of AStarState with the solution sequence 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 /** Search with astar. */ 00104 AStarState * AStar::search( ) 00105 { 00106 AStarState * best = 0; 00107 long key = 0; 00108 std::vector< AStarState * > children; 00109 00110 // while the openlist not is empty 00111 while ( openList.size() > 0 ) 00112 { 00113 // take the best state, and check if it is on closed list 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 // put best state on closed list 00128 closedList[key] = best; 00129 00130 // check if its a goal. 00131 if ( best->isGoal( ) ) 00132 { 00133 return best; 00134 } 00135 // generate all its children 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 /** Generates a solution sequence for a given state 00145 * Initial solution is in solution[0]! 00146 * @param node a pointer of AStarState to the goal 00147 * @return the path from solution to initial solution 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 //delete the states, which are not part of the solution 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 } // end namespace fawkes