rj-action-library/Runtime/Math/Math3D.cs

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

namespace Rokojori
{
  public static class Math3D
  {
    public static Vector3 OnCircleXZ( float radians, float size = 1 )
    {
      var x = Mathf.Cos( radians ) * size;
      var z = Mathf.Sin( radians ) * size;

      return new Vector3( x, 0, z );
    }

    public static Vector3 XYasXZ( Vector2 v )
    {
      return new Vector3( v.X, 0, v.Y );
    }

    public static float Dot( Vector3 a, Vector3 b )
    {
      return a.Dot( b );
    }

    public static Vector3 Cross( Vector3 a, Vector3 b )
    {
      return a.Cross( b );
    }

    public static Vector3 Fract( Vector3 a )
    {
      return new Vector3( MathX.Fract( a.X ), MathX.Fract( a.Y ), MathX.Fract( a.Z ) );
    }

    public static bool IsExactlyZero( Vector3 v )
    {
      return v.X == 0 && v.Y == 0 && v.Z == 0;
    } 

    public static Vector3 ComputeNormalFast( Vector3 a, Vector3 b, Vector3 c )
    {
      return Math3D.Cross( c - b , a - b  ).Normalized();
    }

    public static Vector3 ComputeNormal( Vector3 a, Vector3 b, Vector3 c )
    {
      var cross = Math3D.Cross( c - b , a - b  );

      if ( Math3D.IsExactlyZero( cross ) )
      {
        cross = Math3D.Cross( b - c , a - c  );
      }

      if ( Math3D.IsExactlyZero( cross ) )
      {
        cross = Math3D.Cross( c - a , b - a  );
      }

      return cross.Normalized();
    }


    public static Vector3 SmoothStep( Vector3 a, Vector3 b, Vector3 t )
    {
      var x = MathX.Smoothstep( a.X, b.X, t.X );
      var y = MathX.Smoothstep( a.Y, b.Y, t.Y );
      var z = MathX.Smoothstep( a.Z, b.Z, t.Z ); 

      return new Vector3( x, y, z );
    }

    public static Vector3 Center( params Vector3[] points )
    {
      var center = Vector3.Zero;

      if ( points.Length == 0 )
      {
        return center;
      }

      for ( int i = 0; i < points.Length; i++ )
      {
        center += points[ i ];
      }

      return center / points.Length;
    }

    public static Vector3 Center( List<Vector3> points )
    {
      var center = Vector3.Zero;

      if ( points.Count == 0 )
      {
        return center;
      }

      for ( int i = 0; i < points.Count; i++ )
      {
        center += points[ i ];
      }

      return center / points.Count;
    }

    public static Vector3 GetClosest( Vector3 from, params Vector3[] points )
    {
      var distance = float.MaxValue;
      var index = -1;

      for ( int i = 0; i < points.Length; i++ )
      {
        var d = from.DistanceSquaredTo( points[ i ] );

        if ( d < distance )
        {
          index = i;
          distance = d;
        }
      }

      return points[ index ];
    }

    public static Vector3 GetClosest( Vector3 from, List<Vector3> points )
    {
      var distance = float.MaxValue;
      var index = -1;

      for ( int i = 0; i < points.Count; i++ )
      {
        var d = from.DistanceSquaredTo( points[ i ] );

        if ( d < distance )
        {
          index = i;
          distance = d;
        }
      }

      return points[ index ];
    }


    public static Vector3 LerpUnclamped( Vector3 a, Vector3 b, float amount )
    {
      return a + amount * ( b - a );
    }

    public static Vector3 LerpClamped( Vector3 a, Vector3 b, float amount )
    {
      return LerpUnclamped( a, b, MathX.Clamp01( amount ) );
    }

    public static Quaternion GetGlobalQuaternion( this Node3D node )
    {
      return GetGlobalRotationFrom( node );
    }

    public static Quaternion GetGlobalRotationFrom( Node3D node )
    {
      var quaternion = node.GlobalBasis.GetRotationQuaternion();

      return quaternion;
    } 

    public static Vector3 MinPosition( Node3D a, Node3D b )
    {
      return a.GlobalPosition.Min( b.GlobalPosition );
    }

    public static Vector3 MaxPosition( Node3D a, Node3D b )
    {
      return a.GlobalPosition.Max( b.GlobalPosition );
    }

    public static Vector3 SnapRounded( Vector3 v, Vector3 snapping )
    {
      v.X = MathX.SnapRounded( v.X, snapping.X );
      v.Y = MathX.SnapRounded( v.Y, snapping.Y );
      v.Z = MathX.SnapRounded( v.Z, snapping.Z );

      return v;
    }

    public static Vector3 SnapCeiled( Vector3 v, Vector3 snapping )
    {
      v.X = MathX.SnapCeiled( v.X, snapping.X );
      v.Y = MathX.SnapCeiled( v.Y, snapping.Y );
      v.Z = MathX.SnapCeiled( v.Z, snapping.Z );

      return v;
    }

    public static Vector3 SnapFloored( Vector3 v, Vector3 snapping )
    {
      v.X = MathX.SnapFloored( v.X, snapping.X );
      v.Y = MathX.SnapFloored( v.Y, snapping.Y );
      v.Z = MathX.SnapFloored( v.Z, snapping.Z );

      return v;
    }

