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

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

namespace Rokojori
{
  public class Line3:Curve3
  {
    public Vector3 start = Vector3.Zero;
    public Vector3 end = Vector3.Zero;

    public Line3( Vector3 start, Vector3 end )
    {
      this.start = start;
      this.end = end;
    } 

    public Line3(){}

    public static Line3 Create( Vector3 start, Vector3 end )
    {
      return new Line3( start, end );
    }

    public void Set( Vector3 start, Vector3 end )
    {
      this.start = start;
      this.end = end;
    } 

    public override Vector3 PositionAt( float t )
    {
      return start.Lerp( end, t );
    }

    public Line3 Copy( )
    {
      return new Line3( start, end );
    }

    public Vector3 direction { get { return end - start; } }

    public float length { get { return direction.Length();  } }
    
    public Vector3 GetPointAtParameter( float t )
    {
      return start + direction * t;
    }

    public Vector3 center{ get { return ( start + end ) * 0.5f; } }

    public Line3 ScaleFromCenter( float scale )
    {
      var lineCenter = center;
      var halfDirection = direction * ( 0.5f * scale );

      var scaledStart = lineCenter - halfDirection;
      var scaledEnd   = lineCenter + halfDirection;

      return new Line3( scaledStart, scaledEnd );  
    }

    public Line3 ChangeLengthFromCenter( float lengthChange )
    {
      var lineCenter = center;
      var halfDirection = direction * 0.5f;
      var length = halfDirection.Length();
      length += lengthChange * 0.5f;

      halfDirection = halfDirection.Normalized() * length;      

      var scaledStart = lineCenter - halfDirection;
      var scaledEnd   = lineCenter + halfDirection;

      return new Line3( scaledStart, scaledEnd );  
    }

    public Line3 SetLengthFromCenter( float length )
    {
      var lineCenter = center;
      var halfDirection = direction.Normalized() * ( length * 0.5f );     

      var scaledStart = lineCenter - halfDirection;
      var scaledEnd   = lineCenter + halfDirection;

      return new Line3( scaledStart, scaledEnd );  
    }
    
     
    public Vector3 ClosestPointToPoint( Vector3 point )
    {
      var parameter = MathX.Clamp01( ClostestParameterToPoint( point ) );
      return GetPointAtParameter( parameter );
    }

    public static Vector3 ClosestPointOf( Vector3 p, Vector3 lineStart, Vector3 lineEnd )
    {
      var line = new Line3();
      line.Set( lineStart, lineEnd );

      return line.ClosestPointToPoint( p );
    } 

    public float ClostestParameterToPoint( Vector3 point )
    {
      var startP = point - start;
      var startEnd = end - start;
      var startEnd2 = startEnd.Dot( startEnd );
      var startEnd_startP = startEnd.Dot( startP );

      if ( startEnd2 == 0 )
      {
        return 0;
      }

      var t = startEnd_startP / startEnd2;

      return t;
        
    }

    public float DistanceToPoint( Vector3 point )
    {
      return ( point - ClosestPointToPoint( point ) ).Length();
    }


    public float DistanceToLine( Line3 s )
    {
      var points = ClosestPointsToLine( s );
      return ( points[ 1 ] - points[ 0 ] ).Length();
    }

    public Vector3[] ClosestPointsToLine( Line3 s )
    {
      var parameters = ClosestParametersToLine( s );
      var x = this.GetPointAtParameter( parameters.X );
      var y = s.GetPointAtParameter( parameters.Y );

      return new Vector3[]{ x, y };  
    }

    public Vector3 ClosestPointTo( Line3 other )
    {
      var parameters = ClosestParametersToLine( other );
      return PositionAt( parameters.X );
    }
    
    public Vector3 ClosestPointTo( params Vector3[] points )
    {
      var d = float.MaxValue;
      var id = -1;

      for ( int i = 0; i < points.Length; i++ )
      {
        var pd = DistanceToPoint( points[ i ] );

        if ( pd < d )
        {
          id = i;
          d = pd;
        }
      }

      return points[ id ];
    }

    public Vector3 ClosestPointTo( List<Vector3> points )
    {
      var d = float.MaxValue;
      var id = -1;

      for ( int i = 0; i < points.Count; i++ )
      {
        var pd = DistanceToPoint( points[ i ] );

        if ( pd < d )
        {
          id = i;
          d = pd;
        }
      }

      return points[ id ];
    }


    public Vector2 ClosestParametersToLine( Line3 s )
    {     
      float epsilon = 0.00000001f;

      var u = this.direction;
      var v = s.direction;
      var w = start - s.start;
    
      var a = u.Dot( u );
      var b = u.Dot( v );
      var c = v.Dot( v );
      var d = u.Dot( w );
      var e = v.Dot( w );

      var DD = a * c - b * b;

      var sc = 0f;
      var sN = 0f;
      var sD = 0f;

      var tc = 0f;
      var tN = 0f;
      var tD = 0f;

      sD = DD;
      tD = DD;

      if ( DD < epsilon ) 
      {
        sN = 0.0f;
        sD = 1.0f;
        tN = e;
        tD = c;
      }
      else
      {
        sN = b * e - c * d ;
        tN = a * e - b * d ;

        if ( sN < 0.0f )
        {
          sN = 0.0f;
          tN = e;
          tD = c;
        }
        else if ( sN > sD ) 
        {
          sN = sD;
          tN = e + b;
          tD = c;
        }
      }

      if ( tN < 0f || tN > tD )
      {
        var tN_bigger_tD = tN >= 0f && tN > tD;

        tN = tN_bigger_tD ? tD : 0f;

        var inbetween = tN_bigger_tD ? ( -d + b ) : ( -d );
        var isSmaller = inbetween < 0;
        var isBigger  = inbetween > a;

        sN = isSmaller ? 0f : isBigger ? sD : inbetween; 
        sD = ! isSmaller && ! isBigger ?  a : sD;
      }

      sc = Mathf.Abs( sN ) < epsilon ? 0f : sN / sD;
      tc = Mathf.Abs( tN ) < epsilon ? 0f : tN / tD;

      return new Vector2( sc, tc ); 

    }

  }
}