Subtracting an Obstacle without recalculating the navigation mesh

[BlueOctaveParticipant]

Hi all,
I was wondering if it would be possible to remove an obstacle and have the AI be able to traverse the space that is now opened WITHOUT recalculating the navigation mesh. Any ideas?

[SigilKeymaster]

Yes. The biggest problem with this though is making sure the hole that you are filling is convex. It’s easy enough to find the hole in the mesh (given a hint at least) but filling the hole requires you test the finished polygon for convexity which complicates things.

I’ll need to take a look at some of our code and see if I have what I need for this. I’ll take a look at it tomorrow some time and see if I can quickly put something together for you.

[BlueOctaveParticipant]

Awesome, Thank you Sigil, I appreciate it.

[SigilKeymaster]

Haven’t forgotten about this, just got busy these last couple days. Will probably post something half working for tomorrow.

[BlueOctaveParticipant]

Thanks for the update and taking the time to help me out with this.

[SigilKeymaster]

Let me just throw this out right now while I have it current.

The main class:

using RAIN.Navigation.Graph;
using RAIN.Navigation.NavMesh;
using System;
using System.Collections.Generic;
using UnityEngine;
public class NavMeshFiller : MonoBehaviour
{
    public class ObjectPool<T> where T : class
    {
        Stack<T> _freeList = new Stack<T>();
        public T Allocate()
        {
            if (_freeList.Count > 0)
                return _freeList.Pop();
            else
                return Activator.CreateInstance<T>();
        }
        public void Free(T aObject)
        {
            _freeList.Push(aObject);
        }
    }
    public class ConnectionPool
    {
        Stack<NavigationGraphEdge> _freeList = new Stack<NavigationGraphEdge>();
        public NavigationGraphEdge Allocate()
        {
            if (_freeList.Count > 0)
                return _freeList.Pop();
            else
                return new NavigationGraphEdge(null, null, 0);
        }
        public void Free(NavigationGraphEdge aObject)
        {
            _freeList.Push(aObject);
        }
    }
    [SerializeField]
    private float _radius = 1f;
    [SerializeField]
    private bool _fillHoles = false;
    private NavMeshRig[] _rigs = new NavMeshRig[0];
    private List<List<NavMeshEdge>> _holes = new List<List<NavMeshEdge>>();
    private List<List<NavigationGraphEdge>> _holeConnections = new List<List<NavigationGraphEdge>>();
    private List<List<NavMeshEdge>> _partialHoles = new List<List<NavMeshEdge>>();
    private bool _storedFill = false;
    private Vector3 _storedPosition = Vector3.zero;
    private Vector3 _storedScale = Vector3.zero;
    private List<NavMeshPoly> _workingPolyList = new List<NavMeshPoly>();
    private HashSet<NavMeshEdge> _workingUsedEdges = new HashSet<NavMeshEdge>();
    private ObjectPool<List<NavMeshEdge>> _freeHoles = new ObjectPool<List<NavMeshEdge>>();
    private ConnectionPool _freeConnection = new ConnectionPool();
    private ObjectPool<List<NavigationGraphEdge>> _freeConnectionList = new ObjectPool<List<NavigationGraphEdge>>();
    public float Radius
    {
        get { return _radius; }
        set { _radius = value; }
    }
    public bool FillHoles
    {
        get { return _fillHoles; }
        set
        {
            if (_fillHoles == value)
                return;
            _fillHoles = value;
            UpdateFilledHoles();
        }
    }
    public NavMeshRig[] NavMeshRigs
    {
        get { return _rigs; }
        set { _rigs = value; }
    }
    public List<List<NavMeshEdge>> Holes
    {
        get { return _holes; }
    }
    public List<List<NavMeshEdge>> PartialHoles
    {
        get { return _partialHoles; }
    }
    public float UniformScale
    {
        get
        {
            return Mathf.Min(transform.localScale.x,
                             Mathf.Min(transform.localScale.y, transform.localScale.z));
        }
    }
    public void Start()
    {
        // Store our initial position and scale
        _storedFill = _fillHoles;
        _storedPosition = transform.position;
        _storedScale = transform.localScale * _radius;
        // Update our rigs
        UpdateRigs();
        // Find all holes
        FindAllHoles();
    }
    public void Update()
    {
        // Check for any change in our position or bounds
        if (!Mathf.Approximately(_storedPosition.sqrMagnitude, transform.position.sqrMagnitude) ||
            !Mathf.Approximately(_storedScale.sqrMagnitude, (transform.localScale * _radius).sqrMagnitude))
        {
            _storedFill = _fillHoles;
            _storedPosition = transform.position;
            _storedScale = transform.localScale * _radius;
            // Find them
            FindAllHoles();
        }
        // Check for a change to our fill option (for the editor)
        else if (_storedFill != _fillHoles)
        {
            _storedFill = _fillHoles;
            // Update the holes
            UpdateFilledHoles();
        }
    }
    public void OnDrawGizmosSelected()
    {
        Gizmos.DrawWireSphere(transform.position, UniformScale * _radius);
    }
    public void UpdateRigs()
    {
        _rigs = FindObjectsOfType<NavMeshRig>();
    }
    public void FindAllHoles()
    {
        // Free up any holes we already have
        for (int i = 0; i < _holes.Count; i++)
            _freeHoles.Free(_holes[i]);
        _holes.Clear();
        for (int i = 0; i < _partialHoles.Count; i++)
            _freeHoles.Free(_partialHoles[i]);
        _partialHoles.Clear();
        // Free up our connections too
        for (int i = 0; i < _holeConnections.Count; i++)
            FreeConnections(_holeConnections[i]);
        _holeConnections.Clear();
        for (int i = 0; i < _rigs.Length; i++)
            FindHoles(_rigs[i]);
    }
    public void FindHoles(NavMeshRig aRig)
    {
        Bounds tLocalBounds = new Bounds(aRig.NavMesh.Graph.MountInverseTransform.MultiplyPoint(transform.position),
                                         Vector3.one * UniformScale * _radius * 2);
        // We keep these lists around to save on garbage collection
        _workingPolyList.Clear();
        _workingUsedEdges.Clear();
        // Go straight to the oct tree for our data
        aRig.NavMesh.Graph.PolyTree.GetCollisions(tLocalBounds, _workingPolyList);
        // Go through our polys and find any edges without polys
        for (int i = 0; i < _workingPolyList.Count; i++)
        {
            for (int j = 0; j < _workingPolyList[i].EdgeCount; j++)
            {
                NavMeshEdge tEdge = _workingPolyList[i].GetEdgeNode(j);
                // If our edge only has one poly, we haven't seen it yet,
                // and is in our bounds, trace it
                if (tEdge.PolyCount == 1 &&
                    _workingUsedEdges.Add(tEdge) &&
                    tLocalBounds.Contains(tEdge.PointOne) &&
                    tLocalBounds.Contains(tEdge.PointTwo))
                {
                    // See if we can trace a hole with this edge
                    List<NavMeshEdge> tPossibleHole = _freeHoles.Allocate();
                    tPossibleHole.Clear();
                    // If we can trace it, add it to our holes
                    if (TraceHole(tLocalBounds, _workingPolyList[i], j, tPossibleHole, _workingUsedEdges))
                    {
                        _holes.Add(tPossibleHole);
                        _holeConnections.Add(CreateConnections(tPossibleHole));
                        if (_fillHoles)
                            ConnectHole(_holeConnections[_holeConnections.Count - 1]);
                        else
                            DisconnectHole(_holeConnections[_holeConnections.Count - 1]);
                    }
                    else
                        _partialHoles.Add(tPossibleHole);
                }
            }
        }
    }
    private void UpdateFilledHoles()
    {
        if (_fillHoles)
        {
            for (int i = 0; i < _holeConnections.Count; i++)
                ConnectHole(_holeConnections[i]);
        }
        else
        {
            for (int i = 0; i < _holeConnections.Count; i++)
                DisconnectHole(_holeConnections[i]);
        }
    }
    private bool TraceHole(Bounds aBounds,
                           NavMeshPoly aPoly,
                           int aEdgeIndex,
                           List<NavMeshEdge> aHole,
                           HashSet<NavMeshEdge> aUsedEdges)
    {
        // First one always counts
        aHole.Add(aPoly.GetEdgeNode(aEdgeIndex));
        NavMeshPoly tCurrentPoly = aPoly;
        int tCurrentEdgeIndex = aEdgeIndex;
        // We always skip our first one, as it is either a part of our hole,
        // or a connecting piece
        for (int i = 1; i < tCurrentPoly.EdgeCount; i++)
        {
            NavMeshEdge tNextEdge = tCurrentPoly.GetEdgeNode((tCurrentEdgeIndex + i) % tCurrentPoly.EdgeCount);
            // If we don't have a connection, add it to the list
            if (tNextEdge.PolyCount == 1)
            {
                // If the edge matches our starting point, we're done
                if (tNextEdge == aHole[0])
                    return true;
                // If we haven't seen the edge yet and it's in our
                // bounds, add it
                if (aUsedEdges.Add(tNextEdge) &&
                    aBounds.Contains(tNextEdge.PointOne) &&
                    aBounds.Contains(tNextEdge.PointTwo))
                {
                    aHole.Add(tNextEdge);
                }
                // Otherwise we're done
                else
                    break;
            }
            // Otherwise, move to the next poly and find our first edge
            else
            {
                tCurrentPoly = tNextEdge.GetPolyNode(0) == tCurrentPoly ? tNextEdge.GetPolyNode(1) : tNextEdge.GetPolyNode(0);
                for (tCurrentEdgeIndex = 0; tCurrentEdgeIndex < tCurrentPoly.EdgeCount; tCurrentEdgeIndex++)
                {
                    if (tCurrentPoly.GetEdgeNode(tCurrentEdgeIndex) == tNextEdge)
                        break;
                }
                // Reset our loop and start again
                i = 0;
            }
        }
        return false;
    }
    private List<NavigationGraphEdge> CreateConnections(List<NavMeshEdge> aHole)
    {
        List<NavigationGraphEdge> tConnections = _freeConnectionList.Allocate();
        tConnections.Clear();
        for (int i = 0; i < aHole.Count; i++)
        {
            for (int j = 1; j < aHole.Count; j++)
            {
                NavigationGraphEdge tOneConnection = _freeConnection.Allocate();
                tOneConnection.FromNode = aHole[i];
                tOneConnection.ToNode = aHole[j];
                tOneConnection.StaticCost = (aHole[i].Center - aHole[j].Center).magnitude;
                aHole[i].AddEdgeOut(tOneConnection);
                aHole[j].AddEdgeIn(tOneConnection);
                tConnections.Add(tOneConnection);
                NavigationGraphEdge tTwoConnection = _freeConnection.Allocate();
                tTwoConnection.FromNode = aHole[j];
                tTwoConnection.ToNode = aHole[i];
                tTwoConnection.StaticCost = tOneConnection.StaticCost;
                aHole[j].AddEdgeOut(tTwoConnection);
                aHole[i].AddEdgeIn(tTwoConnection);
                tConnections.Add(tTwoConnection);
            }
        }
        return tConnections;
    }
    private void ConnectHole(List<NavigationGraphEdge> aConnections)
    {
        for (int i = 0; i < aConnections.Count; i++)
            aConnections[i].OverrideCost = -1;
    }
    private void DisconnectHole(List<NavigationGraphEdge> aConnections)
    {
        for (int i = 0; i < aConnections.Count; i++)
            aConnections[i].OverrideCost = float.MaxValue;
    }
    private void FreeConnections(List<NavigationGraphEdge> aConnections)
    {
        for (int i = 0; i < aConnections.Count; i++)
        {
            aConnections[i].FromNode.RemoveEdgeOut(aConnections[i]);
            aConnections[i].ToNode.RemoveEdgeIn(aConnections[i]);
            _freeConnection.Free(aConnections[i]);
        }
        _freeConnectionList.Free(aConnections);
    }
}

