private void autoPilot(AutoPilot pilot, FlightCtrlState controls)
{
controls.killRot = false;
if (!controls.isIdle)
{
Enabled = false;
}
if (attachedVessel == null || !Enabled)
{
return;
}
Vessel vessel = attachedVessel;
CelestialBody body = vessel.mainBody;
Vector3d pos = vessel.GetWorldPos3D() - body.position;
Vector3d airVelocity = vessel.obt_velocity - body.getRFrmVel(body.position + pos);
Transform vesselTransform = vessel.ReferenceTransform;
Vector3d vesselBackward = (Vector3d)(-vesselTransform.up.normalized);
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = (Vector3d)(-vesselTransform.forward.normalized);
Vector3d vesselRight = Vector3d.Cross(vesselUp, vesselBackward).normalized;
Vector3d localVel = new Vector3d(Vector3d.Dot(vesselRight, airVelocity), Vector3d.Dot(vesselUp, airVelocity), Vector3d.Dot(vesselBackward, airVelocity));
Vector3d prograde = localVel.normalized;
Vector3d localGravityUp = new Vector3d(Vector3d.Dot(vesselRight, pos), Vector3d.Dot(vesselUp, pos), Vector3d.Dot(vesselBackward, pos)).normalized;
Vector3d localProgradeRight = Vector3d.Cross(prograde, localGravityUp).normalized;
localGravityUp = Vector3d.Cross(localProgradeRight, prograde);
Vector3d vel = vessel.obt_velocity - body.getRFrmVel(body.position + pos); // air velocity
double AoA = (float)DescentProfile.fetch.GetAngleOfAttack(body, pos, vel);
Vector3d worldTargetDirection = CorrectedDirection;
Vector3d targetDirection = new Vector3d(Vector3d.Dot(vesselRight, worldTargetDirection), Vector3d.Dot(vesselUp, worldTargetDirection), Vector3d.Dot(vesselBackward, worldTargetDirection));
Vector3d targetUp = prograde * (-Math.Sin(AoA)) + localGravityUp * Math.Cos(AoA);
Vector2 correction = Correction;
float dirx = targetDirection.z > 0.0f ? Mathf.Sign((float)targetDirection.x) : (float)targetDirection.x;
float diry = targetDirection.z > 0.0f ? Mathf.Sign((float)targetDirection.y) : (float)targetDirection.y;
float dirz = targetUp.y < 0.0f ? Mathf.Sign((float)targetUp.x) : (float)targetUp.x;
if (targetUp.y > 0.0f)
{
dirz += correction.x; // in case the craft has wings, roll it to use lift for left/right correction
}
float maxSteer = Mathf.Clamp(2.0f - Smoothness * 0.2f, 0.1f, 1.0f);
float warpDamp = 1.0f / TimeWarp.CurrentRate;
controls.pitch = Mathf.Clamp(diry * Strength + vessel.angularVelocity.x * Smoothness, -maxSteer, maxSteer) * warpDamp;
controls.yaw = Mathf.Clamp(-dirx * Strength + vessel.angularVelocity.z * Smoothness, -maxSteer, maxSteer) * warpDamp;
controls.roll = Mathf.Clamp(-dirz * Strength + vessel.angularVelocity.y * Smoothness, -maxSteer, maxSteer) * warpDamp;
}
//formula for drag seems to be drag force = (1/2) * DragMultiplier * (air density) * (mass * max_drag) * (airspeed)^2
//so drag acceleration is (1/2) * DragMultiplier * (air density) * (average max_drag) * (airspeed)^2
//where the max_drag average over parts is weighted by the part mass
public static Vector3d computeDragAccel(Vector3d pos, Vector3d orbitVel, double dragCoeffOverMass, CelestialBody body)
{
double airDensity = FlightGlobals.getAtmDensity(FlightGlobals.getStaticPressure(pos, body));
Vector3d airVel = orbitVel - body.getRFrmVel(pos);
return(-0.5 * FlightGlobals.DragMultiplier * airDensity * dragCoeffOverMass * airVel.magnitude * airVel);
}
public static Vector3 GetCorrectedDirection()
{
var vessel = FlightGlobals.ActiveVessel;
if (vessel == null)
{
return(new Vector3(0, 0, 0));
}
CelestialBody body = vessel.mainBody;
Vector3d pos = vessel.GetWorldPos3D() - body.position;
Vector3d vel = vessel.obt_velocity - body.getRFrmVel(body.position + pos); // air velocity
Vector3 referenceVector = GetPlannedDirection();
Vector3 up = pos.normalized;
Vector3 velRight = Vector3.Cross(vel, up).normalized;
Vector3 refUp = Vector3.Cross(velRight, referenceVector).normalized;
Vector3 refRight = velRight;
Vector2 offsetDir = GetCorrection();
return((referenceVector + refUp * offsetDir.y + refRight * offsetDir.x).normalized);
}
public override OrbitableVelocity GetVelocitiesAtUT(TimeSpan timeStamp)
{
CelestialBody parent = Body.KOSExtensionGetParentBody();
if (parent == null) // only if Body is Sun and therefore has no parent, then do more complex work instead because KSP didn't provide a way itself
{
Vector3d futureOrbitalVel;
CelestialBody soiBody = Shared.Vessel.mainBody;
if (soiBody.orbit != null)
{
futureOrbitalVel = soiBody.orbit.GetFrameVelAtUT(timeStamp.ToUnixStyleTime());
}
else
{
futureOrbitalVel = -1 * VesselTarget.CreateOrGetExisting(Shared.Vessel, Shared).GetVelocitiesAtUT(timeStamp).Orbital.ToVector3D();
}
Vector swappedVel = new Vector(futureOrbitalVel.x, futureOrbitalVel.z, futureOrbitalVel.y); // swap Y and Z because KSP API is weird.
// Also invert directions because the above gives vel of my body rel to sun, and I want vel of sun rel to my body:
return(new OrbitableVelocity(-swappedVel, -swappedVel));
}
var orbVel = new Vector(Orbit.getOrbitalVelocityAtUT(timeStamp.ToUnixStyleTime()));
orbVel = new Vector(orbVel.X, orbVel.Z, orbVel.Y); // swap Y and Z because KSP API is weird.
var surfVel = new Vector(Body.orbit.GetVel() - parent.getRFrmVel(Body.position));
return(new OrbitableVelocity(orbVel, surfVel));
}
internal static void Update()
{
patch = Trajectory.Patches.LastOrDefault();
if ((!Util.IsFlight && !Util.IsTrackingStation) || !Trajectories.IsVesselAttached || !TargetProfile.WorldPosition.HasValue ||
patch == null || !patch.ImpactPosition.HasValue || patch.StartingState.ReferenceBody != TargetProfile.Body || !Ready)
{
SetDisplayEnabled(false);
return;
}
body = Trajectories.AttachedVessel.mainBody;
position = Trajectories.AttachedVessel.GetWorldPos3D() - body.position;
velocity = Trajectories.AttachedVessel.obt_velocity - body.getRFrmVel(body.position + position); // air velocity
up = position.normalized;
vel_right = Vector3d.Cross(velocity, up).normalized;
reference = CalcReference();
SetDisplayEnabled(true);
guide_transform.gameObject.transform.localPosition = navball.attitudeGymbal * (CorrectedDirection.Value * navball.VectorUnitScale);
reference_transform.gameObject.transform.localPosition = navball.attitudeGymbal * (reference * navball.VectorUnitScale);
// hide if behind navball
guide_transform.gameObject.SetActive(guide_transform.gameObject.transform.localPosition.z >= navball.VectorUnitCutoff);
reference_transform.gameObject.SetActive(reference_transform.gameObject.transform.localPosition.z >= navball.VectorUnitCutoff);
}
override public OrbitableVelocity GetVelocitiesAtUT(TimeSpan timeStamp)
{
CelestialBody parent = Body.KOSExtensionGetParentBody();
if (parent == null) // only if Body is Sun and therefore has no parent, then do more complex work instead because KSP didn't provide a way itself
{
Vector3d futureOrbitalVel;
CelestialBody soiBody = Shared.Vessel.mainBody;
if (soiBody.orbit != null)
{
futureOrbitalVel = soiBody.orbit.GetFrameVelAtUT(timeStamp.ToUnixStyleTime());
}
else
{
futureOrbitalVel = (-1) * (new VesselTarget(Shared.Vessel, Shared).GetVelocitiesAtUT(timeStamp).Orbital.ToVector3D());
}
return(new OrbitableVelocity(new Vector(futureOrbitalVel), new Vector(0.0, 0.0, 0.0)));
}
var orbVel = new Vector(Orbit.getOrbitalVelocityAtUT(timeStamp.ToUnixStyleTime()));
orbVel = new Vector(orbVel.X, orbVel.Z, orbVel.Y); // swap Y and Z because KSP API is weird.
var surfVel = new Vector(Body.orbit.GetVel() - parent.getRFrmVel(Body.position));
return(new OrbitableVelocity(orbVel, surfVel));
}
public OrbitableVelocity(CelestialBody b)
{
Orbital = new Vector(b.orbit.GetVel()); // KSP's b.GetObtVelocity() is broken - it causes stack overflow
CelestialBody parent = b.referenceBody;
Surface = parent != null ?
new Vector(b.orbit.GetVel() - parent.getRFrmVel(b.position)) :
new Vector(default(float), default(float), default(float));
}
public void FixedUpdate()
{
if (aerodynamicModel_ != null && vessel_ != null)
{
CelestialBody body = vessel_.orbit.referenceBody;
Vector3d bodySpacePosition = vessel_.GetWorldPos3D() - body.position;
Vector3d bodySpaceVelocity = vessel_.obt_velocity;
double altitudeAboveSea = bodySpacePosition.magnitude - body.Radius;
Vector3d airVelocity = bodySpaceVelocity - body.getRFrmVel(body.position + bodySpacePosition);
#if DEBUG_COMPARE_FORCES
Vector3d FARForce = FARBasicDragModel.debugForceAccumulator + FARWingAerodynamicModel.debugForceAccumulator;
FARBasicDragModel.debugForceAccumulator = new Vector3d(0, 0, 0);
FARWingAerodynamicModel.debugForceAccumulator = new Vector3d(0, 0, 0);
double rho = FARAeroUtil.GetCurrentDensity(body, altitudeAboveSea);
//double rho = vessel_.atmDensity;
//double pressure = FlightGlobals.getStaticPressure(altitudeAboveSea, body);
//double rho = FlightGlobals.getAtmDensity(pressure);
double machNumber = FARAeroUtil.GetMachNumber(body, altitudeAboveSea, airVelocity);
//double machNumber = airVelocity.magnitude / 300.0;
Transform vesselTransform = vessel_.ReferenceTransform;
Vector3d vesselBackward = (Vector3d)(-vesselTransform.up.normalized);
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = (Vector3d)(-vesselTransform.forward.normalized);
Vector3d vesselRight = Vector3d.Cross(vesselUp, vesselBackward).normalized;
double AoA = Math.Acos(Vector3d.Dot(airVelocity.normalized, vesselForward.normalized));
if (Vector3d.Dot(airVelocity, vesselUp) > 0)
{
AoA = -AoA;
}
Vector3d predictedForce = aerodynamicModel_.computeForces_FAR(rho, machNumber, airVelocity, vesselUp, AoA, 0.05);
aerodynamicModel_.Verbose = true;
Vector3d predictedForceWithCache = aerodynamicModel_.computeForces(body, bodySpacePosition, airVelocity, AoA, 0.05);
aerodynamicModel_.Verbose = false;
Vector3d localFARForce = new Vector3d(Vector3d.Dot(FARForce, vesselRight), Vector3d.Dot(FARForce, vesselUp), Vector3d.Dot(FARForce, vesselBackward));
Vector3d localPredictedForce = new Vector3d(Vector3d.Dot(predictedForce, vesselRight), Vector3d.Dot(predictedForce, vesselUp), Vector3d.Dot(predictedForce, vesselBackward));
Vector3d localPredictedForceWithCache = new Vector3d(Vector3d.Dot(predictedForceWithCache, vesselRight), Vector3d.Dot(predictedForceWithCache, vesselUp), Vector3d.Dot(predictedForceWithCache, vesselBackward));
Util.PostSingleScreenMessage("FAR/predict comparison", "air vel=" + Math.Floor(airVelocity.magnitude) + ", AoA=" + (AoA * 180.0 / Math.PI) + ", FAR force=" + localFARForce + ", predicted force=" + localPredictedForce);
Util.PostSingleScreenMessage("predict with cache", "predicted force with cache=" + localPredictedForceWithCache);
#endif
double approximateRho = StockAeroUtil.GetDensity(altitudeAboveSea, body);
double preciseRho = StockAeroUtil.GetDensity(vessel_.GetWorldPos3D(), body);
double actualRho = vessel_.atmDensity;
Util.PostSingleScreenMessage("rho info", "preciseRho=" + preciseRho.ToString("0.0000") + " ; approximateRho=" + approximateRho.ToString("0.0000") + " ; actualRho=" + actualRho.ToString("0.0000"));
}
}
public OrbitableVelocity(CelestialBody b, SharedObjects shared)
{
Orbital = new Vector(b.KOSExtensionGetObtVelocity(shared)); // KSP's b.GetObtVelocity() is broken - it causes stack overflow
CelestialBody parent = b.KOSExtensionGetParentBody();
Surface = (parent != null) ?
new Vector(b.orbit.GetVel() - parent.getRFrmVel(b.position)) :
new Vector(Orbital); // return same velocity as orbit when no parent body to compare against.
InitializeSuffixes();
}
public OrbitableVelocity(CelestialBody b, SharedObjects shared)
{
Orbital = new Vector(b.KOSExtensionGetObtVelocity(shared)); // KSP's b.GetObtVelocity() is broken - it causes stack overflow
CelestialBody parent = b.KOSExtensionGetParentBody();
Surface = (parent != null) ?
new Vector(b.orbit.GetVel() - parent.getRFrmVel(b.position)) :
new Vector(Vector3d.zero);
InitializeSuffixes();
}
static private void EulerStep(
double dt,
Vessel vessel, Vector3d r, // in world space but relative to body.position
Vector3d v, Vector3d att, double totalMass, double minThrust, double maxThrust,
Trajectories.VesselAerodynamicModel aeroModel, CelestialBody body, double t,
BLController controller, Vector3d tgt_r, double aeroFudgeFactor,
out Vector3d steer,
out Vector3d vel_air, out double throttle,
out Vector3d out_r, out Vector3d out_v)
{
double y = r.magnitude - body.Radius;
steer = -Vector3d.Normalize(v);
throttle = 0;
// gravity
double R = r.magnitude;
Vector3d g = r * (-body.gravParameter / (R * R * R));
// Get steer and throttle
bool bailOutLandingBurn = true;
if (controller != null)
{
bool landingGear;
controller.GetControlOutputs(vessel, totalMass, r, v, att, minThrust, maxThrust, t, body, true, out throttle, out steer, out landingGear, bailOutLandingBurn);
// Stop throttle so we don't take off again in timestep, dt
// TODO - Fix HACK!!
if (y < controller.TgtAlt + 50)
{
throttle = 0;
}
}
Vector3d Ft = Vector3d.zero;
if (throttle > 0)
{
Ft = steer * (minThrust + throttle * (maxThrust - minThrust));
}
// TODO: Do repeated calls to GetForces() mess up PID controllers which updates their internal estimates?
vel_air = v - body.getRFrmVel(r + body.position);
if (aeroModel == null)
{
Debug.Log("EulerStep() - No aeroModel");
}
Vector3d F = aeroModel.GetForces(body, r, vel_air, Math.PI) * aeroFudgeFactor + Ft;
Vector3d a = F / totalMass + g;
out_r = r + v * dt + 0.5 * a * dt * dt;
out_v = v + a * dt;
}
// determine average atmospheric absorption factor over the daylight period (not the whole day)
// - by doing an average of values at midday, sunrise and an intermediate value
// - using the current sun direction at the given position to approximate
// the influence of high latitudes and of the inclinaison of the body orbit
public static double AtmosphereFactorAnalytic(CelestialBody body, Vector3d position, Vector3d sun_dir)
{
// only for atmospheric bodies whose rotation period is less than 120 hours
if (body.rotationPeriod > 432000.0)
{
return(AtmosphereFactor(body, position, sun_dir));
}
// get up vector & altitude
Vector3d radialOut = position - body.position;
double altitude = radialOut.magnitude;
radialOut /= altitude; // normalize
altitude -= body.Radius;
altitude = Math.Abs(altitude); //< deal with underwater & fp precision issues
double static_pressure = body.GetPressure(altitude);
if (static_pressure > 0.0)
{
Vector3d[] sunDirs = new Vector3d[3];
// east - sunrise
sunDirs[0] = body.getRFrmVel(position).normalized;
// perpendicular vector
Vector3d sunUp = Vector3d.Cross(sunDirs[0], sun_dir).normalized;
// midday vector (along the radial plane + an angle depending on the original vesselSundir)
sunDirs[1] = Vector3d.Cross(sunUp, sunDirs[0]).normalized;
// invert midday vector if it's pointing toward the ground (checking against radial-out vector)
if (Vector3d.Dot(sunDirs[1], radialOut) < 0.0)
{
sunDirs[1] *= -1.0;
}
// get an intermediate vector between sunrise and midday
sunDirs[2] = (sunDirs[0] + sunDirs[1]).normalized;
double density = body.GetDensity(static_pressure, body.GetTemperature(altitude));
// nonrefracting radially symmetrical atmosphere model [Schoenberg 1929]
double Ra = body.Radius + altitude;
double Ya = body.atmosphereDepth - altitude;
double atmo_factor_analytic = 0.0;
for (int i = 0; i < 3; i++)
{
double q = Ra * Math.Max(0.0, Vector3d.Dot(radialOut, sunDirs[i]));
double path = Math.Sqrt(q * q + 2.0 * Ra * Ya + Ya * Ya) - q;
atmo_factor_analytic += body.GetSolarPowerFactor(density) * Ya / path;
}
atmo_factor_analytic /= 3.0;
return(atmo_factor_analytic);
}
return(1.0);
}
/// <summary>
/// The pair of velocities representing this spot's velocity due to
/// planetary rotation, at this (sea level) altitude:
/// </summary>
/// <returns>velocities pair </returns>
public OrbitableVelocity GetAltitudeVelocities(ScalarValue altitude)
{
Vector3d pos = Body.GetWorldSurfacePosition(Latitude, Longitude, altitude);
Vector3d vel = Body.getRFrmVel(pos);
CelestialBody shipBody = Shared.Vessel.orbit.referenceBody;
if (shipBody == null)
{
return(new OrbitableVelocity(new Vector(vel), new Vector(vel)));
}
Vector3d srfVel = vel - shipBody.getRFrmVel(Shared.Vessel.CoMD);
return(new OrbitableVelocity(new Vector(vel), new Vector(srfVel)));
}
//formula for drag seems to be drag force = (1/2) * DragMultiplier * (air density) * (mass * max_drag) * (airspeed)^2
//so drag acceleration is (1/2) * DragMultiplier * (air density) * (average max_drag) * (airspeed)^2
//where the max_drag average over parts is weighted by the part mass
public static Vector3d DragAccel(this CelestialBody body, Vector3d pos, Vector3d orbitVel, double dragCoeffOverMass)
{
double airPressure = FlightGlobals.getStaticPressure(pos, body);
//Atmosphere cuts out abruptly when pressure falls below 1e-6 * sea level pressure.
if (airPressure < body.atmosphereMultiplier * 1e-6)
{
return(Vector3d.zero);
}
double airDensity = FlightGlobals.getAtmDensity(airPressure);
Vector3d airVel = orbitVel - body.getRFrmVel(pos);
return(-0.5 * FlightGlobals.DragMultiplier * airDensity * dragCoeffOverMass * airVel.magnitude * airVel);
}
// determine average atmospheric absorption factor over the daylight period (not the whole day)
// - by doing an average of values at midday, sunrise and an intermediate value
// - using the current sun direction at the given position to approximate
// the influence of high latitudes and of the inclinaison of the body orbit
public static double AtmosphereFactorAnalytic(CelestialBody body, Vector3d position, Vector3d sun_dir)
{
// only for atmospheric bodies whose rotation period is less than 120 hours
if (body.rotationPeriod > 432000.0)
{
return(AtmosphereFactor(body, position, sun_dir));
}
// get up vector & altitude
Vector3d radialOut = position - body.position;
double altitude = radialOut.magnitude;
radialOut /= altitude; // normalize
altitude -= body.Radius;
altitude = Math.Abs(altitude); //< deal with underwater & fp precision issues
double static_pressure = body.GetPressure(altitude);
if (static_pressure > 0.0)
{
Vector3d[] sunDirs = new Vector3d[3];
// east - sunrise
sunDirs[0] = body.getRFrmVel(position).normalized;
// perpendicular vector
Vector3d sunUp = Vector3d.Cross(sunDirs[0], sun_dir).normalized;
// midday vector (along the radial plane + an angle depending on the original vesselSundir)
sunDirs[1] = Vector3d.Cross(sunUp, sunDirs[0]).normalized;
// invert midday vector if it's pointing toward the ground (checking against radial-out vector)
if (Vector3d.Dot(sunDirs[1], radialOut) < 0.0)
{
sunDirs[1] *= -1.0;
}
// get an intermediate vector between sunrise and midday
sunDirs[2] = (sunDirs[0] + sunDirs[1]).normalized;
double density = body.GetDensity(static_pressure, body.GetTemperature(altitude));
double atmo_factor_analytic = 0.0;
for (int i = 0; i < 3; i++)
{
body.GetSolarAtmosphericEffects(Vector3d.Dot(radialOut, sunDirs[i]), density, out _, out double stockFluxFactor);
atmo_factor_analytic += stockFluxFactor;
}
atmo_factor_analytic /= 3.0;
return(atmo_factor_analytic);
}
return(1.0);
}
override public OrbitableVelocity GetVelocitiesAtUT(TimeSpan timeStamp)
{
var orbVel = new Vector(Orbit.getOrbitalVelocityAtUT(timeStamp.ToUnixStyleTime()));
orbVel = new Vector(orbVel.X, orbVel.Z, orbVel.Y); // swap Y and Z because KSP API is weird.
CelestialBody parent = Body.referenceBody;
if (parent == null) // only if Body is Sun and therefore has no parent.
{
return(new OrbitableVelocity(new Vector(0.0, 0.0, 0.0), new Vector(0.0, 0.0, 0.0)));
}
var surfVel = new Vector(Body.orbit.GetVel() - parent.getRFrmVel(Body.position));
return(new OrbitableVelocity(orbVel, surfVel));
}
/// <summary>
/// Calculates the velocities of this vessel at some future universal timestamp,
/// taking into account all currently predicted SOI transition patches, and also
/// assuming that all the planned maneuver nodes will actually be executed precisely
/// as planned. Note that this cannot "see" into the future any farther than the
/// KSP orbit patches setting allows for.
/// </summary>
/// <param name="timeStamp">The time to predict for. Although the intention is to
/// predict for a future time, it could be used to predict for a past time.</param>
/// <returns>The orbit/surface velocity pair as a user-readable Vector in raw rotation coordinates.</returns>
override public OrbitableVelocity GetVelocitiesAtUT(TimeSpan timeStamp)
{
string blockingTech;
if (!Career.CanMakeNodes(out blockingTech))
{
throw new KOSLowTechException("use VELOCITYAT on a vessel", blockingTech);
}
double desiredUT = timeStamp.ToUnixStyleTime();
Orbit patch = GetOrbitAtUT(desiredUT);
Vector3d orbVel = patch.getOrbitalVelocityAtUT(desiredUT);
// This is an ugly workaround to fix what is probably a bug in KSP's API:
// If looking at a future orbit patch around a child body of the current body, then
// the various get{Thingy}AtUT() methods return numbers calculated incorrectly as
// if the child body was going to remain stationary where it is now, rather than
// taking into account where it will be later when the intercept happens.
// This corrects for that case:
if (Utils.BodyOrbitsBody(patch.referenceBody, Vessel.orbit.referenceBody))
{
Vector3d futureBodyVel = patch.referenceBody.orbit.getOrbitalVelocityAtUT(desiredUT);
orbVel = orbVel + futureBodyVel;
}
// For some weird reason orbital velocities are returned by the KSP API
// with Y and Z swapped, so swap them back:
orbVel = new Vector3d(orbVel.x, orbVel.z, orbVel.y);
CelestialBody parent = patch.referenceBody;
Vector surfVel;
if (parent != null)
{
Vector3d pos = GetPositionAtUT(timeStamp);
surfVel = new Vector(orbVel - parent.getRFrmVel(pos + Shared.Vessel.findWorldCenterOfMass()));
}
else
{
surfVel = new Vector(orbVel.x, orbVel.y, orbVel.z);
}
return(new OrbitableVelocity(new Vector(orbVel), surfVel));
}
/// <summary>
/// Get the velocity pairing of this thing in this orbit at the given
/// time. Note that it does NOT take into account any
/// encounters or maneuver nodes - it assumes the current
/// orbit patch remains followed forever.
/// </summary>
/// <param name="timeStamp">The universal time to query for</param>
/// <returns></returns>
public OrbitableVelocity GetVelocityAtUT(TimeStamp timeStamp)
{
var orbVel = new Vector(orbit.getOrbitalVelocityAtUT(timeStamp.ToUnixStyleTime()));
// For some weird reason orbit returns velocities with Y and Z swapped, so flip them back:
orbVel = new Vector(orbVel.X, orbVel.Z, orbVel.Y);
CelestialBody parent = orbit.referenceBody;
Vector surfVel;
if (parent != null)
{
Vector3d pos = GetPositionAtUT(timeStamp);
surfVel = new Vector(orbVel - parent.getRFrmVel(pos + Shared.Vessel.CoMD));
}
else
{
surfVel = new Vector(orbVel.X, orbVel.Y, orbVel.Z);
}
return(new OrbitableVelocity(orbVel, surfVel));
}
public static Vector3 GetPlannedDirection()
{
var vessel = FlightGlobals.ActiveVessel;
if (vessel == null || Trajectory.Target.Body == null)
{
return(new Vector3(0, 0, 0));
}
CelestialBody body = vessel.mainBody;
Vector3d pos = vessel.GetWorldPos3D() - body.position;
Vector3d vel = vessel.obt_velocity - body.getRFrmVel(body.position + pos); // air velocity
Vector3 up = pos.normalized;
Vector3 velRight = Vector3.Cross(vel, up).normalized;
Vector3 velUp = Vector3.Cross(velRight, vel).normalized;
float plannedAngleOfAttack = (float)DescentProfile.fetch.GetAngleOfAttack(Trajectory.Target.Body, pos, vel);
return(vel.normalized * Mathf.Cos(plannedAngleOfAttack) + velUp * Mathf.Sin(plannedAngleOfAttack));
}
static private Vector3d GetForces(Vessel vessel, Vector3d r, Vector3d v, Vector3d att, double totalMass, double minThrust, double maxThrust,
Trajectories.VesselAerodynamicModel aeroModel, CelestialBody body, double t, double dt,
BLController controller, Vector3d tgt_r, double aeroFudgeFactor,
out Vector3d steer, out Vector3d vel_air, out double throttle)
{
Vector3d F = Vector3d.zero;
double y = r.magnitude - body.Radius;
steer = -Vector3d.Normalize(v);
throttle = 0;
// gravity
double R = r.magnitude;
Vector3d g = r * (-body.gravParameter / (R * R * R));
float lastAng = (float)((-1) * body.angularVelocity.magnitude / Math.PI * 180.0);
Quaternion lastBodyRot = Quaternion.AngleAxis(lastAng, body.angularVelocity.normalized);
vel_air = v - body.getRFrmVel(r + body.position);
if (controller != null)
{
bool bailOutLandingBurn = true;
bool simulate = true;
bool landingGear;
controller.GetControlOutputs(vessel, totalMass, r, v, att, minThrust, maxThrust, t, body, simulate, out throttle, out steer, out landingGear, bailOutLandingBurn);
if (throttle > 0)
{
F = steer * (minThrust + throttle * (maxThrust - minThrust));
}
att = steer; // assume attitude is always correct
}
F = F + aeroModel.GetForces(body, r, vel_air, Math.PI) * aeroFudgeFactor; // retrograde
F = F + g * totalMass;
return(F);
}
public void Apply(PosistionStatistics posistionStatistics, Dictionary <Guid, VesselCtrlUpdate> ctrlUpdate, VesselUpdate previousUpdate, VesselUpdate nextUpdate, Settings dmpSettings)
{
if (HighLogic.LoadedScene == GameScenes.LOADING)
{
return;
}
//Ignore updates to our own vessel if we are in flight and we aren't spectating
if (!vesselWorker.isSpectating && (FlightGlobals.fetch.activeVessel != null ? FlightGlobals.fetch.activeVessel.id == vesselID : false) && HighLogic.LoadedScene == GameScenes.FLIGHT)
{
return;
}
Vessel updateVessel = FlightGlobals.fetch.vessels.FindLast(v => v.id == vesselID);
if (updateVessel == null)
{
//DarkLog.Debug("ApplyVesselUpdate - Got vessel update for " + vesselID + " but vessel does not exist");
return;
}
CelestialBody updateBody = FlightGlobals.Bodies.Find(b => b.bodyName == bodyName);
if (updateBody == null)
{
//DarkLog.Debug("ApplyVesselUpdate - updateBody not found");
return;
}
double interpolatorDelay = 0f;
if (dmpSettings.interpolatorType == InterpolatorType.INTERPOLATE1S)
{
interpolatorDelay = 1f;
}
if (dmpSettings.interpolatorType == InterpolatorType.INTERPOLATE3S)
{
interpolatorDelay = 3f;
}
bool interpolatorEnabled = dmpSettings.interpolatorType == InterpolatorType.INTERPOLATE1S || dmpSettings.interpolatorType == InterpolatorType.INTERPOLATE3S;
bool extrapolatorEnabled = dmpSettings.interpolatorType == InterpolatorType.EXTRAPOLATE_NO_ROT || dmpSettings.interpolatorType == InterpolatorType.EXTRAPOLATE_FULL;
Quaternion normalRotate = Quaternion.identity;
Vector3 oldPos = updateVessel.GetWorldPos3D();
Vector3 oldVelocity = updateVessel.orbitDriver.orbit.GetVel();
//Position/Velocity
if (isSurfaceUpdate)
{
//Get the new position/velocity
double altitudeFudge = 0;
VesselUtil.DMPRaycastPair dmpRaycast = VesselUtil.RaycastGround(position[0], position[1], updateBody);
if (dmpRaycast.altitude != -1d && position[3] != -1d)
{
Vector3 theirNormal = new Vector3(terrainNormal[0], terrainNormal[1], terrainNormal[2]);
altitudeFudge = dmpRaycast.altitude - position[3];
if (Math.Abs(position[2] - position[3]) < 50f)
{
normalRotate = Quaternion.FromToRotation(theirNormal, dmpRaycast.terrainNormal);
}
}
Vector3d updateAcceleration = updateBody.bodyTransform.rotation * new Vector3d(acceleration[0], acceleration[1], acceleration[2]);
Vector3d updateVelocity = updateBody.bodyTransform.rotation * new Vector3d(velocity[0], velocity[1], velocity[2]);
Vector3d updatePostion = updateBody.GetWorldSurfacePosition(position[0], position[1], position[2] + altitudeFudge);
Vector3d newUpdatePostion = updatePostion;
Vector3d newUpdateVelocity = updateVelocity;
double planetariumDifference = Planetarium.GetUniversalTime() - (planetTime + interpolatorDelay);
if (extrapolatorEnabled)
{
if (Math.Abs(planetariumDifference) < 3f)
{
if (dmpSettings.interpolatorType == InterpolatorType.EXTRAPOLATE_NO_ROT || previousUpdate == null)
{
StepExtrapolate(updatePostion, updateVelocity, updateAcceleration, planetariumDifference, out newUpdatePostion, out newUpdateVelocity);
}
if (dmpSettings.interpolatorType == InterpolatorType.EXTRAPOLATE_FULL && previousUpdate != null)
{
StepExtrapolateWithRotation(previousUpdate, updatePostion, updateVelocity, updateAcceleration, planetariumDifference, out newUpdatePostion, out newUpdateVelocity);
}
}
}
if (interpolatorEnabled && nextUpdate != null && (Math.Abs(nextUpdate.planetTime - Planetarium.GetUniversalTime())) < 5f)
{
double scaling = (Planetarium.GetUniversalTime() - interpolatorDelay - planetTime) / (nextUpdate.planetTime - planetTime);
Vector3d nextPosition = updateBody.GetWorldSurfacePosition(nextUpdate.position[0], nextUpdate.position[1], nextUpdate.position[2] + altitudeFudge);
Vector3d nextVelocity = updateBody.bodyTransform.rotation * new Vector3d(nextUpdate.velocity[0], nextUpdate.velocity[1], nextUpdate.velocity[2]);
newUpdatePostion = Vector3d.Lerp(updatePostion, nextPosition, scaling);
newUpdateVelocity = Vector3d.Lerp(updateVelocity, nextVelocity, scaling);
}
Vector3d orbitalPos = newUpdatePostion - updateBody.position;
Vector3d surfaceOrbitVelDiff = updateBody.getRFrmVel(newUpdatePostion);
Vector3d orbitalVel = newUpdateVelocity + surfaceOrbitVelDiff;
updateVessel.orbitDriver.orbit.UpdateFromStateVectors(orbitalPos.xzy, orbitalVel.xzy, updateBody, Planetarium.GetUniversalTime());
}
else
{
updateVessel.orbit.SetOrbit(orbit[0], orbit[1], orbit[2], orbit[3], orbit[4], orbit[5], orbit[6], updateBody);
}
//Updates orbit.pos/vel
updateVessel.orbitDriver.orbit.UpdateFromOrbitAtUT(updateVessel.orbitDriver.orbit, Planetarium.GetUniversalTime(), updateBody);
//Updates vessel pos from the orbit, as if on rails
updateVessel.orbitDriver.updateFromParameters();
//Rotation
Quaternion unfudgedRotation = new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]);
//Rotation extrapolation? :O
if (previousUpdate != null && (extrapolatorEnabled || (interpolatorEnabled && !isSurfaceUpdate)))
{
double deltaUpdateT = planetTime - previousUpdate.planetTime;
double deltaRealT = Planetarium.GetUniversalTime() - previousUpdate.planetTime;
float scaling = (float)(deltaRealT / deltaUpdateT);
if (Math.Abs(deltaRealT) < 3f)
{
Quaternion previousRotation = new Quaternion(previousUpdate.rotation[0], previousUpdate.rotation[1], previousUpdate.rotation[2], previousUpdate.rotation[3]);
unfudgedRotation = RotationLerp(previousRotation, unfudgedRotation, scaling);
}
}
if (nextUpdate != null && interpolatorEnabled && isSurfaceUpdate)
{
double deltaUpdateT = nextUpdate.planetTime - planetTime;
double deltaRealT = Planetarium.GetUniversalTime() - interpolatorDelay - planetTime;
float scaling = (float)(deltaRealT / deltaUpdateT);
if (Math.Abs(deltaRealT) < 3f)
{
Quaternion nextRotation = new Quaternion(nextUpdate.rotation[0], nextUpdate.rotation[1], nextUpdate.rotation[2], nextUpdate.rotation[3]);
unfudgedRotation = RotationLerp(unfudgedRotation, nextRotation, scaling);
}
}
Quaternion updateRotation = normalRotate * unfudgedRotation;
//Rotational error tracking
double rotationalError = Quaternion.Angle(updateVessel.srfRelRotation, updateRotation);
updateVessel.SetRotation(updateVessel.mainBody.bodyTransform.rotation * updateRotation);
updateVessel.srfRelRotation = updateRotation;
Vector3 angularVel = updateVessel.ReferenceTransform.rotation * new Vector3(angularVelocity[0], angularVelocity[1], angularVelocity[2]);
if (updateVessel.parts != null && !updateVessel.packed)
{
for (int i = 0; i < updateVessel.parts.Count; i++)
{
Part thisPart = updateVessel.parts[i];
thisPart.vel = updateVessel.orbit.GetVel() - Krakensbane.GetFrameVelocity();
if (thisPart.orbit.referenceBody.inverseRotation)
{
thisPart.vel -= updateBody.getRFrmVel(thisPart.partTransform.position);
}
if (thisPart.rb != null && thisPart.State != PartStates.DEAD)
{
thisPart.rb.velocity = thisPart.vel;
//Angular Vel
thisPart.rb.angularVelocity = angularVel;
if (thisPart != updateVessel.rootPart)
{
Vector3 diffPos = thisPart.rb.position - updateVessel.CoM;
Vector3 partVelDifference = Vector3.Cross(angularVel, diffPos);
thisPart.rb.velocity = thisPart.rb.velocity + partVelDifference;
}
}
}
}
//Updates Vessel.CoMD, which is used for GetWorldPos3D
updateVessel.precalc.CalculatePhysicsStats();
updateVessel.latitude = updateBody.GetLatitude(updateVessel.GetWorldPos3D());
updateVessel.longitude = updateBody.GetLongitude(updateVessel.GetWorldPos3D());
updateVessel.altitude = updateBody.GetAltitude(updateVessel.GetWorldPos3D());
double distanceError = Vector3d.Distance(oldPos, updateVessel.GetWorldPos3D());
double velocityError = Vector3d.Distance(oldVelocity, updateVessel.orbitDriver.orbit.GetVel());
if (ctrlUpdate != null)
{
if (ctrlUpdate.ContainsKey(updateVessel.id))
{
updateVessel.OnFlyByWire -= ctrlUpdate[updateVessel.id].UpdateControls;
ctrlUpdate.Remove(updateVessel.id);
}
VesselCtrlUpdate vcu = new VesselCtrlUpdate(updateVessel, ctrlUpdate, planetTime + 5, flightState);
ctrlUpdate.Add(updateVessel.id, vcu);
updateVessel.OnFlyByWire += vcu.UpdateControls;
}
//Action group controls
updateVessel.ActionGroups.SetGroup(KSPActionGroup.Gear, actiongroupControls[0]);
updateVessel.ActionGroups.SetGroup(KSPActionGroup.Light, actiongroupControls[1]);
updateVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, actiongroupControls[2]);
updateVessel.ActionGroups.SetGroup(KSPActionGroup.SAS, actiongroupControls[3]);
updateVessel.ActionGroups.SetGroup(KSPActionGroup.RCS, actiongroupControls[4]);
if (sasEnabled)
{
updateVessel.Autopilot.SetMode((VesselAutopilot.AutopilotMode)autopilotMode);
updateVessel.Autopilot.SAS.LockRotation(new Quaternion(lockedRotation[0], lockedRotation[1], lockedRotation[2], lockedRotation[3]));
}
UpdateProtovessel(updateVessel);
posistionStatistics.LogError(updateVessel.id, distanceError, velocityError, rotationalError, planetTime);
}
//formula for drag seems to be drag force = (1/2) * (air density / 125) * (mass * max_drag) * (airspeed)^2
//so drag acceleration is (1/2) * (air density / 125) * (average max_drag) * (airspeed)^2
//where the max_drag average over parts is weighted by the part weight
public static Vector3d computeDragAccel(Vector3d pos, Vector3d orbitVel, double dragCoeffOverMass, CelestialBody body)
{
double airDensity = FlightGlobals.getAtmDensity(FlightGlobals.getStaticPressure(pos, body)) / 125.0;
Vector3d airVel = orbitVel - body.getRFrmVel(pos);
return -0.5 * airDensity * dragCoeffOverMass * airVel.magnitude * airVel;
}
private void FixedUpdate()
{
if (Settings.fetch.NewGui)
{
return;
}
float t = Time.realtimeSinceStartup;
if (t < lastStringRenderTime + stringRenderInterval)
{
return;
}
lastStringRenderTime = t;
Trajectory traj = Trajectory.fetch;
Trajectory.Patch lastPatch = traj.Patches.LastOrDefault();
CelestialBody lastPatchBody = lastPatch?.StartingState.ReferenceBody;
CelestialBody targetBody = Trajectory.Target.Body;
guistring_gForce = (traj.MaxAccel / 9.81).ToString("0.00");
if (lastPatch != null && lastPatch.ImpactPosition.HasValue)
{
Vector3 up = lastPatch.ImpactPosition.Value.normalized;
Vector3 vel = lastPatch.ImpactVelocity.Value - lastPatchBody.getRFrmVel(lastPatch.ImpactPosition.Value + lastPatchBody.position);
float vVelMag = Vector3.Dot(vel, up);
Vector3 vVel = up * vVelMag;
float hVelMag = (vel - vVel).magnitude;
guistring_impactVelocity = String.Format("Impact: V = {0,6:F0}m/s, H = {1,6:F0}m/s", -vVelMag, hVelMag);
}
else
{
guistring_impactVelocity = "";
}
if (Settings.fetch.DisplayTargetGUI)
{
if (lastPatchBody != null && targetBody != null && lastPatch.ImpactPosition.HasValue &&
lastPatchBody == targetBody && Trajectory.Target.WorldPosition.HasValue)
{
// Get Latitude and Longitude from impact position
double impactLat;
double impatLon;
double impactAlt;
// Get Latitude and Longitude from impact position
impactPos = lastPatch.ImpactPosition.Value + lastPatchBody.position;
lastPatchBody.GetLatLonAlt(impactPos, out impactLat, out impatLon, out impactAlt);
// Get Latitude and Longitude for target position
double targetLat;
double targetLon;
double targetAlt;
targetPos = Trajectory.Target.WorldPosition.Value + targetBody.position;
targetBody.GetLatLonAlt(targetPos, out targetLat, out targetLon, out targetAlt);
float targetDistance = (float)(Util.DistanceFromLatitudeAndLongitude(targetBody.Radius + impactAlt,
impactLat, impatLon, targetLat, targetLon) / 1e3);
float targetDistanceNorth = (float)(Util.DistanceFromLatitudeAndLongitude(targetBody.Radius + impactAlt,
impactLat, targetLon, targetLat, targetLon) / 1e3) * ((targetLat - impactLat) < 0 ? -1.0f : +1.0f);
float targetDistanceEast = (float)(Util.DistanceFromLatitudeAndLongitude(targetBody.Radius + impactAlt,
targetLat, impatLon, targetLat, targetLon) / 1e3) * ((targetLon - impatLon) < 0 ? -1.0f : +1.0f);
// format distance to target string
guistring_targetDistance = String.Format("{0,6:F1}km | {1}: {2,6:F1}km | {3}: {4,6:F1}km",
targetDistance,
targetDistanceNorth > 0.0f ? 'N' : 'S',
Math.Abs(targetDistanceNorth),
targetDistanceEast > 0.0f ? 'E' : 'W',
Math.Abs(targetDistanceEast));
}
else
{
guistring_targetDistance = "";
}
}
if (FlightGlobals.ActiveVessel != null)
{
var body = FlightGlobals.ActiveVessel.mainBody;
guistring_Latitude = body.GetLatitude(FlightGlobals.ActiveVessel.GetWorldPos3D()).ToString("000.000000");
guistring_Longitude = body.GetLongitude(FlightGlobals.ActiveVessel.GetWorldPos3D()).ToString("000.000000");
}
}
private IEnumerable <bool> AddPatch(VesselState startingState)
{
if (null == AttachedVessel.patchedConicSolver)
{
AttachedVessel.AttachPatchedConicsSolver();
}
CelestialBody body = startingState.ReferenceBody;
Patch patch = new Patch
{
StartingState = startingState,
IsAtmospheric = false,
SpaceOrbit = startingState.StockPatch ?? CreateOrbitFromState(startingState)
};
patch.EndTime = patch.StartingState.Time + patch.SpaceOrbit.period;
// the flight plan does not always contain the first patches (before the first maneuver node),
// so we populate it with the current orbit and associated encounters etc.
List <Orbit> flightPlan = new List <Orbit>();
for (Orbit orbit = AttachedVessel.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
{
if (AttachedVessel.patchedConicSolver.flightPlan.Contains(orbit))
{
break;
}
flightPlan.Add(orbit);
}
foreach (Orbit orbit in AttachedVessel.patchedConicSolver.flightPlan)
{
flightPlan.Add(orbit);
}
Orbit nextPatch = null;
if (startingState.StockPatch == null)
{
nextPatch = patch.SpaceOrbit.nextPatch;
}
else
{
int planIdx = flightPlan.IndexOf(startingState.StockPatch);
if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
{
nextPatch = flightPlan[planIdx + 1];
}
}
if (nextPatch != null)
{
patch.EndTime = nextPatch.StartUT;
}
double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);
if (!body.atmosphere)
{
maxAtmosphereAltitude = body.pqsController.mapMaxHeight;
}
double minAltitude = patch.SpaceOrbit.PeA;
if (patch.SpaceOrbit.timeToPe < 0 || patch.EndTime < startingState.Time + patch.SpaceOrbit.timeToPe)
{
minAltitude = Math.Min(
patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).magnitude,
patch.SpaceOrbit.getRelativePositionAtUT(patch.StartingState.Time + 1.0).magnitude
) - body.Radius;
}
if (minAltitude < maxAtmosphereAltitude)
{
double entryTime;
if (startingState.Position.magnitude <= body.Radius + maxAtmosphereAltitude)
{
// whole orbit is inside the atmosphere
entryTime = startingState.Time;
}
else
{
entryTime = FindOrbitBodyIntersection(
patch.SpaceOrbit,
startingState.Time, startingState.Time + patch.SpaceOrbit.timeToPe,
body.Radius + maxAtmosphereAltitude);
}
if (entryTime > startingState.Time + 0.1 || !body.atmosphere)
{
if (body.atmosphere)
{
// add the space patch before atmospheric entry
patch.EndTime = entryTime;
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
Position = patch.SpaceOrbit.getRelativePositionAtUT(entryTime).SwapYZ(),
ReferenceBody = body,
Time = entryTime,
Velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime).SwapYZ()
};
}
else
{
// the body has no atmosphere, so what we actually computed is the entry
// inside the "ground sphere" (defined by the maximal ground altitude)
// now we iterate until the inner ground sphere (minimal altitude), and
// check if we hit the ground along the way
double groundRangeExit = FindOrbitBodyIntersection(
patch.SpaceOrbit,
startingState.Time, startingState.Time + patch.SpaceOrbit.timeToPe,
body.Radius - maxAtmosphereAltitude);
if (groundRangeExit <= entryTime)
{
groundRangeExit = startingState.Time + patch.SpaceOrbit.timeToPe;
}
double iterationSize = (groundRangeExit - entryTime) / 100.0;
double t;
bool groundImpact = false;
for (t = entryTime; t < groundRangeExit; t += iterationSize)
{
Vector3d pos = patch.SpaceOrbit.getRelativePositionAtUT(t);
double groundAltitude = GetGroundAltitude(body, CalculateRotatedPosition(body, pos.SwapYZ(), t))
+ body.Radius;
if (pos.magnitude < groundAltitude)
{
t -= iterationSize;
groundImpact = true;
break;
}
}
if (groundImpact)
{
patch.EndTime = t;
patch.RawImpactPosition = patch.SpaceOrbit.getRelativePositionAtUT(t).SwapYZ();
patch.ImpactPosition = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, t);
patch.ImpactVelocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(t).SwapYZ();
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
}
else
{
// no impact, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextPatch != null)
{
AddPatch_outState = new VesselState
{
Position = patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).SwapYZ(),
Velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime).SwapYZ(),
ReferenceBody = nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
}
else
{
AddPatch_outState = null;
}
}
}
}
else
{
if (patch.StartingState.ReferenceBody != AttachedVessel.mainBody)
{
// currently, we can't handle predictions for another body, so we stop
AddPatch_outState = null;
yield break;
}
// simulate atmospheric flight (drag and lift), until impact or atmosphere exit
// (typically for an aerobraking maneuver) assuming a constant angle of attack
patch.IsAtmospheric = true;
patch.StartingState.StockPatch = null;
// lower dt would be more accurate, but a trade-off has to be found between performances and accuracy
double dt = Settings.IntegrationStepSize;
// some shallow entries can result in very long flight. For performances reasons,
// we limit the prediction duration
int maxIterations = (int)(60.0 * 60.0 / dt);
int chunkSize = 128;
// time between two consecutive stored positions (more intermediate positions are computed for better accuracy),
// also used for ground collision checks
double trajectoryInterval = 10.0;
List <Point[]> buffer = new List <Point[]>
{
new Point[chunkSize]
};
int nextPosIdx = 0;
SimulationState state;
state.position = patch.SpaceOrbit.getRelativePositionAtUT(entryTime).SwapYZ();
state.velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime).SwapYZ();
// Initialize a patch with zero acceleration
Vector3d currentAccel = new Vector3d(0.0, 0.0, 0.0);
double currentTime = entryTime;
double lastPositionStoredUT = 0;
Vector3d lastPositionStored = new Vector3d();
bool hitGround = false;
int iteration = 0;
int incrementIterations = 0;
int minIterationsPerIncrement = maxIterations / Settings.MaxFramesPerPatch;
#region Acceleration Functor
// function that calculates the acceleration under current parameters
Vector3d AccelerationFunc(Vector3d position, Vector3d velocity)
{
Profiler.Start("accelerationFunc inside");
// gravity acceleration
double R_ = position.magnitude;
Vector3d accel_g = position * (-body.gravParameter / (R_ * R_ * R_));
// aero force
Vector3d vel_air = velocity - body.getRFrmVel(body.position + position);
double aoa = DescentProfile.GetAngleOfAttack(body, position, vel_air) ?? 0d;
Profiler.Start("GetForces");
Vector3d force_aero = aerodynamicModel_.GetForces(body, position, vel_air, aoa);
Profiler.Stop("GetForces");
Vector3d accel = accel_g + force_aero / aerodynamicModel_.Mass;
accel += API.HandleAccel(AttachedVessel, body, position, velocity, aoa);
Profiler.Stop("accelerationFunc inside");
return(accel);
}
#endregion
#region Integration Loop
while (true)
{
++iteration;
++incrementIterations;
if (incrementIterations > minIterationsPerIncrement && Util.ElapsedMilliseconds(increment_time) > MAX_INCREMENT_TIME)
{
yield return(false);
incrementIterations = 0;
}
double R = state.position.magnitude;
double altitude = R - body.Radius;
double atmosphereCoeff = altitude / maxAtmosphereAltitude;
if (hitGround ||
atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 ||
iteration == maxIterations || currentTime > patch.EndTime)
{
//Util.PostSingleScreenMessage("atmo force", "Atmospheric accumulated force: " + accumulatedForces.ToString("0.00"));
if (hitGround || atmosphereCoeff <= 0.0)
{
patch.RawImpactPosition = state.position;
patch.ImpactPosition = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, currentTime);
patch.ImpactVelocity = state.velocity;
}
patch.EndTime = Math.Min(currentTime, patch.EndTime);
int totalCount = (buffer.Count - 1) * chunkSize + nextPosIdx;
patch.AtmosphericTrajectory = new Point[totalCount];
int outIdx = 0;
foreach (Point[] chunk in buffer)
{
foreach (Point p in chunk)
{
if (outIdx == totalCount)
{
break;
}
patch.AtmosphericTrajectory[outIdx++] = p;
}
}
if (iteration == maxIterations)
{
ScreenMessages.PostScreenMessage("WARNING: trajectory prediction stopped, too many iterations");
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
if (atmosphereCoeff <= 0.0 || hitGround)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
Position = state.position,
Velocity = state.velocity,
ReferenceBody = body,
Time = patch.EndTime
};
yield break;
}
Vector3d lastAccel = currentAccel;
SimulationState lastState = state;
Profiler.Start("IntegrationStep");
// Verlet integration (more precise than using the velocity)
// state = VerletStep(state, accelerationFunc, dt);
state = RK4Step(state, AccelerationFunc, dt, out currentAccel);
currentTime += dt;
// KSP presumably uses Euler integration for position updates. Since RK4 is actually more precise than that,
// we try to reintroduce an approximation of the error.
// The local truncation error for euler integration is:
// LTE = 1/2 * h^2 * y''(t)
// https://en.wikipedia.org/wiki/Euler_method#Local_truncation_error
//
// For us,
// h is the time step of the outer simulation (KSP), which is the physics time step
// y''(t) is the difference of the velocity/acceleration divided by the physics time step
state.position += 0.5 * TimeWarp.fixedDeltaTime * currentAccel * dt;
state.velocity += 0.5 * TimeWarp.fixedDeltaTime * (currentAccel - lastAccel);
Profiler.Stop("IntegrationStep");
// calculate gravity and aerodynamic force
Vector3d gravityAccel = lastState.position * (-body.gravParameter / (R * R * R));
Vector3d aerodynamicForce = (currentAccel - gravityAccel) / aerodynamicModel_.Mass;
// acceleration in the vessel reference frame is acceleration - gravityAccel
maxAccelBackBuffer_ = Math.Max(
(float)(aerodynamicForce.magnitude / aerodynamicModel_.Mass),
maxAccelBackBuffer_);
#region Impact Calculation
Profiler.Start("AddPatch#impact");
double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
if (currentTime >= lastPositionStoredUT + interval)
{
double groundAltitude = GetGroundAltitude(body, CalculateRotatedPosition(body, state.position, currentTime));
if (lastPositionStoredUT > 0)
{
// check terrain collision, to detect impact on mountains etc.
Vector3 rayOrigin = lastPositionStored;
Vector3 rayEnd = state.position;
double absGroundAltitude = groundAltitude + body.Radius;
if (absGroundAltitude > rayEnd.magnitude)
{
hitGround = true;
float coeff = Math.Max(0.01f, (float)((absGroundAltitude - rayOrigin.magnitude)
/ (rayEnd.magnitude - rayOrigin.magnitude)));
state.position = rayEnd * coeff + rayOrigin * (1.0f - coeff);
currentTime = currentTime * coeff + lastPositionStoredUT * (1.0f - coeff);
}
}
lastPositionStoredUT = currentTime;
if (nextPosIdx == chunkSize)
{
buffer.Add(new Point[chunkSize]);
nextPosIdx = 0;
}
Vector3d nextPos = state.position;
if (Settings.BodyFixedMode)
{
nextPos = CalculateRotatedPosition(body, nextPos, currentTime);
}
buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
buffer.Last()[nextPosIdx].orbitalVelocity = state.velocity;
buffer.Last()[nextPosIdx].groundAltitude = (float)groundAltitude;
buffer.Last()[nextPosIdx].time = currentTime;
buffer.Last()[nextPosIdx++].pos = nextPos;
lastPositionStored = state.position;
}
Profiler.Stop("AddPatch#impact");
#endregion
}
#endregion
}
}
else
{
// no atmospheric entry, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextPatch != null)
{
AddPatch_outState = new VesselState
{
Position = patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).SwapYZ(),
Velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime).SwapYZ(),
ReferenceBody = nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
}
else
{
AddPatch_outState = null;
}
}
}
public void Apply()
{
if (HighLogic.LoadedScene == GameScenes.LOADING)
{
return;
}
//Ignore updates to our own vessel if we are in flight and we aren't spectating
if (!vesselWorker.isSpectating && (FlightGlobals.fetch.activeVessel != null ? FlightGlobals.fetch.activeVessel.id == vesselID : false) && HighLogic.LoadedScene == GameScenes.FLIGHT)
{
return;
}
Vessel updateVessel = FlightGlobals.fetch.vessels.FindLast(v => v.id == vesselID);
if (updateVessel == null)
{
//DarkLog.Debug("ApplyVesselUpdate - Got vessel update for " + vesselID + " but vessel does not exist");
return;
}
CelestialBody updateBody = FlightGlobals.Bodies.Find(b => b.bodyName == bodyName);
if (updateBody == null)
{
//DarkLog.Debug("ApplyVesselUpdate - updateBody not found");
return;
}
Quaternion normalRotate = Quaternion.identity;
//Position/Velocity
if (isSurfaceUpdate)
{
//Get the new position/velocity
double altitudeFudge = 0;
VesselUtil.DMPRaycastPair dmpRaycast = VesselUtil.RaycastGround(position[0], position[1], updateBody);
if (dmpRaycast.altitude != -1d && position[3] != -1d)
{
Vector3 theirNormal = new Vector3(terrainNormal[0], terrainNormal[1], terrainNormal[2]);
altitudeFudge = dmpRaycast.altitude - position[3];
if (Math.Abs(position[2] - position[3]) < 50f)
{
normalRotate = Quaternion.FromToRotation(theirNormal, dmpRaycast.terrainNormal);
}
}
double planetariumDifference = Planetarium.GetUniversalTime() - planetTime;
//Velocity fudge
Vector3d updateAcceleration = updateBody.bodyTransform.rotation * new Vector3d(acceleration[0], acceleration[1], acceleration[2]);
Vector3d velocityFudge = Vector3d.zero;
if (Math.Abs(planetariumDifference) < 3f)
{
//Velocity = a*t
velocityFudge = updateAcceleration * planetariumDifference;
}
//Position fudge
Vector3d updateVelocity = updateBody.bodyTransform.rotation * new Vector3d(velocity[0], velocity[1], velocity[2]) + velocityFudge;
Vector3d positionFudge = Vector3d.zero;
if (Math.Abs(planetariumDifference) < 3f)
{
//Use the average velocity to determine the new position
//Displacement = v0*t + 1/2at^2.
positionFudge = (updateVelocity * planetariumDifference) + (0.5d * updateAcceleration * planetariumDifference * planetariumDifference);
}
Vector3d updatePostion = updateBody.GetWorldSurfacePosition(position[0], position[1], position[2] + altitudeFudge) + positionFudge;
double latitude = updateBody.GetLatitude(updatePostion);
double longitude = updateBody.GetLongitude(updatePostion);
double altitude = updateBody.GetAltitude(updatePostion);
updateVessel.latitude = latitude;
updateVessel.longitude = longitude;
updateVessel.altitude = altitude;
updateVessel.protoVessel.latitude = latitude;
updateVessel.protoVessel.longitude = longitude;
updateVessel.protoVessel.altitude = altitude;
if (updateVessel.packed)
{
if (!updateVessel.LandedOrSplashed)
{
//Not landed but under 10km.
Vector3d orbitalPos = updatePostion - updateBody.position;
Vector3d surfaceOrbitVelDiff = updateBody.getRFrmVel(updatePostion);
Vector3d orbitalVel = updateVelocity + surfaceOrbitVelDiff;
updateVessel.orbitDriver.orbit.UpdateFromStateVectors(orbitalPos.xzy, orbitalVel.xzy, updateBody, Planetarium.GetUniversalTime());
updateVessel.orbitDriver.pos = updateVessel.orbitDriver.orbit.pos.xzy;
updateVessel.orbitDriver.vel = updateVessel.orbitDriver.orbit.vel;
}
}
else
{
Vector3d velocityOffset = updateVelocity - updateVessel.srf_velocity;
updateVessel.SetPosition(updatePostion, true);
updateVessel.ChangeWorldVelocity(velocityOffset);
}
}
else
{
Orbit updateOrbit = new Orbit(orbit[0], orbit[1], orbit[2], orbit[3], orbit[4], orbit[5], orbit[6], updateBody);
updateOrbit.Init();
updateOrbit.UpdateFromUT(Planetarium.GetUniversalTime());
double latitude = updateBody.GetLatitude(updateOrbit.pos);
double longitude = updateBody.GetLongitude(updateOrbit.pos);
double altitude = updateBody.GetAltitude(updateOrbit.pos);
updateVessel.latitude = latitude;
updateVessel.longitude = longitude;
updateVessel.altitude = altitude;
updateVessel.protoVessel.latitude = latitude;
updateVessel.protoVessel.longitude = longitude;
updateVessel.protoVessel.altitude = altitude;
if (updateVessel.packed)
{
//The OrbitDriver update call will set the vessel position on the next fixed update
VesselUtil.CopyOrbit(updateOrbit, updateVessel.orbitDriver.orbit);
updateVessel.orbitDriver.pos = updateVessel.orbitDriver.orbit.pos.xzy;
updateVessel.orbitDriver.vel = updateVessel.orbitDriver.orbit.vel;
}
else
{
//Vessel.SetPosition is full of fun and games. Avoid at all costs.
//Also, It's quite difficult to figure out the world velocity due to Krakensbane, and the reference frame.
Vector3d posDelta = updateOrbit.getPositionAtUT(Planetarium.GetUniversalTime()) - updateVessel.orbitDriver.orbit.getPositionAtUT(Planetarium.GetUniversalTime());
Vector3d velDelta = updateOrbit.getOrbitalVelocityAtUT(Planetarium.GetUniversalTime()).xzy - updateVessel.orbitDriver.orbit.getOrbitalVelocityAtUT(Planetarium.GetUniversalTime()).xzy;
//Vector3d velDelta = updateOrbit.vel.xzy - updateVessel.orbitDriver.orbit.vel.xzy;
updateVessel.Translate(posDelta);
updateVessel.ChangeWorldVelocity(velDelta);
}
}
//Rotation
Quaternion unfudgedRotation = new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]);
Quaternion updateRotation = normalRotate * unfudgedRotation;
updateVessel.SetRotation(updateVessel.mainBody.bodyTransform.rotation * updateRotation);
if (updateVessel.packed)
{
updateVessel.srfRelRotation = updateRotation;
updateVessel.protoVessel.rotation = updateVessel.srfRelRotation;
}
//Angular velocity
Vector3 angularVelocity = updateVessel.mainBody.bodyTransform.rotation * updateRotation * new Vector3(this.angularVelocity[0], this.angularVelocity[1], this.angularVelocity[2]);
if (updateVessel.parts != null)
{
foreach (Part vesselPart in updateVessel.parts)
{
if (vesselPart.rb != null && !vesselPart.rb.isKinematic && vesselPart.State == PartStates.ACTIVE)
{
vesselPart.rb.angularVelocity = angularVelocity;
if (vesselPart != updateVessel.rootPart)
{
Vector3 rootPos = FlightGlobals.ActiveVessel.rootPart.rb.position;
Vector3 rootVel = FlightGlobals.ActiveVessel.rootPart.rb.velocity;
Vector3 diffPos = vesselPart.rb.position - rootPos;
Vector3 partVelDifference = Vector3.Cross(angularVelocity, diffPos);
vesselPart.rb.velocity = rootVel + partVelDifference;
}
}
}
}
//Flight state controls (Throttle etc)
if (!vesselWorker.isSpectating)
{
updateVessel.ctrlState.CopyFrom(flightState);
}
else
{
FlightInputHandler.state.CopyFrom(flightState);
}
//Action group controls
updateVessel.ActionGroups.SetGroup(KSPActionGroup.Gear, actiongroupControls[0]);
updateVessel.ActionGroups.SetGroup(KSPActionGroup.Light, actiongroupControls[1]);
updateVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, actiongroupControls[2]);
updateVessel.ActionGroups.SetGroup(KSPActionGroup.SAS, actiongroupControls[3]);
updateVessel.ActionGroups.SetGroup(KSPActionGroup.RCS, actiongroupControls[4]);
}
private static Vector2d GetCorrection()
{
if (!Trajectories.IsVesselAttached)
{
return(Vector2d.zero);
}
Vector3d? targetPosition = TargetProfile.WorldPosition;
CelestialBody body = TargetProfile.Body;
if (!targetPosition.HasValue || patch == null || !patch.ImpactPosition.HasValue || patch.StartingState.ReferenceBody != body || !patch.IsAtmospheric)
{
return(Vector2d.zero);
}
// Get impact position, or, if some point over the trajectory has not enough clearance, smoothly interpolate to that point depending on how much clearance is missing
Vector3d impactPosition = patch.ImpactPosition.Value;
foreach (Trajectory.Point p in patch.AtmosphericTrajectory)
{
double neededClearance = 600.0d;
double missingClearance = neededClearance - (p.pos.magnitude - body.Radius - p.groundAltitude);
if (missingClearance > 0.0d)
{
if (Vector3d.Distance(p.pos, patch.RawImpactPosition.Value) > 3000.0d)
{
double coeff = missingClearance / neededClearance;
Vector3d rotatedPos = p.pos;
if (!Settings.BodyFixedMode)
{
rotatedPos = Trajectory.CalculateRotatedPosition(body, p.pos, p.time);
}
impactPosition = impactPosition * (1.0d - coeff) + rotatedPos * coeff;
}
break;
}
}
Vector3d right = Vector3d.Cross(patch.ImpactVelocity.Value, impactPosition).normalized;
Vector3d behind = Vector3d.Cross(right, impactPosition).normalized;
Vector3d offset = targetPosition.Value - impactPosition;
Vector2d offsetDir = new Vector2d(Vector3d.Dot(right, offset), Vector3d.Dot(behind, offset));
offsetDir *= 0.00005d; // 20km <-> 1 <-> 45° (this is purely indicative, no physical meaning, it would be very complicated to compute an actual correction angle as it depends on the spacecraft behavior in the atmosphere ; a small angle will suffice for a plane, but even a big angle might do almost nothing for a rocket)
Vector3d pos = Trajectories.AttachedVessel.GetWorldPos3D() - body.position;
Vector3d vel = Trajectories.AttachedVessel.obt_velocity - body.getRFrmVel(body.position + pos); // air velocity
double plannedAngleOfAttack = (double)DescentProfile.GetAngleOfAttack(body, pos, vel);
if (plannedAngleOfAttack < Util.HALF_PI)
{
offsetDir.y = -offsetDir.y; // behavior is different for prograde or retrograde entry
}
double maxCorrection = 1.0d;
offsetDir.x = Util.Clamp(offsetDir.x, -maxCorrection, maxCorrection);
offsetDir.y = Util.Clamp(offsetDir.y, -maxCorrection, maxCorrection);
return(offsetDir);
}
protected bool CheckParameters(MonolithState paramState)
{
if (paramState < currentState)
{
return(true);
}
// StarJeb not active vessel
if (starJeb != null && FlightGlobals.ActiveVessel != starJeb ||
candidate != null && FlightGlobals.ActiveVessel != candidate)
{
stepTime = Time.fixedTime;
return(false);
}
// Create the velocity change handler
if (velHdlr == null)
{
LoggingUtil.LogDebug(this, "Adding VelocityHandler");
velHdlr = MapView.MapCamera.gameObject.AddComponent <VelocityHandler>();
velHdlr.param = this;
}
switch (currentState)
{
case MonolithState.STARTED:
// Look for an eva
if (FlightGlobals.ActiveVessel != null && FlightGlobals.ActiveVessel.vesselType == VesselType.EVA)
{
candidate = FlightGlobals.ActiveVessel;
candidateName = candidate.vesselName;
LoggingUtil.LogDebug(this, "Got an eva, starJeb = " + candidate.vesselName);
nextState();
return(true);
}
return(false);
case MonolithState.EVA:
{
Vessel discovery = ContractVesselTracker.Instance.GetAssociatedVessel("Discovery One");
float discoveryDistance = discovery == null ? 10000 : Vector3.Distance(discovery.transform.position, candidate.transform.position);
if (distance < 10000 && discoveryDistance > distance && Time.fixedTime - stepTime > 10.0f || distance < MONOLITH_TOO_CLOSE)
{
// Store Star Jeb's name
starJeb = candidate;
starJebName = candidateName;
PersistentDataStore.Instance.Store <string>("starJebName", starJebName);
// Store Star Jeb's friend's name
ProtoCrewMember protoStarJeb = candidate.GetVesselCrew().First();
if (discovery != null)
{
string trait = protoStarJeb.experienceTrait.TypeName == "Scientist" ? "Pilot" : "Scientist";
ProtoCrewMember notStarJeb = discovery.GetVesselCrew().Where(pcm => pcm.experienceTrait.TypeName == trait).FirstOrDefault();
if (notStarJeb != null)
{
PersistentDataStore.Instance.Store <string>("notStarJebName", notStarJeb.name);
}
}
candidate = null;
nextState();
// Set the right image (male vs. female) for the end sequence
ConfiguredContract contract = Root as ConfiguredContract;
DialogBox dialogBox = contract.Behaviours.Select(b => b as DialogBox).Where(b => b != null).FirstOrDefault();
if (dialogBox != null)
{
FieldInfo detailsField = typeof(DialogBox).GetFields(BindingFlags.Instance | BindingFlags.NonPublic).
Where(fi => fi.FieldType == typeof(List <DialogBox.DialogDetail>)).First();
DialogBox.DialogDetail detail = ((List <DialogBox.DialogDetail>)detailsField.GetValue(dialogBox)).First();
DialogBox.ImageSection starJebImage = detail.sections.First() as DialogBox.ImageSection;
starJebImage.imageURL = protoStarJeb.gender == ProtoCrewMember.Gender.Male ?
"ContractPacks/AnomalySurveyor/Images/starjeb.dds.noload" :
"ContractPacks/AnomalySurveyor/Images/starjeb_female.dds.noload";
}
return(true);
}
}
return(false);
case MonolithState.FULL_OF_STARS1:
{
// Backup progress tracking
progressTreeBackup = new ConfigNode("PROGRESS_TREE_BACKUP");
ProgressTracking.Instance.OnSave(progressTreeBackup);
// Give the first kick away from Jool - this one using regular velocity change
CelestialBody jool = FlightGlobals.Bodies.Where(b => b.name == "Jool").First();
// Find closest point on the jool-monolith line, and throw us away from that (so we don't hit either)
Vector3 line = monolith.transform.position - jool.transform.position;
float t = Vector3.Dot(line, (starJeb.transform.position - jool.transform.position)) / Vector3.Dot(line, line);
Vector3 closest = jool.transform.position + line * t;
velocity = (starJeb.transform.position - (t > 1.0 ? jool.transform.position : closest)).normalized;
velocity += new Vector3(0.0f, 0.1f, 0.0f);
velocity *= 15000;
LoggingUtil.LogDebug(this, "kick magnitude will be: " + velocity);
nextState();
// Camera to target jool
FlightCamera.SetTarget(starJeb.transform);
FlightCamera.fetch.SetCamCoordsFromPosition((starJeb.transform.position - jool.transform.position).normalized * 25.0f);
}
return(false);
case MonolithState.FULL_OF_STARS2:
if (Time.fixedTime - stepTime > 4.0f)
{
// Give the second kick away from Jool - these using anti-kraken velocity change
CelestialBody jool = FlightGlobals.Bodies.Where(b => b.name == "Jool").First();
velocity = (starJeb.transform.position - jool.transform.position).normalized;
velocity += new Vector3(0.0f, 0.1f, 0.0f);
velocity *= 1500000;
LoggingUtil.LogDebug(this, "kick magnitude will be: " + velocity);
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS3:
if (Time.fixedTime - stepTime > 3.0f)
{
// Give the third kick away from Jool
CelestialBody jool = FlightGlobals.Bodies.Where(b => b.name == "Jool").First();
velocity = (starJeb.transform.position - jool.transform.position).normalized;
velocity *= 20000000;
LoggingUtil.LogDebug(this, "kick magnitude will be: " + velocity);
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS4:
if (Time.fixedTime - stepTime > 2.0f)
{
// Give the fourth and final kick away from Jool
CelestialBody jool = FlightGlobals.Bodies.Where(b => b.name == "Jool").First();
velocity = (starJeb.transform.position - jool.transform.position).normalized;
velocity *= 200000000;
LoggingUtil.LogDebug(this, "kick magnitude will be: " + velocity);
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS5:
if (Time.fixedTime - stepTime > 2.0f)
{
// Move along
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_DRES1:
{
// Visit Dres
CelestialBody dres = FlightGlobals.Bodies.Where(b => b.name == "Dres").First();
// Determine which side the sun is on - makes for a better show
CelestialBody sun = FlightGlobals.Bodies.Where(b => b.name == "Sun").First();
Vector3 sunnySide = sun.transform.position - dres.transform.position;
sunnySide.x = 0.0f;
sunnySide.y = 1; // Move across the top of the planet
sunnySide.z = Math.Sign(sunnySide.z);
// Set position for starjeb
float distance = 4.0f * (float)dres.Radius;
starJeb.SetPosition(dres.transform.position + new Vector3(distance, (float)dres.Radius, (float)dres.Radius * sunnySide.z));
velocity = (dres.transform.position - starJeb.transform.position + sunnySide * ((float)dres.Radius)).normalized;
velocity *= distance / 3.0f;
LoggingUtil.LogDebug(this, "kick magnitude will be: " + velocity);
starJeb.SetWorldVelocity(dres.getRFrmVel(starJeb.transform.position));
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_DRES2:
{
// Camera to target Dres - do this on a seperate update to allow KSP to catch up
CelestialBody dres = FlightGlobals.Bodies.Where(b => b.name == "Dres").First();
FlightCamera.SetTarget(starJeb.transform);
FlightCamera.fetch.SetCamCoordsFromPosition(starJeb.transform.position + (starJeb.transform.position - dres.transform.position).normalized * 10.0f);
// Make sure that the camera gets fixed
if (Time.fixedTime - stepTime > 0.1f)
{
nextState();
}
}
return(false);
case MonolithState.FULL_OF_STARS_DRES3:
if (Time.fixedTime - stepTime > 5.5f)
{
// Done with Dres
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_DUNA1:
{
// Start between the sun and Duna
CelestialBody duna = FlightGlobals.Bodies.Where(b => b.name == "Duna").First();
CelestialBody sun = FlightGlobals.Bodies.Where(b => b.name == "Sun").First();
Vector3 sunnySide = sun.transform.position - duna.transform.position;
sunnySide.Normalize();
// Set us up a nice 4 radiuses away...
float distance = 4.0f * (float)duna.Radius;
starJeb.SetPosition(duna.transform.position + sunnySide * distance);
// Go straight at Duna
velocity = (duna.transform.position - starJeb.transform.position).normalized;
velocity *= distance / 3.0f;
LoggingUtil.LogDebug(this, "kick magnitude will be: " + velocity);
// Now offset him down so he doesn't actually hit Duna...
starJeb.SetPosition(starJeb.transform.position + new Vector3(0.0f, -((float)duna.Radius + 55000), 0.0f));
starJeb.SetWorldVelocity(duna.getRFrmVel(starJeb.transform.position));
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_DUNA2:
{
// Camera to target Duna - do this on a seperate update to allow KSP to catch up
CelestialBody duna = FlightGlobals.Bodies.Where(b => b.name == "Duna").First();
FlightCamera.SetTarget(starJeb.transform);
FlightCamera.fetch.SetCamCoordsFromPosition(starJeb.transform.position + (starJeb.transform.position - duna.transform.position).normalized * 25.0f);
// Make sure that the camera gets fixed
if (Time.fixedTime - stepTime > 0.1f)
{
nextState();
}
}
return(false);
case MonolithState.FULL_OF_STARS_DUNA3:
if (Time.fixedTime - stepTime > 5.5f)
{
// Done with Duna
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_EELOO1:
{
// Start perpendicular to the sun and Eeloo
CelestialBody eeloo = FlightGlobals.Bodies.Where(b => b.name == "Eeloo").First();
CelestialBody sun = FlightGlobals.Bodies.Where(b => b.name == "Sun").First();
Vector3 perp = eeloo.transform.position - sun.transform.position;
float tmp = perp.x;
perp.x = -perp.z;
perp.z = tmp;
perp.Normalize();
// Set us up a nice 4 radiuses away...
float distance = 4.0f * (float)eeloo.Radius;
starJeb.SetPosition(eeloo.transform.position + perp * distance);
// Determine which side the sun is on - makes for a better show
Vector3 sunnySide = sun.transform.position - eeloo.transform.position;
sunnySide.Normalize();
// Go straight at Eeloo
velocity = (eeloo.transform.position - starJeb.transform.position).normalized;
velocity *= distance / 3.0f;
LoggingUtil.LogDebug(this, "kick magnitude will be: " + velocity);
// Now offset him down so he doesn't actually hit Eeloo...
starJeb.SetPosition(starJeb.transform.position + sunnySide * ((float)eeloo.Radius * 1.5f));
starJeb.SetWorldVelocity(eeloo.getRFrmVel(starJeb.transform.position));
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_EELOO2:
{
// This time won't target directly towards Eeloo, as the player will have some idea
// what is up by now.
CelestialBody eeloo = FlightGlobals.Bodies.Where(b => b.name == "Eeloo").First();
CelestialBody sun = FlightGlobals.Bodies.Where(b => b.name == "Sun").First();
Vector3 awayFromSun = sun.transform.position - eeloo.transform.position;
awayFromSun.Normalize();
FlightCamera.SetTarget(starJeb.transform);
FlightCamera.fetch.SetCamCoordsFromPosition(starJeb.transform.position + awayFromSun * 50.0f);
// Make sure that the camera gets fixed
if (Time.fixedTime - stepTime > 0.1f)
{
nextState();
}
}
return(false);
case MonolithState.FULL_OF_STARS_EELOO3:
if (Time.fixedTime - stepTime > 5.5f)
{
velocity = null;
// Done with Eeloo
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_EVE1:
{
CelestialBody eve = FlightGlobals.Bodies.Where(b => b.name == "Eve").First();
Vector3 targetPosition = Destination.Value;
Vector3 normal = eve.GetSurfaceNVector(eveLatitude, eveLongitude);
startDistance = 10000000f;
Vector3 start = targetPosition + normal * startDistance;
starJeb.SetPosition(start);
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_EVE2:
{
// Camera straight towards Eve - we're going in!
CelestialBody eve = FlightGlobals.Bodies.Where(b => b.name == "Eve").First();
Vector3 awayFromEve = starJeb.transform.position - eve.transform.position;
awayFromEve.Normalize();
FlightCamera.SetTarget(starJeb.transform);
FlightCamera.fetch.SetCamCoordsFromPosition(starJeb.transform.position + awayFromEve * 15.0f);
// Make sure that the camera gets fixed
if (Time.fixedTime - stepTime > 0.1f)
{
nextState();
}
}
return(false);
case MonolithState.FULL_OF_STARS_EVE3:
// Wait until we've held the position for a split second
if (Time.fixedTime - stepTime >= 9.3f)
{
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_EVE4:
// Give the player a bit to get settled, then let the fun begins
if (Time.fixedTime - stepTime >= 15.0f)
{
// Spawn some asteroids
CelestialBody eve = FlightGlobals.Bodies.Where(b => b.name == "Eve").First();
ScenarioDiscoverableObjects asteroidSpawner = (ScenarioDiscoverableObjects)HighLogic.CurrentGame.scenarios.Find(
s => s.moduleRef is ScenarioDiscoverableObjects).moduleRef;
System.Random random = new System.Random();
// Spawn some more asteroids
for (int i = 0; i < ASTEROID_COUNT; i++)
{
asteroidSpawner.SpawnAsteroid();
}
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_EVE5:
// Wait a full second after spawning the asteroids - we're not allowed to pull
// them off rails until they've been active a bit
if (Time.fixedTime - stepTime > 1.0f)
{
// Spawn some asteroids
CelestialBody eve = FlightGlobals.Bodies.Where(b => b.name == "Eve").First();
System.Random random = new System.Random();
foreach (Vessel asteroid in FlightGlobals.Vessels.Where(v => v.vesselType == VesselType.SpaceObject).Reverse().Take(ASTEROID_COUNT))
{
// Set the position
double r = random.NextDouble() * 0.02 + 0.002;
double theta = random.NextDouble() * 2.0 * Math.PI;
double latitude = starJeb.latitude + r * Math.Sin(theta);
double longitude = starJeb.longitude + r * Math.Cos(theta);
double altitude = starJeb.altitude + 100 + random.NextDouble() * 200;
asteroid.SetPosition(eve.GetWorldSurfacePosition(latitude, longitude, altitude));
asteroid.ChangeWorldVelocity(asteroid.GetSrfVelocity());
asteroid.Load();
asteroid.GoOffRails();
}
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_EVE6:
{
// Determine if there's an asteroid about to kill us
CelestialBody eve = FlightGlobals.Bodies.Where(b => b.name == "Eve").First();
bool killerAsteroid = FlightGlobals.Vessels.Where(v => v.mainBody == eve && v.vesselType == VesselType.SpaceObject &&
Vector3.Distance(starJeb.transform.position, v.transform.position) < 5.5 * ((int)v.DiscoveryInfo.objectSize + 1)).Any();
if (killerAsteroid || Time.fixedTime - stepTime > 20.0f)
{
foreach (Vessel asteroid in FlightGlobals.Vessels.Where(v => v.vesselType == VesselType.SpaceObject).Reverse().Take(ASTEROID_COUNT))
{
asteroid.Die();
}
nextState();
}
}
return(false);
case MonolithState.FULL_OF_STARS_KERBIN1:
{
CheatOptions.NoCrashDamage = false;
// Start between the sun and Kerbin
CelestialBody kerbin = FlightGlobals.Bodies.Where(b => b.name == "Kerbin").First();
CelestialBody sun = FlightGlobals.Bodies.Where(b => b.name == "Sun").First();
Vector3 sunnySide = sun.transform.position - kerbin.transform.position;
sunnySide.Normalize();
// Set us up a nice 4 radiuses away...
float distance = 4.0f * (float)kerbin.Radius;
starJeb.SetPosition(kerbin.transform.position + sunnySide * distance);
// Orient him properly
KerbalEVA keva = starJeb.FindPartModulesImplementing <KerbalEVA>().First();
MethodInfo rotationMethod = typeof(KerbalEVA).GetMethod("correctGroundedRotation", BindingFlags.Instance | BindingFlags.NonPublic);
starJeb.packed = true;
rotationMethod.Invoke(keva, new object[] { });
starJeb.packed = false;
// Hardcode an orbital velocity, because it's late and I'm tired
starJeb.SetWorldVelocity(kerbin.getRFrmVel(starJeb.transform.position).normalized * 1085);
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_KERBIN2:
{
// Camera to target kerbin - do this on a seperate update to allow KSP to catch up
CelestialBody kerbin = FlightGlobals.Bodies.Where(b => b.name == "Kerbin").First();
FlightCamera.SetTarget(starJeb.transform);
FlightCamera.fetch.SetCamCoordsFromPosition(starJeb.transform.position + (starJeb.transform.position - kerbin.transform.position).normalized * 10.0f);
starJeb.SetRotation(FlightCamera.fetch.transform.rotation * Quaternion.AngleAxis(180.0f, FlightCamera.fetch.transform.up));
// Make sure that the camera gets fixed
if (Time.fixedTime - stepTime > 0.1f)
{
nextState();
}
}
return(false);
case MonolithState.FULL_OF_STARS_KERBIN3:
if (Time.fixedTime - stepTime > 2.0f)
{
// Turn into star jeb
CelestialBody kerbin = FlightGlobals.Bodies.Where(b => b.name == "Kerbin").First();
starJeb.vesselName = "The Star Jeb";
Undress(starJeb.gameObject);
FlightCamera.fetch.SetCamCoordsFromPosition(starJeb.transform.position + (starJeb.transform.position - kerbin.transform.position).normalized * 1.5f);
nextState();
}
return(false);
case MonolithState.FULL_OF_STARS_KERBIN4:
if (Time.fixedTime - stepTime < 15.0f)
{
CelestialBody kerbin = FlightGlobals.Bodies.Where(b => b.name == "Kerbin").First();
Vector3 camDirection = starJeb.transform.position + (starJeb.transform.position - kerbin.transform.position).normalized;
}
else
{
nextState();
monolith.Die();
monolith = null;
starJeb.Die();
starJeb = null;
Vessel discovery = ContractVesselTracker.Instance.GetAssociatedVessel("Discovery One");
FlightGlobals.ForceSetActiveVessel(discovery);
}
return(false);
case MonolithState.FULL_OF_STARS_FINAL:
nextState();
return(true);
default:
return(false);
}
}
private IEnumerable <bool> AddPatch(VesselState startingState, DescentProfile profile)
{
if (null == vessel_.patchedConicSolver)
{
UnityEngine.Debug.LogWarning("Trajectories: AddPatch() attempted when patchedConicsSolver is null; Skipping.");
yield break;
}
CelestialBody body = startingState.referenceBody;
var patch = new Patch();
patch.startingState = startingState;
patch.isAtmospheric = false;
patch.spaceOrbit = startingState.stockPatch ?? createOrbitFromState(startingState);
patch.endTime = patch.startingState.time + patch.spaceOrbit.period;
// the flight plan does not always contain the first patches (before the first maneuver node), so we populate it with the current orbit and associated encounters etc.
var flightPlan = new List <Orbit>();
for (var orbit = vessel_.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
{
if (vessel_.patchedConicSolver.flightPlan.Contains(orbit))
{
break;
}
flightPlan.Add(orbit);
}
foreach (var orbit in vessel_.patchedConicSolver.flightPlan)
{
flightPlan.Add(orbit);
}
Orbit nextStockPatch = null;
if (startingState.stockPatch != null)
{
int planIdx = flightPlan.IndexOf(startingState.stockPatch);
if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
{
nextStockPatch = flightPlan[planIdx + 1];
}
}
if (nextStockPatch != null)
{
patch.endTime = nextStockPatch.StartUT;
}
double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);
double minAltitude = patch.spaceOrbit.PeA;
if (patch.endTime < startingState.time + patch.spaceOrbit.timeToPe)
{
minAltitude = patch.spaceOrbit.getRelativePositionAtUT(patch.endTime).magnitude;
}
if (minAltitude < maxAtmosphereAltitude)
{
double entryTime;
if (startingState.position.magnitude <= body.Radius + maxAtmosphereAltitude)
{
// whole orbit is inside the atmosphere
entryTime = startingState.time;
}
else
{
// binary search of entry time in atmosphere
// I guess an analytic solution could be found, but I'm too lazy to search it
double from = startingState.time;
double to = from + patch.spaceOrbit.timeToPe;
int loopCount = 0;
while (to - from > 0.1)
{
++loopCount;
if (loopCount > 1000)
{
UnityEngine.Debug.Log("WARNING: infinite loop? (Trajectories.Trajectory.AddPatch, atmosphere limit search)");
++errorCount_;
break;
}
double middle = (from + to) * 0.5;
if (patch.spaceOrbit.getRelativePositionAtUT(middle).magnitude < body.Radius + maxAtmosphereAltitude)
{
to = middle;
}
else
{
from = middle;
}
}
entryTime = to;
}
if (entryTime > startingState.time + 0.1)
{
// add the space patch before atmospheric entry
patch.endTime = entryTime;
if (body.atmosphere)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)),
referenceBody = body,
time = entryTime,
velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime))
};
yield break;
}
else
{
// the body has no atmosphere, so what we actually computed is the impact on the body surface
// now, go back in time until the impact point is above the ground to take ground height in account
// we assume the ground is horizontal around the impact position
double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)), entryTime)) + body.Radius;
double iterationSize = 1.0;
while (entryTime > startingState.time + iterationSize && patch.spaceOrbit.getRelativePositionAtUT(entryTime).magnitude < groundAltitude)
{
entryTime -= iterationSize;
}
patch.endTime = entryTime;
patch.rawImpactPosition = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, entryTime);
patch.impactVelocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
}
else
{
if (patch.startingState.referenceBody != vessel_.mainBody)
{
// in current aerodynamic prediction code, we can't handle predictions for another body, so we stop here
AddPatch_outState = null;
yield break;
}
// simulate atmospheric flight (drag and lift), until landing (more likely to be a crash as we don't predict user piloting) or atmosphere exit (typically for an aerobraking maneuver)
// the simulation assumes a constant angle of attack
patch.isAtmospheric = true;
patch.startingState.stockPatch = null;
double dt = 0.1; // lower dt would be more accurate, but a tradeoff has to be found between performances and accuracy
int maxIterations = (int)(30.0 * 60.0 / dt); // some shallow entries can result in very long flight, for performances reasons, we limit the prediction duration
int chunkSize = 128;
double trajectoryInterval = 10.0; // time between two consecutive stored positions (more intermediate positions are computed for better accuracy), also used for ground collision checks
var buffer = new List <Point[]>();
buffer.Add(new Point[chunkSize]);
int nextPosIdx = 0;
Vector3d pos = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
Vector3d vel = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
Vector3d prevPos = pos - vel * dt;
//Util.PostSingleScreenMessage("initial vel", "initial vel = " + vel);
double currentTime = entryTime;
double lastPositionStoredUT = 0;
Vector3d lastPositionStored = new Vector3d();
bool hitGround = false;
int iteration = 0;
int incrementIterations = 0;
int minIterationsPerIncrement = maxIterations / Settings.fetch.MaxFramesPerPatch;
while (true)
{
++iteration;
++incrementIterations;
if (incrementIterations > minIterationsPerIncrement && incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
{
yield return(false);
incrementIterations = 0;
}
double R = pos.magnitude;
double altitude = R - body.Radius;
double atmosphereCoeff = altitude / maxAtmosphereAltitude;
if (hitGround || atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 || iteration == maxIterations || currentTime > patch.endTime)
{
if (hitGround || atmosphereCoeff <= 0.0)
{
patch.rawImpactPosition = pos;
patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, currentTime);
patch.impactVelocity = vel;
}
patch.endTime = Math.Min(currentTime, patch.endTime);
int totalCount = (buffer.Count - 1) * chunkSize + nextPosIdx;
patch.atmosphericTrajectory = new Point[totalCount];
int outIdx = 0;
foreach (var chunk in buffer)
{
foreach (var p in chunk)
{
if (outIdx == totalCount)
{
break;
}
patch.atmosphericTrajectory[outIdx++] = p;
}
}
if (iteration == maxIterations)
{
ScreenMessages.PostScreenMessage("WARNING: trajectory prediction stopped, too many iterations");
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
else if (atmosphereCoeff <= 0.0 || hitGround)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
else
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
position = pos,
velocity = vel,
referenceBody = body,
time = patch.endTime
};
yield break;
}
}
Vector3d gravityAccel = pos * (-body.gravParameter / (R * R * R));
//Util.PostSingleScreenMessage("prediction vel", "prediction vel = " + vel);
Vector3d airVelocity = vel - body.getRFrmVel(body.position + pos);
double angleOfAttack = profile.GetAngleOfAttack(body, pos, airVelocity);
Vector3d aerodynamicForce = aerodynamicModel_.GetForces(body, pos, airVelocity, angleOfAttack);
Vector3d acceleration = gravityAccel + aerodynamicForce / aerodynamicModel_.mass;
// acceleration in the vessel reference frame is acceleration - gravityAccel
maxAccelBackBuffer_ = Math.Max((float)(aerodynamicForce.magnitude / aerodynamicModel_.mass), maxAccelBackBuffer_);
//vel += acceleration * dt;
//pos += vel * dt;
// Verlet integration (more precise than using the velocity)
Vector3d ppos = prevPos;
prevPos = pos;
pos = pos + pos - ppos + acceleration * (dt * dt);
vel = (pos - prevPos) / dt;
currentTime += dt;
double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
if (currentTime >= lastPositionStoredUT + interval)
{
double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, pos, currentTime));
if (lastPositionStoredUT > 0)
{
// check terrain collision, to detect impact on mountains etc.
Vector3 rayOrigin = lastPositionStored;
Vector3 rayEnd = pos;
double absGroundAltitude = groundAltitude + body.Radius;
if (absGroundAltitude > rayEnd.magnitude)
{
hitGround = true;
float coeff = Math.Max(0.01f, (float)((absGroundAltitude - rayOrigin.magnitude) / (rayEnd.magnitude - rayOrigin.magnitude)));
pos = rayEnd * coeff + rayOrigin * (1.0f - coeff);
currentTime = currentTime * coeff + lastPositionStoredUT * (1.0f - coeff);
}
}
lastPositionStoredUT = currentTime;
if (nextPosIdx == chunkSize)
{
buffer.Add(new Point[chunkSize]);
nextPosIdx = 0;
}
Vector3d nextPos = pos;
if (Settings.fetch.BodyFixedMode)
{
nextPos = calculateRotatedPosition(body, nextPos, currentTime);
}
buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
buffer.Last()[nextPosIdx].orbitalVelocity = vel;
buffer.Last()[nextPosIdx].groundAltitude = (float)groundAltitude;
buffer.Last()[nextPosIdx].time = currentTime;
buffer.Last()[nextPosIdx++].pos = nextPos;
lastPositionStored = pos;
}
}
}
}
else
{
// no atmospheric entry, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextStockPatch != null)
{
AddPatch_outState = new VesselState
{
position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(patch.endTime)),
velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(patch.endTime)),
referenceBody = nextStockPatch == null ? body : nextStockPatch.referenceBody,
time = patch.endTime,
stockPatch = nextStockPatch
};
yield break;
}
else
{
AddPatch_outState = null;
yield break;
}
}
}
public void FixedUpdate()
{
if (HighLogic.LoadedScene != GameScenes.FLIGHT)
{
return;
}
double now = Planetarium.GetUniversalTime();
double dt = now - PreviousFrameTime;
if (dt > 0.5 || dt < 0.0)
{
Vector3d bodySpacePosition = new Vector3d();
Vector3d bodySpaceVelocity = new Vector3d();
if (aerodynamicModel_ != null && vessel_ != null)
{
CelestialBody body = vessel_.orbit.referenceBody;
bodySpacePosition = vessel_.GetWorldPos3D() - body.position;
bodySpaceVelocity = vessel_.obt_velocity;
double altitudeAboveSea = bodySpacePosition.magnitude - body.Radius;
Vector3d airVelocity = bodySpaceVelocity - body.getRFrmVel(body.position + bodySpacePosition);
#if DEBUG_COMPARE_FORCES
double R = PreviousFramePos.magnitude;
Vector3d gravityForce = PreviousFramePos * (-body.gravParameter / (R * R * R) * vessel_.totalMass);
Quaternion inverseRotationFix = body.inverseRotation ? Quaternion.AngleAxis((float)(body.angularVelocity.magnitude / Math.PI * 180.0 * dt), Vector3.up) : Quaternion.identity;
Vector3d TotalForce = (bodySpaceVelocity - inverseRotationFix * PreviousFrameVelocity) * (vessel_.totalMass / dt);
TotalForce += bodySpaceVelocity * (dt * 0.000015); // numeric precision fix
Vector3d ActualForce = TotalForce - gravityForce;
Transform vesselTransform = vessel_.ReferenceTransform;
Vector3d vesselBackward = (Vector3d)(-vesselTransform.up.normalized);
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = (Vector3d)(-vesselTransform.forward.normalized);
Vector3d vesselRight = Vector3d.Cross(vesselUp, vesselBackward).normalized;
double AoA = Math.Acos(Vector3d.Dot(airVelocity.normalized, vesselForward.normalized));
if (Vector3d.Dot(airVelocity, vesselUp) > 0)
{
AoA = -AoA;
}
VesselAerodynamicModel.DebugParts = true;
Vector3d referenceForce = aerodynamicModel_.ComputeForces(20000, new Vector3d(0, 0, 1500), new Vector3d(0, 1, 0), 0);
VesselAerodynamicModel.DebugParts = false;
Vector3d predictedForce = aerodynamicModel_.ComputeForces(altitudeAboveSea, airVelocity, vesselUp, AoA);
//VesselAerodynamicModel.Verbose = true;
Vector3d predictedForceWithCache = aerodynamicModel_.GetForces(body, bodySpacePosition, airVelocity, AoA);
//VesselAerodynamicModel.Verbose = false;
Vector3d localTotalForce = new Vector3d(Vector3d.Dot(TotalForce, vesselRight), Vector3d.Dot(TotalForce, vesselUp), Vector3d.Dot(TotalForce, vesselBackward));
Vector3d localActualForce = new Vector3d(Vector3d.Dot(ActualForce, vesselRight), Vector3d.Dot(ActualForce, vesselUp), Vector3d.Dot(ActualForce, vesselBackward));
Vector3d localPredictedForce = new Vector3d(Vector3d.Dot(predictedForce, vesselRight), Vector3d.Dot(predictedForce, vesselUp), Vector3d.Dot(predictedForce, vesselBackward));
Vector3d localPredictedForceWithCache = new Vector3d(Vector3d.Dot(predictedForceWithCache, vesselRight), Vector3d.Dot(predictedForceWithCache, vesselUp), Vector3d.Dot(predictedForceWithCache, vesselBackward));
Util.PostSingleScreenMessage("actual/predict comparison", "air vel=" + Math.Floor(airVelocity.magnitude) + " ; AoA=" + (AoA * 180.0 / Math.PI));
//Util.PostSingleScreenMessage("total force", "actual total force=" + localTotalForce.ToString("0.000"));
Util.PostSingleScreenMessage("actual force", "actual force=" + localActualForce.ToString("0.000"));
Util.PostSingleScreenMessage("predicted force", "predicted force=" + localPredictedForce.ToString("0.000"));
Util.PostSingleScreenMessage("predict with cache", "predicted force with cache=" + localPredictedForceWithCache.ToString("0.000"));
Util.PostSingleScreenMessage("reference force", "reference force=" + referenceForce.ToString("0.000"));
Util.PostSingleScreenMessage("current vel", "current vel=" + bodySpaceVelocity.ToString("0.00") + " (mag=" + bodySpaceVelocity.magnitude.ToString("0.00") + ")");
//Util.PostSingleScreenMessage("vel from pos", "vel from pos=" + ((bodySpacePosition - PreviousFramePos) / dt).ToString("0.000") + " (mag=" + ((bodySpacePosition - PreviousFramePos) / dt).magnitude.ToString("0.00") + ")");
Util.PostSingleScreenMessage("force diff", "force ratio=" + (localActualForce.z / localPredictedForce.z).ToString("0.000"));
Util.PostSingleScreenMessage("drag", "physics drag=" + vessel_.rootPart.rb.drag);
#endif
double approximateRho = StockAeroUtil.GetDensity(altitudeAboveSea, body);
double preciseRho = StockAeroUtil.GetDensity(vessel_.GetWorldPos3D(), body);
double actualRho = vessel_.atmDensity;
Util.PostSingleScreenMessage("rho info", /*"preciseRho=" + preciseRho.ToString("0.0000") + " ; " +*/ "rho=" + approximateRho.ToString("0.0000") + " ; actual=" + actualRho.ToString("0.0000") + " ; ratio=" + (actualRho / approximateRho).ToString("0.00"));
}
PreviousFrameVelocity = bodySpaceVelocity;
PreviousFramePos = bodySpacePosition;
PreviousFrameTime = now;
}
}
private static Vector2 GetCorrection()
{
var vessel = FlightGlobals.ActiveVessel;
if (vessel == null)
{
return(new Vector2(0, 0));
}
Vector3? targetPosition = Trajectory.Target.WorldPosition;
var patch = Trajectory.fetch.Patches.LastOrDefault();
CelestialBody body = Trajectory.Target.Body;
if (!targetPosition.HasValue || patch == null || !patch.ImpactPosition.HasValue || patch.StartingState.ReferenceBody != body || !patch.IsAtmospheric)
{
return(new Vector2(0, 0));
}
// Get impact position, or, if some point over the trajectory has not enough clearance, smoothly interpolate to that point depending on how much clearance is missing
Vector3 impactPosition = patch.ImpactPosition.Value;
foreach (var p in patch.AtmosphericTrajectory)
{
float neededClearance = 600.0f;
float missingClearance = neededClearance - (p.pos.magnitude - (float)body.Radius - p.groundAltitude);
if (missingClearance > 0.0f)
{
if (Vector3.Distance(p.pos, patch.RawImpactPosition.Value) > 3000.0f)
{
float coeff = missingClearance / neededClearance;
Vector3 rotatedPos = p.pos;
if (!Settings.fetch.BodyFixedMode)
{
rotatedPos = Trajectory.CalculateRotatedPosition(body, p.pos, p.time);
}
impactPosition = impactPosition * (1.0f - coeff) + rotatedPos * coeff;
}
break;
}
}
Vector3 right = Vector3.Cross(patch.ImpactVelocity.Value, impactPosition).normalized;
Vector3 behind = Vector3.Cross(right, impactPosition).normalized;
Vector3 offset = targetPosition.Value - impactPosition;
Vector2 offsetDir = new Vector2(Vector3.Dot(right, offset), Vector3.Dot(behind, offset));
offsetDir *= 0.00005f; // 20km <-> 1 <-> 45° (this is purely indicative, no physical meaning, it would be very complicated to compute an actual correction angle as it depends on the spacecraft behavior in the atmosphere ; a small angle will suffice for a plane, but even a big angle might do almost nothing for a rocket)
Vector3d pos = vessel.GetWorldPos3D() - body.position;
Vector3d vel = vessel.obt_velocity - body.getRFrmVel(body.position + pos); // air velocity
float plannedAngleOfAttack = (float)DescentProfile.fetch.GetAngleOfAttack(body, pos, vel);
if (plannedAngleOfAttack < Math.PI * 0.5f)
{
offsetDir.y = -offsetDir.y; // behavior is different for prograde or retrograde entry
}
float maxCorrection = 1.0f;
offsetDir.x = Mathf.Clamp(offsetDir.x, -maxCorrection, maxCorrection);
offsetDir.y = Mathf.Clamp(offsetDir.y, -maxCorrection, maxCorrection);
return(offsetDir);
}
internal static void DebugTelemetry()
{
if (!Util.IsFlight)
{
return;
}
double now = Planetarium.GetUniversalTime();
double dt = now - PreviousFrameTime;
if (dt > 0.5 || dt < 0.0)
{
Vector3d bodySpacePosition = new Vector3d();
Vector3d bodySpaceVelocity = new Vector3d();
if (aerodynamicModel_ != null && Trajectories.IsVesselAttached)
{
CelestialBody body = Trajectories.AttachedVessel.orbit.referenceBody;
bodySpacePosition = Trajectories.AttachedVessel.GetWorldPos3D() - body.position;
bodySpaceVelocity = Trajectories.AttachedVessel.obt_velocity;
double altitudeAboveSea = bodySpacePosition.magnitude - body.Radius;
Vector3d airVelocity = bodySpaceVelocity - body.getRFrmVel(body.position + bodySpacePosition);
double R = PreviousFramePos.magnitude;
Vector3d gravityForce = PreviousFramePos * (-body.gravParameter / (R * R * R) * Trajectories.AttachedVessel.totalMass);
Quaternion inverseRotationFix = body.inverseRotation ?
Quaternion.AngleAxis((float)(body.angularVelocity.magnitude / Math.PI * 180.0 * dt), Vector3.up)
: Quaternion.identity;
Vector3d TotalForce = (bodySpaceVelocity - inverseRotationFix * PreviousFrameVelocity) * (Trajectories.AttachedVessel.totalMass / dt);
TotalForce += bodySpaceVelocity * (dt * 0.000015); // numeric precision fix
Vector3d ActualForce = TotalForce - gravityForce;
Transform vesselTransform = Trajectories.AttachedVessel.ReferenceTransform;
Vector3d vesselBackward = (Vector3d)(-vesselTransform.up.normalized);
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = (Vector3d)(-vesselTransform.forward.normalized);
Vector3d vesselRight = Vector3d.Cross(vesselUp, vesselBackward).normalized;
double AoA = Math.Acos(Vector3d.Dot(airVelocity.normalized, vesselForward.normalized));
if (Vector3d.Dot(airVelocity, vesselUp) > 0)
{
AoA = -AoA;
}
VesselAerodynamicModel.DebugParts = true;
Vector3d referenceForce = aerodynamicModel_.ComputeForces(20000, new Vector3d(0, 0, 1500), new Vector3d(0, 1, 0), 0);
VesselAerodynamicModel.DebugParts = false;
Vector3d predictedForce = aerodynamicModel_.ComputeForces(altitudeAboveSea, airVelocity, vesselUp, AoA);
//VesselAerodynamicModel.Verbose = true;
Vector3d predictedForceWithCache = aerodynamicModel_.GetForces(body, bodySpacePosition, airVelocity, AoA);
//VesselAerodynamicModel.Verbose = false;
Vector3d localTotalForce = new Vector3d(
Vector3d.Dot(TotalForce, vesselRight),
Vector3d.Dot(TotalForce, vesselUp),
Vector3d.Dot(TotalForce, vesselBackward));
Vector3d localActualForce = new Vector3d(
Vector3d.Dot(ActualForce, vesselRight),
Vector3d.Dot(ActualForce, vesselUp),
Vector3d.Dot(ActualForce, vesselBackward));
Vector3d localPredictedForce = new Vector3d(
Vector3d.Dot(predictedForce, vesselRight),
Vector3d.Dot(predictedForce, vesselUp),
Vector3d.Dot(predictedForce, vesselBackward));
Vector3d localPredictedForceWithCache = new Vector3d(
Vector3d.Dot(predictedForceWithCache, vesselRight),
Vector3d.Dot(predictedForceWithCache, vesselUp),
Vector3d.Dot(predictedForceWithCache, vesselBackward));
Telemetry.Send("ut", now);
Telemetry.Send("altitude", Trajectories.AttachedVessel.altitude);
Telemetry.Send("airspeed", Math.Floor(airVelocity.magnitude));
Telemetry.Send("aoa", (AoA * 180.0 / Math.PI));
Telemetry.Send("force_actual", localActualForce.magnitude);
Telemetry.Send("force_actual.x", localActualForce.x);
Telemetry.Send("force_actual.y", localActualForce.y);
Telemetry.Send("force_actual.z", localActualForce.z);
//Telemetry.Send("force_total", localTotalForce.magnitude);
//Telemetry.Send("force_total.x", localTotalForce.x);
//Telemetry.Send("force_total.y", localTotalForce.y);
//Telemetry.Send("force_total.z", localTotalForce.z);
Telemetry.Send("force_predicted", localPredictedForce.magnitude);
Telemetry.Send("force_predicted.x", localPredictedForce.x);
Telemetry.Send("force_predicted.y", localPredictedForce.y);
Telemetry.Send("force_predicted.z", localPredictedForce.z);
Telemetry.Send("force_predicted_cache", localPredictedForceWithCache.magnitude);
//Telemetry.Send("force_predicted_cache.x", localPredictedForceWithCache.x);
//Telemetry.Send("force_predicted_cache.y", localPredictedForceWithCache.y);
//Telemetry.Send("force_predicted_cache.z", localPredictedForceWithCache.z);
//Telemetry.Send("force_reference", referenceForce.magnitude);
//Telemetry.Send("force_reference.x", referenceForce.x);
//Telemetry.Send("force_reference.y", referenceForce.y);
//Telemetry.Send("force_reference.z", referenceForce.z);
//Telemetry.Send("velocity.x", bodySpaceVelocity.x);
//Telemetry.Send("velocity.y", bodySpaceVelocity.y);
//Telemetry.Send("velocity.z", bodySpaceVelocity.z);
//Vector3d velocity_pos = (bodySpacePosition - PreviousFramePos) / dt;
//Telemetry.Send("velocity_pos.x", velocity_pos.x);
//Telemetry.Send("velocity_pos.y", velocity_pos.y);
//Telemetry.Send("velocity_pos.z", velocity_pos.z);
Telemetry.Send("drag", Trajectories.AttachedVessel.rootPart.rb.drag);
Telemetry.Send("density", Trajectories.AttachedVessel.atmDensity);
Telemetry.Send("density_calc", StockAeroUtil.GetDensity(altitudeAboveSea, body));
Telemetry.Send("density_calc_precise", StockAeroUtil.GetDensity(Trajectories.AttachedVessel.GetWorldPos3D(), body));
Telemetry.Send("temperature", Trajectories.AttachedVessel.atmosphericTemperature);
Telemetry.Send("temperature_calc", StockAeroUtil.GetTemperature(Trajectories.AttachedVessel.GetWorldPos3D(), body));
}
PreviousFrameVelocity = bodySpaceVelocity;
PreviousFramePos = bodySpacePosition;
PreviousFrameTime = now;
}
}
