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