public NavCubeData(NavCubeType t = NavCubeType.Blocked, int c = int.MaxValue) { type = t; cost = c; }
public void SetData(int x, int y, int z, NavCubeType type, int cost) { data[x, y, z].type = type; data[x, y, z].cost = cost; }
public void SearchPathAsync(NavCubeAgent agent, Vector3Int start, Vector3Int end, NavCubeType type = NavCubeType.Walking, int maxResolveTry = 4096) { if (_AsyncAgentSearch == null) { _AsyncAgentSearch = new Dictionary <NavCubeAgent, Thread>(); } Thread asyncThread = null; if (_AsyncAgentSearch.ContainsKey(agent)) { asyncThread = _AsyncAgentSearch[agent]; if (asyncThread.IsAlive) { asyncThread.Abort(); } } asyncThread = new Thread(() => { try { List <Vector3Int> path = GetPath(start, end, type, maxResolveTry, agent); agent.CompleteSearchPath(path); } catch (ThreadAbortException) { Debug.Log("Thread Exit !"); } }); asyncThread.Start(); if (_AsyncAgentSearch.ContainsKey(agent)) { _AsyncAgentSearch[agent] = asyncThread; } else { _AsyncAgentSearch.Add(agent, asyncThread); } }
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 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 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); } } }