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); }
Vector3 GetPathPoint(float gpprdist, bool curpos = true) { if (path == null) { return(Vector3.zero); } var pp = path.MovePositionAlongPath(gpprdist); if (curpos) { pathpos = pp; pathweg = pp.weg; spathweg = pathweg.frNode.name + " to " + pathweg.toNode.name; wegguid = pp.weg.id; swegguid = wegguid.ToString(); wegdistance = pp.wegDistSoFar; cntgp++; } var pt = new Vector3(pp.pt.x, pp.pt.y + BirdFlyHeight, pp.pt.z); return(pt); }