protected BaseTrajectory(VesselWrapper vsl, Vector3d dV, double startUT)
{
VSL = vsl;
ManeuverDeltaV = dV;
ManeuverDuration = VSL.Engines.TTB((float)ManeuverDeltaV.magnitude);
StartUT = startUT;
TimeToStart = startUT - VSL.Physics.UT;
Body = VSL.vessel.orbitDriver.orbit.referenceBody;
OrigOrbit = VSL.vessel.orbitDriver.orbit;
Orbit = TrajectoryCalculator.NewOrbit(OrigOrbit, ManeuverDeltaV, StartUT);
StartPos = Orbit.getRelativePositionAtUT(StartUT);
StartVel = Orbit.getOrbitalVelocityAtUT(StartUT);
}
void update(bool with_brake)
{
update_from_orbit(Orbit, StartUT);
//correct for brake maneuver
if (with_brake)
{
BrakeEndUT = AtTargetUT - GLB.LTRJ.CorrectionOffset;
BrakeStartUT = BrakeEndUT - MatchVelocityAutopilot.BrakingOffset((float)AtTargetVel.magnitude, VSL, out BrakeDuration);
brake_delta_v = -0.9 * Orbit.getOrbitalVelocityAtUT(BrakeEndUT);
update_from_orbit(TrajectoryCalculator.NewOrbit(Orbit, BrakeDeltaV, BrakeEndUT), BrakeEndUT);
}
else
{
brake_delta_v = -(AtTargetVel + Vector3d.Cross(Body.zUpAngularVelocity, AtTargetPos));
BrakeEndUT = TrajectoryCalculator.FlyAboveUT(Orbit, Target.RelSurfPos(Body).xzy, StartUT);
BrakeStartUT = BrakeEndUT - MatchVelocityAutopilot.BrakingOffset((float)BrakeDeltaV.magnitude, VSL, out BrakeDuration);
}
//compute vessel coordinates at maneuver start
if (VSL.LandedOrSplashed)
{
VslStartLat = Utils.ClampAngle(VSL.vessel.latitude);
VslStartLon = Utils.ClampAngle(VSL.vessel.longitude);
}
else
{
var start_pos = (TrajectoryCalculator.BodyRotationAtdT(Body, -TimeToStart) * StartPos).xzy + Body.position;
VslStartLat = Utils.ClampAngle(Body.GetLatitude(start_pos));
VslStartLon = Utils.ClampAngle(Body.GetLongitude(start_pos));
}
//compute distance to target
DistanceToTarget = Target.AngleTo(SurfacePoint) * Body.Radius;
BrakeEndDeltaAlt = Orbit.getRelativePositionAtUT(BrakeEndUT).magnitude - Body.Radius - TargetAltitude;
//compute distance in lat-lon coordinates
DeltaLat = Utils.AngleDelta(SurfacePoint.Pos.Lat, Target.Pos.Lat) *
Math.Sign(Utils.AngleDelta(Utils.ClampAngle(VslStartLat), SurfacePoint.Pos.Lat));
DeltaLon = Utils.AngleDelta(SurfacePoint.Pos.Lon, Target.Pos.Lon) *
Math.Sign(Utils.AngleDelta(Utils.ClampAngle(VslStartLon), SurfacePoint.Pos.Lon));
//compute distance in radial coordinates
DeltaFi = 90 - Vector3d.Angle(Orbit.GetOrbitNormal(),
TrajectoryCalculator.BodyRotationAtdT(Body, TimeToSurface) *
Body.GetRelSurfacePosition(Target.Pos.Lat, Target.Pos.Lon, TargetAltitude).xzy);
DeltaR = Utils.RadDelta(SurfacePoint.AngleTo(VslStartLat, VslStartLon), Target.AngleTo(VslStartLat, VslStartLon)) * Mathf.Rad2Deg;
}
private bool update_maneuver_node()
{
Node = null;
NodeCB = null;
TargetOrbit = null;
if (Solver != null)
{
if (Solver.maneuverNodes.Count <= 0)
{
return(false);
}
Node = Solver.maneuverNodes[0];
}
else
{
if (VSL.vessel.flightPlanNode.nodes.Count <= 0)
{
return(false);
}
var node = VSL.vessel.flightPlanNode.nodes[0];
Node = new ManeuverNode();
Node.Load(node);
Node.patch = new Orbit(VSL.orbit);
Node.nextPatch =
TrajectoryCalculator.NewOrbit(VSL.orbit, Utils.Node2OrbitalDeltaV(Node), Node.UT);
VSL.vessel.flightPlanNode.RemoveNode(node);
}
NodeCB = Node.patch.referenceBody;
TargetOrbit = Node.nextPatch;
update_node_deltaV();
if (VSL.Engines.MaxDeltaV < Node.DeltaV.magnitude)
{
Status(Colors.Warning,
"WARNING: there may be not enough propellant for the maneuver");
}
return(true);
}
protected BaseTrajectory(VesselWrapper vsl, Vector3d dV, double startUT)
{
VSL = vsl;
var dVm = (float)dV.magnitude;
ManeuverDeltaV = dV;
if (dVm > 0)
{
ManeuverFuel = VSL.Engines.FuelNeeded(dVm);
NotEnoughFuel = ManeuverFuel > VSL.Engines.GetAvailableFuelMass();
ManeuverDuration = VSL.Engines.TTB(dVm);
}
else
{
ManeuverFuel = 0;
ManeuverDuration = 0;
}
StartUT = startUT;
Body = VSL.vessel.orbitDriver.orbit.referenceBody;
OrigOrbit = VSL.vessel.orbitDriver.orbit;
Orbit = TrajectoryCalculator.NewOrbit(OrigOrbit, ManeuverDeltaV, StartUT);
StartPos = Orbit.getRelativePositionAtUT(StartUT);
StartVel = Orbit.getOrbitalVelocityAtUT(StartUT);
}
void update(bool with_brake)
{
update_from_orbit(Orbit, StartUT);
LandingAngle = 90 - Vector3d.Angle(AtTargetPos, -AtTargetVel);
//correct for brake maneuver
if (with_brake)
{
//estimate time needed to rotate the ship downwards
var rotation_time = VSL.Torque.NoEngines?
VSL.Torque.NoEngines.MinRotationTime(90) :
VSL.Torque.MaxPossible.RotationTime(90, 0.1f);
//estimate amount fuel needed for the maneuver
var vertical_vel = Vector3d.Project(AtTargetVel, AtTargetPos);
SetBrakeEndUT(Math.Max(AtTargetUT - GLB.LTRJ.CorrectionOffset + rotation_time, StartUT));
SetBrakeDeltaV(vertical_vel);
if (BrakeFuel > 0)
{
//calculate braking maneuver
var dV = (float)BrakeDeltaV.magnitude;
BrakeDuration = VSL.Engines.AntigravTTB(dV);
BrakeDuration += rotation_time;
//find the appropriate point to perform the maneuver
var brake_end_UT = Math.Max(AtTargetUT - Mathf.Max(GLB.LTRJ.CorrectionOffset, BrakeDuration * 1.1f), StartUT);
var vertical_speed = vertical_vel.magnitude;
double fly_over_error;
do
{
SetBrakeEndUT(brake_end_UT);
fly_over_error = BrakeEndDeltaAlt - GLB.LTRJ.FlyOverAlt;
brake_end_UT -= Math.Abs(fly_over_error / vertical_speed);
} while(brake_end_UT > StartUT && fly_over_error < -1);
SetBrakeDeltaV(-0.9 * Orbit.getOrbitalVelocityAtUT(BrakeEndUT));
//calculate maneuver start time and update landing site
BrakeStartUT = Math.Max(BrakeEndUT - MatchVelocityAutopilot.BrakingOffset((float)BrakeDeltaV.magnitude, VSL, out BrakeDuration), StartUT);
update_from_orbit(TrajectoryCalculator.NewOrbit(Orbit, BrakeDeltaV, BrakeEndUT), BrakeEndUT);
}
else
{
BrakeStartUT = BrakeEndUT;
BrakeDuration = 0;
}
}
else
{
SetBrakeEndUT(TrajectoryCalculator.FlyAboveUT(Orbit, Target.RelOrbPos(Body), StartUT));
SetBrakeDeltaV(-(AtTargetVel + Vector3d.Cross(Body.zUpAngularVelocity, AtTargetPos)));
if (BrakeFuel > 0)
{
var offset = MatchVelocityAutopilot.BrakingOffset((float)BrakeDeltaV.magnitude, VSL, out BrakeDuration);
BrakeStartUT = Math.Max(BrakeEndUT - offset, StartUT);
}
else
{
BrakeStartUT = BrakeEndUT;
BrakeDuration = 0;
}
}
BrakeOffset = (float)Utils.ClampL(BrakeEndUT - BrakeStartUT, 0);
//compute vessel coordinates at maneuver start
if (VSL.LandedOrSplashed)
{
VslStartLat = Utils.ClampAngle(VSL.vessel.latitude);
VslStartLon = Utils.ClampAngle(VSL.vessel.longitude);
}
else
{
var start_pos = (TrajectoryCalculator.BodyRotationAtdT(Body, -TimeToStart) * StartPos).xzy + Body.position;
VslStartLat = Utils.ClampAngle(Body.GetLatitude(start_pos));
VslStartLon = Utils.ClampAngle(Body.GetLongitude(start_pos));
}
//compute distance to target
DistanceToTarget = Target.AngleTo(SurfacePoint) * Body.Radius;
SurfacePoint.Name += string.Format("\n{0} from target", Utils.formatBigValue((float)DistanceToTarget, "m"));
//compute distance in lat-lon coordinates
DeltaLat = Utils.AngleDelta(SurfacePoint.Pos.Lat, Target.Pos.Lat) *
Math.Sign(Utils.AngleDelta(approach.Lat, SurfacePoint.Pos.Lat));
DeltaLon = Utils.AngleDelta(SurfacePoint.Pos.Lon, Target.Pos.Lon) *
Math.Sign(Utils.AngleDelta(approach.Lon, SurfacePoint.Pos.Lon));
//compute distance in radial coordinates
DeltaFi = 90 - Vector3d.Angle(Orbit.GetOrbitNormal(),
TrajectoryCalculator.BodyRotationAtdT(Body, TimeToTarget) *
Body.GetRelSurfacePosition(Target.Pos.Lat, Target.Pos.Lon, TargetAltitude).xzy);
DeltaR = Utils.RadDelta(SurfacePoint.AngleTo(VslStartLat, VslStartLon), Target.AngleTo(VslStartLat, VslStartLon)) * Mathf.Rad2Deg;
// Utils.Log("{}", this);//debug
}
protected BaseTrajectory(VesselWrapper vsl, Vector3d dV, double startUT)
{
VSL = vsl;
var dVm = (float)dV.magnitude;
if (dVm > 0)
{
ManeuverFuel = VSL.Engines.FuelNeeded(dVm);
FullManeuver = ManeuverFuel < VSL.Engines.AvailableFuelMass;
ManeuverDuration = VSL.Engines.TTB_Precise(dVm);
}
else
{
ManeuverFuel = 0;
ManeuverDuration = 0;
FullManeuver = true;
}
StartUT = startUT;
OrigOrbit = VSL.vessel.orbitDriver.orbit;
var obt = StartOrbit;
Body = obt.referenceBody;
if (dVm > 0)
{
ManeuverDeltaV = dV;
NodeDeltaV = Utils.Orbital2NodeDeltaV(obt, ManeuverDeltaV, StartUT);
try
{
Orbit = TrajectoryCalculator.NewOrbit(obt, ManeuverDeltaV, StartUT);
var prev = Orbit;
for (int i = 0; i < 3; i++)
{
if (!PatchedConics.CalculatePatch(prev, prev.nextPatch ?? new Orbit(), prev.epoch, new PatchedConics.SolverParameters(), null))
{
break;
}
prev = prev.nextPatch;
}
Body = Orbit.referenceBody;
// if(Orbit.patchEndTransition != Orbit.PatchTransitionType.FINAL)//debug
// {
// Utils.Log("**************************************************************************************************");//debug
// RendezvousAutopilot.log_patches(Orbit, "Orbit");//deubg
// }
}
catch (ArithmeticException)
{
Orbit = OrigOrbit;
StartUT = VSL.Physics.UT;
ManeuverFuel = 0;
ManeuverDuration = 0;
ManeuverDeltaV = Vector3d.zero;
FullManeuver = true;
}
}
else
{
Orbit = OrigOrbit;
}
StartPos = obt.getRelativePositionAtUT(StartUT);
StartVel = obt.getOrbitalVelocityAtUT(StartUT);
AfterStartVel = Orbit.getOrbitalVelocityAtUT(StartUT);
}
public IEnumerable <double> FromSurfaceTTA(float ApA_offset, double ApA, double alpha, float maxG, float angularV)
{
//Log("FromSurfaceTTA: ApA_offset {}, ApA {}, alpha {}, maxG {}, angularV {}",
//ApA_offset, ApA, alpha*Mathf.Rad2Deg, maxG, angularV*Mathf.Rad2Deg);//debug
var t = 0.0;
var BR = Body.Radius;
var ApR = BR + ApA;
var v = new Vector2d(1e-3, 1e-3);
var r0n = new Vector2d(0, 1);
var r = new Vector2d(0, VSL.orbit.radius);
var r1n = new Vector2d(Math.Sin(alpha), Math.Cos(alpha));
var r1 = r1n * ApR;
var T = new Vector2d(0, 1);
var m = (double)VSL.Physics.M;
var m0 = m;
var mT = eStats.MaxThrust;
var mTm = mT.magnitude;
var mflow = eStats.MaxMassFlow;
var AA = eStats.TorqueInfo.AA_rad;
var s = VSL.Geometry.AreaInDirection(mT);
var thrust = true;
var R = r.magnitude;
var prev_r = r;
var maxR = ApR * 2;
double turn_start = VSL.Altitude.Absolute;
bool turn_started = false;
Orbit obt = null;
while (R > BR && R < maxR &&
Utils.Angle2(r0n, r1n) > Utils.Angle2(r0n, r))
{
yield return(-1);
prev_r = r;
R = r.magnitude;
var h = R - BR;
double time2ApA;
var ApV = getApV(m, s, r, v, C.DeltaTime * 4, out time2ApA);
thrust &= Vector2d.Dot(ApV - r1, r1 - r) < 0;
var srf_dir = -r.Rotate90().normalized;
var thr = thrust ? max_G_throttle((float)Vector2d.Dot(T, r.normalized), m, maxG) : 0;
var nV = v;
if (thrust &&
Vector2d.Dot(r.normalized, v) / VSL.Physics.StG > ToOrbitExecutor.C.MinClimbTime)
{
var rr1 = r1 - r;
solver.Init(Body, r, v, r1);
var minT = solver.ParabolicTime;
var maxT = solver.MinEnergyTime;
nV = solver.dV4TransferME();
while (maxT - minT > 0.1)
{
var curT = (maxT + minT) / 2;
nV = solver.dV4Transfer(curT);
obt = TrajectoryCalculator.NewOrbit(Body, r, v + nV, VSL.Physics.UT, obt);
if (obt.timeToAp > curT)
{
minT = curT;
}
else
{
maxT = curT;
}
}
var neededAoA = Utils.ClampH(Utils.Angle2(rr1, r) / 2, 45);
var angle2Hor = 90 - Utils.Angle2(nV, r);
var AoA = Utils.Angle2(r, v);
pitch.Max = AoA < neededAoA ? 0 : (float)AoA;
pitch.Update((float)angle2Hor);
correction_started.Update(angle2Hor >= 0);
if (AoA < neededAoA && !correction_started)
{
pitch.Action = (float)Utils.Clamp(AoA - neededAoA + angle2Hor,
-AttitudeControlBase.C.MaxAttitudeError,
AoA);
}
if (!turn_started && h < Body.atmosphereDepth)
{
var atm = Body.AtmoParamsAtAltitude(h);
if (atm.Rho > ToOrbitExecutor.C.AtmDensityOffset)
{
turn_start = h;
}
else
{
turn_started = true;
}
}
var startF = Utils.Clamp((h - turn_start) / ApA / ToOrbitExecutor.C.GTurnOffset, 0, 1);
var nT = v.Rotate(pitch.Action * startF);
var atErr = Utils.Angle2Rad(r, T) - Utils.Angle2Rad(r, nT);
T = T.RotateRad(atErr /
Math.Max(C.DeltaTime,
eStats.TorqueInfo.RotationTime3Phase((float)Math.Abs(atErr * Mathf.Rad2Deg),
(float)(AA * m0 / m),
C.RotAccelPhase,
1)) *
C.DeltaTime)
.normalized;
if (Vector2d.Dot(T, r) < 0)
{
T = srf_dir;
}
throttle_correction.Update((float)angle2Hor);
throttle.Update(ApA_offset + throttle_correction.Action - (float)time2ApA);
thr = Utils.ClampH(0.5f + throttle, thr);
}
if (thrust && thr > 0)
{
if (!CheatOptions.InfinitePropellant)
{
var dm = mflow * thr * C.DeltaTime;
if (m < dm)
{
thrust = false;
}
else
{
m -= dm;
}
}
if (thrust)
{
v += T * mTm / m * thr * C.DeltaTime;
}
}
v -= r * Body.gMagnitudeAtCenter / R / R / R * C.DeltaTime;
if (h < Body.atmosphereDepth)
{
var vm = v.magnitude;
if (vm > 0)
{
v -= v / vm * drag(s, h, vm) / m * C.DeltaTime;
}
}
r += v * C.DeltaTime;
r1n = r1n.RotateRad(angularV * C.DeltaTime).normalized;
r1 = r1n * ApR;
t += C.DeltaTime;
//DebugUtils.CSV("ToOrbitSim.csv", t, r);//debug
//DebugUtils.CSV("ToOrbitSim.csv", t, r, v, rel_v, T*mTm/m*thr, h, m, thr, r1, nV);//debug
}
//Log("TimeToApA: {}", t);//debug
yield return(t);
}
public Orbit OrbitFromHere()
{
return(TrajectoryCalculator.NewOrbit(Body, pos, vel, UT));
}
