Nav Graph (C# / Unity)

 
/*!
  \file NavGraph2D.cs
  \author David Knopp
  \details A 2D navigation graph utilizing Unity's PolygonCollider2D
  \copyright All content © 2014-2015 DigiPen (USA) Corporation, all rights reserved.
*/

using UnityEngine;
using System.Collections.Generic;
using Utils;
 
[RequireComponent(typeof(PolygonCollider2D))]
/*!
  \brief 2D navigation graph defined by a PolygonCollider2D
*/
public class NavGraph2D : ISingleton
{
  // public members / properties
  public float nodeOffset = 0.5f; //!< Amount to offset nodes from edges
 
  //!< The polygon defining the graph
  public Polygon2D polygon
  {
    get { return _polys[0]; }
  }
 
  /**
    \brief Calculates the distance between two nodes
    \param nodeA First node
    \param nodeB Second node
    \return Distance between nodeA and nodeB
  */
  public float Distance(int nodeA, int nodeB)
  {
    // get nodes by id
    GraphNode2D a = _graph.GetNode(nodeA);
    GraphNode2D b = _graph.GetNode(nodeB);
 
    return (a.pos - b.pos).magnitude;
  }
 
  /**
    \brief Determines if a given point is on the nav graph
    \param point The point to check
    \return Whether or not the point lies on the graph
  */
  public bool PointOnGraph(Vector2 point)
  {
    if (_polys[0].CollidingWith(point))
    {
      // check if point within an obstacle
      foreach (NavGraph2DObstacle obstacle in _obstacles)
      {
        if (obstacle.polygon.CollidingWith(point))
          return false;
      }
 
      return true;
    }
 
    return false;
  }
 
  /**
    \brief Checks if there is a valid line of sight between two points
    \param start The first point
    \param end The second point
    \return Whether or not there is a valid line of sight
  */
  public bool ValidLOS(Vector2 start, Vector2 end)
  {
    // check intersection between graph's polygons
    foreach (Polygon2D poly in _polys)
    {
      for (int i = 0; i < poly.numPoints; ++i)
      {
        int next = NextIndex(poly, i);
        if (RGMath.LineSegment2DInteresect(start, end, poly.points[i], poly.points[next]))
          return false;
      }
    }
    
    return true;
  }
 
  /**
    \brief Initializes the navigation graph
  */
  private void Awake() 
  {
    _graph = new NavGraph();
    _obstacles = new List(GetComponentsInChildren());
    _polys = new List();
 
    // add mesh polygon
    PolygonCollider2D mesh = GetComponent();
    _polys.Add(new Polygon2D(mesh));
 
    // add obstacle polygons
    foreach (NavGraph2DObstacle obstacle in _obstacles)
    {
      _polys.Add(new Polygon2D(obstacle.mesh, Polygon2D.WindingOrder.CCW));
    }
    
    CreateNodeGraph();
    LinkNodeGraph();
  }
  
  /**
    \brief Constructs nodes at the corners of each polygon
  */
  private void CreateNodeGraph()
  {
    // no nodes possible if mesh is a line
    if (polygon.numPoints < 3)
      return;
 
    // remove existing nodes
    _graph.Clear();
 
    int curID = 0;
    foreach (Polygon2D poly in _polys)
    {
      // store all interior vertices of polygon
      for (int i = 0; i < poly.numPoints; ++i)
      {
        // find prev and next indices
        int prev = PreviousIndex(poly, i);
        int next = NextIndex(poly, i);
 
        if (IsInteriorVertex(poly.points[i], poly.points[prev], poly.points[next]))
        {
          // push node in, away from edges
          Vector2 offset = GetCornerOffset(poly.points[i], poly.points[prev], poly.points[next]);
 
          // add to graph if on mesh
          Vector2 nodePos = poly.points[i] + offset;
          if (PointOnGraph(nodePos))
          {
            int id = curID++;
            _graph.AddNode(new GraphNode2D(nodePos, id), id);
          }
        }
      }
    }
  }
 
  /**
    \brief Links the graph's nodes together
  */
  private void LinkNodeGraph()
  {
    List nodes = _graph.GetNodeList();

    // iterate through all nodes and link if possible
    for (int i = 0; i < nodes.Count; ++i)
    {
      GraphNode2D from = nodes[i];

      // start at i + 1, skipping any duplicate pairs
      for (int j = i + 1; j < nodes.Count; ++j)
      {
        GraphNode2D to = nodes[j];

        // connect nodes if line of sight is valid
        if (ValidLOS(from.pos, to.pos))
        {
          float cost = Distance(from.id, to.id);
          _graph.AddBiDirectionalEdge(from.id, to.id, cost);
        }
      }
    }
  }
 
  /**
    \brief Calculates the offset position of a given point (offset away from edges)
    \param cur The point to calculate the offset of
    \param prev The previous point in the polygon
    \param next The next point in the polygon
    \return The offset of the current position
  */
  private Vector2 GetCornerOffset(Vector2 cur, Vector2 prev, Vector2 next)
  {
    // create vectors to/from neighbor points
    Vector2 fromPrev = cur - prev;
    Vector2 fromNext = cur - next;
    
    // push node in, away from edges
    Vector2 avg = fromPrev.normalized + fromNext.normalized;
    avg.Normalize();
 
    return (avg * nodeOffset);
  }
  
  /**
    \brief Checks if a vertex forms a concave edge on a polygon
    \param cur The point to check
    \param prev The previous point in the polygon
    \param next The next point in the polygon
    \return Whether or not the vertex is interior
  */
  private bool IsInteriorVertex(Vector2 cur, Vector2 prev, Vector2 next)
  {
    // create vectors to/from neighbors
    Vector2 fromPrev = cur - prev;
    Vector2 toNext = next - cur;
 
    float cross = RGMath.Vector2Cross(fromPrev, toNext);
    return (cross > 0.0f);
  }
 
  /**
    \brief Calculates the previous index of a node
    \param poly The polygon used for indexing
    \param i The current index
    \return The previous index (wrapped to the end if necessary)
  */
  private int PreviousIndex(Polygon2D poly, int i)
  {
    int prev = i - 1;
  
    // wrap index to last point
    if (prev < 0)
      prev = poly.numPoints - 1;
 
    return prev;
  }
 
  /**
    \brief Calculates the next index of a node
    \param poly The polygon used for indexing
    \param i The current index
    \return The next index (wrapped to the beginning if necessary)
  */
  private int NextIndex(Polygon2D poly, int i)
  {
    // wrap index back to beginning
    return ((i + 1) % poly.numPoints);
  }
 
  // private members
  private NavGraph _graph; //!< Underlying node graph
  private List _obstacles; //!< Obstacles of the graph
  private List _polys; //!< All polygons that make up the graph
}