/// <summary>Returns the speed to target when going toward a waypoint</summary> /// <param name="wp">Waypoint</param> /// <returns>Target speed</returns> public double GetTargetSpeed(APWaypoint wp) { if (wp == null) { return(0); } switch (wp.Terrain) { case Terrain.Dangerous: return(1); case Terrain.Bad: return(5); case Terrain.Normal: return(10); case Terrain.Good: return(20); case Terrain.Open: default: return(30); } }
bool goToWaypoint(APWaypoint wp, APWaypoint nextWP) { Vector3D route = this.transformer.Pos(wp.WP.Coords) * PLANE; double routeLength = route.Length(); if (this.settings.IsWaypointReached(wp, routeLength)) { return(true); } this.remote.HandBrake = false; double angle = Math.Acos(Vector3D.Normalize(route).Dot(FORWARD)); bool reverseDir = angle > Math.PI / 2; if (reverseDir) { angle = -(angle - Math.PI); } angle *= (route.Dot(RIGHT) > 0) ? 1 : -1; double curSpeed = this.remote.GetShipSpeed() * (this.Reversing ? -1 : 1); double targetSpeed = this.settings.GetTargetSpeed(this.settings.GetTargetSpeed(wp), this.settings.GetTargetSpeed(nextWP), routeLength) * (reverseDir ? -1 : 1); this.wheels.SetPower(this.getPower(targetSpeed, curSpeed)); this.wheels.SetSteer(this.settings.GetSteer(angle, curSpeed)); return(false); }
double getAStarDist(APWaypoint a) { if (this.aStartDistance == null) { this.aStartDistance = (this.WP.Coords - a.WP.Coords).Length(); } return(this.aStartDistance.Value); }
/// <summary>Links this waypoint to another waypoint, denoting the autopilot can go from <see cref="this"/> to <paramref name="wp"/>. It is not bidirectionnal.</summary> /// <param name="wp">Waypoint reachable from this waypoint.</param> public void AddWP(APWaypoint wp) { if (this.waypointsNames == null) { this.waypointsNames = new List <string>(); } this.waypointsNames.Add(wp.Name); this.LinkedWps.Add(wp); }
/// <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(); }
public double GetTargetPrecision(APWaypoint wp) { switch (wp.Type) { case WPType.PrecisePath: return(0.5); case WPType.Maneuvering: return(0.1); case WPType.Path: default: return(3); } }
MyTuple <double, APWaypoint> getDist(Vector3D pos, APWaypoint a, APWaypoint b) { Vector3D ab = b.WP.Coords - a.WP.Coords; Vector3D ap = pos - a.WP.Coords; if (ap.Dot(ab) <= 0) { return(MyTuple.Create(ap.LengthSquared(), a)); } Vector3D bp = pos - b.WP.Coords; return(bp.Dot(ab) >= 0 ? MyTuple.Create(bp.LengthSquared(), b) : MyTuple.Create(ab.Cross(ap).LengthSquared() / ab.LengthSquared(), null as APWaypoint)); }
/// <summary>Requests the Autopilot to go to a waypoint</summary> /// <param name="wpName">Name of the waypoint to reach</param> public void GoTo(string wpName) { if (this.activated) { APWaypoint end = this.Network.GetWaypoint(wpName); if (end == null) { this.log($"Could not find waypoint {wpName}"); } else { this.Network.GetPath(this.remote.GetPosition(), end, this.currentPath); this.log($"Path: {this.currentPath.Count}"); } } }
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); } }
void handle(Process p) { if (!this.activated) { return; } if (this.currentPath.Count > 0) { APWaypoint nextWP = this.currentPath.Count > 1 ? this.currentPath[this.currentPath.Count - 2] : null; if (this.goToWaypoint(this.currentPath.Last(), nextWP)) { this.currentPath.Pop(); } } if (this.currentPath.Count == 0) { this.stop(); } }
/// <summary>Translates the list of waypoint names from the ini string to a list of actual <see cref="APWaypoint"/></summary> /// <param name="network">Network that contains all the waypoints</param> /// <param name="logger">Optional logger</param> public void Update(WPNetwork network, Action <string> logger) { if (this.waypointsNames != null) { int notFound = 0; foreach (string wpName in this.waypointsNames) { APWaypoint wp = network.GetWaypoint(wpName); if (wp == null) { ++notFound; } else { this.LinkedWps.Add(wp); } } if (notFound != 0) { logger?.Invoke($"Waypoint '{this.Name}' links to {notFound} unknown waypoints"); } } }
public bool IsWaypointReached(APWaypoint wp, double distance) => distance < this.GetTargetPrecision(wp);
/// <summary>For A* search</summary> public void SetAStarPrev(APWaypoint prev, APWaypoint end, double fromStart) { this.DistToGoal = fromStart + this.getAStarDist(end); this.PrevWP = prev; this.DistFromStart = fromStart; }
/// <summary>Computes the path to go from a position to a waypoint</summary> /// <param name="pos">Starting position</param> /// <param name="end">Waypoint to reach</param> /// <param name="path">will be filled with a list of waypoint representing the path to follow</param> public void GetPath(Vector3D pos, APWaypoint end, List <APWaypoint> path) { path.Clear(); APWaypoint start = this.getClosest(pos); if (start == null) { return; } this.aStarQueue.Clear(); foreach (APWaypoint wp in this.waypoints.Values) { wp.ResetAStar(); } start.SetAStarPrev(null, end, 0); this.aStarQueue.Add(start.DistToGoal, start); while (this.aStarQueue.Count != 0) { APWaypoint curWP = this.aStarQueue.ElementAt(0).Value; if (curWP == end) { APWaypoint cur = end; while (cur != null) { path.Add(cur); cur = cur.PrevWP; } } this.aStarQueue.RemoveAt(0); curWP.Visit(); foreach (APWaypoint wp in curWP.LinkedWps) { if (wp.Visited) { continue; } double fromStart = curWP.DistFromStart + (curWP.WP.Coords - wp.WP.Coords).Length(); if (wp.PrevWP == null) { wp.SetAStarPrev(curWP, end, fromStart); this.aStarQueue.Add(wp.DistToGoal, wp); } else if (fromStart >= wp.DistFromStart) { continue; } else { this.aStarQueue.Remove(wp.DistToGoal); wp.SetAStarPrev(curWP, end, fromStart); this.aStarQueue.Add(wp.DistToGoal, wp); } } } if (path.Count > 1 && path.Last().WP.Name.StartsWith(",")) { path.Last().WP.Coords = (path.Last().WP.Coords + path[path.Count - 2].WP.Coords) / 2; } }