Skip to content
This repository
Browse code

working on much more elaborate fasr2 demo

  • Loading branch information...
commit 7e37217768346030f047fd1a78a54cbbcbbf31cd 1 parent 6484adb
authored February 19, 2010
BIN  assets/blue.png
BIN  assets/green.png
BIN  assets/red.png
12  examples/ctrl/CMakeLists.txt
@@ -23,9 +23,18 @@ set_source_files_properties( ${PLUGINS} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}
23 23
 
24 24
 foreach( PLUGIN ${PLUGINS} )
25 25
   ADD_LIBRARY( ${PLUGIN} MODULE ${PLUGIN}.cc )
  26
+endforeach( PLUGIN )
  27
+				
  28
+ADD_LIBRARY( fasr2 MODULE fasr2.cc astar/findpath.cpp )
  29
+
  30
+# add fasr2 to the list of plugins
  31
+SET( PLUGINS ${PLUGINS} fasr2 )
  32
+
  33
+foreach( PLUGIN ${PLUGINS} )
26 34
   TARGET_LINK_LIBRARIES( ${PLUGIN} stage )
27 35
 endforeach( PLUGIN )
28 36
 
  37
+
29 38
 # delete the "lib" prefix from the plugin libraries
30 39
 SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" )
31 40
 
@@ -37,6 +46,7 @@ SET_TARGET_PROPERTIES( ${PLUGINS} PROPERTIES PREFIX "" )
37 46
 #endif( PLAYER_FOUND )
38 47
 
39 48
 
  49
+
40 50
 # install in <prefix>/lib
41  
-install( TARGETS ${PLUGINS} DESTINATION lib)
  51
+install( TARGETS ${PLUGINS} fasr2 DESTINATION lib)
42 52
 
16  examples/ctrl/astar/astar.h
... ...
@@ -0,0 +1,16 @@
  1
+
  2
+class point_t 
  3
+{ 
  4
+public:
  5
+  unsigned int x, y; 
  6
+
  7
+  point_t( unsigned int x, unsigned int y ) : x(x), y(y) {}
  8
+};
  9
+
  10
+bool astar( uint8_t* map, 
  11
+				unsigned int width, 
  12
+				unsigned int height,
  13
+				const point_t start, 
  14
+				const point_t goal, 
  15
+				std::vector<point_t>& path );
  16
+
350  examples/ctrl/astar/findpath.cpp
... ...
@@ -0,0 +1,350 @@
  1
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  2
+
  3
+// STL A* Search implementation
  4
+// (C)2001 Justin Heyes-Jones
  5
+//
  6
+// Finding a path on a simple grid maze
  7
+// This shows how to do shortest path finding using A*
  8
+
  9
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  10
+
  11
+#include "stlastar.h" // See header for copyright and usage information
  12
+
  13
+#include <iostream>
  14
+#include <math.h>
  15
+
  16
+#define DEBUG_LISTS 0
  17
+#define DEBUG_LIST_LENGTHS_ONLY 0
  18
+
  19
+using namespace std;
  20
+
  21
+
  22
+// map helper functions
  23
+
  24
+static uint8_t* _map = NULL;
  25
+static unsigned int _map_width=0, _map_height=0;
  26
+
  27
+uint8_t GetMap( unsigned int x, unsigned int y )
  28
+{
  29
+  assert(_map);
  30
+  
  31
+  if( x < 0 ||
  32
+		x >= _map_width ||
  33
+		y < 0 ||
  34
+		y >= _map_height
  35
+		)
  36
+	 {
  37
+		return 9;	 
  38
+	 }
  39
+  
  40
+	return _map[(y*_map_width)+x];
  41
+}
  42
+
  43
+// Definitions
  44
+
  45
+class MapSearchNode
  46
+{
  47
+public:
  48
+	unsigned int x;	 // the (x,y) positions of the node
  49
+	unsigned int y;	
  50
+	
  51
+	MapSearchNode() { x = y = 0; }
  52
+	MapSearchNode( unsigned int px, unsigned int py ) { x=px; y=py; }
  53
+
  54
+	float GoalDistanceEstimate( MapSearchNode &nodeGoal );
  55
+	bool IsGoal( MapSearchNode &nodeGoal );
  56
+	bool GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node );
  57
+	float GetCost( MapSearchNode &successor );
  58
+	bool IsSameState( MapSearchNode &rhs );
  59
+
  60
+	void PrintNodeInfo(); 
  61
+
  62
+
  63
+};
  64
+
  65
+bool MapSearchNode::IsSameState( MapSearchNode &rhs )
  66
+{
  67
+
  68
+	// same state in a maze search is simply when (x,y) are the same
  69
+	if( (x == rhs.x) &&
  70
+		(y == rhs.y) )
  71
+	{
  72
+		return true;
  73
+	}
  74
+	else
  75
+	{
  76
+		return false;
  77
+	}
  78
+
  79
+}
  80
+
  81
+void MapSearchNode::PrintNodeInfo()
  82
+{
  83
+  //cout << x << "\t" << y << endl;
  84
+  cout << "Node position : (" << x << ", " << y << ")" << endl;
  85
+}
  86
