/// <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;
}
}
