public bool FindGoal(PathType pathType, Vector2Int goal, Vector2Int start) { if (pathGrids[pathType][start.x, start.y].index == indexes[pathType]) { return(true); } Vector2Int pos = start; List <PathNode> checkNodes = new List <PathNode>(); List <PathNode> doneNodes = new List <PathNode>(); PathNode newNode; BuildingBase building; Vector3 rayPos; RaycastHit hit; newNode = new PathNode(pos.x, pos.y) { distance = Mathf.RoundToInt(TerrainGen.GetHexDistance(pos, TerrainGen.GetGridPosition2D(PathKeeper.goal))) }; newNode.sortCost = newNode.distance; checkNodes.Add(newNode); Vector2Int checkStep = new Vector2Int(); int rounds = 0; // while (pos != goal && checkNodes.Count > 0) { checkNodes.Sort(); if (debug) { Vector3 startDraw = TerrainGen.GetHexPosition(pos); Vector3 endDraw = TerrainGen.GetHexPosition(checkNodes[0].position); startDraw.y += 0.01f; endDraw.y += 0.01f; Debug.DrawLine(startDraw, endDraw, Color.green, 3); } pos = checkNodes[0].position; int ind = 0; if (pos.y % 2 == 0) { ind += 6; } for (int i = 0; i < 6; i++) { checkStep.x = pos.x + MyMath.hexOffSetGrid[ind + i].x; checkStep.y = pos.y + MyMath.hexOffSetGrid[ind + i].z; if (pathGrids[pathType][pos.x, pos.y].index == indexes[pathType]) { continue; } newNode = new PathNode(checkStep.x, checkStep.y); if (debug) { Debug.DrawLine(TerrainGen.GetHexPosition(pos), TerrainGen.GetHexPosition(checkStep), Color.red, 2); } newNode.distance = Mathf.RoundToInt(TerrainGen.GetHexDistance(checkStep, TerrainGen.GetGridPosition2D(PathKeeper.goal))); newNode.sortCost = CalcCost(pathType, pos, checkStep) + checkNodes[0].sortCost; newNode.cost = CalcCost(pathType, pos, checkStep) + newNode.distance; newNode.index = indexes[pathType]; newNode.from = pos; rayPos = TerrainGen.GetHexPosition(checkStep); rayPos.y += 2; if (Physics.Raycast(rayPos, Vector3.down, out hit, 3)) { building = hit.collider.GetComponentInParent <BuildingBase>(); if (building != null) { if (pathType != PathType.flight && building.HasProperty(BuildingProperties.blocking)) { newNode.sortCost += 100000; } } } int exists = checkNodes.FindIndex(a => a.position == newNode.position); if (exists == -1 && doneNodes.FindIndex(node => node.position == newNode.position) == -1) { checkNodes.Add(newNode); } else if (exists != -1) { PathNode existingNode = checkNodes[exists]; if (existingNode.sortCost > newNode.sortCost) { checkNodes.Remove(existingNode); checkNodes.Add(newNode); } } } doneNodes.Add(checkNodes[0]); checkNodes.RemoveAt(0); if (rounds > 500) { Debug.Log("Ending Pathfind with " + checkNodes.Count + " / " + doneNodes.Count); break; } rounds++; } doneNodes.Sort(); PathNode thisNode; PathNode fromNode; List <PathNode> adjacentNodes; bool atStart = false; rounds = 0; thisNode = doneNodes[0]; while (!atStart && rounds < 500 && thisNode != null) { fromNode = doneNodes.Find(node => node.position == thisNode.from); if (fromNode != null) { //fromNode.target = thisNode.position; //Find all nodes that came from fromNode adjacentNodes = doneNodes.FindAll(node => node.from == fromNode.position); foreach (PathNode node in adjacentNodes) { //See if the node is adjacent to out new target if (MyMath.hexOffSetGrid2D.Contains(thisNode.position - node.position)) { if (debug) { Vector3 drawStart = TerrainGen.GetHexPosition(node.position); Vector3 drawEnd = drawStart; drawEnd.y += 0.1f; Debug.DrawLine(drawStart, drawEnd, Color.magenta, 3); } if (node.target == node.position) { Debug.DrawRay(TerrainGen.GetHexPosition(node.position), Vector3.up, Color.red, 5); } node.target = thisNode.position; pathGrids[pathType][thisNode.x, thisNode.z] = node; //doneNodes.Remove(node); } else { if (debug) { Vector3 drawStart = TerrainGen.GetHexPosition(node.position); Vector3 drawEnd = drawStart; drawEnd.y += 0.1f; Debug.DrawLine(drawStart, drawEnd, Color.yellow, 3); } node.target = fromNode.position; pathGrids[pathType][thisNode.x, thisNode.z] = node; //doneNodes.Remove(node); } } } else { Debug.Log("No node found"); //pathGrids[pathType][thisNode.x, thisNode.z] = thisNode; } if (thisNode.target == Vector2Int.zero && thisNode.position != goal) { //Find best target by cost; Debug.Log("we got a lonly one"); } pathGrids[pathType][thisNode.x, thisNode.z] = thisNode; rounds++; thisNode = fromNode; } foreach (PathNode node in checkNodes) { //updateNodes[pathType].Add(node.position); } return(false); }