+
  87
+// Here's the heuristic function that estimates the distance from a Node
  88
+// to the Goal. 
  89
+
  90
+float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
  91
+{
  92
+  float xd = fabs(float(((float)x - (float)nodeGoal.x)));
  93
+  float yd = fabs(float(((float)y - (float)nodeGoal.y)));
  94
+  return xd + yd;	
  95
+  //return 1.001 * (xd + yd );
  96
+}
  97
+
  98
+bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal )
  99
+{
  100
+
  101
+	if( (x == nodeGoal.x) &&
  102
+		(y == nodeGoal.y) )
  103
+	{
  104
+		return true;
  105
+	}
  106
+
  107
+	return false;
  108
+}
  109
+
  110
+// This generates the successors to the given Node. It uses a helper function called
  111
+// AddSuccessor to give the successors to the AStar class. The A* specific initialisation
  112
+// is done for each node internally, so here you just set the state information that
  113
+// is specific to the application
  114
+bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node )
  115
+{
  116
+
  117
+	int parent_x = -1; 
  118
+	int parent_y = -1; 
  119
+
  120
+	if( parent_node )
  121
+	{
  122
+		parent_x = parent_node->x;
  123
+		parent_y = parent_node->y;
  124
+	}
  125
+	
  126
+
  127
+	MapSearchNode NewNode;
  128
+
  129
+	// push each possible move except allowing the search to go backwards
  130
+
  131
+	if( (GetMap( x-1, y ) < 9) 
  132
+		&& !((parent_x == x-1) && (parent_y == y))
  133
+	  ) 
  134
+	{
  135
+		NewNode = MapSearchNode( x-1, y );
  136
+		astarsearch->AddSuccessor( NewNode );
  137
+	}	
  138
+
  139
+	if( (GetMap( x, y-1 ) < 9) 
  140
+		&& !((parent_x == x) && (parent_y == y-1))
  141
+	  ) 
  142
+	{
  143
+		NewNode = MapSearchNode( x, y-1 );
  144
+		astarsearch->AddSuccessor( NewNode );
  145
+	}	
  146
+
  147
+	if( (GetMap( x+1, y ) < 9)
  148
+		&& !((parent_x == x+1) && (parent_y == y))
  149
+	  ) 
  150
+	{
  151
+		NewNode = MapSearchNode( x+1, y );
  152
+		astarsearch->AddSuccessor( NewNode );
  153
+	}	
  154
+
  155
+		
  156
+	if( (GetMap( x, y+1 ) < 9) 
  157
+		&& !((parent_x == x) && (parent_y == y+1))
  158
+		)
  159
+	{
  160
+		NewNode = MapSearchNode( x, y+1 );
  161
+		astarsearch->AddSuccessor( NewNode );
  162
+	}	
  163
+
  164
+	return true;
  165
+}
  166
+
  167
+// given this node, what does it cost to move to successor. In the case
  168
+// of our map the answer is the map terrain value at this node since that is 
  169
+// conceptually where we're moving
  170
+
  171
+float MapSearchNode::GetCost( MapSearchNode &successor )
  172
+{
  173
+	return (float) GetMap( x, y );
  174
+
  175
+}
  176
+
  177
+
  178
+// Main
  179
+
  180
+#include "astar.h"
  181
+
  182
+bool astar( uint8_t* map, 
  183
+				unsigned int width, 
  184
+				unsigned int height,
  185
+				const point_t start, 
  186
+				const point_t goal, 
  187
+				std::vector<point_t>& path )
  188
