public void AddNeighbour(NavCubeNode current, Vector3Int end, Vector3Int dir, float cost, NavCubeType searchType, NavCubeAgent agent)
{
Vector3Int pos = current.position + dir;
NavCubeChunk.NavCubeData data = GetData(pos);
if (agent != null)
{
for (int i = 0; i < agent.height; i++)
{
int ii = i - agent.offset;
if (GetData(pos + Vector3Int.up * ii).type == NavCubeType.Blocked)
{
return;
}
}
}
else
{
if (data.type == NavCubeType.Blocked)
{
return;
}
}
float heuristic = pos.Manhattan(end);
NavCubeNode ncn = new NavCubeNode(pos, current.cost + data.cost * cost, heuristic, current);
if (closedList.Contains(ncn))
{
return;
}
if (openList.Contains(ncn))
{
NavCubeNode ol_ncn = openList.Find(x => x.position.Equals(pos));
if (ol_ncn.total > ncn.total)
{
ol_ncn.cost = ncn.cost;
ol_ncn.heuristic = ncn.heuristic;
ol_ncn.parent = ncn.parent;
}
return;
}
if (searchType == NavCubeType.Walking)
{
if (data.type == NavCubeType.Walking)
{
openList.Add(ncn);
}
}
else
{
openList.Add(ncn);
}
}
public List <Vector3Int> GetPath(Vector3Int start, Vector3Int end, NavCubeType type = NavCubeType.Walking, int maxResolveTry = 4096, NavCubeAgent agent = null)
{
closedList = new List <NavCubeNode>();
openList = new List <NavCubeNode>();
List <Vector3Int> path = null;
if (agent != null)
{
for (int i = 0; i < agent.height; i++)
{
int ii = i - agent.offset;
if (GetData(start + Vector3Int.up * ii).type == NavCubeType.Blocked || GetData(end + Vector3Int.up * ii).type == NavCubeType.Blocked)
{
return(null);
}
}
}
else
{
if (GetData(start).type == NavCubeType.Blocked || GetData(end).type == NavCubeType.Blocked)
{
return(null);
}
}
openList.Add(new NavCubeNode(start.Copy(), 0.0f, 0.0f));
NavCubeNode current = null;
while (openList.Count > 0)
{
current = openList[0];
openList.RemoveAt(0);
if (current.Equals(end))
{
break;
}
if (agent == null || (agent.movedir & 1) == 1)
{
AddNeighbour(current, end, new Vector3Int(1, 0, 0), NavCubeNode.Cost100, type, agent);
AddNeighbour(current, end, new Vector3Int(-1, 0, 0), NavCubeNode.Cost100, type, agent);
AddNeighbour(current, end, new Vector3Int(0, 1, 0), NavCubeNode.Cost100, type, agent);
AddNeighbour(current, end, new Vector3Int(0, -1, 0), NavCubeNode.Cost100, type, agent);
AddNeighbour(current, end, new Vector3Int(0, 0, 1), NavCubeNode.Cost100, type, agent);
AddNeighbour(current, end, new Vector3Int(0, 0, -1), NavCubeNode.Cost100, type, agent);
}
if (agent == null || (agent.movedir & 2) == 2)
{
AddNeighbour(current, end, new Vector3Int(1, -1, 0), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(-1, -1, 0), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(0, -1, 1), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(0, -1, -1), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(-1, 0, -1), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(1, 0, -1), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(1, 0, 1), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(-1, 0, 1), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(1, 1, 0), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(-1, 1, 0), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(0, 1, 1), NavCubeNode.Cost101, type, agent);
AddNeighbour(current, end, new Vector3Int(0, 1, -1), NavCubeNode.Cost101, type, agent);
}
if (agent == null || (agent.movedir & 4) == 4)
{
AddNeighbour(current, end, new Vector3Int(-1, -1, -1), NavCubeNode.Cost111, type, agent);
AddNeighbour(current, end, new Vector3Int(1, -1, -1), NavCubeNode.Cost111, type, agent);
AddNeighbour(current, end, new Vector3Int(1, -1, 1), NavCubeNode.Cost111, type, agent);
AddNeighbour(current, end, new Vector3Int(-1, -1, 1), NavCubeNode.Cost111, type, agent);
AddNeighbour(current, end, new Vector3Int(-1, 1, -1), NavCubeNode.Cost111, type, agent);
AddNeighbour(current, end, new Vector3Int(1, 1, -1), NavCubeNode.Cost111, type, agent);
AddNeighbour(current, end, new Vector3Int(1, 1, 1), NavCubeNode.Cost111, type, agent);
AddNeighbour(current, end, new Vector3Int(-1, 1, 1), NavCubeNode.Cost111, type, agent);
}
AddPathLink(current, end, type, agent);
openList = openList.OrderBy(x => x.total).ToList <NavCubeNode>();
closedList.Add(current);
if (closedList.Count >= maxResolveTry)
{
break;
}
}
if (current != null && current.Equals(end))
{
Debug.Log($"Path found ! (Search Stack: {closedList.Count})");
path = new List <Vector3Int>();
NavCubeNode node = current;
while (node != null)
{
path.Insert(0, node.position);
node = node.parent;
}
}
else
{
Debug.Log("Path not found !");
}
closedList.Clear();
openList.Clear();
return(path);
}
public void AddPathLink(NavCubeNode current, Vector3Int end, NavCubeType searchType, NavCubeAgent agent)
{
if (links == null)
{
links = new List <MavCubeLink>();
return;
}
foreach (MavCubeLink link in links)
{
Vector3Int nextPos;
if (link.InPosA(current.position))
{
nextPos = current.position + link.AtoB;
}
else if (link.InPosB(current.position))
{
nextPos = current.position + link.BtoA;
}
else
{
continue;
}
NavCubeChunk.NavCubeData data = GetData(nextPos);
if (agent != null)
{
bool noValise = false;
for (int i = 0; i < agent.height; i++)
{
int ii = i - agent.offset;
if (GetData(nextPos + Vector3Int.up * ii).type == NavCubeType.Blocked)
{
noValise = true;
break;
}
}
if (noValise)
{
continue;
}
}
else
{
if (data.type == NavCubeType.Blocked)
{
continue;
}
}
float heuristic = nextPos.Manhattan(end);
NavCubeNode ncn = new NavCubeNode(nextPos, current.cost + link.cost * link.movingCost, heuristic, current);
if (closedList.Contains(ncn))
{
continue;
}
if (openList.Contains(ncn))
{
NavCubeNode ol_ncn = openList.Find(x => x.position.Equals(nextPos));
if (ol_ncn.total > ncn.total)
{
ol_ncn.cost = ncn.cost;
ol_ncn.heuristic = ncn.heuristic;
ol_ncn.parent = ncn.parent;
}
continue;
}
if (searchType == NavCubeType.Walking)
{
if (data.type == NavCubeType.Walking)
{
openList.Add(ncn);
}
}
else
{
openList.Add(ncn);
}
}
}