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 void SetFrom( MeshGeometry mg, int a, int b, int c )
    {
      this.a = mg.vertices[ a ];
      this.b = mg.vertices[ b ];
      this.c = mg.vertices[ c ];

      _needsUpdate = true;
    }

    public static Triangle3 CreateFrom( List<Vector3> points, int offset = 0 )
    {
      var t = new Triangle3( 
        points[ 0 ],
        points[ 1 ],
        points[ 2 ]
      );

      return t;
    }

    public static Triangle3 CreateFrom( MeshGeometry mg, int a, int b, int c )
    {
      var t = new Triangle3( 
        mg.vertices[ a ],
        mg.vertices[ b ],
        mg.vertices[ c ]
      );

      return t;
    }

    public static Triangle3 CreateFrom( MeshGeometry mg, int triangleIndex )
    {
      var triangleOffset = triangleIndex;

      var t = new Triangle3( 
        mg.vertices[ triangleOffset ],
        mg.vertices[ triangleOffset + 1],
        mg.vertices[ triangleOffset + 2 ]
      );

      return t;
    }

    public void SetFrom( MeshGeometry mg, int triangleIndex )
    {
      var triangleOffset = triangleIndex;

      SetFrom( mg, triangleOffset, triangleOffset + 1, triangleOffset + 2 );
    }

    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;
    Vector3 _normal;

    bool _needsUpdate = false;

    public void NeedsUpdate()
    {
      _needsUpdate = true;
    }


    public Vector3 center
    {
      get 
      {
        Update();
        return _center;
      }
    }

    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 );

      _normal = ( b - a ).Cross( c - a ).Normalized(); 

      _center = ( a + b + c ) / 3f;

      _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 float GetHeightOfPoint( int i )
    {
      var p = GetPoint( i );

      var b = GetPoint( ( i + 1 ) % 3 );
      var a = GetPoint( ( i + 2 ) % 3 );

      var ab = Line3.ClosestPointOf( p, a, b );

      return ( p - ab ).Length();
    }

    public float GetArea( int i = 0 )
    {
      var p = GetPoint( i );

      var b = GetPoint( ( i + 1 ) % 3 );
      var a = GetPoint( ( i + 2 ) % 3 );

      var ab = Line3.ClosestPointOf( p, a, b );

      var h =  ( p - ab ).Length();

      return ( ( a - b ).Length() * h ) / 2f;
    }

    public LerpCurve3 GetOutline()
    {
      return LerpCurve3.FromPoints( GetPoint( 0 ), GetPoint( 1 ), GetPoint( 2 ) );
    }

    public List<Vector3> points => new List<Vector3>(){ a, b, c };
    

    

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

      return a;
    }

    public Line3 GetEdge( int i )
    {
      var p0 = GetPoint( i );
      var p1 = GetPoint( i % 3 );

      return Line3.Create( p0, p1 );
    }

    public float GetEdgeLength( int i )
    {
      return GetEdge( i ).length;
    }

    public float longestEdgeLength => MathX.Min( GetEdgeLength( 0 ),  GetEdgeLength( 1 ), GetEdgeLength( 2 ) );
    public float shortestEdgeLength => MathX.Max( GetEdgeLength( 0 ),  GetEdgeLength( 1 ), GetEdgeLength( 2 ) );

    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 void Rotate( Quaternion q, Vector3? pivot = null )
    {
      if ( pivot != null )
      {
        Translate( -( (Vector3)pivot ));
      }

      a = a * q;
      b = b * q;
      c = c * q;

      if ( pivot != null )
      {
        Translate( ( (Vector3)pivot ));
      }
    }

    public Triangle3 Clone()
    {
      return new Triangle3( a, b, c );
    }

    public Vector3 normal 
    {
      get
      {
        Update();
        return _normal;
      }

    }
    
    public Quaternion GetXZAlignmentRotation()
    {
      return Math3D.AlignUp( normal );
    }

    public void ScaleFromCenter( float scale )
    {
      var cnt = center;
      a = ( a - cnt ) * scale + cnt; 
      b = ( b - cnt ) * scale + cnt; 
      c = ( c - cnt ) * scale + cnt;       
    }

    public Triangle3 Shrink( float distance )
    {
      var n = normal;

      var t = Offset( distance );

      if ( t == null || Math3D.LookingAtEachOther( t.normal, n ) )
      {
        return null;
      }

      return t;
      
    }

    public Triangle3 Offset( float distance )
    {
      var rotationXZ = GetXZAlignmentRotation();
      var clone = Clone();
      var originalCenter = center;

      clone.Translate( - originalCenter );
      clone.Rotate( rotationXZ );
      

      var t2 = Triangle2.AsXZ( clone );

      var shrinked = t2.Shrink( distance );

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

      clone = XYasXZ( shrinked );
      clone.Rotate( rotationXZ.Inverse() );
      clone.Translate( originalCenter );

      return clone;
    }

    public void Translate( Vector3 translation )
    {
      a += translation;
      b += translation;
      c += translation;
    }

    public static Triangle3 XYasXZ( Triangle2 t )
    {
      return new Triangle3( Math3D.XYasXZ( t.a ), Math3D.XYasXZ( t.b ), Math3D.XYasXZ( t.c ) );
    }

    public List<Triangle3> CenterSplit()
    {
      var m = center;

      var subs = new List<Triangle3>
      {
        new Triangle3( a, b, m ),
        new Triangle3( b, c, m ),
        new Triangle3( c, a, m )
      };

      return subs;
    }

    public void GetSubdivisionPoints( List<Vector3> output, float minArea )
    {
      var processingList = new List<Triangle3>();

      processingList.Add( this );

      while ( processingList.Count > 0 )
      {
        var tri = processingList.Shift();
        output.Add( tri.center );
        
        var subs = tri.CenterSplit();

        subs.ForEach( 
          st => 
          {
            if ( st.area > minArea ) 
            {
              processingList.Add( st );
            }
          } 
        );

      }



    }


    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 );
  	}

    public Vector3? GetBaryCentricCoordinate( Vector3 point )
    {
      return GetBaryCentricCoordinate( point, a, b, c );
    }

    public Vector4? Lerp( Vector3 p, Vector4 x, Vector4 y, Vector4 z )
    {
      var bary = GetBaryCentricCoordinate( p );
      
      if ( bary == null )
      {
        return null;
      }

      var w = ( Vector3 ) bary;

      return w.Lerp( x, y, z );
    } 

    public Vector3? Lerp( Vector3 p, Vector3 x, Vector3 y, Vector3 z )
    {
      var bary = GetBaryCentricCoordinate( p );
      
      if ( bary == null )
      {
        return null;
      }

      var w = ( Vector3 ) bary;

      return w.Lerp( x, y, z );
    } 

    public Vector2? Lerp( Vector3 p, Vector2 x, Vector2 y, Vector2 z )
    {
      var bary = GetBaryCentricCoordinate( p );
      
      if ( bary == null )
      {
        return null;
      }

      var w = ( Vector3 ) bary;

      return w.Lerp( x, y, z );
    } 

    public float? Lerp( Vector3 p, float x, float y, float z )
    {
      var bary = GetBaryCentricCoordinate( p );
      
      if ( bary == null )
      {
        return null;
      }

      var w = ( Vector3 ) bary;

      return w.Lerp( x, y, z );
    }
  }

}