An editor for it (put in an editor folder):

using RAIN.Navigation.Graph;
using RAIN.Navigation.NavMesh;
using RAINEditor.Utility;
using System.Collections.Generic;
using UnityEditor;
using UnityEngine;
[CustomEditor(typeof(NavMeshFiller))]
public class NavMeshFillerEditor : Editor
{
    private Material _lineMaterial = null;
    public void OnSceneGUI()
    {
        NavMeshFiller tFiller = (NavMeshFiller)target;
        DrawHoles(tFiller.Holes, Color.green);
        DrawConnections(tFiller.Holes);
        DrawHoles(tFiller.PartialHoles, Color.red);
    }
    private void DrawHoles(List<List<NavMeshEdge>> aHoles, Color aColor)
    {
        if (_lineMaterial == null)
        {
            _lineMaterial = new Material(Shader.Find("Unlit/Color"));
            _lineMaterial.hideFlags = HideFlags.HideAndDontSave;
        }
        _lineMaterial.color = aColor;
        _lineMaterial.SetPass(0);
        for (int i = 0; i < aHoles.Count; i++)
        {
            NavMeshRig tRig = Visualization.GetNavMeshRigForGraph((NavMeshPathGraph)aHoles[i][0].Graph);
            Matrix4x4 tOffset = Matrix4x4.TRS(new Vector3(0, tRig.NavMesh.VisualHeightOffset + 0.1f, 0), Quaternion.identity, Vector3.one);
            GL.PushMatrix();
            GL.MultMatrix(aHoles[i][0].Graph.MountTransform * tOffset);
            GL.Begin(GL.LINES);
            for (int j = 0; j < aHoles[i].Count; j++)
            {
                GL.Vertex(aHoles[i][j].PointOne);
                GL.Vertex(aHoles[i][j].PointTwo);
            }
            GL.End();
            GL.PopMatrix();
        }
    }
    private void DrawConnections(List<List<NavMeshEdge>> aHoles)
    {
        if (_lineMaterial == null)
        {
            _lineMaterial = new Material(Shader.Find("Unlit/Color"));
            _lineMaterial.hideFlags = HideFlags.HideAndDontSave;
        }
        for (int i = 0; i < 2; i++)
        {
            _lineMaterial.color = i == 0 ? Color.white : Color.red;
            _lineMaterial.SetPass(0);
            for (int j = 0; j < aHoles.Count; j++)
            {
                NavMeshRig tRig = Visualization.GetNavMeshRigForGraph((NavMeshPathGraph)aHoles[j][0].Graph);
                Matrix4x4 tOffset = Matrix4x4.TRS(new Vector3(0, tRig.NavMesh.VisualHeightOffset + 0.1f, 0), Quaternion.identity, Vector3.one);
                GL.PushMatrix();
                GL.MultMatrix(aHoles[j][0].Graph.MountTransform * tOffset);
                GL.Begin(GL.LINES);
                for (int k = 0; k < aHoles[j].Count; k++)
                {
                    for (int m = 0; m < aHoles[j][k].OutEdgeCount; m++)
                    {
                        NavigationGraphEdge tEdge = aHoles[j][k].EdgeOut(m);
                        // We don't want to display internal edges, just our hole
                        if (((NavMeshEdge)tEdge.FromNode).PolyCount != 1 || ((NavMeshEdge)tEdge.ToNode).PolyCount != 1)
                            continue;
                        if (((NavMeshEdge)tEdge.FromNode).GetPolyNode(0) == ((NavMeshEdge)tEdge.ToNode).GetPolyNode(0))
                            continue;
                        if ((i == 0 && tEdge.Cost < float.MaxValue) || (i == 1 && tEdge.Cost == float.MaxValue))
                        {
                            GL.Vertex(tEdge.FromNode.LocalPosition);
                            GL.Vertex(tEdge.ToNode.LocalPosition);
                        }
                    }
                }
                GL.End();
                GL.PopMatrix();
            }
        }
    }
}

