122 lines
2.6 KiB
C#
122 lines
2.6 KiB
C#
using Godot;
|
|
|
|
namespace Rokojori
|
|
{
|
|
public class Plane3
|
|
{
|
|
public Vector3 normal = Vector3.Up;
|
|
public float constant = 0;
|
|
|
|
|
|
public Plane3(){}
|
|
|
|
public void Set( Vector3 normal, float constant = 0 )
|
|
{
|
|
this.normal = normal;
|
|
this.constant = constant;
|
|
}
|
|
|
|
public Vector3 ConstrainToPlane( Vector3 p, float distance = 10000 )
|
|
{
|
|
var line3 = new Line3();
|
|
line3.Set( p - normal * distance, p + normal * distance );
|
|
|
|
var intersection = IntersectLine( line3 );
|
|
|
|
if ( intersection == null )
|
|
{
|
|
return p;
|
|
}
|
|
|
|
return (Vector3) intersection;
|
|
}
|
|
|
|
public Vector3 ClosestPointTo( Vector3 p )
|
|
{
|
|
return ConstrainToPlane( p );
|
|
}
|
|
|
|
public float DistanceTo( Vector3 p, float normalIntersectionDistance = 5000 )
|
|
{
|
|
var line = Line3.Create( p - normal * normalIntersectionDistance, p + normal * normalIntersectionDistance );
|
|
var isc = IntersectLine( line );
|
|
|
|
if ( isc == null )
|
|
{
|
|
return float.PositiveInfinity;
|
|
}
|
|
|
|
var xp = (Vector3)isc;
|
|
|
|
return ( p - xp ).Length();
|
|
}
|
|
|
|
public Ray3 GetIntersectionRay( Plane3 other )
|
|
{
|
|
var lineDirection = normal.Cross( other.normal );
|
|
|
|
var det = lineDirection.LengthSquared();
|
|
|
|
if ( det == 0 )
|
|
{
|
|
return null;
|
|
}
|
|
|
|
var r = new Ray3();
|
|
r.offset = ( ( lineDirection.Cross( other.normal ) * constant ) +
|
|
( normal.Cross( lineDirection ) * other.constant ) ) / det;
|
|
|
|
r.direction = lineDirection;
|
|
|
|
return r;
|
|
}
|
|
|
|
public Vector3? IntersectLine( Line3 line )
|
|
{
|
|
var direction = line.direction;
|
|
|
|
var denominator = Math3D.Dot( normal, direction );
|
|
|
|
if ( denominator == 0 )
|
|
{
|
|
return null;
|
|
}
|
|
|
|
var t = - ( Math3D.Dot( line.start, normal ) + constant ) / denominator;
|
|
|
|
if ( t < 0 || t > 1 )
|
|
{
|
|
return null;
|
|
}
|
|
|
|
return direction * t + line.start;
|
|
}
|
|
|
|
public static Plane3 GetZeroUp()
|
|
{
|
|
var plane = new Plane3();
|
|
plane.Set( Vector3.Up, 0 );
|
|
return plane;
|
|
}
|
|
|
|
public void SetFromNormalAndCoplanarPoint( Vector3 normal, Vector3 point )
|
|
{
|
|
this.normal = normal;
|
|
this.constant = - Math3D.Dot( point, this.normal );
|
|
}
|
|
|
|
public void SetFromCoplanarPoints( Vector3 a, Vector3 b, Vector3 c )
|
|
{
|
|
var normal = Math3D.ComputeNormal( a, b, c );
|
|
SetFromNormalAndCoplanarPoint( normal, a );
|
|
}
|
|
|
|
public static Plane3 FromCoplanarPoints( Vector3 a, Vector3 b, Vector3 c )
|
|
{
|
|
var p = new Plane3();
|
|
p.SetFromCoplanarPoints( a, b, c );
|
|
|
|
return p;
|
|
}
|
|
}
|
|
} |