+{
  189
+  //cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";
  190
+
  191
+	// set the static vars
  192
+	_map = map;
  193
+	_map_width = width;
  194
+	_map_height = height;
  195
+
  196
+	// Our sample problem defines the world as a 2d array representing a terrain
  197
+	// Each element contains an integer from 0 to 5 which indicates the cost 
  198
+	// of travel across the terrain. Zero means the least possible difficulty 
  199
+	// in travelling (think ice rink if you can skate) whilst 5 represents the 
  200
+	// most difficult. 9 indicates that we cannot pass.
  201
+
  202
+	// Create an instance of the search class...
  203
+
  204
+	AStarSearch<MapSearchNode> astarsearch;
  205
+
  206
+	unsigned int SearchCount = 0;
  207
+
  208
+	const unsigned int NumSearches = 1;
  209
+	
  210
+	bool path_found = false;
  211
+
  212
+	while(SearchCount < NumSearches)
  213
+	{
  214
+	  
  215
+		// Create a start state
  216
+		MapSearchNode nodeStart;
  217
+		nodeStart.x = start.x;
  218
+		nodeStart.y = start.y;
  219
+
  220
+		// Define the goal state
  221
+		MapSearchNode nodeEnd;
  222
+		nodeEnd.x = goal.x;
  223
+		nodeEnd.y = goal.y;
  224
+		
  225
+		// Set Start and goal states
  226
+		
  227
+		astarsearch.SetStartAndGoalStates( nodeStart, nodeEnd );
  228
+
  229
+		unsigned int SearchState;
  230
+		unsigned int SearchSteps = 0;
  231
+
  232
+		do
  233
+		{
  234
+			SearchState = astarsearch.SearchStep();
  235
+
  236
+			SearchSteps++;
  237
+
  238
+	#if DEBUG_LISTS
  239
+
  240
+			cout << "Steps:" << SearchSteps << "\n";
  241
+
  242
+			int len = 0;
  243
+
  244
+			cout << "Open:\n";
  245
+			MapSearchNode *p = astarsearch.GetOpenListStart();
  246
+			while( p )
  247
+			{
  248
+				len++;
  249
+	#if !DEBUG_LIST_LENGTHS_ONLY			
  250
+				((MapSearchNode *)p)->PrintNodeInfo();
  251
+	#endif
  252
+				p = astarsearch.GetOpenListNext();
  253
+				
  254
+			}
  255
+
  256
+			cout << "Open list has " << len << " nodes\n";
  257
+
  258
+			len = 0;
  259
+
  260
+			cout << "Closed:\n";
  261
+			p = astarsearch.GetClosedListStart();
  262
+			while( p )
  263
+			{
  264
+				len++;
  265
+	#if !DEBUG_LIST_LENGTHS_ONLY			
  266
+				p->PrintNodeInfo();
  267
+	#endif			
  268
+				p = astarsearch.GetClosedListNext();
  269
+			}
  270
+
  271
+			cout << "Closed list has " << len << " nodes\n";
  272
+	#endif
  273
+
  274
+		}
  275
+		while( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SEARCHING );
  276
+
  277
+		if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
  278
+		{
  279
+		  //cout << "Search found goal state\n";
  280
+
  281
+				MapSearchNode *node = astarsearch.GetSolutionStart();
  282
+				
  283
+#if DISPLAY_SOLUTION
  284
+				cout << "Displaying solution\n";
  285
+#endif
  286
+				int steps = 0;
  287
+				
  288
+				//node->PrintNodeInfo();
  289
+				
  290
+				path.push_back( point_t( node->x, node->y ) );
  291
+				
  292
+				for( ;; )
  293
+				  {
  294
+					 node = astarsearch.GetSolutionNext();
  295
+					 
  296
+					 if( !node )
  297
+						{
  298
+						  break;
  299
+						}
  300
+					 
  301
+					 //node->PrintNodeInfo();
  302
+					 
  303
+					 path.push_back( point_t( node->x, node->y ) );
  304
+					 
  305
+					 steps ++;
  306
+					 
  307
+				  };
  308
+				
  309
+				//cout << "Solution steps " << steps << endl;
  310
+				
  311
+				// Once you're done with the solution you can free the nodes up
  312
+				astarsearch.FreeSolutionNodes();
  313
+				
  314
+				path_found = true;
  315
+				
  316
+		}
  317
+		else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED ) 
  318
+		  {
  319
+			 cout << "Search terminated. Did not find goal state\n";
  320
+			 
  321
+		  }
  322
+		
  323
+		// Display the number of loops the search went through
  324
+		//cout << "SearchSteps : " << SearchSteps << "\n";
  325
+		
  326
+		SearchCount ++;
  327
+		
  328
+		astarsearch.EnsureMemoryFreed();
  329
+	}
  330
+	
  331
+	return path_found;  
  332
+}
  333
+
  334
+//   // STL container iterator macros
  335
+// #define VAR(V,init) __typeof(init) V=(init)
  336
+// #define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
  337
+
  338
+// int main( int argc, char *argv[] )
  339
+// {
  340
+//   std::vector<point_t> path;
  341
+
  342
+//   bool result = astar( map, 30, 30, point_t( 1,1 ), point_t( 25,20 ), path );
  343
+
  344
+//   printf( "#%s:\n", result ? "PATH" : "NOPATH" );
  345
+  
  346
+//   FOR_EACH( it, path )
  347
+// 	 printf( "%d, %d\n", it->x, it->y );
  348
+//   puts("");
  349
+// }
  350
+
250  examples/ctrl/astar/fsa.h
... ...
@@ -0,0 +1,250 @@
  1
+/* 
  2
+
  3
+A* Algorithm Implementation using STL is
  4
+Copyright (C)2001-2005 Justin Heyes-Jones
  5
+
  6
+Permission is given by the author to freely redistribute and 
  7
+include this code in any program as long as this credit is 
  8
+given where due.
  9
+ 
  10
+  COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, 
  11
+  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, 
  12
+  INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE 
  13
+  IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
  14
+  OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND 
  15
+  PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED 
  16
+  CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL 
  17
+  DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY 
  18
+  NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF 
  19
+  WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE 
  20
+  OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
  21
+  THIS DISCLAIMER.
  22
+ 
  23
+  Use at your own risk!
  24
+
  25
+
  26
+
  27
+  FixedSizeAllocator class
  28
+  Copyright 2001 Justin Heyes-Jones
  29
+
  30
+  This class is a constant time O(1) memory manager for objects of 
  31
+  a specified type. The type is specified using a template class.
  32
+
  33
+  Memory is allocated from a fixed size buffer which you can specify in the
  34
+  class constructor or use the default.
  35
+
  36
+  Using GetFirst and GetNext it is possible to iterate through the elements
  37
+  one by one, and this would be the most common use for the class. 
  38
+
  39
+  I would suggest using this class when you want O(1) add and delete
  40
+  and you don't do much searching, which would be O(n). Structures such as binary 
  41
+  trees can be used instead to get O(logn) access time.
  42
+
  43
+*/
  44
