/// <summary>
/// Find the closest point on an offmesh connection, which is in between the two points.
/// </summary>
/// <param name="poly">Current polygon</param>
/// <param name="pos">Current position</param>
/// <param name="closest">Resulting point that is closest.</param>
public void ClosestPointOnPolyOffMeshConnection(Poly poly, Vector3 pos, out Vector3 closest)
{
Vector3 v0 = Verts[poly.Verts[0]];
Vector3 v1 = Verts[poly.Verts[1]];
float d0 = (pos - v0).Length();
float d1 = (pos - v1).Length();
float u = d0 / (d0 + d1);
closest = Vector3.Lerp(v0, v1, u);
}
/// <summary>
/// Given a point, find the closest point on that poly.
/// </summary>
/// <param name="poly">The current polygon.</param>
/// <param name="pos">The current position</param>
/// <param name="closest">Reference to the closest point</param>
public void ClosestPointOnPolyBoundary(NavPoly poly, Vector3 pos, out Vector3 closest, out bool insidePoly)
{
insidePoly = false;
//Clamp point to be inside the polygon
Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
float[] edgeDistance = new float[PathfindingCommon.VERTS_PER_POLYGON];
float[] edgeT = new float[PathfindingCommon.VERTS_PER_POLYGON];
int numPolyVerts = poly.VertCount;
for (int i = 0; i < numPolyVerts; i++)
{
verts[i] = Verts[poly.Verts[i]];
}
bool inside = Distance.PointToPolygonEdgeSquared(pos, verts, numPolyVerts, edgeDistance, edgeT);
if (inside)
{
//Point is inside the polygon
closest = pos;
insidePoly = false;
}
else
{
//Point is outside the polygon
//Clamp to nearest edge
float minDistance = float.MaxValue;
int minIndex = -1;
for (int i = 0; i < numPolyVerts; i++)
{
if (edgeDistance[i] < minDistance)
{
minDistance = edgeDistance[i];
minIndex = i;
}
}
Vector3 va = verts[minIndex];
Vector3 vb = verts[(minIndex + 1) % numPolyVerts];
closest = Vector3.Lerp(va, vb, edgeT[minIndex]);
}
}
public static Vector3 Lerp(Vector3 v1, Vector3 v2, float amount)
{
return(new Vector3(Vector.Lerp(v1, v2, amount)));
}
