public Astar(Node startnode, Node endnode, matrix Ma, Dictionary<string, Node> obstacal)
{
this.startNode = startnode;
this.endNode = endnode;
this.Matrix = Ma;
obstancleDic = obstacal;
}
public Node(float x, float y)
{
_parent = null;
_hasParent = false;
_coords = new Vector2D(x, y);
_g = 0;
}
public Node(int x, int y, int g, Node father)
{
this.x = x;
this.y = y;
this.G = g;
this.Father = father;
}
public void AddAdjacentNodesToOpenList(Node node)
{
int x = node.x;
int y = node.y;
_openList.Remove(node);
tempGrid[x, y] = 3;
if (x == (int)_currentCoord.y && y == (int)_currentCoord.x)
{
_path = node;
_foundPath = true;
}
if (!_foundPath)
{
CheckAdjacentTile((x + 1), y, node);
CheckAdjacentTile((x - 1), y, node);
CheckAdjacentTile(x, (y + 1), node);
CheckAdjacentTile(x, (y - 1), node);
Node next = _openList[0];
for (int i = 1; i > _openList.Count; i++)
{
if (next.f > _openList[i].f)
next = _openList[i];
}
AddAdjacentNodesToOpenList(next);
}
}
public void CheckAdjacentTile(int x, int y, Node node)
{
int heuristic;
if (tempGrid[x, y] < 1)
{
heuristic = (Math.Abs((int)_currentCoord.y - x) + Math.Abs((int)_currentCoord.x - y)) * 10;
Node nextNewNode = new Node(node, x, y, heuristic);
if (tempGrid[x, y] == -1)
{
for (int i = 0; i > _openList.Count; i++)
{
if (_openList[i].x == x && _openList[i].y == y)
{
if (_openList[i].g <= nextNewNode.g)
break;
else
_openList[i] = nextNewNode;
}
}
}
else
{
_openList.Add(nextNewNode);
tempGrid[x, y] = -1;
}
}
}
public List<Node> GetNeighbours(Node node)
{
List<Node> neighbours = new List<Node>();
for(int x = -1; x <= 1; x++)
{
for(int y = -1; y <= 1; y++)
{
if (y == -1 || y == 1)
{
if (x == 1 || x == -1) continue;
}
if (x == -1 || x == 1)
{
if (y == 1 || y == -1) continue;
}
if(x == 0 && y == 0) continue;
int checkX = node.gridX + x;
int checkY = node.gridY + y;
if(checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
neighbours.Add(grid[checkX, checkY]);
}
}
}
return neighbours;
}
public void CreateGrid()
{
grid = new Node[gridSizeX, gridSizeY];
// world bottom left change to fit with xy not xz
Vector3 bottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.up * gridWorldSize.y / 2;
for(int x = 0; x < gridSizeX; x++)
{
for(int y = 0; y < gridSizeY; y++)
{
// node pts
Vector3 worldPoint = bottomLeft + (Vector3.right * (x * nodeDiameter + nodeRadius)) + (Vector3.up * (y * nodeDiameter + nodeRadius));
bool walkable = true;
if (map != null)
{
MapData mapData = map.GetMapData ();
if (mapData[y,x].IsCollidable())
{
walkable = false;
}
}
else walkable = !Physics.CheckSphere(worldPoint, nodeRadius-0.05f, unwalkableMask);
grid[x, y] = new Node(walkable, worldPoint, x, y);
}
}
}
public Node(Node parent, int heuristic)
{
_parent = parent;
_hasParent = true;
_g = _parent._g + 10;
_heuristic = heuristic;
_f = _g + _heuristic;
}
private static void GetEndpoints()
{
Console.WriteLine("Where do you want to start?");
char s = Console.ReadLine()[0];
Console.WriteLine("Where do you want to end?");
char e = Console.ReadLine()[0];
start = nodes[s - 'a'];
end = nodes[e - 'a'];
}
public Node(Node parent, float x, float y, int heuristic)
{
_parent = parent;
_hasParent = true;
_g = _parent._g + 10;
_heuristic = heuristic;
_f = _g + _heuristic;
_coords = new Vector2D(x, y);
}
public static Path TracePath(Node startNode, Node endNode)
{
Path path = new Path();
Node currentNode = endNode;
while(currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.parent;
}
path.Reverse();
grid.path = path;
return path;
}
/// <summary>
/// Returns the distance in miles or kilometers of any two
/// latitude / longitude points.
/// </summary>
/// <param name=”node1″></param>
/// <param name=”node2″></param>
/// <param name=”type”></param>
/// <returns></returns>
public static double Distance(Node node1, Node node2, DistanceType type)
{
double R = (type == DistanceType.ml) ? 3960 : 6371;
double dLat = ToRadian(node2.Latitude - node1.Latitude);
double dLon = ToRadian(node2.Longitude - node1.Longitude);
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
Math.Cos(ToRadian(node1.Latitude)) * Math.Cos(ToRadian(node2.Latitude)) *
Math.Sin(dLon / 2) * Math.Sin(dLon / 2);
double c = 2 * Math.Asin(Math.Min(1, Math.Sqrt(a)));
double d = R * c;
return d;
}
public void AStar()
{
Node tempNode = new Node();
Node temp = new Node();
currentNode = this.getStartNode();
openDic.Add(currentNode.getKey(), currentNode);
do
{
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
tempNode.X = currentNode.X + i - 1;
tempNode.Y = currentNode.Y + j - 1;
if (!obstancleDic.ContainsKey(tempNode.getKey()) && !clostDic.ContainsKey(tempNode.getKey()) && tempNode.X >= 0 && tempNode.X <= Matrix.x && tempNode.Y >= 0 && tempNode.Y <= Matrix.y && (i != 1 || j != 1))
{
if (openDic.TryGetValue(tempNode.getKey(), out temp))
{
int gF = temp.getG(temp.getFather());
int gC = temp.getG(currentNode);
if (gF > gC)
{
temp.setFather(currentNode);
}
}
else
{
tempNode.setFather(currentNode);
openDic.Add(tempNode.getKey(), tempNode);
}
tempNode = new Node();
}
}
}
openDic.Remove(currentNode.getKey());
clostDic.Add(currentNode.getKey(), currentNode);
currentNode = this.getMinNode(openDic, this.getEndNode());
} while (!clostDic.ContainsKey(this.getEndNode().getKey()) && openDic.Count != 0);
}
static void Main(string[] args)
{
//地图
matrix Matrix = new matrix(7, 3);
//开始点
Node startNode = new Node(2,2,0,new Node());
//结束点
Node endNode = new Node(6, 2, 0, new Node());
//阻挡点
Node obstalNode1 = new Node(4, 1, 0, new Node());
Node obstalNode2 = new Node(4, 2, 0, new Node());
Node obstalNode3 = new Node(4, 3, 0, new Node());
Dictionary<string, Node> obstalDic = new Dictionary<string,Node>();
obstalDic.Add(obstalNode1.getKey(), obstalNode1);
obstalDic.Add(obstalNode2.getKey(), obstalNode1);
obstalDic.Add(obstalNode3.getKey(), obstalNode1);
Astar astar = new Astar(startNode, endNode, Matrix, obstalDic);
astar.AStar();
astar.getShortCutRoute();
Console.Read();
}
public void setFather(Node father)
{
this.Father = father;
this.getG();
}
public int getH(Node endNode)
{
return this.ManHaTen(this, endNode);
}
public int getG(Node from)
{
G = (((x - from.x) * (y - from.y) == 0) ? 10 : 14) + from.G;
return G;
}
public int getF(Node endNode)
{
int tem = this.getG() + this.getH(endNode);
return tem;
}
private Node getMinNode(Dictionary<string, Node> inputNodeDic, Node endNode)
{
Node minNode = new Node();
if (inputNodeDic.Count > 0)
{
minNode = inputNodeDic.ElementAt(0).Value;
foreach (KeyValuePair<string, Node> item in inputNodeDic)
{
if (minNode.getF(endNode) > item.Value.getF(endNode))
{
minNode = item.Value;
}
}
}
return minNode;
}
public bool Contains(Node n)
{
return list.Contains(n);
}
/// <summary>
/// Adds a new TNode to the NodeList.
/// </summary>
public virtual void Add(Node n)
{
data.Add(n.Key, n);
}
public bool Search(
IStateTransition transition,
IState initialState,
IState goalState,
HeuristicDelegate heuristic,
out Node result, double initialCost=0)
{
result = null;
var fringe = new BinaryHeap<SearchNode>((x, y) => y.Value > x.Value);
fringe.Insert(new SearchNode()
{
f = heuristic(initialState, goalState),
g = initialCost,
State = initialState
});
var visitedNodes = new Dictionary<IState, SearchNode>();
visitedNodes.Add(initialState, new SearchNode()
{
f = heuristic(initialState,
goalState),
g = initialCost,
State = initialState
});
var history = new Dictionary<IState, Node>();
history.Add(initialState, new Node() { State = initialState });
while (true)
{
if (fringe.Count == 0)
{
return false;
}
var node = fringe.Remove();
if (transition.IsGoal(node.State, goalState))
{
result = history[goalState];
return true;
}
foreach (var x in transition.Expand(node.State))
{
var state = x.Item1;
var _g = x.Item3 + node.g;
var _f = _g + heuristic(state, goalState);
if (transition.IsGoal(state, goalState))
_f = _g;
var action = x.Item2;
SearchNode visitedNode;
Node hnode;
if (visitedNodes.TryGetValue(state, out visitedNode))
{
if (visitedNode.g < _g)
hnode = history[state];
else
continue;
}
else
{
visitedNode = new SearchNode();
visitedNodes.Add(state, visitedNode);
hnode = new Node();
history.Add(state, hnode);
}
visitedNode.f = _f;
visitedNode.g = _g;
visitedNode.State = state;
fringe.Insert(visitedNode);
hnode.Previous = history[node.State];
hnode.Cost = _g;
hnode.PreviousAction = action;
hnode.State = state;
}
}
}
public Point[] findPath(Point start, Point end, int X, int Y, int[,] Map)
{
List<Point> p = new List<Point>();
Node current = new Node(start, 0, 0, 0);
current.H = calcH(start, end);
current.F = current.H;
openList.Add(current);
bool ex = false;
bool can = true;
while (!ex)
{
int i = findMinF(openList);
current = openList[i];
openList.RemoveAt(i);
closeList.Add(current);
Point nb = current.P;
nbPoint(new Point(nb.X + 1, nb.Y), current, end, X, Y, Map);
nbPoint(new Point(nb.X + 1, nb.Y - 1), current, end, X, Y, Map);
nbPoint(new Point(nb.X, nb.Y - 1), current, end, X, Y, Map);
nbPoint(new Point(nb.X - 1, nb.Y - 1), current, end, X, Y, Map);
nbPoint(new Point(nb.X - 1, nb.Y), current, end, X, Y, Map);
nbPoint(new Point(nb.X - 1, nb.Y + 1), current, end, X, Y, Map);
nbPoint(new Point(nb.X, nb.Y + 1), current, end, X, Y, Map);
nbPoint(new Point(nb.X + 1, nb.Y + 1), current, end, X, Y, Map);
if (openList.Count == 0)
{
ex = true;
can = false;
}
if (ifExistsInList(end, openList))
ex = true;
}
if (can)
{
List<Node> list = new List<Node>();
foreach (Node n in openList)
list.Add(n);
foreach (Node n in closeList)
list.Add(n);
p.Add(end);
Point pp = end;
int i = 0;
while (pp != start)
{
i = inList(pp, list);
if (i < 0)
break;
pp = list[i].Parent;
p.Add(pp);
list.RemoveAt(i);
}
}
Point[] ret = new Point[p.Count];
for (int i = 0; i < p.Count; i++)
ret[i] = p[p.Count - i - 1];
return ret;
}
private int calcG(Node node)
{
int g = 0;
foreach (Node n in closeList)
g += n.G;
Point p = closeList[closeList.Count - 1].P;
if (p.X == node.P.X && p.Y != node.P.Y)
g += 10;
else if (p.X != node.P.X && p.Y == node.P.Y)
g += 10;
else if (p.X != node.P.X && p.Y != node.P.Y)
g += 22;
return g;
}
int GetH(Node node)
{
//return 0;
return((int)(Math.Sqrt((node.x - goal_.x) * (node.x - goal_.x) + (node.y - goal_.y) * (node.y - goal_.y)) * 9.5f));
}
public Node()
{
_parent = null;
_hasParent = false;
_g = 0;
}
/// <summary>
/// Removes a TNode from the NodeList.
/// </summary>
public virtual void Remove(Node n)
{
data.Remove(n.Key);
}
public void getShortCutRoute()
{
Node endNode = new Node();
if (clostDic.TryGetValue(this.getEndNode().getKey(), out endNode))
{
endNode.showShortCutRout();
}
}
private int ManHaTen(Node me, Node endNode)
{
return (System.Math.Abs(endNode.x - me.x) + System.Math.Abs(endNode.y - me.y)) * 10;
}
private void nbPoint(Point nb, Node current, Point end, int X, int Y, int[,] Map)
{
if (nb.X >= X || nb.X < 0)
return;
if (nb.Y >= Y || nb.Y < 0)
return;
if (ifExistsInList(nb, closeList))
return;
if (Map[nb.X, nb.Y] == 1)
return;
if (!ifExistsInList(nb, openList))
{
Node n = new Node(nb, 0, 0, 0);
n.H = calcH(nb, end);
n.G = calcG(n);
n.F = n.G + n.H;
n.Parent = current.P;
openList.Add(n);
}
else
{
Node n = openList[inList(nb, openList)];
if (n.G < calcG(n))
{
n.Parent = current.P;
n.G = calcG(n);
n.F = n.G + n.H;
int j = inList(nb, openList);
openList.RemoveAt(j);
openList.Add(n);
}
}
}
public void Add(Node n)
{
list.Add(n);
}