+
  45
+#ifndef FSA_H
  46
+#define FSA_H
  47
+
  48
+#include <string.h>
  49
+#include <stdio.h>
  50
+
  51
+template <class USER_TYPE> class FixedSizeAllocator
  52
+{
  53
+
  54
+public: 
  55
+	// Constants
  56
+	enum
  57
+	{
  58
+		FSA_DEFAULT_SIZE = 100
  59
+	};
  60
+
  61
+	// This class enables us to transparently manage the extra data 
  62
+	// needed to enable the user class to form part of the double-linked
  63
+	// list class
  64
+	struct FSA_ELEMENT
  65
+	{
  66
+		USER_TYPE UserType;
  67
+		
  68
+		FSA_ELEMENT *pPrev;
  69
+		FSA_ELEMENT *pNext;
  70
+	};
  71
+
  72
+public: // methods
  73
+	FixedSizeAllocator( unsigned int MaxElements = FSA_DEFAULT_SIZE ) :
  74
+	m_MaxElements( MaxElements ),
  75
+	m_pFirstUsed( NULL )
  76
+	{
  77
+		// Allocate enough memory for the maximum number of elements
  78
+
  79
+		m_pMemory = (FSA_ELEMENT *) (new char[ m_MaxElements * sizeof(FSA_ELEMENT) ]); 
  80
+
  81
+		// Set the free list first pointer
  82
+		m_pFirstFree = m_pMemory;
  83
+
  84
+		// Clear the memory
  85
+		memset( m_pMemory, 0, sizeof( FSA_ELEMENT ) * m_MaxElements );
  86
+
  87
+		// Point at first element
  88
+		FSA_ELEMENT *pElement = m_pFirstFree;
  89
+
  90
+		// Set the double linked free list
  91
+		for( unsigned int i=0; i<m_MaxElements; i++ )
  92
+		{
  93
+			pElement->pPrev = pElement-1;
  94
+			pElement->pNext = pElement+1;
  95
+
  96
+			pElement++;
  97
+		}
  98
+
  99
+		// first element should have a null prev
  100
+		m_pFirstFree->pPrev = NULL;
  101
+		// last element should have a null next
  102
+		(pElement-1)->pNext = NULL;
  103
+
  104
+	}
  105
+
  106
+
  107
+	~FixedSizeAllocator()
  108
+	{
  109
+		// Free up the memory
  110
+		delete [] (char *) m_pMemory;
  111
+	}
  112
+
  113
+	// Allocate a new USER_TYPE and return a pointer to it 
  114
+	USER_TYPE *alloc()
  115
+	{
  116
+
  117
+		FSA_ELEMENT *pNewNode = NULL;
  118
+
  119
+		if( !m_pFirstFree )
  120
+		{
  121
+			return NULL;
  122
+		}
  123
+		else
  124
+		{
  125
+			pNewNode = m_pFirstFree;
  126
+			m_pFirstFree = pNewNode->pNext;
  127
+
  128
+			// if the new node points to another free node then
  129
+			// change that nodes prev free pointer...
  130
+			if( pNewNode->pNext )
  131
+			{
  132
+				pNewNode->pNext->pPrev = NULL;
  133
+			}
  134
+
  135
+			// node is now on the used list
  136
+
  137
+			pNewNode->pPrev = NULL; // the allocated node is always first in the list
  138
+
  139
+			if( m_pFirstUsed == NULL )
  140
+			{
  141
+				pNewNode->pNext = NULL; // no other nodes
  142
+			}
  143
+			else
  144
+			{
  145
+				m_pFirstUsed->pPrev = pNewNode; // insert this at the head of the used list
  146
+				pNewNode->pNext = m_pFirstUsed;
  147
+			}
  148
+
  149
+			m_pFirstUsed = pNewNode;
  150
+		}	
  151
+		
  152
+		return reinterpret_cast<USER_TYPE*>(pNewNode);
  153
+	}
  154
+
  155
+	// Free the given user type
  156
+	// For efficiency I don't check whether the user_data is a valid
  157
+	// pointer that was allocated. I may add some debug only checking
  158
+	// (To add the debug check you'd need to make sure the pointer is in 
  159
+	// the m_pMemory area and is pointing at the start of a node)
  160
+	void free( USER_TYPE *user_data )
  161
+	{
  162
+		FSA_ELEMENT *pNode = reinterpret_cast<FSA_ELEMENT*>(user_data);
  163
+
  164
+		// manage used list, remove this node from it
  165
+		if( pNode->pPrev )
  166
+		{
  167
+			pNode->pPrev->pNext = pNode->pNext;
  168
+		}
  169
+		else
  170
+		{
  171
+			// this handles the case that we delete the first node in the used list
  172
+			m_pFirstUsed = pNode->pNext;
  173
+		}
  174
+
  175
+		if( pNode->pNext )
  176
+		{
  177
+			pNode->pNext->pPrev = pNode->pPrev;
  178
+		}
  179
+
  180
+		// add to free list
  181
+		if( m_pFirstFree == NULL ) 
  182
+		{
  183
+			// free list was empty
  184
+			m_pFirstFree = pNode;
  185
+			pNode->pPrev = NULL;
  186
+			pNode->pNext = NULL;
  187
+		}
  188
+		else
  189
+		{
  190
+			// Add this node at the start of the free list
  191
+			m_pFirstFree->pPrev = pNode;
  192
+			pNode->pNext = m_pFirstFree;
  193
+			m_pFirstFree = pNode;
  194
+		}
  195
+
  196
+	}
  197
+
  198
+	// For debugging this displays both lists (using the prev/next list pointers)
  199
+	void Debug()
  200
+	{
  201
+		printf( "free list " );
  202
+
  203
+		FSA_ELEMENT *p = m_pFirstFree;
  204
+		while( p )
  205
+		{
  206
+			printf( "%x!%x ", p->pPrev, p->pNext );
  207
+			p = p->pNext;
  208
+		}
  209
+		printf( "\n" );
  210
+
  211
+		printf( "used list " );
  212
+
  213
+		p = m_pFirstUsed;
  214
+		while( p )
  215
+		{
  216
+			printf( "%x!%x ", p->pPrev, p->pNext );
  217
+			p = p->pNext;
  218
+		}
  219
+		printf( "\n" );
  220
+	}
  221
+
  222
+	// Iterators
  223
+
  224
+	USER_TYPE *GetFirst()
  225
+	{
  226
+		return reinterpret_cast<USER_TYPE *>(m_pFirstUsed);
  227
+	}
  228
+
  229
+	USER_TYPE *GetNext( USER_TYPE *node )
  230
+	{
  231
+		return reinterpret_cast<USER_TYPE *>
  232
+			(
  233
+				(reinterpret_cast<FSA_ELEMENT *>(node))->pNext
  234
+			);
  235
+	}
  236
+
  237
+public: // data
  238
+
  239
+private: // methods
  240
+
  241
+private: // data
  242
+
  243
+	FSA_ELEMENT *m_pFirstFree;
  244
+	FSA_ELEMENT *m_pFirstUsed;
  245
+	unsigned int m_MaxElements;
  246
+	FSA_ELEMENT *m_pMemory;
  247
+
  248
+};
  249