    public static Quaternion AlignUp( Quaternion rotation, Vector3 upDirection )
    {
      var basis = new Basis( rotation );
      var aligned = AlignUp( basis, upDirection );
      return aligned.GetRotationQuaternion();
    }

    public static Basis AlignUp( Basis basis, Vector3 upDirection )
    {
      basis.Y = upDirection;
      basis.X = - basis.Z.Cross( upDirection );
      return basis.Orthonormalized();
    }

    public static Vector3 Lerp( Vector3 a, Vector3 b, float lerp )
    {
      return a.Lerp( b, lerp );
    }

    public static Quaternion LookRotation( Vector3 direction, Vector3 up )
    {
      if ( direction.Length() == 0 )
      {
        return Quaternion.Identity;
      }
      
      var t = new Transform3D();
      t.Basis = Basis.Identity;
      t.Origin = Vector3.Zero;    

      t = t.LookingAt( direction, up );

      return t.Basis.GetRotationQuaternion();
    }

    public static Vector3 LerpComponents( Vector3 a, Vector3 b, Vector3 t )
    {
      return new Vector3( Mathf.Lerp( a.X, b.X, t.X ), Mathf.Lerp( a.Y, b.Y, t.Y ), Mathf.Lerp( a.Z, b.Z, t.Z ) );
    }

    public static Vector4 AsVector4( this Quaternion q)
    {
      return new Vector4( q.X, q.Y, q.Z, q.W );
    }

    public static void SetGlobalQuaternion( this Node3D node, Quaternion quaternion )
    {
      var offset = node.GlobalPosition;

      var localScale = node.Scale;
      node.GlobalBasis = new Basis( quaternion );
      node.GlobalPosition = offset;
      node.Scale = localScale;



      //SetGlobalRotationTo( node, quaternion );
    }

    public static void SetGlobalRotationTo( Node3D node, Quaternion quaternion )
    {
      var forward = quaternion * Vector3.Forward;
      var up      = quaternion * Vector3.Up;

      node.LookAt( node.GlobalPosition + forward, up );
    }

    public static void LookTowards( this Node3D node, Vector3 forwardDirection, Vector3 upDirection, Quaternion rotation )
    {
      //forwardDirection = rotation * forwardDirection;
      //upDirection      = rotation * upDirection;

      node.LookTowards( forwardDirection, upDirection );  
      node.SetGlobalQuaternion( node.GetGlobalQuaternion() * rotation );
    }

    public static void LookTowards( this Node3D node, Vector3 forward, Vector3 up )
    {
      node.LookAt( forward + node.GlobalPosition, up );
    }


    public static Quaternion GetDifference( this Quaternion q, Quaternion other )
    {
      return GetQuaternionDifference( q, other );
    }

    public static Quaternion GetQuaternionDifference( Quaternion a, Quaternion b )
    {
      return b.Inverse() * a;
    } 

    public static Quaternion GetQuaternionFraction( Quaternion q, float fraction )
    {
      return Quaternion.Identity.Slerp( q, fraction );
    }


    public static Vector3 GlobalForward( this Node3D node )
    {
      return GetGlobalForward( node );
    }

    public static Vector3 GetGlobalForward( Node3D node )
    {
      return -node.GlobalBasis.Z;
    }

    public static Vector3 GlobalUp( this Node3D node )
    {
      return GetGlobalUp( node );
    }

    public static Vector3 GetGlobalUp( Node3D node )
    {
      return node.GlobalBasis.Y;
    }

     public static Vector3 GlobalRight( this Node3D node )
    {
      return GetGlobalRight( node );
    }

    public static Vector3 GetGlobalRight( Node3D node )
    {
      return node.GlobalBasis.X;
    }

    public static Vector3 GetYPlaneForward( Node3D node )
    {
      var forward = GetGlobalForward( node );

      forward.Y = 0;

      return forward.Normalized();
    }

    public static Vector3 GetYPlaneRight( Node3D node )
    {
      var right = GetGlobalRight( node );

      right.Y = 0;

      return right.Normalized();
    }

    public static void SetGlobalX( this Node3D node, float x )
    {
      var gp = node.GlobalPosition;

      gp.X = x;

      node.GlobalPosition = gp;
    }

    public static void SetGlobalY( this Node3D node, float y )
    {
      var gp = node.GlobalPosition;

      gp.Y = y;

      node.GlobalPosition = gp;
    }

    public static void SetGlobalZ( this Node3D node, float z )
    {
      var gp = node.GlobalPosition;

      gp.Z = z;

      node.GlobalPosition = gp;
    }

    public static Aabb? GetWorldBounds( this Node3D node )
    {
      return GetWorldBoundsFrom( node );
    }

    public static Aabb? GetWorldBoundsFrom( Node3D node )
    {
      Aabb? worldBounds = null;

      Nodes.ForEach<VisualInstance3D>( node,
        ( vi )=>
        {
          var nBounds = vi.GetAabb();

          worldBounds = worldBounds == null ? nBounds : ( ((Aabb)worldBounds).Merge( nBounds ) );
        }
      );

      return worldBounds;
    }
  }

}