2017-11-05 11:37:59 +01:00
|
|
|
using System;
|
2019-08-08 03:29:40 +02:00
|
|
|
using System.Runtime.InteropServices;
|
2018-03-01 08:51:35 +01:00
|
|
|
#if REAL_T_IS_DOUBLE
|
|
|
|
using real_t = System.Double;
|
|
|
|
#else
|
|
|
|
using real_t = System.Single;
|
|
|
|
#endif
|
|
|
|
|
2017-11-05 11:37:59 +01:00
|
|
|
namespace Godot
|
|
|
|
{
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Plane represents a normalized plane equation.
|
|
|
|
/// "Over" or "Above" the plane is considered the side of
|
|
|
|
/// the plane towards where the normal is pointing.
|
|
|
|
/// </summary>
|
2019-08-08 03:29:40 +02:00
|
|
|
[Serializable]
|
|
|
|
[StructLayout(LayoutKind.Sequential)]
|
2017-11-05 11:37:59 +01:00
|
|
|
public struct Plane : IEquatable<Plane>
|
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
private Vector3 _normal;
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
2020-07-21 20:07:00 +02:00
|
|
|
/// The normal of the plane, which must be normalized.
|
|
|
|
/// In the scalar equation of the plane `ax + by + cz = d`, this is
|
2020-07-22 02:13:45 +02:00
|
|
|
/// the vector `(a, b, c)`, where `d` is the <see cref="D"/> property.
|
2020-07-07 23:29:35 +02:00
|
|
|
/// </summary>
|
|
|
|
/// <value>Equivalent to `x`, `y`, and `z`.</value>
|
2018-05-17 17:04:20 +02:00
|
|
|
public Vector3 Normal
|
|
|
|
{
|
|
|
|
get { return _normal; }
|
|
|
|
set { _normal = value; }
|
|
|
|
}
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// The X component of the plane's normal vector.
|
|
|
|
/// </summary>
|
|
|
|
/// <value>Equivalent to <see cref="Normal"/>'s X value.</value>
|
2018-03-01 08:51:35 +01:00
|
|
|
public real_t x
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
|
|
|
get
|
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
return _normal.x;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
set
|
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
_normal.x = value;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// The Y component of the plane's normal vector.
|
|
|
|
/// </summary>
|
|
|
|
/// <value>Equivalent to <see cref="Normal"/>'s Y value.</value>
|
2018-03-01 08:51:35 +01:00
|
|
|
public real_t y
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
|
|
|
get
|
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
return _normal.y;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
set
|
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
_normal.y = value;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// The Z component of the plane's normal vector.
|
|
|
|
/// </summary>
|
|
|
|
/// <value>Equivalent to <see cref="Normal"/>'s Z value.</value>
|
2018-03-01 08:51:35 +01:00
|
|
|
public real_t z
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
|
|
|
get
|
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
return _normal.z;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
set
|
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
_normal.z = value;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// The distance from the origin to the plane (in the direction of
|
|
|
|
/// <see cref="Normal"/>). This value is typically non-negative.
|
2020-07-21 20:07:00 +02:00
|
|
|
/// In the scalar equation of the plane `ax + by + cz = d`,
|
|
|
|
/// this is `d`, while the `(a, b, c)` coordinates are represented
|
2020-07-22 02:13:45 +02:00
|
|
|
/// by the <see cref="Normal"/> property.
|
2020-07-07 23:29:35 +02:00
|
|
|
/// </summary>
|
|
|
|
/// <value>The plane's distance from the origin.</value>
|
2020-05-10 16:47:11 +02:00
|
|
|
public real_t D { get; set; }
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// The center of the plane, the point where the normal line intersects the plane.
|
|
|
|
/// </summary>
|
|
|
|
/// <value>Equivalent to <see cref="Normal"/> multiplied by `D`.</value>
|
2017-11-05 11:37:59 +01:00
|
|
|
public Vector3 Center
|
|
|
|
{
|
|
|
|
get
|
|
|
|
{
|
2020-05-10 16:47:11 +02:00
|
|
|
return _normal * D;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
2020-07-07 23:29:35 +02:00
|
|
|
set
|
|
|
|
{
|
|
|
|
_normal = value.Normalized();
|
|
|
|
D = value.Length();
|
|
|
|
}
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Returns the shortest distance from this plane to the position `point`.
|
|
|
|
/// </summary>
|
2020-07-22 22:46:04 +02:00
|
|
|
/// <param name="point">The position to use for the calculation.</param>
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <returns>The shortest distance.</returns>
|
2018-03-01 08:51:35 +01:00
|
|
|
public real_t DistanceTo(Vector3 point)
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2020-05-10 16:47:11 +02:00
|
|
|
return _normal.Dot(point) - D;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Returns true if point is inside the plane.
|
|
|
|
/// Comparison uses a custom minimum epsilon threshold.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="point">The point to check.</param>
|
|
|
|
/// <param name="epsilon">The tolerance threshold.</param>
|
|
|
|
/// <returns>A bool for whether or not the plane has the point.</returns>
|
2018-03-01 08:51:35 +01:00
|
|
|
public bool HasPoint(Vector3 point, real_t epsilon = Mathf.Epsilon)
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2020-05-10 16:47:11 +02:00
|
|
|
real_t dist = _normal.Dot(point) - D;
|
2017-11-21 23:32:19 +01:00
|
|
|
return Mathf.Abs(dist) <= epsilon;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Returns the intersection point of the three planes: `b`, `c`,
|
|
|
|
/// and this plane. If no intersection is found, `null` is returned.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="b">One of the three planes to use in the calculation.</param>
|
|
|
|
/// <param name="c">One of the three planes to use in the calculation.</param>
|
|
|
|
/// <returns>The intersection, or `null` if none is found.</returns>
|
2019-10-09 04:47:22 +02:00
|
|
|
public Vector3? Intersect3(Plane b, Plane c)
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
real_t denom = _normal.Cross(b._normal).Dot(c._normal);
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2019-10-09 04:47:22 +02:00
|
|
|
if (Mathf.IsZeroApprox(denom))
|
2020-07-07 23:29:35 +02:00
|
|
|
{
|
2019-10-09 04:47:22 +02:00
|
|
|
return null;
|
2020-07-07 23:29:35 +02:00
|
|
|
}
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2020-05-10 16:47:11 +02:00
|
|
|
Vector3 result = b._normal.Cross(c._normal) * D +
|
|
|
|
c._normal.Cross(_normal) * b.D +
|
|
|
|
_normal.Cross(b._normal) * c.D;
|
2017-11-05 11:37:59 +01:00
|
|
|
|
|
|
|
return result / denom;
|
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Returns the intersection point of a ray consisting of the
|
|
|
|
/// position `from` and the direction normal `dir` with this plane.
|
|
|
|
/// If no intersection is found, `null` is returned.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="from">The start of the ray.</param>
|
|
|
|
/// <param name="dir">The direction of the ray, normalized.</param>
|
|
|
|
/// <returns>The intersection, or `null` if none is found.</returns>
|
2019-10-09 04:47:22 +02:00
|
|
|
public Vector3? IntersectRay(Vector3 from, Vector3 dir)
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
real_t den = _normal.Dot(dir);
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2019-10-09 04:47:22 +02:00
|
|
|
if (Mathf.IsZeroApprox(den))
|
2020-07-07 23:29:35 +02:00
|
|
|
{
|
2019-10-09 04:47:22 +02:00
|
|
|
return null;
|
2020-07-07 23:29:35 +02:00
|
|
|
}
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2020-05-10 16:47:11 +02:00
|
|
|
real_t dist = (_normal.Dot(from) - D) / den;
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2018-01-18 21:37:17 +01:00
|
|
|
// This is a ray, before the emitting pos (from) does not exist
|
2017-11-05 11:37:59 +01:00
|
|
|
if (dist > Mathf.Epsilon)
|
2020-07-07 23:29:35 +02:00
|
|
|
{
|
2019-10-09 04:47:22 +02:00
|
|
|
return null;
|
2020-07-07 23:29:35 +02:00
|
|
|
}
|
2017-11-05 11:37:59 +01:00
|
|
|
|
|
|
|
return from + dir * -dist;
|
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Returns the intersection point of a line segment from
|
|
|
|
/// position `begin` to position `end` with this plane.
|
|
|
|
/// If no intersection is found, `null` is returned.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="begin">The start of the line segment.</param>
|
|
|
|
/// <param name="end">The end of the line segment.</param>
|
|
|
|
/// <returns>The intersection, or `null` if none is found.</returns>
|
2019-10-09 04:47:22 +02:00
|
|
|
public Vector3? IntersectSegment(Vector3 begin, Vector3 end)
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
|
|
|
Vector3 segment = begin - end;
|
2018-05-17 17:04:20 +02:00
|
|
|
real_t den = _normal.Dot(segment);
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2019-10-09 04:47:22 +02:00
|
|
|
if (Mathf.IsZeroApprox(den))
|
2020-07-07 23:29:35 +02:00
|
|
|
{
|
2019-10-09 04:47:22 +02:00
|
|
|
return null;
|
2020-07-07 23:29:35 +02:00
|
|
|
}
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2020-05-10 16:47:11 +02:00
|
|
|
real_t dist = (_normal.Dot(begin) - D) / den;
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2019-10-09 04:47:22 +02:00
|
|
|
// Only allow dist to be in the range of 0 to 1, with tolerance.
|
2018-04-08 05:39:35 +02:00
|
|
|
if (dist < -Mathf.Epsilon || dist > 1.0f + Mathf.Epsilon)
|
2020-07-07 23:29:35 +02:00
|
|
|
{
|
2019-10-09 04:47:22 +02:00
|
|
|
return null;
|
2020-07-07 23:29:35 +02:00
|
|
|
}
|
2017-11-05 11:37:59 +01:00
|
|
|
|
|
|
|
return begin + segment * -dist;
|
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Returns true if `point` is located above the plane.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="point">The point to check.</param>
|
|
|
|
/// <returns>A bool for whether or not the point is above the plane.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
public bool IsPointOver(Vector3 point)
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2020-05-10 16:47:11 +02:00
|
|
|
return _normal.Dot(point) > D;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Returns the plane scaled to unit length.
|
|
|
|
/// </summary>
|
|
|
|
/// <returns>A normalized version of the plane.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
public Plane Normalized()
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
real_t len = _normal.Length();
|
2017-11-05 11:37:59 +01:00
|
|
|
|
|
|
|
if (len == 0)
|
2020-07-07 23:29:35 +02:00
|
|
|
{
|
2017-11-05 11:37:59 +01:00
|
|
|
return new Plane(0, 0, 0, 0);
|
2020-07-07 23:29:35 +02:00
|
|
|
}
|
2017-11-05 11:37:59 +01:00
|
|
|
|
2020-05-10 16:47:11 +02:00
|
|
|
return new Plane(_normal / len, D / len);
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Returns the orthogonal projection of `point` into the plane.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="point">The point to project.</param>
|
|
|
|
/// <returns>The projected point.</returns>
|
2017-11-21 23:32:19 +01:00
|
|
|
public Vector3 Project(Vector3 point)
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
return point - _normal * DistanceTo(point);
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
2018-11-20 11:14:07 +01:00
|
|
|
|
2018-08-21 07:34:34 +02:00
|
|
|
// Constants
|
|
|
|
private static readonly Plane _planeYZ = new Plane(1, 0, 0, 0);
|
|
|
|
private static readonly Plane _planeXZ = new Plane(0, 1, 0, 0);
|
|
|
|
private static readonly Plane _planeXY = new Plane(0, 0, 1, 0);
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// A plane that extends in the Y and Z axes (normal vector points +X).
|
|
|
|
/// </summary>
|
|
|
|
/// <value>Equivalent to `new Plane(1, 0, 0, 0)`.</value>
|
2018-08-21 07:34:34 +02:00
|
|
|
public static Plane PlaneYZ { get { return _planeYZ; } }
|
2020-07-07 23:29:35 +02:00
|
|
|
|
|
|
|
/// <summary>
|
|
|
|
/// A plane that extends in the X and Z axes (normal vector points +Y).
|
|
|
|
/// </summary>
|
|
|
|
/// <value>Equivalent to `new Plane(0, 1, 0, 0)`.</value>
|
2018-08-21 07:34:34 +02:00
|
|
|
public static Plane PlaneXZ { get { return _planeXZ; } }
|
2020-07-07 23:29:35 +02:00
|
|
|
|
|
|
|
/// <summary>
|
|
|
|
/// A plane that extends in the X and Y axes (normal vector points +Z).
|
|
|
|
/// </summary>
|
|
|
|
/// <value>Equivalent to `new Plane(0, 0, 1, 0)`.</value>
|
2018-08-21 07:34:34 +02:00
|
|
|
public static Plane PlaneXY { get { return _planeXY; } }
|
2018-11-20 11:14:07 +01:00
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Constructs a plane from four values. `a`, `b` and `c` become the
|
|
|
|
/// components of the resulting plane's <see cref="Normal"/> vector.
|
|
|
|
/// `d` becomes the plane's distance from the origin.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="a">The X component of the plane's normal vector.</param>
|
|
|
|
/// <param name="b">The Y component of the plane's normal vector.</param>
|
|
|
|
/// <param name="c">The Z component of the plane's normal vector.</param>
|
|
|
|
/// <param name="d">The plane's distance from the origin. This value is typically non-negative.</param>
|
2020-05-10 16:47:11 +02:00
|
|
|
public Plane(real_t a, real_t b, real_t c, real_t d)
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
_normal = new Vector3(a, b, c);
|
2020-05-10 16:47:11 +02:00
|
|
|
this.D = d;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
2020-07-07 23:29:35 +02:00
|
|
|
|
|
|
|
/// <summary>
|
|
|
|
/// Constructs a plane from a normal vector and the plane's distance to the origin.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="normal">The normal of the plane, must be normalized.</param>
|
|
|
|
/// <param name="d">The plane's distance from the origin. This value is typically non-negative.</param>
|
2020-05-10 16:47:11 +02:00
|
|
|
public Plane(Vector3 normal, real_t d)
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
this._normal = normal;
|
2020-05-10 16:47:11 +02:00
|
|
|
this.D = d;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Constructs a plane from the three points, given in clockwise order.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="v1">The first point.</param>
|
|
|
|
/// <param name="v2">The second point.</param>
|
|
|
|
/// <param name="v3">The third point.</param>
|
2017-11-05 11:37:59 +01:00
|
|
|
public Plane(Vector3 v1, Vector3 v2, Vector3 v3)
|
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
_normal = (v1 - v3).Cross(v1 - v2);
|
|
|
|
_normal.Normalize();
|
2020-05-10 16:47:11 +02:00
|
|
|
D = _normal.Dot(v1);
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
public static Plane operator -(Plane plane)
|
|
|
|
{
|
2020-05-10 16:47:11 +02:00
|
|
|
return new Plane(-plane._normal, -plane.D);
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
public static bool operator ==(Plane left, Plane right)
|
|
|
|
{
|
|
|
|
return left.Equals(right);
|
|
|
|
}
|
|
|
|
|
|
|
|
public static bool operator !=(Plane left, Plane right)
|
|
|
|
{
|
|
|
|
return !left.Equals(right);
|
|
|
|
}
|
|
|
|
|
|
|
|
public override bool Equals(object obj)
|
|
|
|
{
|
|
|
|
if (obj is Plane)
|
|
|
|
{
|
|
|
|
return Equals((Plane)obj);
|
|
|
|
}
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
public bool Equals(Plane other)
|
|
|
|
{
|
2020-05-10 16:47:11 +02:00
|
|
|
return _normal == other._normal && D == other.D;
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
2020-07-07 23:29:35 +02:00
|
|
|
/// <summary>
|
|
|
|
/// Returns true if this plane and `other` are approximately equal, by running
|
|
|
|
/// <see cref="Mathf.IsEqualApprox(real_t, real_t)"/> on each component.
|
|
|
|
/// </summary>
|
|
|
|
/// <param name="other">The other plane to compare.</param>
|
|
|
|
/// <returns>Whether or not the planes are approximately equal.</returns>
|
2019-10-14 22:33:45 +02:00
|
|
|
public bool IsEqualApprox(Plane other)
|
|
|
|
{
|
2020-05-10 16:47:11 +02:00
|
|
|
return _normal.IsEqualApprox(other._normal) && Mathf.IsEqualApprox(D, other.D);
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
public override int GetHashCode()
|
|
|
|
{
|
2020-05-10 16:47:11 +02:00
|
|
|
return _normal.GetHashCode() ^ D.GetHashCode();
|
2017-11-05 11:37:59 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
public override string ToString()
|
|
|
|
{
|
2021-02-25 15:54:50 +01:00
|
|
|
return String.Format("{0}, {1}", new object[]
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
_normal.ToString(),
|
2020-05-10 16:47:11 +02:00
|
|
|
D.ToString()
|
2017-11-05 11:37:59 +01:00
|
|
|
});
|
|
|
|
}
|
|
|
|
|
|
|
|
public string ToString(string format)
|
|
|
|
{
|
2021-02-25 15:54:50 +01:00
|
|
|
return String.Format("{0}, {1}", new object[]
|
2017-11-05 11:37:59 +01:00
|
|
|
{
|
2018-05-17 17:04:20 +02:00
|
|
|
_normal.ToString(format),
|
2020-05-10 16:47:11 +02:00
|
|
|
D.ToString(format)
|
2017-11-05 11:37:59 +01:00
|
|
|
});
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|