astar.cpp

ｍａｔａｌａｂ的算法

		return GetOpenListNext( f,g,h );
	}

	UserState *GetOpenListNext( float &f, float &g, float &h )
	{
		iterDbgOpen++;
		if( iterDbgOpen != m_OpenList.end() )
		{
			f = (*iterDbgOpen)->f;
			g = (*iterDbgOpen)->g;
			h = (*iterDbgOpen)->h;
			return &(*iterDbgOpen)->m_UserState;
		}

		return NULL;
	}

	UserState *GetClosedListStart()
	{
		float f,g,h;
		return GetClosedListStart( f,g,h );
	}

	UserState *GetClosedListStart( float &f, float &g, float &h )
	{
		iterDbgClosed = m_ClosedList.begin();
		if( iterDbgClosed != m_ClosedList.end() )
		{
			f = (*iterDbgClosed)->f;
			g = (*iterDbgClosed)->g;
			h = (*iterDbgClosed)->h;

			return &(*iterDbgClosed)->m_UserState;
		}

		return NULL;
	}

	UserState *GetClosedListNext()
	{
		float f,g,h;
		return GetClosedListNext( f,g,h );
	}

	UserState *GetClosedListNext( float &f, float &g, float &h )
	{
		iterDbgClosed++;
		if( iterDbgClosed != m_ClosedList.end() )
		{
			f = (*iterDbgClosed)->f;
			g = (*iterDbgClosed)->g;
			h = (*iterDbgClosed)->h;

			return &(*iterDbgClosed)->m_UserState;
		}

		return NULL;
	}

	// Get the number of steps

	int GetStepCount() { return m_Steps; }

private: // methods

	// This is called when a search fails or is cancelled to free all used
	// memory 
	void FreeAllNodes()
	{
		// iterate open list and delete all nodes
		vector< Node * >::iterator iterOpen = m_OpenList.begin();

		while( iterOpen != m_OpenList.end() )
		{
			Node *n = (*iterOpen);
			FreeNode( n );

			iterOpen ++;
		}

		m_OpenList.clear();

		// iterate closed list and delete unused nodes
		vector< Node * >::iterator iterClosed;

		for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
		{
			Node *n = (*iterClosed);
			FreeNode( n );
		}

		m_ClosedList.clear();
	}


	// This call is made by the search class when the search ends. A lot of nodes may be
	// created that are still present when the search ends. They will be deleted by this 
	// routine once the search ends
	void FreeUnusedNodes()
	{
		// iterate open list and delete unused nodes
		vector< Node * >::iterator iterOpen = m_OpenList.begin();

		while( iterOpen != m_OpenList.end() )
		{
			Node *n = (*iterOpen);

			if( !n->child )
			{
				FreeNode( n );

				n = NULL;
			}

			iterOpen ++;
		}

		m_OpenList.clear();

		// iterate closed list and delete unused nodes
		vector< Node * >::iterator iterClosed;

		for( iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed ++ )
		{
			Node *n = (*iterClosed);

			if( !n->child )
			{
				FreeNode( n );
				n = NULL;

			}
		}

		m_ClosedList.clear();

	}

	// Node memory management
	Node *AllocateNode()
	{

#if !USE_FSA_MEMORY
		Node *p = new Node;
		return p;
#else
		Node *address = m_FixedSizeAllocator.alloc();

		if( !address )
		{
			return NULL;
		}
		m_AllocateNodeCount ++;
		Node *p = new (address) Node;
		return p;
#endif
	}

	void FreeNode( Node *node )
	{

		m_FreeNodeCount ++;

#if !USE_FSA_MEMORY
		delete node;
#else
		m_FixedSizeAllocator.free( node );
#endif
	}

public: // data

	// Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article)
	vector< Node *> m_OpenList;

	// Closed list is a vector.
	vector< Node * > m_ClosedList; 

	// Successors is a vector filled out by the user each type successors to a node
	// are generated
	vector< Node * > m_Successors;

	// State
	unsigned int m_State;

	// Counts steps
	int m_Steps;

	// Start and goal state pointers
	Node *m_Start;
	Node *m_Goal;

	Node *m_CurrentSolutionNode;

	// Memory
 	FixedSizeAllocator<Node> m_FixedSizeAllocator;
	
	//Debug : need to keep these two iterators around
	// for the user Dbg functions
	vector< Node * >::iterator iterDbgOpen;
	vector< Node * >::iterator iterDbgClosed;

	// debugging : count memory allocation and free's
	int m_AllocateNodeCount;
	int m_FreeNodeCount;

	bool m_CancelRequest;

};

// Definitions

class MapSearchNode
{
public:
	double x;	 // the (x,y) positions of the node
	double y;	
	double id; // the index of the node
	double g; // cumulative cost from s to this node
	
	MapSearchNode() { x = y = 0; id = 0; g = NumofNodes;}
	MapSearchNode( double px, double py,  double pid, double pg) { x=px; y=py; id=pid; g = pg;}

	double GoalDistanceEstimate( MapSearchNode &nodeGoal );
	bool IsGoal( MapSearchNode &nodeGoal );
	bool GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node );
	double 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) && (id == rhs.id))
	{
		return true;
	}
	else
	{
		return false;
	}

}

void MapSearchNode::PrintNodeInfo()
{
	char str[100];
	sprintf( str, "Node position : (%f,%f)\n", x,y );

	cout << str;
}

// Here's the heuristic function that estimates the distance from a Node
// to the Goal. 

double MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal )
{
	double xd = double( ( x - nodeGoal.x ) );
	double yd = double( ( y - nodeGoal.y) );

	return (float)(sqrt((xd*xd) + (yd*yd)));  

}

bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal )
{

	if( (x == nodeGoal.x) &&
		(y == nodeGoal.y) && id == nodeGoal.id )
	{
		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 )
{

	double parent_x = -1; 
	double parent_y = -1; 
	double parent_id = -1;
	int i;

	if( parent_node )
	{
		parent_x = parent_node->x;
		parent_y = parent_node->y;
		parent_id = parent_node->id;
	}
	

	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 );
	}	
*/

	for(i=0; i<NumofNodes; i++){
		if(GetMap(id, i)==1 && (parent_id != i)){
			NewNode = MapSearchNode(GetX(i), GetY(i), i, NumofNodes);
			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

double MapSearchNode::GetCost( MapSearchNode &successor )
{
	//note that, the estimate distance based on the real coordination, so our cost 
	//should also be based on the coordination
	return GoalDistanceEstimate(successor);
	
}


   
void mexFunction(int nOutArray, mxArray *OutArray[], int nInArray,
		 const mxArray *InArray[])
{
	
	double *pI1;
	double *pI2; 
	double *pI3;
	double *pO1, *pO2;
	int size1, size2;
	int i;
	int steps = 0;
	double cost = 0;
	
	//coordination = (float *)&InArray[0];
	//connect = (int *)&InArray[1];
  pI1=mxGetPr(InArray[0]);
  pI2=mxGetPr(InArray[1]);
  pI3=mxGetPr(InArray[2]);
  //pI1 = (float *)InArray[0];
  //pI2 = (int *)InArray[1];
 /*
  size1 = mxGetNumberOfElements(InArray[0]);
  size2 = mxGetNumberOfElements(InArray[1]);
  
  OutArray[0]=mxCreateDoubleMatrix(1,size1, mxREAL);
  pO1=mxGetPr(OutArray[0]);	
  OutArray[1]=mxCreateDoubleMatrix(1,size2, mxREAL);
  pO2=mxGetPr(OutArray[1]);	
  
  for(i=0; i<size1; i++){
  	*pO1 = *pI1;
  	pI1++; pO1++;
  }
  	
  for(i=0; i<size2; i++){
  	*pO2 = *pI2;
  	pI2++; pO2++;
  }
  */
  
  coordination = pI1;
  connect = pI2;
  
  NumofNodes = *pI3;
  
	cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n";

	// 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;

	// Create a start state
	MapSearchNode nodeStart;
	nodeStart.id = *(pI3+1); 
	nodeStart.x = GetX(nodeStart.id);
	nodeStart.y = GetY(nodeStart.id);
	nodeStart.g = 0;
	//nodeStart.id = *mxGetPr(InArray[3]); 
	//nodeStart.x = GetX(nodeStart.id);
	//nodeStart.y = GetY(nodeStart.id);
	// Define the goal state
	MapSearchNode nodeEnd;

	nodeEnd.id = *(pI3 + 2);
	nodeEnd.x = GetX(nodeEnd.id);							
	nodeEnd.y = GetY(nodeEnd.id);	
	nodeEnd.g = NumofNodes;
	//nodeEnd.id = *mxGetPr(InArray[4]);
	//nodeEnd.x = GetX(nodeEnd.id);							
	//nodeEnd.y = GetY(nodeEnd.id);
	// Set Start and goal states
	
	AvgDist = *(pI3 + 3);
	
	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
			double *temp = mxGetPr(mxCreateDoubleMatrix(1,NumofNodes, mxREAL));
			node->PrintNodeInfo();
			*temp = node->id + 1; // difference between c++ and matlab
		  temp++;
			for( ;; )
			{
				node = astarsearch.GetSolutionNext();

				if( !node )
				{
					break;
				}
				*temp = node->id + 1; // difference between c++ and matlab
				temp++;
				node->PrintNodeInfo();
				steps ++;
				cost = node->g;
			};
			//cost = astarsearch.GetSolutionEnd()->g;

			cout << "Solution steps " << steps << endl;
			
			// Once you're done with the solution you can free the nodes up
			astarsearch.FreeSolutionNodes();
			
			temp = temp - steps - 1;
			OutArray[0]=mxCreateDoubleMatrix(1,steps+1, mxREAL);
  		pO1=mxGetPr(OutArray[0]);
  		for(i=0; i<steps+1; i++)
  			*(pO1+i) = *(temp+i);
	
	}
	else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED ) 
	{
		cout << "Search terminated. Did not find goal state\n";
		OutArray[0]=mxCreateDoubleMatrix(1,1, mxREAL);
  	pO1=mxGetPr(OutArray[0]);
  	*pO1 = -1;
	}

	// Display the number of loops the search went through
	cout << "SearchSteps : " << SearchSteps << "\n";
	OutArray[1] = mxCreateDoubleMatrix(1,1,mxREAL);
  pO2=mxGetPr(OutArray[1]);
  *pO2 = SearchSteps;
  //*pO2 = cost;
  
  /*
	if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_SUCCEEDED )
	{
		

  	MapSearchNode *mynode = astarsearch.GetSolutionStart();

		*pO1 = mynode->id + 1; // difference between c++ and matlab
		pO1++;
		for( ;; )
		{
				mynode = astarsearch.GetSolutionNext();

				if( !mynode )
				{
					break;
				}
				*pO1 = mynode->id + 1; // difference between c++ and matlab
				pO1++;
				
		};
		astarsearch.FreeSolutionNodes();
  }
  else if( SearchState == AStarSearch<MapSearchNode>::SEARCH_STATE_FAILED ){
  	OutArray[0]=mxCreateDoubleMatrix(1,1, mxREAL);
  	pO1=mxGetPr(OutArray[0]);
  	*pO1 = -1;
  }
 */
  
	return;
}