a*寻路算法

之前做unity3d游戏时用到的a*寻路算法，整理出来记录一下

public struct MyVector
{
  public int x;
 public int y;
 public MyVector( int _x, int _y )
 {
  x = _x;
  y = _y;
 }
};
 
public class Point
{
 public MyVector position; 
 public int h;      
 public int g;      
 public Point father;  
 
 public Point()
 {
 }
 public Point( MyVector _position, int _h, int _g, Point F )
 {
  position = _position;
  h = _h;
  g = _g;
  father = F;
 }
}
 
public class WayFinding
{ 
 public List<Point> Open_List = new List<Point>();//open list 
 public List<Point> Close_List = new List<Point>();//close list
 
 private Point GetMinF()
 {
  Point _pMin = null;
  foreach( Point p in Open_List )
  {
   if ( _pMin == null || _pMin.g + _pMin.h > p.g + p.h )
   {
    _pMin = p;
   }
  }
  return _pMin;
 }
 private bool IsBar( Point p, int[,] map )
 {
  int x = p.position.x;
  int y = p.position.y;
  if ( map[x, y] == 0 )
  {
   return true;
  }
  return false;
 }
 private bool IsInCloseList( MyVector p )
 {
  foreach ( Point _p in Close_List )
  {
   if ( p.x == _p.position.x && p.y == _p.position.y )
   {
    return true;
   }
  }
  return false;
 }
 private Point GetPointFromCloseList( MyVector p )
 {
  foreach ( Point _p in Close_List )
  {
   if ( p.x == _p.position.x && p.y == _p.position.y )
   {
    return _p;
   }
  }
  return null;
 }
 private bool IsInOpenList( MyVector p )
 {
  foreach ( Point _p in Open_List )
  {
   if ( p.x == _p.position.x && p.y == _p.position.y )
   {
    return true;
   }
  }
  return false;
 }
 private Point GetPointFromOpenList( MyVector p )
 {
  foreach ( Point _p in Open_List )
  {
   if ( p.x == _p.position.x && p.y  == _p.position.y )
   {
    return _p;
   }
  }
  return null;
 }
 private int GetG( Point p )
 {
  if ( p.father == null )
   return 0;
  if ( p.position.x == p.father.position.x || p.position.y == p.father.position.y  )
  {
   return ( p.father.g + 10 );
  }
  else 
  {
   return ( p.father.g + 14 );
  }
 }
 private int GetH( MyVector p, MyVector cp )
 {
  return ( Mathf.Abs( p.x - cp.x ) + Mathf.Abs( p.y -cp.y ) )*10;
 }
 private void CheckPoint( Point p0, Ball[,] map, Point pa, ref Point pb )
 {
  int x = 0, y = 0;
  for ( int i = 0; i < 4; i++ )
  {
   switch(i)
   {
   case 0:
    x = p0.position.x + 1;
    y = p0.position.y;
    break;
   case 1:
    x = p0.position.x - 1;
    y = p0.position.y;
    break;
   case 2:
    x = p0.position.x;
    y = p0.position.y - 1;
    break;
   case 3:
    x = p0.position.x;
    y = p0.position.y + 1;
    break;
   }
   if ( x >= 0 && x < 9 && y >= 0 && y < 9 )
   {
    if ( !map[x,y].isExist && !IsInCloseList( new MyVector( x , y ) ) )
    {
     if ( IsInOpenList( new MyVector( x , y ) ) )
     {
      Point pt = GetPointFromOpenList( new MyVector( x, y  ) ); 
      int G_new = 0;
      
      G_new = GetG( pt );
      if ( G_new < pt.g )
      {
       Open_List.Remove( pt );
       pt.father = p0;
       pt.g = G_new;
       Open_List.Add ( pt );
      }
     }
     else
     {
      Point pt = new Point ();
      pt.position.x = x;
      pt.position.y = y;
      pt.father = p0;
      pt.g = GetG( pt );
      pt.h = GetH( pt.position, pb.position );
      Open_List.Add( pt );
     }
    }
   }
  }
 }
 
 public Point FindWay( Ball[,] balls, Point pa, Point pb )
 {
  Point head = new Point ();
  Open_List.Add(pa);
  
  while ( !( IsInOpenList( pb.position ) || Open_List.Count == 0 ) ) 
  {
    Point p0 = GetMinF();
    if ( p0 == null )
    {
     break;
    }
    Open_List.Remove( p0 );
    Close_List.Add( p0 );
    CheckPoint( p0, balls, pa, ref pb );
  }
  Point p = GetPointFromOpenList( pb.position );
  
  Point tempP = new Point ();
  if ( p != null )
  {
   tempP.position.x = p.position.x;
   tempP.position.y = p.position.y;
   tempP.father = null;
    
   head = tempP;
   p = p.father;
  }
  else
  {
   return null;
  }
  
  
  while ( p.father != null )
  {
   Point temp = new Point ();
   
   temp.position.x = p.position.x;
   temp.position.y = p.position.y;
   temp.father = head;
   
   head = temp; 
   p = p.father;
  }
  
        return head;
 }
}