+
  250
+#endif // defined FSA_H
20  examples/ctrl/astar/license.txt
... ...
@@ -0,0 +1,20 @@
  1
+Copyright (c) 2006 Justin Heyes-Jones
  2
+
  3
+Permission is hereby granted, free of charge, to any person obtaining
  4
+a copy of this software and associated documentation files (the "Software"), 
  5
+to deal in the Software without restriction, including without limitation
  6
+ the rights to use, copy, modify, merge, publish, distribute, sublicense, 
  7
+and or sell copies of the Software, and to permit persons to whom the 
  8
+Software is furnished to do so, subject to the following conditions:
  9
+
  10
+The above copyright notice and this permission notice shall be included 
  11
+in all copies or substantial portions of the Software.
  12
+
  13
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  14
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  15
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
  16
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
  17
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
  18
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
  19
+DEALINGS IN THE SOFTWARE.
  20
+
145  examples/ctrl/astar/readme.txt
... ...
@@ -0,0 +1,145 @@
  1
+**********************************
  2
+A* Algorithm by Justin Heyes-Jones
  3
+**********************************
  4
+
  5
+***********
  6
+Description
  7
+***********
  8
+
  9
+This implementation is intended to be simple to read yet fairly
  10
+efficient. To build it you can compile, with any recent C++ compiler,
  11
+the following files :
  12
+
  13
+For 8-puzzle solver
  14
+
  15
+	8puzzle.cpp
  16
+	stlastar.h
  17
+	optionally fsa.h
  18
+
  19
+Command line 
  20
+
  21
+	8puzzle with no arguments runs with one of the boards in the cpp file, you can
  22
+	select the one you want changing the conditional compiliation instructions. Or if you
  23
+	prefer pass in a board on the command line using digits for the tile positions, where
  24
+	zero is the space. The board runs from left to right, each row at a time:
  25
+	
  26
+		8puzzle 013824765
  27
+
  28
+For path finder 
  29
+
  30
+	findpath.cpp
  31
+	stlastar.h
  32
+	optionally fsa.h
  33
+
  34
+	pathfind has no arguments. You can edit the simple map in pathfind.cpp and the start 
  35
+	and goal co-ordinates to experiement with the pathfinder.
  36
+
  37
+Fixed size allocator notes: As mentioned briefly in the tutorial you can enable and disable the
  38
+faster memory allocation. This allocates a fixed size block of memory, so you have to specify this size
  39
+with the astar constructor. You need to enlarge it if you hit an out of memory assert during the
  40
