/// <summary>
/// Attempts to find a path to the destination node using <paramref name="currentNode"/> as the starting location
/// </summary>
/// <param name="currentNode">The node from which to find a path</param>
/// <returns>True if a path to the destination has been found, otherwise false</returns>
private bool Search(Node currentNode)
{
// Set the current node to Closed since it cannot be traversed more than once
currentNode.State = NodeState.Closed;
List<Node> nextNodes = GetAdjacentWalkableNodes(currentNode);
// Sort by F-value so that the shortest possible routes are considered first
nextNodes.Sort((node1, node2) => node1.F.CompareTo(node2.F));
foreach (var nextNode in nextNodes)
{
// Check whether the end node has been reached
if (nextNode.Location == this.endNode.Location)
{
return true;
}
else
{
// If not, check the next set of nodes
if (Search(nextNode)) // Note: Recurses back into Search(Node)
return true;
}
}
// The method returns false if this path leads to be a dead end
return false;
}
/// <summary>
/// Create a new instance of PathFinder
/// </summary>
/// <param name="searchParameters"></param>
public PathFinder(SearchParameters searchParameters)
{
this.searchParameters = searchParameters;
InitializeNodes(searchParameters.Map);
this.startNode = this.nodes[searchParameters.StartLocation.X, searchParameters.StartLocation.Y];
this.startNode.State = NodeState.Open;
this.endNode = this.nodes[searchParameters.EndLocation.X, searchParameters.EndLocation.Y];
}
/// <summary>
/// Find Direct
/// </summary>
/// <param name="searchParameters"></param>
public List<Point> FindPath(Point Start, Point End )
{
this.startNode = this.nodes[Start.X, Start.Y];
this.startNode.State = NodeState.Open;
this.startNode.IsWalkable = true;
this.endNode = this.nodes[End.X, End.Y];
this.endNode.IsWalkable = true;
return FindPath ();
}
public int nMaxStep; // limit of path find
/// <summary>
/// Create a new instance of PathFinder
/// </summary>
/// <param name="searchParameters"></param>
public PathFinder(SearchParameters searchParameters)
{
this.searchParameters = searchParameters;
InitializeNodes(searchParameters.Map); // it create node.
this.startNode = this.nodes[searchParameters.StartLocation.X, searchParameters.StartLocation.Y];
this.startNode.State = NodeState.Open;
this.endNode = this.nodes[searchParameters.EndLocation.X, searchParameters.EndLocation.Y];
// path find cache
openlst = new List<Node>();
movelst = new List<Point>();
bIsDirty = true;
}
/// <summary>
/// Returns any nodes that are adjacent to <paramref name="fromNode"/> and may be considered to form the next step in the path
/// </summary>
/// <param name="fromNode">The node from which to return the next possible nodes in the path</param>
/// <returns>A list of next possible nodes in the path</returns>
private List<Node> GetAdjacentWalkableNodes(Node fromNode)
{
List<Node> walkableNodes = new List<Node>();
IEnumerable<Point> nextLocations = GetAdjacentLocations(fromNode.Location);
foreach (var location in nextLocations)
{
int x = location.X;
int y = location.Y;
// Stay within the grid's boundaries
if (x < 0 || x >= this.width || y < 0 || y >= this.height)
continue;
Node node = this.nodes[x, y];
// Ignore non-walkable nodes
if (!node.IsWalkable)
continue;
// Ignore already-closed nodes
if (node.State == NodeState.Closed)
continue;
// Already-open nodes are only added to the list if their G-value is lower going via this route.
if (node.State == NodeState.Open)
{
float traversalCost = Node.GetTraversalCost(node.Location, node.ParentNode.Location);
float gTemp = fromNode.G + traversalCost;
if (gTemp < node.G)
{
node.ParentNode = fromNode;
walkableNodes.Add(node);
}
}
else
{
// If it's untested, set the parent and flag it as 'Open' for consideration
node.ParentNode = fromNode;
node.State = NodeState.Open;
walkableNodes.Add(node);
}
}
return walkableNodes;
}
/// <summary>
/// Find Direct
/// </summary>
/// <param name="searchParameters"></param>
public List<Point> MoveAble(Point Start , int nDist )
{
this.startNode = this.nodes[Start.X, Start.Y];
this.startNode.State = NodeState.Open;
this.startNode.IsWalkable = true;
Node parentnode;
// open list
bool bFind = false;
bool bStop = false;
int nNewG = 0;
openlst.Clear();
movelst.Clear();
// List<Node> closelst = new List<Node>();
// List<Node> maxsteplst = new List<Node>(); // record the max step node
Node currentNode = startNode;
openlst.Add (currentNode); // push
while ( openlst.Count > 0 ) {
parentnode = openlst[ 0 ] ; // GET FIRST NODE for short len
//openlst.RemoveAt[ openlst.Count-1 ]; // pop
openlst.Remove( parentnode );
parentnode.State = NodeState.Closed; // close this
// closelst.Add( parentnode ); // add x to closedset //将x节点插入已经被估算的节点
// if need check max step
if( parentnode.G >= nDist ){
// maxsteplst.Add( parentnode );
continue;
}
// find all next node L + G
List<Node> nextNodes = GetAdjacentWalkableNodes(parentnode); // close node won't return
foreach (Node nextNode in nextNodes)
{
//if( closelst.IndexOf(nextNode)>=0 ) //if y in closedset //若y已被估值，跳过
// continue;
// if( nextNode.State == NodeState.Closed ){
// continue;
// }
openlst.Add( nextNode );
//nextNode.State = NodeState.Closed; // close this
// add to next moveable list
if( nextNode.G <= nDist ){ // add if it can move
movelst.Add( new Point( nextNode.Location.X , nextNode.Location.Y ) );
}
}
}
bIsDirty = true;
return movelst;
}
private bool FastSearch(Node currentNode)
{
//bool bFind = false;
int nDiffX = (endNode.Location.X - currentNode.Location.X);
int nDiffY = (endNode.Location.Y - currentNode.Location.Y);
int nAbsX = Math.Abs ( nDiffX );
int nAbsY = Math.Abs ( nDiffY );
Node parentNode = null;
int nDeltX = nDiffX > 0 ? 1 : -1;
int nDeltY = nDiffY > 0 ? 1 : -1;
// try x first
int x = currentNode.Location.X;
int y = currentNode.Location.Y;
parentNode = currentNode;
for (int i=0 ; i< nAbsX; i++ ,x=x+nDeltX) {
Node node = this.nodes[x, y];
if( node.IsWalkable == false ) {
break;
}
else{
Node node2 = new Node( x , y , true );
node2.ParentNode = parentNode;
parentNode = node2;
}
// check is target node
if( (parentNode.Location.X==endNode.Location.X) && (parentNode.Location.Y==endNode.Location.Y) )
{
endNode = parentNode; // set end to new node for trace parent outside
return true;
}
}
if (x == endNode.Location.X) {
for (int j=0; j<= nAbsY; j++ , y+=nDeltY) {
Node node = this.nodes[x, y];
if( node.IsWalkable == false ) {
break;
}
else{
Node node2 = new Node( x , y , true );
node2.ParentNode = parentNode;
parentNode = node2;
}
// check is target node
if( (parentNode.Location.X==endNode.Location.X) && (parentNode.Location.Y==endNode.Location.Y) )
{
endNode = parentNode; // set end to new node for trace parent outside
return true;
}
}
}
// test y first
x = currentNode.Location.X;
y = currentNode.Location.Y;
parentNode = currentNode;
for (int j=0; j< nAbsY; j++ , y+=nDeltY) {
Node node = this.nodes[x, y];
if( node.IsWalkable == false ) {
break;
}
else{
Node node2 = new Node( x , y , true );
node2.ParentNode = parentNode;
parentNode = node2;
}
// check is target node
if( (parentNode.Location.X==endNode.Location.X) && (parentNode.Location.Y==endNode.Location.Y) )
{
endNode = parentNode; // set end to new node for trace parent outside
return true;
}
}
if (y == endNode.Location.Y) {
for (int i=0 ; i<= nAbsX; i++ ,x=x+nDeltX) {
Node node = this.nodes[x, y];
if( node.IsWalkable == false ) {
break;
}
else{
Node node2 = new Node( x , y , true );
node2.ParentNode = parentNode;
parentNode = node2;
}
// check is target node
if( (parentNode.Location.X==endNode.Location.X) && (parentNode.Location.Y==endNode.Location.Y) )
{
endNode = parentNode; // set end to new node for trace parent outside
return true;
}
}
}
return false;
}
private bool AStarSearch(Node currentNode)
{
Node parentnode;
// open list
bool bFind = false;
bool bStop = false;
int nNewG = 0;
// List<Node> openlst = new List<Node>();
// List<Node> closelst = new List<Node>();
// List<Node> maxsteplst = new List<Node>(); // record the max step node
openlst.Clear();
openlst.Add (currentNode); // push
while ( openlst.Count > 0 ) {
parentnode = openlst[ 0 ] ; // GET FIRST NODE for short len
if (parentnode.Location == this.endNode.Location){
bFind = true;
break; // find
}
//openlst.RemoveAt[ openlst.Count-1 ]; // pop
openlst.Remove( parentnode );
parentnode.State = NodeState.Closed; // close this
// closelst.Add( parentnode ); // add x to closedset //将x节点插入已经被估算的节点
// if need check max step
if( nMaxStep > 0 )
{
if( parentnode.G >= nMaxStep ){
// maxsteplst.Add( parentnode );
continue;
}
}
// find all next node L + G
List<Node> nextNodes = GetAdjacentWalkableNodes(parentnode); // close node won't return
foreach (var nextNode in nextNodes)
{
//if( closelst.IndexOf(nextNode)>=0 ) //if y in closedset //若y已被估值，跳过
// continue;
openlst.Add( nextNode );
//nextNode.State = NodeState.Closed; // close this
}
}
bIsDirty = true;
return bFind;
}
public void ApplyMap( bool[,] map )
{
this.width = map.GetLength(0);
this.height = map.GetLength(1);
if( this.nodes == null )
this.nodes = new Node[this.width, this.height];
for (int y = 0; y < this.height; y++)
{
for (int x = 0; x < this.width; x++)
{
Node node = this.nodes[x, y];
if( node == null )
{
node = new Node(x, y, map[x, y] );
this.nodes[x, y] = node;
}
else {
node.Reset();
node.IsWalkable = map[x, y];
}
}
}
}
public void Reset()
{
State = NodeState.Untested;
parentNode = null;
IsWalkable = true;
H = 0;
G = 0;
}