257 lines
8.8 KiB
C#
257 lines
8.8 KiB
C#
// Copyright (c) 2014-2015 Robert Rouhani <robert.rouhani@gmail.com> and other contributors (see CONTRIBUTORS file).
|
|
// Licensed under the MIT License - https://raw.github.com/Robmaister/SharpNav/master/LICENSE
|
|
|
|
using System;
|
|
|
|
#if MONOGAME
|
|
using Vector3 = Microsoft.Xna.Framework.Vector3;
|
|
#elif OPENTK
|
|
using Vector3 = OpenTK.Vector3;
|
|
#elif SHARPDX
|
|
using Vector3 = SharpDX.Vector3;
|
|
#endif
|
|
|
|
namespace SharpNav.Geometry
|
|
{
|
|
/// <summary>
|
|
/// Contains helper methods to calculate the distance between two objects.
|
|
/// </summary>
|
|
internal static class Distance
|
|
{
|
|
/// <summary>
|
|
/// Find the 3D distance between a point (x, y, z) and a segment PQ
|
|
/// </summary>
|
|
/// <param name="pt">The coordinate of the point.</param>
|
|
/// <param name="p">The coordinate of point P in the segment PQ.</param>
|
|
/// <param name="q">The coordinate of point Q in the segment PQ.</param>
|
|
/// <returns>The distance between the point and the segment.</returns>
|
|
internal static float PointToSegmentSquared(ref Vector3 pt, ref Vector3 p, ref Vector3 q)
|
|
{
|
|
//distance from P to Q
|
|
Vector3 pq = q - p;
|
|
|
|
//disance from P to the lone point
|
|
float dx = pt.X - p.X;
|
|
float dy = pt.Y - p.Y;
|
|
float dz = pt.Z - p.Z;
|
|
|
|
float segmentMagnitudeSquared = pq.LengthSquared();
|
|
float t = pq.X * dx + pq.Y * dy + pq.Z * dz;
|
|
|
|
if (segmentMagnitudeSquared > 0)
|
|
t /= segmentMagnitudeSquared;
|
|
|
|
//keep t between 0 and 1
|
|
if (t < 0)
|
|
t = 0;
|
|
else if (t > 1)
|
|
t = 1;
|
|
|
|
dx = p.X + t * pq.X - pt.X;
|
|
dy = p.Y + t * pq.Y - pt.Y;
|
|
dz = p.Z + t * pq.Z - pt.Z;
|
|
|
|
return dx * dx + dy * dy + dz * dz;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find the 2d distance between a point (x, z) and a segment PQ, where P is (px, pz) and Q is (qx, qz).
|
|
/// </summary>
|
|
/// <param name="x">The X coordinate of the point.</param>
|
|
/// <param name="z">The Z coordinate of the point.</param>
|
|
/// <param name="px">The X coordinate of point P in the segment PQ.</param>
|
|
/// <param name="pz">The Z coordinate of point P in the segment PQ.</param>
|
|
/// <param name="qx">The X coordinate of point Q in the segment PQ.</param>
|
|
/// <param name="qz">The Z coordinate of point Q in the segment PQ.</param>
|
|
/// <returns>The distance between the point and the segment.</returns>
|
|
internal static float PointToSegment2DSquared(int x, int z, int px, int pz, int qx, int qz)
|
|
{
|
|
float segmentDeltaX = qx - px;
|
|
float segmentDeltaZ = qz - pz;
|
|
float dx = x - px;
|
|
float dz = z - pz;
|
|
float segmentMagnitudeSquared = segmentDeltaX * segmentDeltaX + segmentDeltaZ * segmentDeltaZ;
|
|
float t = segmentDeltaX * dx + segmentDeltaZ * dz;
|
|
|
|
//normalize?
|
|
if (segmentMagnitudeSquared > 0)
|
|
t /= segmentMagnitudeSquared;
|
|
|
|
//0 < t < 1
|
|
if (t < 0)
|
|
t = 0;
|
|
else if (t > 1)
|
|
t = 1;
|
|
|
|
dx = px + t * segmentDeltaX - x;
|
|
dz = pz + t * segmentDeltaZ - z;
|
|
|
|
return dx * dx + dz * dz;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find the 2d distance between a point and a segment PQ
|
|
/// </summary>
|
|
/// <param name="pt">The coordinate of the point.</param>
|
|
/// <param name="p">The coordinate of point P in the segment PQ.</param>
|
|
/// <param name="q">The coordinate of point Q in the segment PQ.</param>
|
|
/// <returns>The distance between the point and the segment.</returns>
|
|
internal static float PointToSegment2DSquared(ref Vector3 pt, ref Vector3 p, ref Vector3 q)
|
|
{
|
|
float t = 0;
|
|
return PointToSegment2DSquared(ref pt, ref p, ref q, out t);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find the 2d distance between a point and a segment PQ
|
|
/// </summary>
|
|
/// <param name="pt">The coordinate of the point.</param>
|
|
/// <param name="p">The coordinate of point P in the segment PQ.</param>
|
|
/// <param name="q">The coordinate of point Q in the segment PQ.</param>
|
|
/// <param name="t">Parameterization ratio t</param>
|
|
/// <returns>The distance between the point and the segment.</returns>
|
|
internal static float PointToSegment2DSquared(ref Vector3 pt, ref Vector3 p, ref Vector3 q, out float t)
|
|
{
|
|
//distance from P to Q in the xz plane
|
|
float segmentDeltaX = q.X - p.X;
|
|
float segmentDeltaZ = q.Z - p.Z;
|
|
|
|
//distance from P to lone point in xz plane
|
|
float dx = pt.X - p.X;
|
|
float dz = pt.Z - p.Z;
|
|
|
|
float segmentMagnitudeSquared = segmentDeltaX * segmentDeltaX + segmentDeltaZ * segmentDeltaZ;
|
|
t = segmentDeltaX * dx + segmentDeltaZ * dz;
|
|
|
|
if (segmentMagnitudeSquared > 0)
|
|
t /= segmentMagnitudeSquared;
|
|
|
|
//keep t between 0 and 1
|
|
if (t < 0)
|
|
t = 0;
|
|
else if (t > 1)
|
|
t = 1;
|
|
|
|
dx = p.X + t * segmentDeltaX - pt.X;
|
|
dz = p.Z + t * segmentDeltaZ - pt.Z;
|
|
|
|
return dx * dx + dz * dz;
|
|
}
|
|
|
|
internal static float PointToPolygonSquared(Vector3 point, Vector3[] verts, int vertCount)
|
|
{
|
|
float dmin = float.MaxValue;
|
|
bool c = false;
|
|
|
|
for (int i = 0, j = vertCount - 1; i < vertCount; j = i++)
|
|
{
|
|
Vector3 vi = verts[i];
|
|
Vector3 vj = verts[j];
|
|
|
|
if (((vi.Z > point.Z) != (vj.Z > point.Z)) && (point.X < (vj.X - vi.X) * (point.Z - vi.Z) / (vj.Z - vi.Z) + vi.X))
|
|
c = !c;
|
|
|
|
dmin = Math.Min(dmin, Distance.PointToSegment2DSquared(ref point, ref vj, ref vi));
|
|
}
|
|
|
|
return c ? -dmin : dmin;
|
|
}
|
|
|
|
//TOOD where did these come from?
|
|
|
|
/// <summary>
|
|
/// Finds the squared distance between a point and the nearest edge of a polygon.
|
|
/// </summary>
|
|
/// <param name="pt">A point.</param>
|
|
/// <param name="verts">A set of vertices that define a polygon.</param>
|
|
/// <param name="nverts">The number of vertices to use from <c>verts</c>.</param>
|
|
/// <returns>The squared distance between a point and the nearest edge of a polygon.</returns>
|
|
internal static float PointToPolygonEdgeSquared(Vector3 pt, Vector3[] verts, int nverts)
|
|
{
|
|
float dmin = float.MaxValue;
|
|
for (int i = 0, j = nverts - 1; i < nverts; j = i++)
|
|
dmin = Math.Min(dmin, Distance.PointToSegment2DSquared(ref pt, ref verts[j], ref verts[i]));
|
|
|
|
return Containment.PointInPoly(pt, verts, nverts) ? -dmin : dmin;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the distance between a point and the nearest edge of a polygon.
|
|
/// </summary>
|
|
/// <param name="pt">A point.</param>
|
|
/// <param name="verts">A set of vertices that define a polygon.</param>
|
|
/// <param name="nverts">The number of vertices to use from <c>verts</c>.</param>
|
|
/// <param name="edgeDist">A buffer for edge distances to be stored in.</param>
|
|
/// <param name="edgeT">A buffer for parametrization ratios to be stored in.</param>
|
|
/// <returns>A value indicating whether the point is contained in the polygon.</returns>
|
|
internal static bool PointToPolygonEdgeSquared(Vector3 pt, Vector3[] verts, int nverts, float[] edgeDist, float[] edgeT)
|
|
{
|
|
for (int i = 0, j = nverts - 1; i < nverts; j = i++)
|
|
edgeDist[j] = PointToSegment2DSquared(ref pt, ref verts[j], ref verts[i], out edgeT[j]);
|
|
|
|
return Containment.PointInPoly(pt, verts, nverts);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the distance between a point and triangle ABC.
|
|
/// </summary>
|
|
/// <param name="p">A point.</param>
|
|
/// <param name="a">The first vertex of the triangle.</param>
|
|
/// <param name="b">The second vertex of the triangle.</param>
|
|
/// <param name="c">The third vertex of the triangle.</param>
|
|
/// <returns>The distnace between the point and the triangle.</returns>
|
|
internal static float PointToTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
|
|
{
|
|
//If the point lies inside the triangle, return the interpolated y-coordinate
|
|
float h;
|
|
if (PointToTriangle(p, a, b, c, out h))
|
|
{
|
|
return Math.Abs(h - p.Y);
|
|
}
|
|
|
|
return float.MaxValue;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finds the distance between a point and triangle ABC.
|
|
/// </summary>
|
|
/// <param name="p">A point.</param>
|
|
/// <param name="a">The first vertex of the triangle.</param>
|
|
/// <param name="b">The second vertex of the triangle.</param>
|
|
/// <param name="c">The third vertex of the triangle.</param>
|
|
/// <param name="height">The height between the point and the triangle.</param>
|
|
/// <returns>A value indicating whether the point is contained within the triangle.</returns>
|
|
internal static bool PointToTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c, out float height)
|
|
{
|
|
Vector3 v0 = c - a;
|
|
Vector3 v1 = b - a;
|
|
Vector3 v2 = p - a;
|
|
|
|
float dot00, dot01, dot02, dot11, dot12;
|
|
|
|
Vector3Extensions.Dot2D(ref v0, ref v0, out dot00);
|
|
Vector3Extensions.Dot2D(ref v0, ref v1, out dot01);
|
|
Vector3Extensions.Dot2D(ref v0, ref v2, out dot02);
|
|
Vector3Extensions.Dot2D(ref v1, ref v1, out dot11);
|
|
Vector3Extensions.Dot2D(ref v1, ref v2, out dot12);
|
|
|
|
//compute barycentric coordinates
|
|
float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
|
|
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
|
|
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
|
|
|
|
const float EPS = 1E-4f;
|
|
|
|
//if point lies inside triangle, return interpolated y-coordinate
|
|
if (u >= -EPS && v >= -EPS && (u + v) <= 1 + EPS)
|
|
{
|
|
height = a.Y + v0.Y * u + v1.Y * v;
|
|
return true;
|
|
}
|
|
|
|
height = float.MaxValue;
|
|
return false;
|
|
}
|
|
}
|
|
}
|