+search.
  41
+
  42
+Compilation notes:
  43
+
  44
+Microsoft Visual C++ : Confirmed working with version 8.0.50727 with some deprecation warnings
  45
+I'm going to leave the deprecation warnings in so that it still works cleanly with GCC. 
  46
+TODO Make a non-deprecated compliant version using compiler checking
  47
+
  48
+GCC notes : Compiled using version 3.4.5 (MingW)
  49
+
  50
+Please let me know if it doesn't work for you and I will try to help. I cannot help if you are using
  51
+an old compiler such as Turbo C++, since I update the code to meet Ansi Standard C++ as required.
  52
+
  53
+At least in as far as the Microsoft and GCC compilers adhere to Ansi and add breaking changes.
  54
+
  55
+History:
  56
+
  57
+Updated 1th February 2009
  58
+**********************************
  59
+
  60
+Fixed Manhattan distance bug. Should use absolute values.
  61
+
  62
+Got rid of sprintfs, use cout instead.
  63
+
  64
+Updated 3rd August 2006
  65
+**********************************
  66
+
  67
+Fixed memory leak
  68
+Fixed special case handling for finding better path to a closed node
  69
+Fixed bug with comparing the start node with a new node by pointer instead of value
  70
+Changed code to use Manhattan distance heuristic with pathfind.cpp as it is more correct
  71
+
  72
+
  73
+Updated 27th September 2005
  74
+**********************************
  75
+
  76
+Thanks to Gyan singh for pointing out the code no longer compiles under GCC.
  77
+
  78
+Well, that was the case. I've removed a Gnu offending conio.h include, and added lots more typename
  79
+keywords, which seem to be required now when making an iterator using a template type.
  80
+
  81
+If anyone knows what the breaking change to the compiler was for, let me know.
  82
+
  83
+Updated 6th September 2005
  84
+**********************************
  85
+
  86
+	Finally set the path to fsa.h correctly, sorry about that.
  87
+	8puzzle.cpp now defaults to using a demo puzzle that solves in a short time.
  88
+	Added typename keyword to comply with latest ISO standards. 
  89
+
  90
+Updated November 26th 2001
  91
+**********************************
  92
+
  93
+	Fixed a bug. When a node is deleted from the Open list I did sort_heap when make_heap
  94
+	is needed to keep the heap structure valid. This causes the search to go awry under some
  95
+	conditions. Thanks to Mike Ryynanen for tracking this down.
  96
+
  97
+justinhj@hotmail.com
  98
+
  99
+http://www.geocities.com/jheyesjones/astar.html
  100
+
  101
+or
  102
+
  103
+http://www.heyes-jones.com/astar.html
  104
+
  105
+A* Algorithm Implementation using STL is
  106
+Copyright (C)2001-2005 Justin Heyes-Jones
  107
+
  108
+Permission is given by the author to freely redistribute and 
  109
+include this code in any program as long as this credit is 
  110
+given where due.
  111
+ 
  112
+  COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, 
  113
+  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, 
  114
+  INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE 
  115
+  IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
  116
+  OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND 
  117
+  PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED 
  118
+  CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL 
  119
+  DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY 
  120
+  NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF 
  121
+  WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE 
  122
+  OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
  123
+  THIS DISCLAIMER.
  124
+ 
  125
+  Use at your own risk!
  126
+
  127
+
  128
+
  129
+
  130
+
  131
+
  132
+
  133
+
  134
+
  135
+
  136
+
  137
+
  138
+
  139
+
  140
+
  141
+
  142
+
  143
+
  144
+
  145
+
754  examples/ctrl/astar/stlastar.h
... ...
@@ -0,0 +1,754 @@
  1
+/*
  2
+A* Algorithm Implementation using STL is
  3
+Copyright (C)2001-2005 Justin Heyes-Jones
  4
+
  5
+Permission is given by the author to freely redistribute and 
  6
+include this code in any program as long as this credit is 
  7
+given where due.
  8
+ 
  9
+  COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, 
  10
+  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, 
  11
+  INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE 
  12
+  IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE
  13
+  OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND 
  14
+  PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED 
  15
+  CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL 
  16
+  DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY 
  17
+  NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF 
  18
+  WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE 
  19
+  OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER
  20
+  THIS DISCLAIMER.
  21
+ 
  22
+  Use at your own risk!
  23
+
  24
+*/
  25
+
  26
+// used for text debugging
  27
+#include <iostream>
  28
+#include <stdio.h>
  29
+//#include <conio.h>
  30
+#include <assert.h>
  31
+
  32
+// stl includes
  33
+#include <algorithm>
  34
+#include <set>
  35
+#include <vector>
  36
+
  37
+using namespace std;
  38
+
  39
+// fast fixed size memory allocator, used for fast node memory management
  40
+#include "fsa.h"
  41
+
  42
+// Fixed size memory allocator can be disabled to compare performance
  43
+// Uses std new and delete instead if you turn it off
  44
+#define USE_FSA_MEMORY 1
  45
+
  46
+// disable warning that debugging information has lines that are truncated
  47
