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