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