+// occurs in stl headers
  48
+#pragma warning( disable : 4786 )
  49
+
  50
+// The AStar search class. UserState is the users state space type
  51
+template <class UserState> class AStarSearch
  52
+{
  53
+
  54
+public: // data
  55
+
  56
+	enum
  57
+	{
  58
+		SEARCH_STATE_NOT_INITIALISED,
  59
+		SEARCH_STATE_SEARCHING,
  60
+		SEARCH_STATE_SUCCEEDED,
  61
+		SEARCH_STATE_FAILED,
  62
+		SEARCH_STATE_OUT_OF_MEMORY,
  63
+		SEARCH_STATE_INVALID
  64
+	};
  65
+
  66
+
  67
+	// A node represents a possible state in the search
  68
+	// The user provided state type is included inside this type
  69
+
  70
+	public:
  71
+
  72
+	class Node
  73
+	{
  74
+		public:
  75
+
  76
+			Node *parent; // used during the search to record the parent of successor nodes
  77
+			Node *child; // used after the search for the application to view the search in reverse
  78
+			
  79
+			float g; // cost of this node + it's predecessors
  80
+			float h; // heuristic estimate of distance to goal
  81
+			float f; // sum of cumulative cost of predecessors and self and heuristic
  82
+
  83
+			Node() :
  84
+				parent( 0 ),
  85
+				child( 0 ),
  86
+				g( 0.0f ),
  87
+				h( 0.0f ),
  88
+				f( 0.0f )
  89
+			{			
  90
+			}
  91
+
  92
+			UserState m_UserState;
  93
+	};
  94
+
  95
+
  96
+	// For sorting the heap the STL needs compare function that lets us compare
  97
+	// the f value of two nodes
  98
+
  99
+	class HeapCompare_f 
  100
+	{
  101
+		public:
  102
+
  103
+			bool operator() ( const Node *x, const Node *y ) const
  104
+			{
  105
+				return x->f > y->f;
  106
+			}
  107
+	};
  108
+
  109
+
  110
+public: // methods
  111
+
  112
+
  113
+	// constructor just initialises private data
  114
+	AStarSearch( int MaxNodes = 1000 ) :
  115
+		m_AllocateNodeCount(0),
  116
+#if USE_FSA_MEMORY
  117
+		m_FixedSizeAllocator( MaxNodes ),
  118
+#endif
  119
+		m_State( SEARCH_STATE_NOT_INITIALISED ),
  120
+		m_CurrentSolutionNode( NULL ),
  121
+		m_CancelRequest( false )
  122
+	{
  123
+	}
  124
+
  125
+	// call at any time to cancel the search and free up all the memory
  126
+	void CancelSearch()
  127
+	{
  128
+		m_CancelRequest = true;
  129
+	}
  130
+
  131
+	// Set Start and goal states
  132
+	void SetStartAndGoalStates( UserState &Start, UserState &Goal )
  133
+	{
  134
+		m_CancelRequest = false;
  135
+
  136
+		m_Start = AllocateNode();
  137
+		m_Goal = AllocateNode();
  138
+
  139
+		assert((m_Start != NULL && m_Goal != NULL));
  140
+		
  141
+		m_Start->m_UserState = Start;
  142
+		m_Goal->m_UserState = Goal;
  143
+
  144
+		m_State = SEARCH_STATE_SEARCHING;
  145
+		
  146
+		// Initialise the AStar specific parts of the Start Node
  147
+		// The user only needs fill out the state information
  148
+
  149
+		m_Start->g = 0; 
  150
+		m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
  151
+		m_Start->f = m_Start->g + m_Start->h;
  152
+		m_Start->parent = 0;
  153
+
  154
+		// Push the start node on the Open list
  155
+
  156
+		m_OpenList.push_back( m_Start ); // heap now unsorted
  157
+
  158
+		// Sort back element into heap
  159
+		push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
  160
+
  161
+		// Initialise counter for search steps
  162
+		m_Steps = 0;
  163
+	}
  164
+
  165
+	// Advances search one step 
  166
+	unsigned int SearchStep()
  167
+	{
  168
+		// Firstly break if the user has not initialised the search
  169
+		assert( (m_State > SEARCH_STATE_NOT_INITIALISED) &&
  170
+				(m_State < SEARCH_STATE_INVALID) );
  171
+
  172
+		// Next I want it to be safe to do a searchstep once the search has succeeded...
  173
+		if( (m_State == SEARCH_STATE_SUCCEEDED) ||
  174
+			(m_State == SEARCH_STATE_FAILED) 
  175
+		  )
  176
+		{
  177
+			return m_State; 
  178
+		}
  179
+
  180
+		// Failure is defined as emptying the open list as there is nothing left to 
  181
+		// search...
  182
+		// New: Allow user abort
  183
+		if( m_OpenList.empty() || m_CancelRequest )
  184
+		{
  185
+			FreeAllNodes();
  186
+			m_State = SEARCH_STATE_FAILED;
  187
+			return m_State;
  188
+		}
  189
+		
  190
+		// Incremement step count
  191
+		m_Steps ++;
  192
+
  193
+		// Pop the best node (the one with the lowest f) 
  194
+		Node *n = m_OpenList.front(); // get pointer to the node
  195
+		pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
  196
+		m_OpenList.pop_back();
  197
+
  198
+		// Check for the goal, once we pop that we're done
  199
+		if( n->m_UserState.IsGoal( m_Goal->m_UserState ) )
  200
+		{
  201
+			// The user is going to use the Goal Node he passed in 
  202
+			// so copy the parent pointer of n 
  203
+			m_Goal->parent = n->parent;
  204
+
  205
+			// A special case is that the goal was passed in as the start state
  206
+			// so handle that here
  207
+			if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) )
  208
+			{
  209
+				FreeNode( n );
  210
+
  211
+				// set the child pointers in each node (except Goal which has no child)
  212
+				Node *nodeChild = m_Goal;
  213
+				Node *nodeParent = m_Goal->parent;
  214
+
  215
+				do 
  216
+				{
  217
+					nodeParent->child = nodeChild;
  218
+
  219
+					nodeChild = nodeParent;
  220
+					nodeParent = nodeParent->parent;
  221
+				
  222
+				} 
  223
+				while( nodeChild != m_Start ); // Start is always the first node by definition
  224
+
  225
+			}
  226
+
  227
+			// delete nodes that aren't needed for the solution
  228
+			FreeUnusedNodes();
  229
+
  230
+			m_State = SEARCH_STATE_SUCCEEDED;
  231
+
  232
+			return m_State;
  233
+		}
  234
