//*******************************************************
public static Vector3d SimAeroForce(Vector3d v_wrld_vel, double altitude, double latitude = 0.0)
{
Profiler.Start("SimAeroForce");
CelestialBody body = Trajectories.AttachedVessel.mainBody;
double pressure = body.GetPressure(altitude);
// Lift and drag for force accumulation.
Vector3d total_lift = Vector3d.zero;
Vector3d total_drag = Vector3d.zero;
// dynamic pressure for standard drag equation
double rho = GetDensity(altitude, body);
double dyn_pressure = 0.0005 * rho * v_wrld_vel.sqrMagnitude;
if (rho <= 0)
{
return(Vector3.zero);
}
double soundSpeed = body.GetSpeedOfSound(pressure, rho);
double mach = v_wrld_vel.magnitude / soundSpeed;
if (mach > 25.0)
{
mach = 25.0;
}
// Loop through all parts, accumulating drag and lift.
for (int i = 0; i < Trajectories.AttachedVessel.Parts.Count; ++i)
{
// need checks on shielded components
Part p = Trajectories.AttachedVessel.Parts[i];
#if DEBUG
TrajectoriesDebug partDebug = VesselAerodynamicModel.DebugParts ? p.FindModuleImplementing <TrajectoriesDebug>() : null;
if (partDebug != null)
{
partDebug.Drag = 0;
partDebug.Lift = 0;
}
#endif
if (p.ShieldedFromAirstream || p.Rigidbody == null)
{
continue;
}
// Get Drag
Vector3d sim_dragVectorDir = v_wrld_vel.normalized;
Vector3d sim_dragVectorDirLocal = -(p.transform.InverseTransformDirection(sim_dragVectorDir));
Vector3d liftForce = Vector3d.zero;
Vector3d dragForce;
Profiler.Start("SimAeroForce#drag");
switch (p.dragModel)
{
case Part.DragModel.DEFAULT:
case Part.DragModel.CUBE:
DragCubeList cubes = p.DragCubes;
DragCubeList.CubeData p_drag_data = new DragCubeList.CubeData();
double drag;
if (cubes.None) // since 1.0.5, some parts don't have drag cubes (for example fuel lines and struts)
{
drag = p.maximum_drag;
}
else
{
try
{
cubes.AddSurfaceDragDirection(-sim_dragVectorDirLocal, (float)mach, ref p_drag_data);
}
catch (Exception e)
{
cubes.SetDrag(sim_dragVectorDirLocal, (float)mach);
cubes.ForceUpdate(true, true);
cubes.AddSurfaceDragDirection(-sim_dragVectorDirLocal, (float)mach, ref p_drag_data);
Util.DebugLogError("Exception {0} on drag initialization", e);
}
double pseudoreynolds = rho * Math.Abs(v_wrld_vel.magnitude);
double pseudoredragmult = PhysicsGlobals.DragCurvePseudoReynolds.Evaluate((float)pseudoreynolds);
drag = p_drag_data.areaDrag * PhysicsGlobals.DragCubeMultiplier * pseudoredragmult;
liftForce = p_drag_data.liftForce;
}
double sim_dragScalar = dyn_pressure * drag * PhysicsGlobals.DragMultiplier;
dragForce = -sim_dragVectorDir * sim_dragScalar;
break;
case Part.DragModel.SPHERICAL:
dragForce = -sim_dragVectorDir * p.maximum_drag;
break;
case Part.DragModel.CYLINDRICAL:
dragForce = -sim_dragVectorDir *Mathf.Lerp(p.minimum_drag, p.maximum_drag, Mathf.Abs(Vector3.Dot(p.partTransform.TransformDirection(p.dragReferenceVector), sim_dragVectorDir)));
break;
case Part.DragModel.CONIC:
dragForce = -sim_dragVectorDir *Mathf.Lerp(p.minimum_drag, p.maximum_drag, Vector3.Angle(p.partTransform.TransformDirection(p.dragReferenceVector), sim_dragVectorDir) / 180f);
break;
default:
// no drag to apply
dragForce = Vector3d.zero;
break;
}
Profiler.Stop("SimAeroForce#drag");
#if DEBUG
if (partDebug != null)
{
partDebug.Drag += (float)dragForce.magnitude;
}
#endif
total_drag += dragForce;
// If it isn't a wing or lifter, get body lift.
if (!p.hasLiftModule)
{
Profiler.Start("SimAeroForce#BodyLift");
float simbodyLiftScalar = p.bodyLiftMultiplier * PhysicsGlobals.BodyLiftMultiplier * (float)dyn_pressure;
simbodyLiftScalar *= PhysicsGlobals.GetLiftingSurfaceCurve("BodyLift").liftMachCurve.Evaluate((float)mach);
Vector3 bodyLift = p.transform.rotation * (simbodyLiftScalar * liftForce);
bodyLift = Vector3.ProjectOnPlane(bodyLift, sim_dragVectorDir);
// Only accumulate forces for non-LiftModules
total_lift += bodyLift;
Profiler.Stop("SimAeroForce#BodyLift");
}
Profiler.Start("SimAeroForce#LiftingSurface");
// Find ModuleLifingSurface for wings and liftforce.
// Should catch control surface as it is a subclass
for (int j = 0; j < p.Modules.Count; ++j)
{
var m = p.Modules[j];
float mcs_mod;
if (m is ModuleLiftingSurface)
{
mcs_mod = 1.0f;
double liftQ = dyn_pressure * 1000;
ModuleLiftingSurface wing = (ModuleLiftingSurface)m;
Vector3 nVel = Vector3.zero;
Vector3 liftVector = Vector3.zero;
float liftdot;
float absdot;
wing.SetupCoefficients(v_wrld_vel, out nVel, out liftVector, out liftdot, out absdot);
double prevMach = p.machNumber;
p.machNumber = mach;
Vector3 local_lift = mcs_mod * wing.GetLiftVector(liftVector, liftdot, absdot, liftQ, (float)mach);
Vector3 local_drag = mcs_mod * wing.GetDragVector(nVel, absdot, liftQ);
p.machNumber = prevMach;
total_lift += local_lift;
total_drag += local_drag;
#if DEBUG
if (partDebug != null)
{
partDebug.Lift += (float)local_lift.magnitude;
partDebug.Drag += (float)local_drag.magnitude;
}
#endif
}
}
Profiler.Stop("SimAeroForce#LiftingSurface");
}
// RETURN STUFF
Vector3 force = total_lift + total_drag;
Profiler.Stop("SimAeroForce");
return(force);
}
private static IEnumerable <bool> AddPatch(VesselState startingState)
{
if (null == Trajectories.AttachedVessel.patchedConicSolver)
{
Util.LogWarning("patchedConicsSolver is null, Skipping.");
yield break;
}
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 = Trajectories.AttachedVessel.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
{
if (Trajectories.AttachedVessel.patchedConicSolver.flightPlan.Contains(orbit))
{
break;
}
flightPlan.Add(orbit);
}
foreach (Orbit orbit in Trajectories.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 = 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 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, Util.SwapYZ(pos), t))
+ body.Radius;
if (pos.magnitude < groundAltitude)
{
t -= iterationSize;
groundImpact = true;
break;
}
}
if (groundImpact)
{
patch.EndTime = t;
patch.RawImpactPosition = Util.SwapYZ(patch.SpaceOrbit.getRelativePositionAtUT(t));
patch.ImpactPosition = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, t);
patch.ImpactVelocity = Util.SwapYZ(patch.SpaceOrbit.getOrbitalVelocityAtUT(t));
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
else
{
// no impact, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextPatch != null)
{
AddPatch_outState = new VesselState
{
Position = Util.SwapYZ(patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime)),
Velocity = Util.SwapYZ(patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime)),
ReferenceBody = nextPatch == null ? body : nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
yield break;
}
else
{
AddPatch_outState = null;
yield break;
}
}
}
}
else
{
if (patch.StartingState.ReferenceBody != Trajectories.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 = Util.SwapYZ(patch.SpaceOrbit.getRelativePositionAtUT(entryTime));
state.velocity = Util.SwapYZ(patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime));
// 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
Func <Vector3d, Vector3d, Vector3d> accelerationFunc = (position, 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;
Profiler.Stop("accelerationFunc inside");
return(accel);
};
#endregion
#region Integration Loop
while (true)
{
++iteration;
++incrementIterations;
if (incrementIterations > minIterationsPerIncrement && incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
{
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;
}
else if (atmosphereCoeff <= 0.0 || hitGround)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
else
{
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 = Util.SwapYZ(patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime)),
Velocity = Util.SwapYZ(patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime)),
ReferenceBody = nextPatch == null ? body : nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
yield break;
}
else
{
AddPatch_outState = null;
yield break;
}
}
}
public void Dispose()
{
Profiler.Stop(name);
}
/// <summary>
/// Compute the aerodynamic forces that would be applied to the vessel if it was in the specified situation (air velocity, altitude and angle of attack).
/// </summary>
/// <returns>The computed aerodynamic forces in world space</returns>
public Vector3d ComputeForces(double altitude, Vector3d airVelocity, Vector3d vup, double angleOfAttack)
{
Profiler.Start("ComputeForces");
if (!Trajectories.IsVesselAttached || !Trajectories.AttachedVessel.mainBody.atmosphere || altitude >= body_.atmosphereDepth)
{
return(Vector3d.zero);
}
Transform vesselTransform = Trajectories.AttachedVessel.ReferenceTransform;
if (vesselTransform == null)
{
return(Vector3d.zero);
}
// this is weird, but the vessel orientation does not match the reference transform (up is forward), this code fixes it but I don't know if it'll work in all cases
Vector3d vesselBackward = -vesselTransform.up.normalized;
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = -vesselTransform.forward.normalized;
Vector3d vesselRight = Vector3d.Cross(vesselUp, vesselBackward).normalized;
Vector3d airVelocityForFixedAoA = (vesselForward * Math.Cos(-angleOfAttack) + vesselUp * Math.Sin(-angleOfAttack)) * airVelocity.magnitude;
Vector3d totalForce = ComputeForces_Model(airVelocityForFixedAoA, altitude);
if (double.IsNaN(totalForce.x) || double.IsNaN(totalForce.y) || double.IsNaN(totalForce.z))
{
Util.LogWarning("{0} totalForce is NaN (altitude={1}, airVelocity={2}, angleOfAttack={3}", AerodynamicModelName, altitude, airVelocity.magnitude, angleOfAttack);
return(Vector3d.zero); // Don't send NaN into the simulation as it would cause bad things (infinite loops, crash, etc.). I think this case only happens at the atmosphere edge, so the total force should be 0 anyway.
}
// convert the force computed by the model (depends on the current vessel orientation, which is irrelevant for the prediction) to the predicted vessel orientation (which depends on the predicted velocity)
Vector3d localForce = new Vector3d(Vector3d.Dot(vesselRight, totalForce), Vector3d.Dot(vesselUp, totalForce), Vector3d.Dot(vesselBackward, totalForce));
//if (Double.IsNaN(localForce.x) || Double.IsNaN(localForce.y) || Double.IsNaN(localForce.z))
// throw new Exception("localForce is NAN");
Vector3d velForward = airVelocity.normalized;
Vector3d velBackward = -velForward;
Vector3d velRight = Vector3d.Cross(vup, velBackward);
if (velRight.sqrMagnitude < 0.001)
{
velRight = Vector3d.Cross(vesselUp, velBackward);
if (velRight.sqrMagnitude < 0.001)
{
velRight = Vector3d.Cross(vesselBackward, velBackward).normalized;
}
else
{
velRight = velRight.normalized;
}
}
else
{
velRight = velRight.normalized;
}
Vector3d velUp = Vector3d.Cross(velBackward, velRight).normalized;
Vector3d predictedVesselForward = velForward * Math.Cos(angleOfAttack) + velUp * Math.Sin(angleOfAttack);
Vector3d predictedVesselBackward = -predictedVesselForward;
Vector3d predictedVesselRight = velRight;
Vector3d predictedVesselUp = Vector3d.Cross(predictedVesselBackward, predictedVesselRight).normalized;
Vector3d res = predictedVesselRight * localForce.x + predictedVesselUp * localForce.y + predictedVesselBackward * localForce.z;
if (double.IsNaN(res.x) || double.IsNaN(res.y) || double.IsNaN(res.z))
{
Util.LogWarning("{0} res is NaN (altitude={1}, airVelocity={2}, angleOfAttack={3}", AerodynamicModelName, altitude, airVelocity.magnitude, angleOfAttack);
return(new Vector3d(0, 0, 0)); // Don't send NaN into the simulation as it would cause bad things (infinite loops, crash, etc.). I think this case only happens at the atmosphere edge, so the total force should be 0 anyway.
}
Profiler.Stop("ComputeForces");
return(res);
}