NavMeshLinksGenerator.cs

using System;
using UnityEngine;
using System.Collections.Generic;
using System.Linq;
using UnityEngine.AI;


public class NavMeshLinksGenerator : MonoBehaviour
{
    [Serializable]
    public class Edge
    {
        public Vector3 start;
        public Vector3 end;
        public float length;
        public Quaternion facingNormal;
    }
    
    public Transform linkPrefab;
    public float tileWidth = 5f;

    [Header( "Settings" )] 
    public float maxDropHeight = 3f;
    public float maxJumpHeight = 3f;
    public float maxJumpLength = 3f;
    public LayerMask raycastLayerMask = -1;


    [Header( "EdgeNormal" )] 
    public bool invertFacingNormal = false;// not sure why this is used
    public bool dontAllignYAxis = false; // note sure why this is used
    
    Mesh mesh;
    List< Edge > edges = new List< Edge >();
    List< Edge > edgesToDelete = new List< Edge >();
    float agentRadius;
    const float edgeDuplicateTolerance = 0.01f;

    [InspectorButton("Generate")] public bool _GenerateLinks;
    [InspectorButton("ClearLinks")] public bool _ClearLinks;
    
    public void Generate()
    {
        DebugDrawHelper.Clear();
        
        ClearLinks(); 
        FindEdges();
        PlaceLinksAlongEdges();
        
        EditorSafeUtils.SetSceneDirty(gameObject);
        Debug.Log("Generated " + transform.GetComponentsInChildren<NavMeshLink>().Length + " links.");
    }

    public void ClearLinks()
    {
        List< NavMeshLink > navMeshLinkList = GetComponentsInChildren<NavMeshLink> (true ).ToList ( );
        while ( navMeshLinkList.Count > 0 )
        {
            GameObject obj = navMeshLinkList[0].gameObject;
            if ( obj != null ) DestroyImmediate ( obj );
            navMeshLinkList.RemoveAt ( 0 );
        }
        
        // !!!
        transform.Clear();
    }

    void FindEdges()
    {
        edges.Clear();
        edgesToDelete.Clear();
        var tr = NavMesh.CalculateTriangulation();

        mesh = new Mesh()
        {
            vertices = tr.vertices,
            triangles = tr.indices
        };
        
        // find each edge of a triangle, and figure out if it's an edge of the nav mesh
        for ( int i = 0; i < mesh.triangles.Length - 1; i += 3 )
        {
            GetEdgeFromTrianglePoint( mesh, i, i + 1 );
            GetEdgeFromTrianglePoint( mesh, i + 1, i + 2 );
            GetEdgeFromTrianglePoint( mesh, i + 2, i );
        }
        
        // calculate lengths and normals
        foreach ( Edge edge in edges )
        {
            edge.length = Vector3.Distance(edge.start,edge.end);
            edge.facingNormal = Quaternion.LookRotation( Vector3.Cross( edge.end - edge.start, Vector3.up ) );
            if( dontAllignYAxis )
            {
                edge.facingNormal = Quaternion.LookRotation(
                    edge.facingNormal * Vector3.forward,
                    Quaternion.LookRotation( edge.end - edge.start ) * Vector3.up
                );
            }
            if( invertFacingNormal ) edge.facingNormal = Quaternion.Euler( Vector3.up * 180 ) * edge.facingNormal;

            // mark invalid edges for discard
            NavMeshHit hit;
            if (NavMesh.SamplePosition(Vector3Extension.Average(edge.start, edge.end) + edge.facingNormal * Vector3.forward * 0.1f,
                out hit, 0.05f, NavMesh.AllAreas))
            {
                edgesToDelete.Add(edge);
            }
            
        }
        
        foreach (var edge in edgesToDelete)
        {
            edges.Remove(edge);
        }
        
        // Debug
        foreach (var edge in edges)
        {
            Debug.DrawLine(edge.start + Vector3.up * 0.1f, edge.end + Vector3.up * 0.2f, Color.red, 5);
            DebugExtension.DrawPoint(edge.start + Vector3.up * 0.1f, 0.05f, Color.red, 5);
            DebugExtension.DrawPoint(edge.end + Vector3.up * 0.2f, 0.05f, Color.red, 5);
        }
        Debug.Log("Edges: " + edges.Count);
        Debug.Log("Edges To Delete: " + edgesToDelete.Count);
    }

  // Finds all triangle edges of the nav mesh and figures out if they are edges
    void GetEdgeFromTrianglePoint( Mesh currMesh, int n1, int n2 )
    {
        Vector3 start = currMesh.vertices[ currMesh.triangles[ n1 ] ] ;
        Vector3 end = currMesh.vertices[ currMesh.triangles[ n2 ] ] ;

        Edge newEdge = new Edge() { start = start, end = end };

        var shouldAdd = true;
        
        //remove duplicates edges
        foreach ( Edge edge in edges )
        {
            
            var startOverlapsStart = MathExtension.CompareWithTolerance(edge.start, start, edgeDuplicateTolerance);
            var startOverlapsEnd = MathExtension.CompareWithTolerance(edge.start, end, edgeDuplicateTolerance);
            var endOverlapsStart = MathExtension.CompareWithTolerance(edge.end, start, edgeDuplicateTolerance);
            var endOverlapsEnd = MathExtension.CompareWithTolerance(edge.end, end, edgeDuplicateTolerance);

            // both points overlap, never add either
            if ((startOverlapsStart || startOverlapsEnd) && (endOverlapsStart || endOverlapsEnd))
            {
                shouldAdd = false;
                edges.Remove( edge );
                break;
            }
        }
        
        if(shouldAdd)
        {
            
            edges.Add(newEdge);
        }
    }
    
