string GetDirectionText(AstarBaseNode node) { if (node == null || node.nextNode == null) { return(""); } Vector2 direction = node.nextNode.position - node.position; if (direction == Vector2.up) { return("↑"); } if (direction == Vector2.right) { return("→"); } if (direction == Vector2.down) { return("↓"); } if (direction == Vector2.left) { return("←"); } return(""); }
public void FindPath(Vector2 startPosition, Vector2 endPosition) { if (!InMatrix(startPosition) || !InMatrix(endPosition) || !GetNode(startPosition).canThrough || !GetNode(endPosition).canThrough) { return; } ResetMatrix(_matrix.GetLength(0), _matrix.GetLength(1)); _openedList = new List <AstarBaseNode>(); _closedList = new List <AstarBaseNode>(); _startPosition = new Vector2((int)startPosition.x, (int)startPosition.y); //这步极其重要,一定要把起点终点的坐标进行精确,否则算成本的时候会出现不可预料的小数进而造成误差 _endPosition = new Vector2((int)endPosition.x, (int)endPosition.y); _openedList.Add(GetNode(_startPosition)); //将起点加入开表 while (_openedList.Count > 0) { AstarBaseNode currentNode = GetMinimumCostNodeInOpenedList(); if (IsDestination(currentNode)) { break; } OpenNode(currentNode, Vector2.up); OpenNode(currentNode, Vector2.right); OpenNode(currentNode, Vector2.down); OpenNode(currentNode, Vector2.left); CloseNode(currentNode); } }
public void Reset() //寻路开始时清空成本和指向 { _nextNode = null; _startToCost = 0; _toEndCost = 0; _totalCost = 0; }
public void Pathfind_CantFind() { AStarBase aStar = new AStarBase(7, 7); Vector2 startPosition = new Vector2(0, 0); Vector2 endPosition = new Vector2(6, 6); aStar.SetObstacle(new Vector2(0, 1)); aStar.SetObstacle(new Vector2(1, 1)); aStar.SetObstacle(new Vector2(2, 1)); aStar.SetObstacle(new Vector2(4, 1)); aStar.SetObstacle(new Vector2(4, 2)); aStar.SetObstacle(new Vector2(4, 3)); aStar.SetObstacle(new Vector2(5, 3)); aStar.SetObstacle(new Vector2(6, 3)); aStar.SetObstacle(new Vector2(1, 4)); aStar.SetObstacle(new Vector2(2, 4)); aStar.SetObstacle(new Vector2(3, 4)); aStar.SetObstacle(new Vector2(2, 5)); aStar.SetObstacle(new Vector2(0, 6)); aStar.SetObstacle(new Vector2(1, 6)); aStar.FindPath(startPosition, endPosition); /* * N N 0 0 0 0 E * * ↓ ← N 0 0 0 0 * * ↓ N N N 0 0 0 * * → → → ↓ N N N * * → → → ↓ N ↓ ← * * N N N ↓ N ↓ ← * * S ← ← ← ← ← ← */ AstarBaseNode[,] matrix = aStar.matrix; AstarBaseNode[,] nextNodes = new AstarBaseNode[7, 7] { { null, matrix[0, 0], matrix[1, 0], matrix[2, 0], matrix[3, 0], matrix[4, 0], matrix[5, 0] }, { null, null, null, matrix[3, 0], null, matrix[5, 0], matrix[5, 1] }, { matrix[1, 2], matrix[2, 2], matrix[3, 2], matrix[3, 1], null, matrix[5, 1], matrix[5, 2] }, { matrix[1, 3], matrix[2, 3], matrix[3, 3], matrix[3, 2], null, null, null }, { matrix[0, 3], null, null, null, null, null, null }, { matrix[0, 4], matrix[0, 5], null, null, null, null, null }, { null, null, null, null, null, null, null } }; for (int x = 0; x < 7; x++) { for (int y = 0; y < 7; y++) { Assert.AreEqual(nextNodes[y, x], aStar.matrix[x, y].nextNode); //横y竖x,何等反人类,而且竟然严格符合了数组下标顺序:前面是第一层,后面是第二层 } } }
Color GetNodeColor(AstarBaseNode node, float maxStartToCost, float maxToEndCost) { if (!node.canThrough) { return(Color.black); } Color nodeRed = Color.red * node.startToCost / maxStartToCost; Color nodeBlue = Color.blue * node.toEndCost / maxToEndCost; return(nodeRed + nodeBlue + Color.gray * 0.5f); }
void OpenNode(AstarBaseNode currentNode, Vector2 direction) //有缺陷,假设一个格子已经开了但有更低价的格子可以指向,缺少对这时候的处理,这个处理在后面几个里有 { AstarBaseNode newNode = GetNode(currentNode, direction); if (newNode == null || !newNode.canThrough) { return; //如果没获取到新节点或新节点不能通行则直接return } if (!_openedList.Contains(newNode) && !_closedList.Contains(newNode)) //不在开表里也不在闭表里,说明是未开启状态 { DoOpenNode(currentNode, newNode); } }
void DisplayPath() { AstarBaseNode currentNode = _aStar.GetNode(_endPosition); if (currentNode == null || currentNode.nextNode == null) { return; } while (currentNode.nextNode != null) { AddGreen(currentNode); currentNode = currentNode.nextNode; } AddGreen(currentNode); }
AstarBaseNode GetMinimumCostNodeInOpenedList() { AstarBaseNode minimumCostNode = _openedList[0]; foreach (AstarBaseNode currentNode in _openedList) { if (currentNode.totalCost < minimumCostNode.totalCost) { minimumCostNode = currentNode; } else if (currentNode.totalCost == minimumCostNode.totalCost && currentNode.toEndCost < minimumCostNode.toEndCost) //当前节点总成本和最小成本节点总成本相同,但距离终点更近的话 { minimumCostNode = currentNode; } } return(minimumCostNode); }
public void Pathfind_CanFind_NoObstacle() { AStarBase aStar = new AStarBase(7, 7); Vector2 startPosition = new Vector2(0, 0); Vector2 endPosition = new Vector2(6, 6); aStar.FindPath(startPosition, endPosition); /* * ↓ ← ← ← ← ← E * * ↓ ← ↑ ↑ ↑ ↑ 0 * * ↓ ← 0 0 0 0 0 * * ↓ ← 0 0 0 0 0 * * ↓ ← 0 0 0 0 0 * * ↓ ← 0 0 0 0 0 * * S ← 0 0 0 0 0 */ AstarBaseNode[,] matrix = aStar.matrix; AstarBaseNode[,] nextNodes = new AstarBaseNode[7, 7] { { null, matrix[0, 0], null, null, null, null, null }, { matrix[0, 0], matrix[0, 1], null, null, null, null, null }, { matrix[0, 1], matrix[0, 2], null, null, null, null, null }, { matrix[0, 2], matrix[0, 3], null, null, null, null, null }, { matrix[0, 3], matrix[0, 4], null, null, null, null, null }, { matrix[0, 4], matrix[0, 5], matrix[2, 6], matrix[3, 6], matrix[4, 6], matrix[5, 6], null }, { matrix[0, 5], matrix[0, 6], matrix[1, 6], matrix[2, 6], matrix[3, 6], matrix[4, 6], matrix[5, 6] } }; for (int x = 0; x < 7; x++) { for (int y = 0; y < 7; y++) { Assert.AreEqual(nextNodes[y, x], aStar.matrix[x, y].nextNode); //二维数组赋值是按照横竖x的方式进行的,也就是说上面的矩阵写错了,要转置,这就是一个用 [y, x] 一个用 [x, y] 的原因 } } }
public AStarBase(int width, int height) { if (width < 0) { width = 0; //防止传入负数 } if (height < 0) { height = 0; //防止传入负数 } _matrix = new AstarBaseNode[width, height]; for (int x = 0; x < width; x++) { for (int y = 0; y < height; y++) { _matrix[x, y] = new AstarBaseNode(x, y); } } }
void MouseMiddleDown() { Vector2 position = GetMousePosition(); AstarBaseNode node = _aStar.GetNode(position); if (node == null) { return; } if (node.canThrough) { _aStar.SetObstacle(position); } else { _aStar.RemoveObstacle(position); } DoFindPath(); }
void DisplayANode(AstarBaseNode node, GameObject displayer, float maxStartToCost, float maxToEndCost) { displayer.GetComponentInChildren <Image>().color = GetNodeColor(node, maxStartToCost, maxToEndCost); displayer.GetComponentInChildren <Text>().text = GetDirectionText(node); }
public void Open(AstarBaseNode nextNode, Vector2 destination) { _nextNode = nextNode; ComputeCost(destination); }
public AstarBaseNode GetNode(AstarBaseNode node, Vector2 direction) { return(GetNode(node.position + direction)); }
void CloseNode(AstarBaseNode node) { _openedList.Remove(node); _closedList.Add(node); }
void DoOpenNode(AstarBaseNode currentNode, AstarBaseNode openNode) { openNode.Open(currentNode, _endPosition); _openedList.Add(openNode); }
bool IsDestination(AstarBaseNode currentNode) { return(currentNode == GetNode(_endPosition)); }
void AddGreen(AstarBaseNode currentNode) { Vector2 position = currentNode.position; _displayMatrix[(int)position.x, (int)position.y].GetComponentInChildren <Image>().color += Color.green; }