Permalink
Browse files

working on much more elaborate fasr2 demo

  • Loading branch information...
1 parent 6484adb commit 7e37217768346030f047fd1a78a54cbbcbbf31cd rtv committed Feb 19, 2010
View
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
View
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
View
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
@@ -23,9 +23,18 @@ set_source_files_properties( ${PLUGINS} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}
foreach( PLUGIN ${PLUGINS} )
ADD_LIBRARY( ${PLUGIN} MODULE ${PLUGIN}.cc )
+endforeach( PLUGIN )
+
+ADD_LIBRARY( fasr2 MODULE fasr2.cc astar/findpath.cpp )
+
+# add fasr2 to the list of plugins
+SET( PLUGINS ${PLUGINS} fasr2 )
+
+foreach( PLUGIN ${PLUGINS} )
TARGET_LINK_LIBRARIES( ${PLUGIN} stage )
endforeach( PLUGIN )
+
# delete the "lib" prefix from the plugin libraries
SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" )
@@ -37,6 +46,7 @@ SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" )
#endif( PLAYER_FOUND )
+
# install in <prefix>/lib
-install( TARGETS ${PLUGINS} DESTINATION lib)
+install( TARGETS ${PLUGINS} fasr2 DESTINATION lib)
@@ -0,0 +1,16 @@
+
+class point_t
+{
+public:
+ unsigned int x, y;
+
+ point_t( unsigned int x, unsigned int y ) : x(x), y(y) {}
+};
+
+bool astar( uint8_t* map,
+ unsigned int width,
+ unsigned int height,
+ const point_t start,
+ const point_t goal,
+ std::vector<point_t>& path );
+
@@ -0,0 +1,350 @@
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// STL A* Search implementation
+// (C)2001 Justin Heyes-Jones
+//
+// Finding a path on a simple grid maze
+// This shows how to do shortest path finding using A*
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#include "stlastar.h" // See header for copyright and usage information
+
+#include <iostream>
+#include <math.h>
+
+#define DEBUG_LISTS 0
+#define DEBUG_LIST_LENGTHS_ONLY 0
+
+using namespace std;
+
+
+// map helper functions
+
+static uint8_t* _map = NULL;
+static unsigned int _map_width=0, _map_height=0;
+
+uint8_t GetMap( unsigned int x, unsigned int y )
+{
+ assert(_map);
+
+ if( x < 0 ||
+ x >= _map_width ||
+ y < 0 ||
+ y >= _map_height
+ )
+ {
+ return 9;
+ }
+
+ return _map[(y*_map_width)+x];
+}
+
+// Definitions
+
+class MapSearchNode
+{
+public:
+ unsigned int x; // the (x,y) positions of the node
+ unsigned int y;
+
+ MapSearchNode() { x = y = 0; }
+ MapSearchNode( unsigned int px, unsigned int py ) { x=px; y=py; }
+
+ float GoalDistanceEstimate( MapSearchNode &nodeGoal );
+ bool IsGoal( MapSearchNode &nodeGoal );
+ bool GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node );
+ float GetCost( MapSearchNode &successor );
+ bool IsSameState( MapSearchNode &rhs );
+
+ void PrintNodeInfo();
+
+
+};
+
+bool MapSearchNode::IsSameState( MapSearchNode &rhs )
+{
+
+ // same state in a maze search is simply when (x,y) are the same
+ if( (x == rhs.x) &&
+ (y == rhs.y) )
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+
+}
+
+void MapSearchNode::PrintNodeInfo()
+{
+ //cout << x << "\t" << y << endl;
+ cout << "Node position : (" << x << ", " << y << ")" << endl;
+}
+
+// Here's the heuristic function that estimates the distance from a Node
+// to the Goal.
+
+float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
+{
+ float xd = fabs(float(((float)x - (float)nodeGoal.x)));
+ float yd = fabs(float(((float)y - (float)nodeGoal.y)));
+ return xd + yd;
+ //return 1.001 * (xd + yd );
+}
+
+bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal )
+{
+
+ if( (x == nodeGoal.x) &&
+ (y == nodeGoal.y) )
+ {
+ return true;
+ }
+
+ return false;
+}
+
+// This generates the successors to the given Node. It uses a helper function called
+// AddSuccessor to give the successors to the AStar class. The A* specific initialisation
+// is done for each node internally, so here you just set the state information that
+// is specific to the application
+bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node )
+{
+
+ int parent_x = -1;
+ int parent_y = -1;
+
+ if( parent_node )
+ {
+ parent_x = parent_node->x;
+ parent_y = parent_node->y;
+ }
+
+
+ MapSearchNode NewNode;
+
+ // push each possible move except allowing the search to go backwards
+
+ if( (GetMap( x-1, y ) < 9)
+ && !((parent_x == x-1) && (parent_y == y))
+ )
+ {
+ NewNode = MapSearchNode( x-1, y );
+ astarsearch->AddSuccessor( NewNode );
+ }
+
+ if( (GetMap( x, y-1 ) < 9)
+ && !((parent_x == x) && (parent_y == y-1))
+ )
+ {
+ NewNode = MapSearchNode( x, y-1 );
+ astarsearch->AddSuccessor( NewNode );
+ }
+
+ if( (GetMap( x+1, y ) < 9)
+ && !((parent_x == x+1) && (parent_y == y))
+ )
+ {
+ NewNode = MapSearchNode( x+1, y );
+ astarsearch->AddSuccessor( NewNode );
+ }
+
+
+ if( (GetMap( x, y+1 ) < 9)
+ && !((parent_x == x) && (parent_y == y+1))
+ )
+ {
+ NewNode = MapSearchNode( x, y+1 );
+ astarsearch->AddSuccessor( NewNode );
+ }
+
+ return true;
+}
+
+// given this node, what does it cost to move to successor. In the case
+// of our map the answer is the map terrain value at this node since that is
+// conceptually where we're moving
+
+float MapSearchNode::GetCost( MapSearchNode &successor )
+{
+ return (float) GetMap( x, y );
+
+}
+
+
+// Main
+
+#include "astar.h"
+
+bool astar( uint8_t* map,
+ unsigned int width,
+ unsigned int height,
+ const point_t start,
+ const point_t goal,
+ std::vector<point_t>& path )
+{
+ //cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";
+
+ // set the static vars
+ _map = map;
+ _map_width = width;
+ _map_height = height;
+
+ // Our sample problem defines the world as a 2d array representing a terrain
+ // Each element contains an integer from 0 to 5 which indicates the cost
+ // of travel across the terrain. Zero means the least possible difficulty
+ // in travelling (think ice rink if you can skate) whilst 5 represents the
+ // most difficult. 9 indicates that we cannot pass.
+
+ // Create an instance of the search class...
+
+ AStarSearch<MapSearchNode> astarsearch;
+
+ unsigned int SearchCount = 0;
+
+ const unsigned int NumSearches = 1;
+
+ bool path_found = false;
+
+ while(SearchCount < NumSearches)
+ {
+
+ // Create a start state
+ MapSearchNode nodeStart;
+ nodeStart.x = start.x;
+ nodeStart.y = start.y;
+
+ // Define the goal state
+ MapSearchNode nodeEnd;
+ nodeEnd.x = goal.x;
+ nodeEnd.y = goal.y;
+
+ // Set Start and goal states
+
+ astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
+
+ unsigned int SearchState;
+ unsigned int SearchSteps = 0;
+
+ do
+ {
+ SearchState = astarsearch.SearchStep();
+
+ SearchSteps++;
+
+ #if DEBUG_LISTS
+
+ cout << "Steps:" << SearchSteps << "\n";
+
+ int len = 0;
+
+ cout << "Open:\n";
+ MapSearchNode *p = astarsearch.GetOpenListStart();
+ while( p )
+ {
+ len++;
+ #if !DEBUG_LIST_LENGTHS_ONLY
+ ((MapSearchNode *)p)->PrintNodeInfo();
+ #endif
+ p = astarsearch.GetOpenListNext();
+
+ }
+
+ cout << "Open list has " << len << " nodes\n";
+
+ len = 0;
+
+ cout << "Closed:\n";
+ p = astarsearch.GetClosedListStart();
+ while( p )
+ {
+ len++;
+ #if !DEBUG_LIST_LENGTHS_ONLY
+ p->PrintNodeInfo();
+ #endif
+ p = astarsearch.GetClosedListNext();
+ }
+
+ cout << "Closed list has " << len << " nodes\n";
+ #endif
+
+ }
+ while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );
+
+ if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
+ {
+ //cout << "Search found goal state\n";
+
+ MapSearchNode *node = astarsearch.GetSolutionStart();
+
+#if DISPLAY_SOLUTION
+ cout << "Displaying solution\n";
+#endif
+ int steps = 0;
+
+ //node->PrintNodeInfo();
+
+ path.push_back( point_t( node->x, node->y ) );
+
+ for( ;; )
+ {
+ node = astarsearch.GetSolutionNext();
+
+ if( !node )
+ {
+ break;
+ }
+
+ //node->PrintNodeInfo();
+
+ path.push_back( point_t( node->x, node->y ) );
+
+ steps ++;
+
+ };
+
+ //cout << "Solution steps " << steps << endl;
+
+ // Once you're done with the solution you can free the nodes up
+ astarsearch.FreeSolutionNodes();
+
+ path_found = true;
+
+ }
+ else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED )
+ {
+ cout << "Search terminated. Did not find goal state\n";
+
+ }
+
+ // Display the number of loops the search went through
+ //cout << "SearchSteps : " << SearchSteps << "\n";
+
+ SearchCount ++;
+
+ astarsearch.EnsureMemoryFreed();
+ }
+
+ return path_found;
+}
+
+// // STL container iterator macros
+// #define VAR(V,init) __typeof(init) V=(init)
+// #define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
+
+// int main( int argc, char *argv[] )
+// {
+// std::vector<point_t> path;
+
+// bool result = astar( map, 30, 30, point_t( 1,1 ), point_t( 25,20 ), path );
+
+// printf( "#%s:\n", result ? "PATH" : "NOPATH" );
+
+// FOR_EACH( it, path )
+// printf( "%d, %d\n", it->x, it->y );
+// puts("");
+// }
+
Oops, something went wrong.

0 comments on commit 7e37217

Please sign in to comment.