示例#1
0
    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);
        }
    }
示例#2
0
    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);
    }
示例#3
0
    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);
            }
        }
    }