549 lines
13 KiB
C#
549 lines
13 KiB
C#
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using Godot;
|
|
using System.Text;
|
|
using System;
|
|
|
|
namespace Rokojori
|
|
{
|
|
public static class Math3D
|
|
{
|
|
public static Transform3D TRS( Vector3 translation, Quaternion rotation, Vector3 scale )
|
|
{
|
|
var trsf = new Transform3D( new Basis( rotation ), translation );
|
|
trsf.ScaledLocal( scale );
|
|
|
|
return trsf;
|
|
}
|
|
|
|
public static Transform3D TRS( Pose pose, Vector3 scale )
|
|
{
|
|
return TRS( pose.position, pose.rotation, scale );
|
|
}
|
|
|
|
public static Vector3 Clamp01( Vector3 v )
|
|
{
|
|
return new Vector3(
|
|
MathX.Clamp01( v.X ),
|
|
MathX.Clamp01( v.Y ),
|
|
MathX.Clamp01( v.Z )
|
|
);
|
|
}
|
|
|
|
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 OnCircleXY( float radians, float size = 1 )
|
|
{
|
|
var x = Mathf.Cos( radians ) * size;
|
|
var y = Mathf.Sin( radians ) * size;
|
|
|
|
return new Vector3( x, y, 0 );
|
|
}
|
|
|
|
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 Quaternion AlignUp( Vector3 upDirection, Quaternion? q = null )
|
|
{
|
|
var quaternion = q == null ? Quaternion.Identity : (Quaternion) q;
|
|
return AlignUp( quaternion, upDirection );
|
|
}
|
|
|
|
public static float AngleXY( Vector3 direction )
|
|
{
|
|
return Mathf.Atan2( direction.Y, direction.X );
|
|
}
|
|
|
|
public static float AngleXZ( Vector3 direction )
|
|
{
|
|
return Mathf.Atan2( direction.Z, direction.X );
|
|
}
|
|
|
|
public static Vector3 NormalAngle( float angle )
|
|
{
|
|
var y = Mathf.Sin( angle );
|
|
var z = Mathf.Cos( angle );
|
|
|
|
return new Vector3( 0, y, z );
|
|
}
|
|
|
|
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 Quaternion RotateX( float radians )
|
|
{
|
|
if ( radians == 0 ) { return Quaternion.Identity; }
|
|
|
|
return Quaternion.FromEuler( new Vector3( radians, 0, 0 ) );
|
|
}
|
|
|
|
public static Quaternion RotateXDegrees( float degrees )
|
|
{
|
|
return RotateX( Mathf.DegToRad( degrees ) );
|
|
}
|
|
|
|
public static Quaternion RotateY( float radians )
|
|
{
|
|
if ( radians == 0 ) { return Quaternion.Identity; }
|
|
|
|
return Quaternion.FromEuler( new Vector3( 0, radians, 0 ) );
|
|
}
|
|
|
|
public static Quaternion RotateYDegrees( float degrees )
|
|
{
|
|
return RotateY( Mathf.DegToRad( degrees ) );
|
|
}
|
|
|
|
public static Quaternion RotateZ( float radians )
|
|
{
|
|
if ( radians == 0 ) { return Quaternion.Identity; }
|
|
|
|
return Quaternion.FromEuler( new Vector3( 0, 0, radians ) );
|
|
}
|
|
|
|
public static Quaternion RotateZDegrees( float degrees )
|
|
{
|
|
return RotateZ( Mathf.DegToRad( degrees ) );
|
|
}
|
|
|
|
public static Quaternion YawPitchRotation( float yaw, float pitch )
|
|
{
|
|
return RotateXDegrees( pitch ) * RotateYDegrees( yaw );
|
|
}
|
|
|
|
public static void SetGlobalQuaternion( this Node3D node, Quaternion quaternion )
|
|
{
|
|
var localScale = node.Scale;
|
|
node.GlobalBasis = new Basis( quaternion );
|
|
node.Scale = localScale;
|
|
}
|
|
|
|
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 GetGlobalScale( Node3D node )
|
|
{
|
|
return node.GlobalTransform.Basis.Scale;
|
|
}
|
|
|
|
public static float GetGlobalUniScale( Node3D node )
|
|
{
|
|
var scale3 = GetGlobalScale( node );
|
|
return MathX.Max( scale3.X, scale3.Y, scale3.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 Vector3 Average( List<Vector3> vectors )
|
|
{
|
|
var average = Vector3.Zero;
|
|
|
|
vectors.ForEach( v => average += v );
|
|
|
|
if ( average == Vector3.Zero )
|
|
{
|
|
return vectors[ 0 ];
|
|
}
|
|
|
|
return ( average / vectors.Count ).Normalized();
|
|
}
|
|
|
|
public static Vector3 BlendNormals( Vector3 a, Vector3 b, float amount )
|
|
{
|
|
if ( amount <= 0 )
|
|
{
|
|
return a;
|
|
}
|
|
|
|
if ( amount >= 1 )
|
|
{
|
|
return b;
|
|
}
|
|
|
|
var n = a.Lerp( b, amount );
|
|
var length = n.Length();
|
|
|
|
if ( length > 0 )
|
|
{
|
|
return n / length;
|
|
}
|
|
|
|
return amount <= 0.5 ? a : b;
|
|
}
|
|
|
|
public static Aabb? GetWorldBounds( this Node3D node, bool onlyVisible = true )
|
|
{
|
|
return GetWorldBoundsFrom( node, onlyVisible );
|
|
}
|
|
|
|
public static Aabb? GetWorldBoundsFrom( Node3D node, bool onlyVisible = true )
|
|
{
|
|
Aabb? worldBounds = null;
|
|
|
|
Nodes.ForEach<VisualInstance3D>( node,
|
|
( vi )=>
|
|
{
|
|
if ( onlyVisible && ! vi.IsVisibleInTree() )
|
|
{
|
|
return;
|
|
}
|
|
|
|
var nBounds = vi.GetAabb();
|
|
|
|
nBounds.Size *= GetGlobalUniScale( vi );
|
|
nBounds.Position += vi.GlobalPosition;
|
|
nBounds.End += vi.GlobalPosition;
|
|
|
|
worldBounds = worldBounds == null ? nBounds : ( ((Aabb)worldBounds).Merge( nBounds ) );
|
|
}
|
|
);
|
|
|
|
return worldBounds;
|
|
}
|
|
}
|
|
|
|
} |