Fawkes API Fawkes Development Version

astar.cpp

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
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends