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

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


namespace Rokojori
{

  public class Triangle3
  {   
    public Vector3 a;
    public Vector3 b;
    public Vector3 c;

    public Triangle3( Vector3 a, Vector3 b, Vector3 c )
    {
      this.a = a;
      this.b = b;
      this.c = c;

      _needsUpdate = true;
    }

    public float perimeter
    {
      get
      {
        var sideA = ( b - a ).Length();
        var sideB = ( c - b ).Length();
        var sideC = ( a - c ).Length();
      
        return sideA + sideB + sideC;
      }
    }

    public float semiperimeter => perimeter * 0.5f;

    Vector3 _center;
    Sphere _boundingSphere;
    Plane3 _plane;

    bool _needsUpdate = false;

    public void NeedsUpdate()
    {
      _needsUpdate = true;
    }

    public Sphere boundingSphere 
    {
      get 
      {      
        Update();
        return _boundingSphere;
      }
    }

    void Update()
    {
      if ( ! _needsUpdate )
      {
        return;
      }

      var m = Math3D.Center( a, b, c );
      var radius = Mathf.Sqrt( MathX.Min( ( m - a ).LengthSquared(), ( m - b ).LengthSquared(), ( m  -c ).LengthSquared() ) );
      _boundingSphere = new Sphere( m, radius );

      _plane = new Plane3(); 
      _plane.SetFromCoplanarPoints( a, b, c );

      _needsUpdate = false;
    }

    public static float ComputeTriangleArea( Vector3 a, Vector3 b, Vector3 c )
    {
      var sideA = ( b - a ).Length();
      var sideB = ( c - b ).Length();
      var sideC = ( a - c ).Length();
    

      var perimeter = sideA + sideB + sideC;
      var semiperimeter = 0.5f * perimeter;
      var areaValue = Mathf.Sqrt( semiperimeter * ( semiperimeter - sideA ) 
                                                * ( semiperimeter - sideB ) 
                                                * ( semiperimeter - sideC )) ;

      return areaValue;   
    }

    public float area => ComputeTriangleArea( a, b, c );
   
    public bool Intersects( Line3 line )
    {
      var planePoint = _plane.IntersectLine( line );

      if ( planePoint == null )
      {
        return false;
      }

      return InsideTriangle( (Vector3) planePoint, a, b, c );
    } 

    public Vector3? GetIntersection( Line3 line )
    {
      Update();

      var planePoint = _plane.IntersectLine( line );

      if ( planePoint == null )
      {
        return null;
      }

      if ( ! InsideTriangle( (Vector3) planePoint, a, b, c ) )
      {
        return null;
      }

      return planePoint;
    }

    public bool PointInside( Vector3 p )
    {
      return InsideTriangle( p, a, b, c );
    } 
  
    public Vector3 ClosestPoint( Vector3 p )
    {
      Update();

      var planePoint = _plane.ClosestPointTo( p );

      if ( PointInside( planePoint ) )
      {
        return planePoint;
      }

      return ClosestPointToEdges( p );

    }

    public Vector3? GetPointOnPlaneInsideTriangle( Vector3 p )
    {
      Update();

      var planePoint = _plane.ClosestPointTo( p );

      if ( PointInside( planePoint ) )
      {
        return planePoint;
      }

      return null;
    }

    Vector3[] closestPoints = new Vector3[3];

    public Vector3 ClosestPointToEdges( Vector3 p )
    {
      closestPoints[ 0 ] = Line3.ClosestPointOf( p, a, b );
      closestPoints[ 1 ] = Line3.ClosestPointOf( p, b, c );
      closestPoints[ 2 ] = Line3.ClosestPointOf( p, c, a );

      return Math3D.GetClosest( p, closestPoints );
    }

    public Vector3 ClosestPoint( Line3 line )
    {
      var intersection = GetIntersection( line );

      if ( intersection != null )
      {
        return (Vector3) intersection;
      }

      var ab = new Line3( a, b ).ClosestPointTo( line );
      var bc = new Line3( b, c ).ClosestPointTo( line );
      var ca = new Line3( c, a ).ClosestPointTo( line );

      return line.ClosestPointTo( ab, bc, ca );
    }

    public Vector3 GetPoint( int i )
    {
      if ( i == 0 )
      {
        return a;
      }
      else if ( i == 1 )
      {
        return b;
      }
      else if ( i == 2 )
      {
        return c;
      }

      return a;
    }

    void SetPointsToLine( Line3 line, int index )
    {
      line.Set( GetPoint( index ), GetPoint( ( index + 1 ) % 3 ) );
    }

    public Line3 GetIntersectionLine( Triangle3 other )
    {     
      var ownSphere   = boundingSphere;
      var otherSphere = other.boundingSphere;

      if ( ! ownSphere.IntersectsSphere( otherSphere ) )
      {
        return null;
      }

      var line = new Line3();

      var intersections = new List<Vector3>();

      for ( int i = 0; i < 3; i++ )
      {
        SetPointsToLine( line, i );
        var intersection = other.GetIntersection( line );        

        if ( intersection != null )
        {
          intersections.Add( (Vector3) intersection );
        }
      }

      if ( intersections.Count == 2 )
      {
        line.Set( intersections[ 0 ], intersections[ 1 ] );
        return line;
      }

      for ( int i = 0; i < 3; i++ )
      {
        other.SetPointsToLine( line, i );
        var intersection = GetIntersection( line );        

        if ( intersection != null )
        {
          intersections.Add( (Vector3) intersection );
        }
      }

      if ( intersections.Count == 2 )
      {
        line.Set( intersections[ 0 ], intersections[ 1 ] );
        return line;
      }

      return null;


    }

    public bool Intersects( Triangle3 other )
    {
      var ownSphere   = boundingSphere;
      var otherSphere = other.boundingSphere;

      if ( ! ownSphere.IntersectsSphere( otherSphere ) )
      {
        return false;
      }

      var line = new Line3();
      
      for ( int i = 0; i < 3; i++ )
      {
        if ( PointInside( other.GetPoint( i ) ) )
        {
          return true;
        }

        if ( other.PointInside( GetPoint( i ) ) )
        {
          return true;
        }

        SetPointsToLine( line, i );

        if ( other.Intersects( line ) )
        {
          return true;
        }

        other.SetPointsToLine( line, i );

        if ( Intersects( line ) )
        {
          return true;
        }
      }

      return false;
    }

    
    public static bool InsideTriangle( Vector3 point, Vector3 a, Vector3 b, Vector3 c ) 
    {
      var bary = GetBaryCentricCoordinate( point, a, b, c );

      if ( bary == null )
      {
        return false;
      }

      var values = (Vector3) bary;

      var insideV = Range.Contains( values.Y, 0, 1 );
      var insideU = Range.Contains( values.Z, 0, 1 );
      var insideX = Range.Contains( values.X, 0, 1 );

      var result = insideX && insideV && insideU;
      return result;

    }

    public static Vector3? GetBaryCentricCoordinate( Vector3 point, Vector3 a, Vector3 b, Vector3 c ) 
    {
  		var _v0 = c - a;
  		var _v1 = b - a;
  		var _v2 = point - a;

  		var dot00 = Math3D.Dot( _v0, _v0 );
  		var dot01 = Math3D.Dot( _v0, _v1 );
  		var dot02 = Math3D.Dot( _v0, _v2 );
  		var dot11 = Math3D.Dot( _v1, _v1 );
  		var dot12 = Math3D.Dot( _v1, _v2 );

  		var denom = ( dot00 * dot11 - dot01 * dot01 );

  		if ( denom == 0 ) 
      {
  			return null; 
  		}

  		var invDenom = 1f / denom;

  		var u = ( dot11 * dot02 - dot01 * dot12 ) * invDenom;
  		var v = ( dot00 * dot12 - dot01 * dot02 ) * invDenom;

  		return new Vector3( 1f - u - v, v, u );
  	}

  }

}