public Vector3 FindClosestPointOnPath(Vector3 pt, out float minpathlength)
{
var rpt = Vector3.zero;
float mindist = 9e30f;
minpathlength = 9e30f;
float curpathlength = 0;
foreach (var w in _path.waypts)
{
var wp1 = sman.rgo.transform.TransformPoint(w.frNode.pt);
var wp2 = sman.rgo.transform.TransformPoint(w.toNode.pt);
var lamb = (float)GraphUtil.FindClosestLambClampedTo01(pt, wp1, wp2);
var nearpt = lamb * (wp2 - wp1) + wp1;
var dist = Vector3.Distance(pt, nearpt);
if (dist < mindist)
{
rpt = nearpt;
mindist = dist;
minpathlength = curpathlength + lamb * w.distance;
}
curpathlength += w.distance;
}
return(rpt);
}
public Weg FindClosestWegOnPath(Vector3 pt, out float minpathlength)
{
Weg rweg = null;
float mindist = 9e30f;
minpathlength = 9e30f;
float curpathlength = 0;
if (_path != null)
{
foreach (var w in _path.waypts)
{
var wp1 = sman.rgo.transform.TransformPoint(w.frNode.pt);
var wp2 = sman.rgo.transform.TransformPoint(w.toNode.pt);
var lamb = (float)GraphUtil.FindClosestLambClampedTo01(pt, wp1, wp2);
var nearpt = lamb * (wp2 - wp1) + wp1;
var dist = Vector3.Distance(pt, nearpt);
if (dist < mindist)
{
rweg = w;
mindist = dist;
minpathlength = curpathlength + lamb * w.distance;
}
curpathlength += w.distance;
}
}
//RegionMan.Log("fclp:" + i);
return(rweg);
}