    void PlaceLinksAlongEdges()
    {
        agentRadius = NavMesh.GetSettingsByIndex( 0 ).agentRadius;
        foreach ( Edge edge in edges )
        {
            int tilesInEdge = ( int ) Mathf.Clamp( edge.length / tileWidth, 0, 10000 );
            for ( int i = 0; i < tilesInEdge; i++ ) //every edge length segment
            {
                Vector3 position = Vector3.Lerp(edge.start, edge.end, ( float ) i / ( float ) tilesInEdge);
                MakeLinksFromPosition( position, edge.facingNormal );
            }
        }
    }

    void MakeLinksFromPosition( Vector3 startPos, Quaternion normal )
    {
        var amountOfRays = (int)(maxJumpLength / tileWidth);
        var alreadyHitHeights = new List<float>();

        // create folder
        var parentFolder = new GameObject("LinkFolder").transform;
        parentFolder.SetParent(transform);
        int counter = 0;
        
        // do a bunch of vertical rays, goign down
        // horizontally spread out
        for (int i = 0; i < amountOfRays; i++)
        {
            // increment distance for next ray
            var distance = Mathf.Lerp(0, maxJumpLength, i / (float) amountOfRays);
            Vector3 rayStart = startPos + normal * Vector3.forward * (agentRadius * 2 + distance);
            Vector3 rayEnd = rayStart - Vector3.up * maxDropHeight * 1.1f;

            // raycast down in physics world, and then map it to navmesh
            RaycastHit raycastHit = new RaycastHit();
            if( Physics.Raycast( rayStart,(rayEnd - rayStart).normalized, out raycastHit,  Vector3.Distance(rayStart, rayEnd), raycastLayerMask.value))
            {
                NavMeshHit navMeshHit;
                if( NavMesh.SamplePosition( raycastHit.point, out navMeshHit, 0.5f, NavMesh.AllAreas ) )
                {
                    var endPos = navMeshHit.position;
                    
                    // check if valid
                    if( Vector3.Distance( startPos, navMeshHit.position ) > tileWidth    // jump at least 1 tile 
                        && !alreadyHitHeights.Any(hh => Mathf.Abs(hh - navMeshHit.position.y) < tileWidth))    // make sure we're not already good for this height...
                    {
                        // check if we're not inside something
                        
                        // figure out who's up 
                        var highPoint = startPos;
                        var lowPoint = endPos;
                        if (lowPoint.y > highPoint.y)
                        {
                            highPoint = endPos;
                            lowPoint = startPos;
                        }

                        // figure out, bidirectionally, if there's any colliders between start and end
                        // but we need to do this at a 90 degree angle rather than direct, to avoid intersecting corners
                        if (PhysicsExtension.Raycast(highPoint + Vector3.up * 0.1f,
                                lowPoint.withY(highPoint.y) + Vector3.up * 0.1f, out raycastHit, raycastLayerMask.value) ||    // chech horizontal
                            PhysicsExtension.Raycast(lowPoint.withY(highPoint.y) + Vector3.up * 0.1f,
                                lowPoint + Vector3.up * 0.1f, out raycastHit, raycastLayerMask.value)                    ||    // check vertical
                            PhysicsExtension.Raycast(lowPoint.withY(highPoint.y) + Vector3.up * 0.1f, 
                                highPoint + Vector3.up * 0.1f,out raycastHit, raycastLayerMask.value) ||    // chech horizontal reverse
                            PhysicsExtension.Raycast(lowPoint + Vector3.up * 0.1f,
                                lowPoint.withY(highPoint.y) + Vector3.up * 0.1f,out raycastHit, raycastLayerMask.value)) // chek vertical reverse
                        {
                            continue;
                        }
                        else
                        {
                            // Add the link!
                            alreadyHitHeights.Add(endPos.y);
                            var position = startPos - normal * Vector3.forward * 0.02f;
                            Transform newLinkTransform =
                                Instantiate(linkPrefab.transform, position, normal) as Transform;
                            var newLink = newLinkTransform.GetComponent<NavMeshLink>();
                            newLink.startPoint = Vector3.zero;
                            newLink.endPoint = newLink.transform.InverseTransformPoint(endPos);
                            newLink.UpdateLink();
                            newLink.bidirectional = (startPos.y - endPos.y) < maxJumpHeight;
                            newLinkTransform.SetParent(parentFolder);
                            newLinkTransform.gameObject.name = "Navmesh Link " + i;
                            counter++;
                        }
                    }
                }
            }
        }
        
        // cleanup
        if(counter > 0)
            parentFolder.name += " (" + counter + ")";
        else
            DestroyImmediate(parentFolder.gameObject);
    }
}