rj-action-library/Runtime/Math/Geometry/Path2.cs

using System.Collections;
using System.Collections.Generic;
using Godot;

namespace Rokojori
{
  public enum PointInPathResult
  {
    INSIDE,
    OUTSIDE,
    ERROR
  }

  

  public class Path2
  {
    float POINT_TO_CLOSE_DISTANCE = 0.0001f;
    float ANGLE_TO_SIMILAR_TRESHOLD = 0.00001f / 180f;
    int NUM_POINT_IN_PATH_TEST_RETRIES = 5;

    List<Vector2> _points = new List<Vector2>();

    public List<Vector2> points => _points;
    

    public Path2( List<Vector2> points )
    {
      this._points = points;
    }

    public int numPoints => _points.Count;
    public bool empty => _points.Count == 0;

    public bool cacheBounds = true;
    
    Vector2? min;
    Vector2? max;

    public Vector2 leftTop
    {
      get 
      {
        if ( cacheBounds && min != null )
        {
          return (Vector2) min;
        }
        
        var _min = _points[ 0 ];
        
        for ( int i = 1; i < _points.Count; i++ )
        {
          var point = _points[ i ];
          _min.X = Mathf.Min( point.X, _min.X );
          _min.Y = Mathf.Min( point.Y, _min.Y );
        } 

        min = _min;

        return (Vector2) min;

      }
    }

    public Vector2 rightBottom
    {
      get 
      {

         if ( cacheBounds && max != null )
        {
          return (Vector2) max;
        }

        var _max = _points[ 0 ];
        
        for ( int i = 1; i < _points.Count; i++ )
        {
          var point = _points[ i ];
          _max.X = Mathf.Max( point.X, _max.X );
          _max.Y = Mathf.Max( point.Y, _max.Y );
        }

        max = _max;
        
        return (Vector2) max;

      }
    }

    public void AddToNavigationPolygon( NavigationPolygon polygon )
    { 
      var convexPolygons = Geometry2D.DecomposePolygonInConvex( points.ToArray() );

      for ( int i = 0; i < convexPolygons.Count; i++ )
      {
        var vertices = convexPolygons[ i ];
        polygon.SetVertices( vertices );
        polygon.AddPolygon( CreateIndices( vertices.Length ) );
      }
      
    }

    public static int[] CreateIndices( int amount )
    {
      var indices = new int[ amount ];

      for ( int i = 0; i < amount; i++ )
      {
        indices[ i ] = i;
      }

      return indices;
    } 

    public static Path2 AsXZFrom( Curve3D curve3D, bool closed, int resolution = 20 )
    {
      var points = new List<Vector2>();

      var numPoints = resolution;

      var normalizer = 1f / resolution;

      for ( int i = 0; i < numPoints; i++ )
      {
        points.Add( Math2D.XZ( curve3D.Samplef( i * normalizer ) ) );
      }

      if ( closed )
      {
        points.Add( points[ 0 ] );
      }

      return new Path2( points );
    }

    public static Path2 AsXZFrom( Curve3 curve3, bool closed, int resolution = 20 )
    {
      var points = new List<Vector2>();

      var numPoints = resolution;

      var normalizer = 1f / resolution;

      for ( int i = 0; i < numPoints; i++ )
      {
        points.Add( Math2D.XZ( curve3.SampleAt( i * normalizer ) ) );
      }

      if ( closed )
      {
        points.Add( points[ 0 ] );
      }

      return new Path2( points );
    }

    public bool PointInPath( Vector2 p, bool fast = true, bool checkBoundingBox = true )
    {
      if ( fast )
      {
        return PointInPathFast( p, checkBoundingBox );
      }
      else
      {
        return PointInPathReliable( p, checkBoundingBox );
      }
    }

    public bool PointInPathFast( Vector2 point, bool checkBoundingBox = true )
    { 
      var min = this.leftTop;
      var max = this.rightBottom;

      if ( checkBoundingBox )
      {        
        if ( ! Box2.ContainsPoint( min, max, point ) )
        {
          return false;
        }
      }

      return CheckPointInPathFast( point, max + new Vector2( 122.133544f, 129.45423f ) );
    }

    public bool PointInPathReliable( Vector2 point, bool checkBoundingBox = true )
    {
      var min = this.leftTop;
      var max = this.rightBottom;

      if ( checkBoundingBox )
      {        
        if ( ! Box2.ContainsPoint( min, max, point ) )
        {
          return false;
        }

      }

      var endPoint = max + new Vector2( 0.1234235f, 0.4211322f );     
      var result = CheckPointInPath( point, endPoint );

      if ( PointInPathResult.ERROR != result )
      { 
        return PointInPathResult.INSIDE == result;
      }
      
      var rightTop = new Vector2( max.X, min.Y );
      endPoint = rightTop + new Vector2( 0.03234235f, -0.90211322f );   
      result = CheckPointInPath( point, endPoint );
      
      if ( PointInPathResult.ERROR != result )
      {
        return PointInPathResult.INSIDE == result;
      }
      
      var centerTop = new Vector2( ( min.X + max.X ) * 0.5f , min.Y );
      endPoint = centerTop + new Vector2( 0.013234235f, -0.90211322f );   
      result = CheckPointInPath( point, endPoint );
      
      if ( PointInPathResult.ERROR != result )
      {
        return PointInPathResult.INSIDE == result;
      }
     
      var rightMiddle = new Vector2( max.X , ( min.Y + max.Y ) * 0.5f);
      endPoint = rightMiddle + new Vector2( 0.013234235f, 0.00211322f );   
      result = CheckPointInPath( point, endPoint );      

      if ( PointInPathResult.ERROR != result )
      {
        return PointInPathResult.INSIDE == result;
      }

      endPoint = leftTop + new Vector2( -0.01053535f, -0.41005465f );   
      result = CheckPointInPath( point, endPoint );

      if ( PointInPathResult.ERROR != result )
      {
        return PointInPathResult.INSIDE == result;
      }

      for ( int i = 0; i < NUM_POINT_IN_PATH_TEST_RETRIES; i++ )
      {
        var randomX = GodotRandom.Get().Next();
        var randomY = GodotRandom.Get().Next();

        var randomPoint = max + new Vector2( randomX, randomY );     
        result = CheckPointInPath( point, endPoint );

        if ( PointInPathResult.ERROR != result )
        {
          return PointInPathResult.INSIDE == result;
        }
      } 

      return false; 
    }

    public bool CheckPointInPathFast( Vector2 point, Vector2 endPoint )
    {
      var pointLine = new Line2( point, endPoint );

      var iterationLine = new Line2();
            
      var intersections = 0;

      for ( int i = 0; i < _points.Count; i++ )
      {
        var start = _points[ i ];

        var nextIndex = ( i == _points.Count - 1 ) ? 0 : ( i + 1 );
        var end   = _points[ nextIndex ];     
        

        iterationLine.start = start;        
        iterationLine.end   = end;

        var intersects = pointLine.IntersectsWith( iterationLine );

        if ( intersects )
        {
          intersections ++;
        }
        
      }

      
      return intersections % 2 != 0;
    }

    PointInPathResult CheckPointInPath( Vector2 point, Vector2 endPoint )
    {
      var pointLine = new Line2( point, endPoint );
      var pointAngle = pointLine.angle;
      var pointAngle2 = pointLine.reverseAngle;

      var iterationLine = new Line2();
      
      var intersections = 0;

      for ( int i = 0; i < _points.Count; i++ )
      {
        var start = _points[ i ];

        if ( pointLine.DistanceToPoint( start ) < POINT_TO_CLOSE_DISTANCE )
        {
          return PointInPathResult.ERROR;
        } 

        var nextIndex = ( i == _points.Count - 1 ) ? 0 : ( i + 1 );
        var end   = _points[ nextIndex ];     
        

        iterationLine.start = _points[ i ];        
        iterationLine.end = _points[ nextIndex ];

        var iterationAngle = iterationLine.angle;
        var angleDifference = AbsoluteDeltaAngleFromRadiansToDegrees( pointAngle, iterationAngle );

        if ( angleDifference < ANGLE_TO_SIMILAR_TRESHOLD )
        {
          return PointInPathResult.ERROR;
        } 

        angleDifference = AbsoluteDeltaAngleFromRadiansToDegrees( pointAngle2, iterationAngle );

        if ( angleDifference < ANGLE_TO_SIMILAR_TRESHOLD )
        {
          return PointInPathResult.ERROR;
        } 


        var intersects = pointLine.IntersectsWith( iterationLine );

        if ( intersects )
        {
          intersections ++;
        }
        
      }

      if ( intersections % 2 != 0 )
      {
        return PointInPathResult.INSIDE;
      }

      return PointInPathResult.OUTSIDE;

    }

    public static Path2 Circle( Vector2 center, float radius = 1, int resolution = 36 )
    {
      var points = new List<Vector2>();

      for ( int i = 0; i < resolution; i++ )
      {
        var t = i / ( float ) ( resolution );
        var phase = t * Mathf.Pi * 2;
        var x = Mathf.Cos( phase ) * radius;
        var y = Mathf.Sin( phase ) * radius;

        points.Add( new Vector2( x, y ) + center );
      }

      return new Path2( points );
    }
    

    public static float AbsoluteDeltaAngleFromRadiansToDegrees( float rA, float rB )
    {
      var dA = Mathf.RadToDeg( rA );
      var dB = Mathf.RadToDeg( rB );

      return MathX.AbsoluteDeltaAngle( dA, dB );
    }


  } 
}