/// <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 (!vessel_.mainBody.atmosphere)
{
return(Vector3d.zero);
}
if (altitude >= body_.atmosphereDepth)
{
return(Vector3d.zero);
}
Transform vesselTransform = vessel_.ReferenceTransform;
// 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 = (Vector3d)(-vesselTransform.up.normalized);
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = (Vector3d)(-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))
{
Debug.Log(string.Format("Trajectories: WARNING: {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))
{
Debug.Log("Trajectories: res is NaN (altitude=" + altitude + ", airVelocity=" + airVelocity.magnitude + ", angleOfAttack=" + 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);
}
public void Dispose()
{
Profiler.Stop(name);
}
//*******************************************************
public static Vector3 SimAeroForce(Vessel _vessel, Vector3 v_wrld_vel, double altitude, double latitude = 0.0)
{
Profiler.Start("SimAeroForce");
CelestialBody body = _vessel.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 < _vessel.Parts.Count; ++i)
{
// need checks on shielded components
Part p = _vessel.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
Vector3 sim_dragVectorDir = v_wrld_vel.normalized;
Vector3 sim_dragVectorDirLocal = -(p.transform.InverseTransformDirection(sim_dragVectorDir));
Vector3 liftForce = new Vector3(0, 0, 0);
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();
float 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)
{
cubes.SetDrag(sim_dragVectorDirLocal, (float)mach);
cubes.ForceUpdate(true, true);
cubes.AddSurfaceDragDirection(-sim_dragVectorDirLocal, (float)mach, ref p_drag_data);
//Debug.Log(String.Format("Trajectories: Caught NRE on Drag Initialization. Should be fixed now. {0}", e));
}
float pseudoreynolds = (float)(rho * Mathf.Abs(v_wrld_vel.magnitude));
float pseudoredragmult = PhysicsGlobals.DragCurvePseudoReynolds.Evaluate(pseudoreynolds);
drag = p_drag_data.areaDrag * PhysicsGlobals.DragCubeMultiplier * pseudoredragmult;
liftForce = p_drag_data.liftForce;
}
double sim_dragScalar = dyn_pressure * (double)drag * PhysicsGlobals.DragMultiplier;
dragForce = -(Vector3d)sim_dragVectorDir * sim_dragScalar;
break;
case Part.DragModel.SPHERICAL:
dragForce = -(Vector3d)sim_dragVectorDir * (double)p.maximum_drag;
break;
case Part.DragModel.CYLINDRICAL:
dragForce = -(Vector3d)sim_dragVectorDir * (double)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 = -(Vector3d)sim_dragVectorDir * (double)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 = new Vector3d();
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);
}
public ProfileScope(string name)
{
this.name = name;
Profiler.Start(name);
}