+		else // not goal
  235
+		{
  236
+
  237
+			// We now need to generate the successors of this node
  238
+			// The user helps us to do this, and we keep the new nodes in
  239
+			// m_Successors ...
  240
+
  241
+			m_Successors.clear(); // empty vector of successor nodes to n
  242
+
  243
+			// User provides this functions and uses AddSuccessor to add each successor of
  244
+			// node 'n' to m_Successors
  245
+			bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL ); 
  246
+
  247
+			if( !ret )
  248
+			{
  249
+
  250
+			    typename vector< Node * >::iterator successor;
  251
+
  252
+				// free the nodes that may previously have been added 
  253
+				for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
  254
+				{
  255
+					FreeNode( (*successor) );
  256
+				}
  257
+
  258
+				m_Successors.clear(); // empty vector of successor nodes to n
  259
+
  260
+				// free up everything else we allocated
  261
+				FreeAllNodes();
  262
+
  263
+				m_State = SEARCH_STATE_OUT_OF_MEMORY;
  264
+				return m_State;
  265
+			}
  266
+			
  267
+			// Now handle each successor to the current node ...
  268
+			for( typename vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
  269
+			{
  270
+
  271
+				// 	The g value for this successor ...
  272
+				float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState );
  273
+
  274
+				// Now we need to find whether the node is on the open or closed lists
  275
+				// If it is but the node that is already on them is better (lower g)
  276
+				// then we can forget about this successor
  277
+
  278
+				// First linear search of open list to find node
  279
+
  280
+				typename vector< Node * >::iterator openlist_result;
  281
+
  282
+				for( openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result ++ )
  283
+				{
  284
+					if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
  285
+					{
  286
+						break;					
  287
+					}
  288
+				}
  289
+
  290
+				if( openlist_result != m_OpenList.end() )
  291
+				{
  292
+
  293
+					// we found this state on open
  294
+
  295
+					if( (*openlist_result)->g <= newg )
  296
+					{
  297
+						FreeNode( (*successor) );
  298
+
  299
+						// the one on Open is cheaper than this one
  300
+						continue;
  301
+					}
  302
+				}
  303
+
  304
+				typename vector< Node * >::iterator closedlist_result;
  305
+
  306
+				for( closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result ++ )
  307
+				{
  308
+					if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
  309
+					{
  310
+						break;					
  311
+					}
  312
+				}
  313
+
  314
+				if( closedlist_result != m_ClosedList.end() )
  315
+				{
  316
+
  317
+					// we found this state on closed
  318
+
  319
+					if( (*closedlist_result)->g <= newg )
  320
+					{
  321
+						// the one on Closed is cheaper than this one
  322
+						FreeNode( (*successor) );
  323
+
  324
+						continue;
  325
+					}
  326
+				}
  327
+
  328
+				// This node is the best node so far with this particular state
  329
+				// so lets keep it and set up its AStar specific data ...
  330
+
  331
+				(*successor)->parent = n;
  332
+				(*successor)->g = newg;
  333
+				(*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
  334
+				(*successor)->f = (*successor)->g + (*successor)->h;
  335
+
  336
+				// Remove successor from closed if it was on it
  337
+
  338
+				if( closedlist_result != m_ClosedList.end() )
  339
+				{
  340
+					// remove it from Closed
  341
+					FreeNode(  (*closedlist_result) ); 
  342
+					m_ClosedList.erase( closedlist_result );
  343
+
  344
+					// Fix thanks to ...
  345
+					// Greg Douglas <gregdouglasmail@gmail.com>
  346
+					// who noticed that this code path was incorrect