public IEnumerator StartFinding(PathFinding pathFinding) { _searchingList = new List <MapPosition>(); _searchingMap = new AStarScore[pathFinding.OriginMap.GetLength(0), pathFinding.OriginMap.GetLength(1)]; _searchingMap[pathFinding.StartPoint.PosX, pathFinding.StartPoint.PosY] = new AStarScore(0, 0); _searchingList.Add(pathFinding.StartPoint); var checkingPos = _searchingList[0]; _searchingList.RemoveAt(0); while (!checkingPos.Equals(pathFinding.EndPoint)) { if (CheckPos(-1, 0)) { yield break; } if (CheckPos(1, 0)) { yield break; } if (CheckPos(0, -1)) { yield break; } if (CheckPos(0, 1)) { yield break; } _searchingList.Sort((MapPosition posA, MapPosition posB) => { var aStarA = _searchingMap[posA.PosX, posA.PosY]; var aStarB = _searchingMap[posB.PosX, posB.PosY]; return(aStarA.CompareTo(aStarB)); }); checkingPos = _searchingList[0]; _searchingList.RemoveAt(0); yield return(null); } bool CheckPos(int offsetX, int offsetY) { int nextX = checkingPos.PosX + offsetX; int nextY = checkingPos.PosY + offsetY; if (nextX < 0 || nextX >= PathFinding.MAP_HEIGHT) { return(false); } if (nextY < 0 || nextY >= PathFinding.MAP_WIDTH) { return(false); } var tempScore = _searchingMap[nextX, nextY]; //if (tempScore != null) { return false; } var checkingScore = _searchingMap[checkingPos.PosX, checkingPos.PosY]; var tempPos = new MapPosition(nextX, nextY); if (pathFinding.OriginMap[nextX, nextY] == PathFinding.POINT_END) { var a = new AStarScore(checkingScore.G + 1, 0); a.SetParent(checkingPos); _searchingMap[nextX, nextY] = a; UpdateSearchBlock(pathFinding, nextX, nextY); pathFinding.SetIsFound(true); _searchingList.Clear(); return(true); } if (pathFinding.OriginMap[nextX, nextY] == PathFinding.POINT_EMPTY) { if (tempScore == null) { var a = new AStarScore(checkingScore.G + 1, MapPosition.AStarDistance(tempPos, pathFinding.EndPoint)); a.SetParent(checkingPos); _searchingMap[nextX, nextY] = a; _searchingList.Add(tempPos); } else if (tempScore.G > checkingScore.G + 1) { tempScore.SetGScore(checkingScore.G + 1); tempScore.SetParent(checkingPos); if (!_searchingList.Contains(tempPos)) { _searchingList.Add(tempPos); } } UpdateSearchBlock(pathFinding, nextX, nextY); } return(false); } }