public void WaypointOverlay()
{
if (PN == null || TCA == null || !TCA.Available || !GUIWindowBase.HUD_enabled)
{
return;
}
if (SelectingTarget)
{
var coords = MapView.MapIsEnabled?
Coordinates.GetAtPointer(vessel.mainBody) :
Coordinates.GetAtPointerInFlight();
if (coords != null)
{
var t = new WayPoint(coords);
Markers.DrawCBMarker(vessel.mainBody, coords, new Color(1.0f, 0.56f, 0.0f), WayPointMarker);
DrawLabelAtPointer(coords.FullDescription(vessel), t.DistanceTo(vessel));
if (!clicked)
{
if (Input.GetMouseButtonDown(0))
{
clicked = true;
}
else if (Input.GetMouseButtonDown(1))
{
clicked_time = DateTime.Now; clicked = true;
}
}
else
{
if (Input.GetMouseButtonUp(0))
{
t.Update(VSL);
t.Movable = true;
if (select_single)
{
t.Name = "Target";
SelectingTarget = false;
select_single = false;
VSL.SetTarget(null, t);
MapView.ExitMapView();
}
else
{
t.Name = "Waypoint " + (CFG.Path.Count + 1);
AddTargetDamper.Run(() => CFG.Path.Enqueue(t));
}
CFG.ShowPath = true;
clicked = false;
}
if (Input.GetMouseButtonUp(1))
{
SelectingTarget &= (DateTime.Now - clicked_time).TotalSeconds >= GLB.ClickDuration;
clicked = false;
}
}
}
}
bool current_target_drawn = false;
var camera = Markers.CurrentCamera;
if (CFG.ShowPath)
{
var i = 0;
var num = (float)(CFG.Path.Count - 1);
WayPoint wp0 = null;
var total_dist = 0f;
var dist2cameraF = Mathf.Pow(Mathf.Max((camera.transform.position -
VSL.vessel.transform.position).magnitude, 1),
GLB.CameraFadeinPower);
foreach (var wp in CFG.Path)
{
current_target_drawn |= wp.Equals(CFG.Target);
wp.UpdateCoordinates(vessel.mainBody);
var dist = -1f;
if (wp0 != null && wp != selected_waypoint)
{
total_dist += (float)wp.DistanceTo(wp0, VSL.Body) / dist2cameraF;
dist = total_dist;
}
var r = Markers.DefaultIconSize;
var c = marker_color(i, num, dist);
if (wp0 == null)
{
DrawPath(vessel, wp, c);
}
else
{
DrawPath(vessel.mainBody, wp0, wp, c);
}
if (wp == edited_waypoint)
{
c = edited_color; r = Markers.DefaultIconSize * 2;
}
else if (wp.Land)
{
r = Markers.DefaultIconSize * 2;
}
if (DrawWayPoint(wp, c, size:r))
{
DrawLabelAtPointer(wp.FullInfo(vessel), wp.DistanceTo(vessel));
select_waypoint(wp);
}
else if (wp == selected_waypoint)
{
DrawLabelAtPointer(wp.FullInfo(vessel), wp.DistanceTo(vessel));
}
wp0 = wp;
i++;
}
}
//current target and anchor
if (CFG.Anchor != null)
{
DrawWayPoint(CFG.Anchor, Color.cyan, "Anchor");
current_target_drawn |= CFG.Anchor.Equals(CFG.Target);
}
if (CFG.Target != null && !current_target_drawn &&
(!CFG.Target.IsVessel || CFG.Target.GetVessel().LandedOrSplashed))
{
DrawWayPoint(CFG.Target, Color.magenta, "Target");
}
//custom markers
VSL.Info.CustomMarkersWP.ForEach(m => DrawWayPoint(m, Color.red, m.Name));
VSL.Info.CustomMarkersVec.ForEach(m => Markers.DrawWorldMarker(m, Color.red, "Custom WayPoint", WayPointMarker));
//modify the selected waypoint
if (!SelectingTarget && selected_waypoint != null)
{
if (changing_altitude)
{
var dist2camera = (selected_waypoint.GetTransform().position - camera.transform.position).magnitude;
var dy = (Input.mousePosition.y - last_mouse_y) / Screen.height *
dist2camera * Math.Tan(camera.fieldOfView * Mathf.Deg2Rad) * 2;
last_mouse_y = Input.mousePosition.y;
selected_waypoint.Pos.Alt += dy;
selected_waypoint.Update(VSL);
}
else
{
var coords = MapView.MapIsEnabled?
Coordinates.GetAtPointer(vessel.mainBody) :
Coordinates.GetAtPointerInFlight();
if (coords != null)
{
selected_waypoint.Pos = coords;
selected_waypoint.Update(VSL);
}
}
if (Input.GetMouseButtonUp(0) ||
(changing_altitude && Input.GetMouseButtonUp(2)))
{
if (changing_altitude)
{
selected_waypoint.Pos.Alt = Math.Max(selected_waypoint.Pos.Alt,
selected_waypoint.Pos.SurfaceAlt(vessel.mainBody));
}
selected_waypoint = null;
orig_coordinates = null;
changing_altitude = false;
}
else if (Input.GetMouseButtonDown(1))
{
selected_waypoint.Pos = orig_coordinates;
selected_waypoint.Update(VSL);
selected_waypoint = null;
orig_coordinates = null;
changing_altitude = false;
}
}
#if DEBUG
// VSL.Engines.All.ForEach(e => e.engine.thrustTransforms.ForEach(t => DrawWorldMarker(t.position, Color.red, e.name)));
// DrawWorldMarker(VSL.vessel.transform.position, Color.yellow, "Vessel");
// DrawWorldMarker(VSL.Physics.wCoM, Color.green, "CoM");
#endif
}
public static void DrawPath(Vessel v, WayPoint wp1, Color c)
{
DrawPath(v.mainBody, new WayPoint(v.latitude, v.longitude, v.altitude), wp1, c, 1);
}
public void WaypointList()
{
if (PN == null)
{
return;
}
var WPM = WaypointManager.Instance();
if (CFG.Path.Count == 0)
{
GUILayout.BeginHorizontal();
if (TCAScenario.Paths.Count > 0)
{
Utils.ButtonSwitch("Navigation Paths", ref show_path_library, "", GUILayout.ExpandWidth(true));
}
if (WPM != null && WPM.Waypoints.Count > 0)
{
Utils.ButtonSwitch("Contract Waypoints", ref show_stock_waypoints, "", GUILayout.ExpandWidth(true));
}
GUILayout.EndHorizontal();
stock_waypoints(WPM);
path_library();
return;
}
GUILayout.BeginVertical();
GUILayout.BeginHorizontal();
if (GUILayout.Button(CFG.ShowPath? "Hide Waypoints" : "Show Waypoints",
Styles.active_button,
GUILayout.ExpandWidth(true)))
{
CFG.ShowPath = !CFG.ShowPath;
}
if (TCAScenario.Paths.Empty)
{
GUILayout.Label("Load Path", Styles.inactive_button, GUILayout.ExpandWidth(false));
}
else
{
Utils.ButtonSwitch("Load Path", ref show_path_library, "", GUILayout.ExpandWidth(false));
}
if (WPM == null || WPM.Waypoints.Count == 0)
{
GUILayout.Label("Add From Contracts", Styles.inactive_button, GUILayout.ExpandWidth(false));
}
else
{
Utils.ButtonSwitch("Add From Contracts", ref show_stock_waypoints, "", GUILayout.ExpandWidth(false));
}
GUILayout.EndHorizontal();
stock_waypoints(WPM);
path_library();
if (CFG.ShowPath)
{
GUILayout.BeginVertical(Styles.white);
waypointsScroll = GUILayout
.BeginScrollView(waypointsScroll,
GUILayout.Height(Utils.ClampH(TCAGui.LineHeight * (CFG.Path.Count + 1),
TCAGui.ControlsHeightHalf)));
GUILayout.BeginVertical();
int i = 0;
var num = (float)(CFG.Path.Count - 1);
var col = GUI.contentColor;
WayPoint del = null;
WayPoint up = null;
foreach (var wp in CFG.Path)
{
GUI.contentColor = marker_color(i, num);
var label = string.Format("{0}) {1}", 1 + i, wp.GetName());
if (wp == edited_waypoint)
{
label += " *";
}
if (CFG.Target == wp)
{
var hd = (float)wp.DistanceTo(vessel);
var vd = (float)(wp.Pos.Alt - vessel.altitude);
var info = string.Format("Distance: ◀ {0} ▲ {1}",
Utils.formatBigValue(hd, "m"),
Utils.formatBigValue(vd, "m"));
if (VSL.HorizontalSpeed.Absolute > 0.1)
{
info += string.Format(", ETA {0:c}", new TimeSpan(0, 0, (int)(hd / VSL.HorizontalSpeed.Absolute)));
}
GUILayout.Label(info, Styles.white, GUILayout.ExpandWidth(true));
}
GUILayout.BeginHorizontal();
if (GUILayout.Button(new GUIContent(label, string.Format("{0}\nPush to target this waypoint", wp.SurfaceDescription(vessel))),
GUILayout.ExpandWidth(true)))
{
FlightGlobals.fetch.SetVesselTarget(wp.GetTarget());
}
GUI.contentColor = col;
GUILayout.FlexibleSpace();
if (GUILayout.Button("Edit", Styles.normal_button))
{
edit_waypoint(wp);
}
if (GUILayout.Button(new GUIContent("^", "Move up"), Styles.normal_button))
{
up = wp;
}
if (LND != null &&
Utils.ButtonSwitch("Land", wp.Land, "Land on arrival"))
{
wp.Land = !wp.Land;
}
if (Utils.ButtonSwitch("||", wp.Pause, "Pause on arrival", GUILayout.Width(25)))
{
wp.Pause = !wp.Pause;
}
if (GUILayout.Button(new GUIContent("X", "Delete waypoint"),
Styles.danger_button, GUILayout.Width(25)))
{
del = wp;
}
GUILayout.EndHorizontal();
i++;
}
GUI.contentColor = col;
if (del != null)
{
CFG.Path.Remove(del);
}
else if (up != null)
{
CFG.Path.MoveUp(up);
}
if (CFG.Path.Count == 0 && CFG.Nav)
{
CFG.HF.XOn(HFlight.Stop);
}
GUILayout.EndVertical();
GUILayout.EndScrollView();
GUILayout.BeginHorizontal();
path_name = GUILayout.TextField(path_name, GUILayout.ExpandWidth(true));
if (string.IsNullOrEmpty(path_name))
{
GUILayout.Label("Save Path", Styles.inactive_button, GUILayout.Width(70));
}
else
{
var existing = TCAScenario.Paths.Contains(path_name);
if (GUILayout.Button(existing? "Overwrite" : "Save Path",
existing? Styles.danger_button : Styles.enabled_button,
GUILayout.Width(70)))
{
CFG.Path.Name = path_name;
TCAScenario.Paths.SavePath(CFG.Path);
}
}
if (GUILayout.Button("Clear Path", Styles.danger_button, GUILayout.ExpandWidth(false)))
{
CFG.Path.Clear();
}
GUILayout.EndHorizontal();
GUILayout.EndVertical();
}
GUILayout.EndVertical();
}
void waypoint_editor(int windowID)
{
var close = false;
GUILayout.BeginVertical();
GUILayout.BeginHorizontal();
GUILayout.Label("Name:", GUILayout.Width(70));
edited_waypoint_name = GUILayout.TextField(edited_waypoint_name, GUILayout.ExpandWidth(true));
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Latitude:", GUILayout.Width(70));
LatField.Draw("°", false, 1, "F1");
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Longitude:", GUILayout.Width(70));
LonField.Draw("°", false, 1, "F1");
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
GUILayout.Label("Altitude:", GUILayout.Width(70));
AltField.Draw("°", false, 100, "F0");
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button("Delete", Styles.danger_button))
{
CFG.Path.Remove(edited_waypoint);
close = true;
}
GUILayout.FlexibleSpace();
if (GUILayout.Button("Cancel", Styles.active_button))
{
close = true;
}
GUILayout.FlexibleSpace();
if (LND != null &&
Utils.ButtonSwitch("Land", edited_waypoint.Land, "Land on arrival"))
{
edited_waypoint.Land = !edited_waypoint.Land;
}
if (Utils.ButtonSwitch("||", edited_waypoint.Pause, "Pause on arrival", GUILayout.Width(25)))
{
edited_waypoint.Pause = !edited_waypoint.Pause;
}
if (GUILayout.Button("Apply", Styles.confirm_button))
{
LatField.UpdateValue(); LonField.UpdateValue(); AltField.UpdateValue();
edited_waypoint.Name = edited_waypoint_name;
edited_waypoint.Pos.Lat = Utils.ClampAngle(LatField.Value);
edited_waypoint.Pos.Lon = Utils.ClampAngle(LonField.Value);
edited_waypoint.Pos.Alt = Math.Max(AltField.Value,
edited_waypoint.Pos.SurfaceAlt(vessel.mainBody));
edited_waypoint.Update(VSL);
close = true;
}
GUILayout.EndHorizontal();
GUILayout.EndVertical();
GUIWindowBase.TooltipsAndDragWindow();
if (close)
{
edited_waypoint = null;
}
}
protected override void Update()
{
if (CFG.Nav.Paused)
{
return;
}
//differentiate between flying in formation and going to the target
var vdistance = 0f; //vertical distance to the target
if (CFG.Nav[Navigation.FollowTarget])
{
update_formation_info();
}
else
{
VSL.Altitude.LowerThreshold = (float)CFG.Target.Pos.Alt;
if (ALT != null && CFG.VF[VFlight.AltitudeControl] && CFG.AltitudeAboveTerrain)
{
vdistance = VSL.Altitude.LowerThreshold + CFG.DesiredAltitude - VSL.Altitude.Absolute;
}
tVSL = null;
tPN = null;
}
//calculate direct distance
var vdir = Vector3.ProjectOnPlane(CFG.Target.GetTransform().position + formation_offset - VSL.Physics.wCoM, VSL.Physics.Up);
var hdistance = Utils.ClampL(vdir.magnitude - VSL.Geometry.R, 0);
var bearing_threshold = Utils.Clamp(1 / VSL.Torque.MaxCurrent.AngularAccelerationAroundAxis(VSL.Engines.CurrentDefThrustDir),
C.BearingCutoffCos, 0.98480775f); //10deg yaw error
//update destination
if (tPN != null && tPN.Valid && !tPN.VSL.Info.Destination.IsZero())
{
VSL.Info.Destination = tPN.VSL.Info.Destination;
}
else
{
VSL.Info.Destination = vdir;
}
//handle flying in formation
var tvel = Vector3.zero;
var vel_is_set = false;
var end_distance = CFG.Target.AbsRadius;
var dvel = VSL.HorizontalSpeed.Vector;
if (CFG.Target.Land)
{
end_distance /= 4;
}
if (tVSL != null && tVSL.loaded)
{
if (formation_offset.IsZero())
{
end_distance *= all_followers.Count / 2f;
}
tvel = Vector3d.Exclude(VSL.Physics.Up, tVSL.srf_velocity);
dvel -= tvel;
var tvel_m = tvel.magnitude;
var dir2vel_cos = Vector3.Dot(vdir.normalized, tvel.normalized);
var lat_dir = Vector3.ProjectOnPlane(vdir - VSL.HorizontalSpeed.Vector * C.LookAheadTime, tvel);
var lat_dist = lat_dir.magnitude;
FormationBreakTimer.RunIf(() => keep_formation = false,
tvel_m < C.FormationSpeedCutoff);
Maneuvering = CanManeuver && lat_dist > CFG.Target.AbsRadius && hdistance < CFG.Target.AbsRadius * 3;
if (keep_formation && tvel_m > 0 &&
(!CanManeuver ||
dir2vel_cos <= bearing_threshold ||
lat_dist < CFG.Target.AbsRadius * 3))
{
if (CanManeuver)
{
HSC.AddWeightedCorrection(lat_dir.normalized * Utils.ClampH(lat_dist / CFG.Target.AbsRadius, 1) *
tvel_m * C.FormationFactor * (Maneuvering? 1 : 0.5f));
}
hdistance = Utils.ClampL(Mathf.Abs(dir2vel_cos) * hdistance - VSL.Geometry.R, 0);
if (dir2vel_cos < 0)
{
if (hdistance < CFG.Target.AbsRadius)
{
HSC.AddRawCorrection(tvel * Utils.Clamp(-hdistance / CFG.Target.AbsRadius * C.FormationFactor, -C.FormationFactor, 0));
}
else if (Vector3.Dot(vdir, dvel) < 0 &&
(dvel.magnitude > C.FollowerMaxAwaySpeed ||
hdistance > CFG.Target.AbsRadius * 5))
{
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(vdir);
return;
}
else
{
HSC.AddRawCorrection(tvel * (C.FormationFactor - 1));
}
hdistance = 0;
}
vdir = tvel;
}
}
//if the distance is greater that the threshold (in radians), use the Great Circle navigation
if (hdistance / VSL.Body.Radius > C.DirectNavThreshold)
{
var next = CFG.Target.PointFrom(VSL.vessel, 0.1);
hdistance = (float)CFG.Target.DistanceTo(VSL.vessel);
vdir = Vector3.ProjectOnPlane(VSL.Body.GetWorldSurfacePosition(next.Lat, next.Lon, VSL.vessel.altitude)
- VSL.vessel.transform.position, VSL.Physics.Up);
tvel = Vector3.zero;
}
else if (!VSL.IsActiveVessel && hdistance > GLB.UnpackDistance)
{
VSL.SetUnpackDistance(hdistance * 1.2f);
}
vdir.Normalize();
//check if we have arrived to the target and stayed long enough
if (hdistance < end_distance)
{
if (CFG.Nav[Navigation.FollowTarget])
{
var prev_needed_speed = VSL.HorizontalSpeed.NeededVector.magnitude;
if (prev_needed_speed < 1 && !CFG.HF[HFlight.Move])
{
CFG.HF.OnIfNot(HFlight.Move);
}
else if (prev_needed_speed > 10 && !CFG.HF[HFlight.NoseOnCourse])
{
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
}
if (tvel.sqrMagnitude > 1)
{
//set needed velocity and starboard to match that of the target
keep_formation = true;
VSL.HorizontalSpeed.SetNeeded(tvel);
HSC.AddRawCorrection((tvel - VSL.HorizontalSpeed.Vector) * 0.9f);
}
else
{
VSL.HorizontalSpeed.SetNeeded(Vector3d.zero);
}
vel_is_set = true;
}
else
{
CFG.HF.OnIfNot(HFlight.Move);
VSL.Altitude.DontCorrectIfSlow();
VSL.HorizontalSpeed.SetNeeded(Vector3d.zero);
vel_is_set = true;
if (vdistance <= 0 && vdistance > -VSL.Geometry.R)
{
if (CFG.Nav[Navigation.FollowPath] && CFG.Path.Count > 0)
{
if (CFG.Path.Peek() == CFG.Target)
{
if (CFG.Path.Count > 1)
{
CFG.Path.Dequeue();
if (on_arrival())
{
return;
}
start_to(CFG.Path.Peek());
return;
}
if (ArrivedTimer.TimePassed)
{
CFG.Path.Clear();
if (on_arrival())
{
return;
}
finish();
return;
}
}
else
{
if (on_arrival())
{
return;
}
start_to(CFG.Path.Peek());
return;
}
}
else if (ArrivedTimer.TimePassed)
{
if (on_arrival())
{
return;
}
finish();
return;
}
}
}
}
else
{
ArrivedTimer.Reset();
CFG.HF.OnIfNot(HFlight.NoseOnCourse);
//if we need to make a sharp turn, stop and turn, then go on
var heading_dir = Vector3.Dot(VSL.OnPlanetParams.Heading, vdir);
var hvel_dir = Vector3d.Dot(VSL.HorizontalSpeed.normalized, vdir);
var sharp_turn_allowed = !CFG.Nav[Navigation.FollowTarget] ||
hdistance < end_distance *Utils.ClampL(all_followers.Count / 2, 2);
if (heading_dir < bearing_threshold &&
hvel_dir < bearing_threshold &&
sharp_turn_allowed)
{
SharpTurnTimer.Start();
}
if (SharpTurnTimer.Started)
{
VSL.HorizontalSpeed.SetNeeded(vdir);
Maneuvering = false;
vel_is_set = true;
if (heading_dir < bearing_threshold || sharp_turn_allowed &&
VSL.HorizontalSpeed.Absolute > 1 && Math.Abs(hvel_dir) < C.BearingCutoffCos)
{
SharpTurnTimer.Restart();
}
else if (SharpTurnTimer.TimePassed)
{
SharpTurnTimer.Reset();
}
}
// Log("timer: {}\nheading*dir {} < {}, vel {} > 1, vel*dir {} < {}",
// SharpTurnTimer,
// Vector3.Dot(VSL.OnPlanetParams.Heading, vdir), bearing_threshold,
// VSL.HorizontalSpeed.Absolute, Vector3d.Dot(VSL.HorizontalSpeed.normalized, vdir), bearing_threshold);//debug
}
var cur_vel = (float)Vector3d.Dot(dvel, vdir);
if (!vel_is_set)
{
//don't slow down on intermediate waypoints too much
var min_dist = C.OnPathMinDistance * VSL.Geometry.R;
if (!CFG.Target.Land && CFG.Nav[Navigation.FollowPath] &&
CFG.Path.Count > 1 && hdistance < min_dist)
{
WayPoint next_wp = null;
if (CFG.Path.Peek() == CFG.Target)
{
var iwp = CFG.Path.GetEnumerator();
try
{
iwp.MoveNext(); iwp.MoveNext();
next_wp = iwp.Current;
}
catch {}
}
else
{
next_wp = CFG.Path.Peek();
}
if (next_wp != null)
{
next_wp.Update(VSL);
var next_dist = Vector3.ProjectOnPlane(next_wp.GetTransform().position - CFG.Target.GetTransform().position, VSL.Physics.Up);
var angle2next = Utils.Angle2(vdir, next_dist);
var minD = Utils.ClampL(min_dist * (1 - angle2next / 180 / VSL.Torque.MaxPitchRoll.AA_rad * C.PitchRollAAf), CFG.Target.AbsRadius);
if (minD > hdistance)
{
hdistance = minD;
}
}
else
{
hdistance = min_dist;
}
}
else if (CFG.Nav.Not(Navigation.FollowTarget))
{
hdistance = Utils.ClampL(hdistance - end_distance + VSL.Geometry.D, 0);
}
//tune maximum speed and PID
if (CFG.MaxNavSpeed < 10)
{
CFG.MaxNavSpeed = 10;
}
DistancePID.Min = HorizontalSpeedControl.C.TranslationMinDeltaV + 0.1f;
DistancePID.Max = CFG.MaxNavSpeed;
if (CFG.Nav[Navigation.FollowTarget])
{
DistancePID.P = C.DistancePID.P / 2;
DistancePID.D = DistancePID.P / 2;
}
else
{
DistancePID.P = C.DistancePID.P;
DistancePID.D = C.DistancePID.D;
}
if (cur_vel > 0)
{
var mg2 = VSL.Physics.mg * VSL.Physics.mg;
var brake_thrust = Mathf.Min(VSL.Physics.mg, VSL.Engines.MaxThrustM / 2 * VSL.OnPlanetParams.TWRf);
var max_thrust = Mathf.Min(Mathf.Sqrt(brake_thrust * brake_thrust + mg2), VSL.Engines.MaxThrustM * 0.99f);
var manual_thrust = VSL.Engines.ManualThrustLimits.Project(VSL.LocalDir(vdir)).magnitude;
if (manual_thrust > brake_thrust)
{
brake_thrust = manual_thrust;
}
else
{
manual_thrust = -1;
}
if (brake_thrust > 0)
{
var brake_accel = brake_thrust / VSL.Physics.M;
var prep_time = 0f;
if (manual_thrust < 0)
{
var brake_angle = Utils.Angle2(VSL.Engines.CurrentDefThrustDir, vdir) - 45;
if (brake_angle > 0)
{
//count rotation of the vessel to braking position
var axis = Vector3.Cross(VSL.Engines.CurrentDefThrustDir, vdir);
if (VSL.Torque.Slow)
{
prep_time = VSL.Torque.NoEngines.RotationTime3Phase(brake_angle, axis, C.RotationAccelPhase);
//also count time needed for the engines to get to full thrust
prep_time += Utils.LerpTime(VSL.Engines.Thrust.magnitude, VSL.Engines.MaxThrustM, max_thrust, VSL.Engines.AccelerationSpeed);
}
else
{
prep_time = VSL.Torque.MaxCurrent.RotationTime2Phase(brake_angle, axis, VSL.OnPlanetParams.GeeVSF);
}
}
}
var prep_dist = cur_vel * prep_time + CFG.Target.AbsRadius;
var eta = hdistance / cur_vel;
max_speed.TauUp = C.MaxSpeedFilterUp / eta / brake_accel;
max_speed.TauDown = eta * brake_accel / C.MaxSpeedFilterDown;
max_speed.Update(prep_dist < hdistance?
(1 + Mathf.Sqrt(1 + 2 / brake_accel * (hdistance - prep_dist))) * brake_accel :
2 * brake_accel);
CorrectionPID.Min = -VSL.HorizontalSpeed.Absolute;
if (max_speed < cur_vel)
{
CorrectionPID.Update(max_speed - cur_vel);
}
else
{
CorrectionPID.IntegralError *= (1 - TimeWarp.fixedDeltaTime * C.CorrectionEasingRate);
CorrectionPID.Update(0);
}
HSC.AddRawCorrection(CorrectionPID.Action * VSL.HorizontalSpeed.Vector.normalized);
}
if (max_speed < CFG.MaxNavSpeed)
{
DistancePID.Max = Mathf.Max(DistancePID.Min, max_speed);
}
}
//take into account vertical distance and obstacle
var rel_ahead = VSL.Altitude.Ahead - VSL.Altitude.Absolute;
// Log("vdist {}, rel.ahead {}, vF {}, aF {}", vdistance, rel_ahead,
// Utils.ClampL(1 - Mathf.Atan(vdistance/hdistance)/Utils.HalfPI, 0),
// Utils.ClampL(1 - rel_ahead/RAD.DistanceAhead, 0));//debug
vdistance = Mathf.Max(vdistance, rel_ahead);
if (vdistance > 0)
{
hdistance *= Utils.ClampL(1 - Mathf.Atan(vdistance / hdistance) / (float)Utils.HalfPI, 0);
}
if (RAD != null && rel_ahead > 0 && RAD.DistanceAhead > 0)
{
hdistance *= Utils.ClampL(1 - rel_ahead / RAD.DistanceAhead, 0);
}
//update the needed velocity
DistancePID.Update(hdistance);
var nV = vdir * DistancePID.Action;
//correcto for Follow Target program
if (CFG.Nav[Navigation.FollowTarget] && Vector3d.Dot(tvel, vdir) > 0)
{
nV += tvel;
}
VSL.HorizontalSpeed.SetNeeded(nV);
}
//correct for lateral movement
var latV = -Vector3d.Exclude(vdir, VSL.HorizontalSpeed.Vector);
var latF = (float)Math.Min((latV.magnitude / Math.Max(VSL.HorizontalSpeed.Absolute, 0.1)), 1);
LateralPID.P = C.LateralPID.P * latF;
LateralPID.I = Math.Min(C.LateralPID.I, latF);
LateralPID.D = C.LateralPID.D * latF;
LateralPID.Update(latV);
HSC.AddWeightedCorrection(LateralPID.Action);
// Log("\ndir v {}\nlat v {}\nact v {}\nlatPID {}",
// VSL.HorizontalSpeed.NeededVector, latV,
// LateralPID.Action, LateralPID);//debug
}
protected TargetedTrajectory(VesselWrapper vsl, Vector3d dV, double startUT, WayPoint target)
: base(vsl, dV, startUT)
{
Target = target;
}
protected void SetTarget(WayPoint wp = null)
{
VSL.SetTarget(this, wp);
}
public SurfaceNode(WayPoint wp, CelestialBody body)
{
Pos = wp.Pos;
Body = body;
}
public void StartFrom(WayPoint wp)
{
StartNode = new SurfaceNode(wp, VSL.Body);
}