Add the NavMeshFiller to an obstacle that is already creating a hole in the navigation mesh. Adjust the radius on the NavMeshFiller to make the sphere encompass the entire hole. When you press play you can fill/unfill the hole by checking the fill holes (or setting in code).

So a few things about the NavMeshFiller: It is dynamic in the sense that you can move it around at runtime, but it isn’t super fast as the radius gets large; It’ll fill more than one hole if the sphere is large enough (which may or may not be desired); and it doesn’t differentiate between holes and borders right now (may never), which shouldn’t matter as long as the radius is small.

Three big issues that I will try to address: 1) the hole has to be convex 2) the resulting connections are complex, and 3) there currently isn’t a way to mark a navigation mesh as changed.

1) If you attempt to fill a concave hole, it’ll create some invalid connections, may result in an infinite path find (not positive on that), will probably be wrong no matter what though.
2) The connections are complex in that there is a large number of them, but likely it won’t slow things down due to the way AStar works.
3) If you fill a hole while an AI is pathing it won’t know there is a better path at that point unless you tell the AI to repath.

I have things that can be done for 2 and 3, not positive about 1 though (it’s doable, but how hard is the question). Well I will get this generally working and then probably let it go, as I’m thinking of including some of this in RAIN pretty soon, so hopefully you’ll be able to discard this custom work for an official release soon.

Try it with a simple scene first, just to see how it works: plane for ground/navigation mesh, cube for obstacle/filler, cube for AI, and a simple behavior tree to show the pathing. Get that going so you see how it works, and then work it into your larger project.

Let me know if you have questions.

• This reply was modified 6 months, 1 week ago by  Sigil. Reason: general stuff

[BlueOctaveParticipant]

I can’t seem to be able to get this to work. I’ll wait until you get this worked into the release build and comment on it then. Thanks again for all your help, I can’t wait until the next version is released!

[Author]

Posts