/// <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))); }