fork(3) download
copy 
  1. /*
  2. A* Algorithm Implementation using STL is
  3. Copyright (C)2001-2005 Justin Heyes-Jones
  4.  
  5. A* Algorithm Implementation not using STL is
  6. Copyright (C)2008 Konstantin Stupnik
  7. Optimized version of original implementation.
  8.  
  9.  
  10. Permission is given by the author to freely redistribute and
  11. include this code in any program as long as this credit is
  12. given where due.
  13.  
  14.   COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS,
  15.   WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED,
  16.   INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE
  17.   IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
  18.   OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND
  19.   PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED
  20.   CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL
  21.   DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY
  22.   NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF
  23.   WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE
  24.   OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
  25.   THIS DISCLAIMER.
  26.  
  27.   Use at your own risk!
  28.  
  29. */
  30.  
  31. #ifndef __ASTARSEARCH_HPP__
  32. #define __ASTARSEARCH_HPP__
  33.  
  34. #include "IntrSortedList.hpp"
  35. #include <sys/types.h>
  36. #ifdef DEBUGASTAR
  37. #include <stdio.h>
  38. #define asdebugprintf(x) printf x
  39. #else
  40. #define asdebugprintf(x)
  41. #endif
  42.  
  43. template <class UserState,class CostType>
  44. struct StateCostTmpl{
  45.   UserState state;
  46.   CostType cost;
  47.   StateCostTmpl(){}
  48.   StateCostTmpl(const UserState& argState,CostType argCost):state(argState),cost(argCost){}
  49. };
  50.  
  51.  
  52. template <class MapClass,class UserState,typename CostType=int,int maxSuccessors=8>
  53. class AStarSearch
  54. {
  55. public: // data
  56.  
  57.   enum AStarSearchState
  58.   {
  59.     SEARCH_STATE_NOT_INITIALISED,
  60.     SEARCH_STATE_SEARCHING,
  61.     SEARCH_STATE_SUCCEEDED,
  62.     SEARCH_STATE_FAILED,
  63.     SEARCH_STATE_OUT_OF_MEMORY,
  64.     SEARCH_STATE_INVALID
  65.   };
  66.  
  67.  
  68.   // constructor just initialises private data
  69.   AStarSearch() :
  70.     userMap(0),
  71.     nodesMap(0),
  72.     nodesMapSize(0),
  73.     state( SEARCH_STATE_NOT_INITIALISED ),
  74.     invokeId(0),
  75.     start(0),
  76.     goal(0),
  77.     currentSolutionNode( NULL ),
  78.     cancelRequest( false )
  79.   {
  80.   }
  81.  
  82.   ~AStarSearch()
  83.   {
  84.     if(nodesMap)
  85.     {
  86.       delete [] nodesMap;
  87.     }
  88.   }
  89.  
  90.   void assignMap(MapClass* argUserMap)
  91.   {
  92.     userMap=argUserMap;
  93.   }
  94.  
  95.  
  96.   // call at any time to cancel the search
  97.   void cancelSearch()
  98.   {
  99.     cancelRequest = true;
  100.   }
  101.  
  102.   void setMaxIndex(size_t maxIndex)
  103.   {
  104.     if(nodesMapSize>maxIndex)return;
  105.     if(nodesMap)delete [] nodesMap;
  106.     nodesMapSize=maxIndex+1;
  107.     nodesMap=new Node[nodesMapSize];
  108.     memset(nodesMap,0,sizeof(Node)*nodesMapSize);
  109.   }
  110.  
  111.   // Set Start and goal states
  112.   void setStartAndGoalStates( const UserState &argStart, const UserState &argGoal )
  113.   {
  114.     //setMaxIndex wasn't called and memory wasn't allocated
  115.     if(nodesMapSize==0)
  116.     {
  117.       state=SEARCH_STATE_OUT_OF_MEMORY;
  118.       return;
  119.     }
  120.  
  121.     // increment invokeId for next search
  122.     invokeId++;
  123.  
  124.     asdebugprintf(("start=(%s), end=(%s)\n",argStart.toString().c_str(),argGoal.toString().c_str()));
  125.  
  126.     // Clear open list after previous searches
  127.     openList.Clear();
  128.  
  129.     cancelRequest = false;
  130.  
  131. #ifdef DEBUGASTAR
  132.     if(userMap->getIndex(argStart)>=nodesMapSize || userMap->getIndex(argGoal)>=nodesMapSize)
  133.     {
  134.       asdebugprintf(("Start or Goal state index is out of range!"));
  135.       state=SEARCH_STATE_OUT_OF_MEMORY;
  136.       return;
  137.     }
  138. #endif
  139.  
  140.     // allocate start end goal nodes
  141.     start = nodesMap+userMap->getIndex(argStart);
  142.     goal = nodesMap+userMap->getIndex(argGoal);
  143.  
  144.     start->userState = argStart;
  145.     goal->userState = argGoal;
  146.  
  147.     //goal is the same as start
  148.     //do not need to do anything
  149.     if(userMap->getIndex(argStart)==userMap->getIndex(argGoal))
  150.     {
  151.       state = SEARCH_STATE_SUCCEEDED;
  152.       start->child=0;
  153.       return;
  154.     }
  155.     else
  156.     {
  157.       state = SEARCH_STATE_SEARCHING;
  158.     }
  159.  
  160.     // Initialise the AStar specific parts of the Start Node
  161.     // The user only needs fill out the state information
  162.  
  163.     start->g = 0;
  164.     start->h = start->userState.goalDistanceEstimate( goal->userState );
  165.     start->f = start->g + start->h;
  166.     start->parent = 0;
  167.  
  168.     // Push the start node on the Open list
  169.  
  170.     start->invokeId=invokeId;
  171.     start->na=naOpen;
  172.     openList.Push( start );
  173.  
  174.     asdebugprintf(("push:%d:%s\n",start->f,start->userState.toString().c_str()));
  175.  
  176.     // Initialise counter for search steps
  177.     steps = 0;
  178.   }
  179.  
  180.   // Advances search one step
  181.   AStarSearchState searchStep()
  182.   {
  183.     // Firstly break if the user has not initialised the search
  184.  
  185.     if( state != SEARCH_STATE_SEARCHING )
  186.     {
  187.       return state;
  188.     }
  189.  
  190.     // Failure is defined as emptying the open list as there is nothing left to
  191.     // search...
  192.     // New: Allow user abort
  193.     if( openList.isEmpty() || cancelRequest )
  194.     {
  195.       state = SEARCH_STATE_FAILED;
  196.       return state;
  197.     }
  198.  
  199.     // Incremement step count
  200.     steps ++;
  201.  
  202.     // Pop the best node (the one with the lowest f)
  203.     Node *n= openList.Pop();
  204.  
  205.     asdebugprintf(("Pop:%d:%s\n",n->f,n->userState.toString().c_str()));
  206.  
  207.     // Check for the goal, once we pop that we're done
  208.     //if( n->userState.isEqual( &goal->userState ) )
  209.     if( n==goal )
  210.     {
  211.       // The user is going to use the Goal Node he passed in
  212.       // so copy the parent pointer of n
  213.       //goal->parent = n->parent;
  214.  
  215.       // set the child pointers in each node (except Goal which has no child)
  216.       Node *nodeChild = goal;
  217.       Node *nodeParent = goal->parent;
  218.       do
  219.       {
  220.         nodeParent->child = nodeChild;
  221.         nodeChild = nodeParent;
  222.         nodeParent = nodeParent->parent;
  223.       }
  224.       while( nodeChild != start ); // Start is always the first node by definition
  225.  
  226.       state = SEARCH_STATE_SUCCEEDED;
  227.  
  228.       return state;
  229.     }
  230.     else // not goal
  231.     {
  232.  
  233.       // We now need to generate the successors of this node
  234.       // The user helps us to do this, and we keep the new nodes in
  235.       // successors ...
  236.  
  237.       // User provides this functions and uses AddSuccessor to add each successor of
  238.       // node 'n' to successors
  239.  
  240.       successorsSize=userMap->getSuccessors( successors, &n->userState,n->parent?&n->parent->userState:0);
  241.  
  242.       StateCost* it=successors;
  243.       StateCost* end=successors+successorsSize;
  244.  
  245.       //Node tempNode;
  246.       Node* successor;
  247.       CostType newg;
  248.       bool previousFound;
  249.  
  250.       // Now handle each successor to the current node ...
  251.       for( ; it != end; it++ )
  252.       {
  253. #ifdef DEBUGASTAR
  254.         if(userMap->getIndex(it->state)>=nodesMapSize)
  255.         {
  256.           asdebugprintf(("State index is out of range:%s(%d)\n",it->state.toString().c_str(),(int)userMap->getIndex(it->state)));
  257.           state=SEARCH_STATE_OUT_OF_MEMORY;
  258.           return state;
  259.         }
  260. #endif
  261.  
  262.         successor=nodesMap+userMap->getIndex(it->state);
  263.  
  264.         //  The g value for this successor ...
  265.         newg = n->g + it->cost;
  266.  
  267.         asdebugprintf(("successor: g=%d, %s\n",newg,it->state.toString().c_str()));
  268.  
  269.         // Now we need to find whether the node is on the open or closed lists
  270.         // If it is but the node that is already on them is better (lower g)
  271.         // then we can forget about this successor
  272.         previousFound=successor->invokeId==invokeId;
  273.  
  274.         if( previousFound )
  275.         {
  276.           asdebugprintf(("found in previous: %s, g=%d\n",(successor->na==naOpen)?"open":((successor->na==naClosed)?"closed":"none"),successor->g));
  277.  
  278.           if(successor->na==naOpen && successor->g <= newg)
  279.           {
  280.             // we found this state on open
  281.             // the one on Open is cheaper than this one
  282.             continue;
  283.           }
  284.           if(successor->na==naClosed && successor->g <= newg)
  285.           {
  286.             // the one on Closed is cheaper than this one
  287.             continue;
  288.           }
  289.         }else
  290.         {
  291.           successor->invokeId=invokeId;
  292.           successor->child=0;
  293.           successor->userState=it->state;
  294.         }
  295.  
  296.         // This node is the best node so far with this particular state
  297.         // so lets keep it and set up its AStar specific data ...
  298.  
  299.         successor->parent = n;
  300.         successor->g = newg;
  301.         successor->h = successor->userState.goalDistanceEstimate( goal->userState );
  302.         successor->f = successor->g + successor->h;
  303.  
  304.         //previous version of this state found in open list,
  305.         //but this one is better. let's update it.
  306.         if(previousFound && successor->na==naOpen)
  307.         {
  308.           openList.Update( successor );
  309.           asdebugprintf(("update:%d\n",(int)openList.getSize()));
  310.         }else
  311.         {
  312.           successor->na=naOpen;
  313.           openList.Push( successor );
  314.           asdebugprintf(("push:%d,%d\n",successor->f,(int)openList.getSize()));
  315.         }
  316.       }
  317.  
  318.       // mark n as Closed, as we have expanded it now
  319.  
  320.       n->na=naClosed;
  321.  
  322.     } // end else (not goal so expand)
  323.  
  324.     return state; // state is searching at this point
  325.  
  326.   }
  327.  
  328.   // Functions for traversing the solution
  329.  
  330.   // Get start node
  331.   UserState *getSolutionStart()
  332.   {
  333.     currentSolutionNode = start;
  334.     if( start )
  335.     {
  336.       return &start->userState;
  337.     }
  338.     else
  339.     {
  340.       return NULL;
  341.     }
  342.   }
  343.  
  344.   // Get next node
  345.   UserState *getSolutionNext()
  346.   {
  347.     if( currentSolutionNode )
  348.     {
  349.       if( currentSolutionNode->child )
  350.       {
  351.  
  352.         currentSolutionNode = currentSolutionNode->child;
  353.  
  354.         return &currentSolutionNode->userState;
  355.       }
  356.     }
  357.  
  358.     return NULL;
  359.   }
  360.  
  361.   // Get end node
  362.   UserState *getSolutionEnd()
  363.   {
  364.     currentSolutionNode = goal;
  365.     if( goal )
  366.     {
  367.       return &goal->userState;
  368.     }
  369.     else
  370.     {
  371.       return NULL;
  372.     }
  373.   }
  374.  
  375.   // Step solution iterator backwards
  376.   UserState *getSolutionPrev()
  377.   {
  378.     if( currentSolutionNode )
  379.     {
  380.       if( currentSolutionNode->parent )
  381.       {
  382.  
  383.         currentSolutionNode = currentSolutionNode->parent;
  384.  
  385.         return &currentSolutionNode->userState;
  386.       }
  387.     }
  388.  
  389.     return NULL;
  390.   }
  391.  
  392.   // Get the number of steps
  393.  
  394.   int getStepCount()const
  395.   {
  396.     return steps;
  397.   }
  398.  
  399. private: // methods
  400.  
  401.   typedef StateCostTmpl<UserState,CostType> StateCost;
  402.   // A node represents a possible state in the search
  403.   // The user provided state type is included inside this type
  404.  
  405.   enum NodeAffilation{
  406.     naNone,
  407.     naClosed,
  408.     naOpen
  409.   };
  410.  
  411.  
  412.   struct Node:
  413.   IntrSortedListNodeBase<Node>
  414.   {
  415.       Node *parent; // used during the search to record the parent of successor nodes
  416.       Node *child; // used after the search for the application to view the search in reverse
  417.  
  418.       CostType g; // cost of this node + it's predecessors
  419.       CostType h; // heuristic estimate of distance to goal
  420.       CostType f; // sum of cumulative cost of predecessors and self and heuristic
  421.  
  422.       int invokeId;
  423.  
  424.       NodeAffilation na;
  425.  
  426.  
  427.       bool lessThan(const Node* rhs)const
  428.       {
  429.         return f < rhs->f;
  430.       }
  431.  
  432.       UserState userState;
  433.   };
  434.  
  435.   //private copy constructor
  436.   //class cannot be copied
  437.   AStarSearch(const AStarSearch&);
  438.  
  439.   //use provided class that represent access to map
  440.  
  441.   MapClass* userMap;
  442.  
  443.   //sorted list for open nodes
  444.   typedef IntrSortedList<Node> OpenNodeContainer;
  445.   OpenNodeContainer openList;
  446.  
  447.   //plain array of search nodes
  448.   Node* nodesMap;
  449.   size_t nodesMapSize;
  450.  
  451.   //current node to add successors to
  452.   StateCost successors[maxSuccessors];
  453.   size_t successorsSize;
  454.  
  455.   // State
  456.   AStarSearchState state;
  457.  
  458.   // Counts steps
  459.   int steps;
  460.   //current invokeId to track nodes reusage
  461.   int invokeId;
  462.  
  463.   // Start and goal state pointers
  464.   Node *start;
  465.   Node *goal;
  466.   Node *currentSolutionNode;
  467.  
  468.   bool cancelRequest;
  469.  
  470. };
  471.  
  472. #undef asdebugprintf
  473.  
  474. #endif
  475.  
Not running #stdin #stdout 0s 0KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
Standard output is empty
https://ideone.com/gITAuf
language:
C++ (gcc 8.3)
created:
12 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!