APWaypoint getClosest(Vector3D pos)
{
double maxDist = double.MaxValue;
APWaypoint cA = null, cB = null;
foreach (APWaypoint wp in this.waypoints.Values)
{
wp.ResetAStar();
}
foreach (APWaypoint wpA in this.waypoints.Values)
{
wpA.Visit();
foreach (APWaypoint wpB in wpA.LinkedWps.Where(w => !w.Visited))
{
MyTuple <double, APWaypoint> dist = this.getDist(pos, wpA, wpB);
if (dist.Item1 < maxDist)
{
if (dist.Item2 == null)
{
cA = wpA;
cB = wpB;
}
else
{
cA = dist.Item2;
cB = null;
}
maxDist = dist.Item1;
}
}
}
if (cB == null)
{
return(cA);
}
else
{
Vector3D b = cB.WP.Coords;
var ba = Vector3D.Normalize(cA.WP.Coords - b);
var res = new APWaypoint(new MyWaypointInfo(",TMPSTART", b + ((pos - b).Dot(ba) * ba)));
res.AddWP(cA);
res.AddWP(cB);
return(res);
}
}
/// <summary>Creates a new waypoint from the position of the <see cref="remote"/> that is reachable from the waypoint named <paramref name="linkedWpN"/></summary>
/// <param name="name">Name of the new waypoint</param>
/// <param name="linkedWpN">Name of the waypoint that is biderctionally reachable</param>
/// <param name="terrain">Terrain type of the new waypoint</param>
/// <param name="type">Type of the new waypoint</param>
public void AddLinkedWP(string name, string linkedWpN, Terrain terrain = Terrain.Dangerous, WPType type = WPType.PrecisePath)
{
APWaypoint linkedWP = this.waypoints[linkedWpN];
var wp = new APWaypoint(new MyWaypointInfo(name, this.remote.GetPosition()), terrain, type);
linkedWP.AddWP(wp);
wp.AddWP(linkedWP);
this.waypoints[name] = wp;
this.save();
}