//*******************************************************
public static Vector3 SimAeroForce(Vessel _vessel, Vector3 v_wrld_vel, double altitude, double latitude = 0)
{
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 (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;
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;
}
total_drag += dragForce;
// If it isn't a wing or lifter, get body lift.
if (!p.hasLiftModule)
{
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
//Debug.Log("[Trajectories] bodyLift=" + bodyLift);
total_lift += bodyLift;
}
// 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;
}
}
}
// RETURN STUFF
//Debug.Log("[Trajectories] total_lift" + total_lift + " total_drag=" + total_drag);
Vector3 force = total_lift + total_drag;
return(force);
}
public static Vector3 get_part_torque(CenterOfLiftQuery qry, Part p, Vector3 CoM, ref float lift, ref float drag)
{
if (p == null || (p.Rigidbody != p.rb) && !PhysicsGlobals.ApplyDragToNonPhysicsParts)
{
return(Vector3.zero);
}
Vector3 lift_pos = Vector3.zero;
Vector3 drag_pos = Vector3.zero;
if (!p.ShieldedFromAirstream)
{
var providers = p.FindModulesImplementing <ModuleLiftingSurface>();
if ((providers != null) && providers.Count > 0)
{
p.hasLiftModule = true;
}
Vector3 res = Vector3.zero;
if (p.hasLiftModule && providers[0] is ModuleControlSurface)
{
p.DragCubes.SetCubeWeight("neutral", 1.5f);
p.DragCubes.SetCubeWeight("fullDeflectionPos", 0.0f);
p.DragCubes.SetCubeWeight("fullDeflectionNeg", 0.0f);
}
// drag from drag-cubes
if (!p.DragCubes.None)
{
Vector3 drag_force = Vector3.zero;
p.dragVector = qry.refVector;
p.dragVectorSqrMag = p.dragVector.sqrMagnitude;
p.dragVectorMag = Mathf.Sqrt(p.dragVectorSqrMag);
p.dragVectorDir = p.dragVector / p.dragVectorMag;
p.dragVectorDirLocal = -p.partTransform.InverseTransformDirection(p.dragVectorDir);
p.dynamicPressurekPa = qry.refAirDensity * 0.0005 * p.dragVectorSqrMag;
if (p.rb != p.Rigidbody && PhysicsGlobals.ApplyDragToNonPhysicsPartsAtParentCoM)
{
drag_pos = p.Rigidbody.worldCenterOfMass;
lift_pos = drag_pos;
}
else
{
lift_pos = p.partTransform.TransformPoint(p.CoLOffset);
drag_pos = p.partTransform.TransformPoint(p.CoPOffset);
}
p.DragCubes.SetDrag(p.dragVectorDirLocal, mach);
float pseudoreynolds = (float)(density * Mathf.Abs(speed));
float pseudoredragmult = PhysicsGlobals.DragCurvePseudoReynolds.Evaluate(pseudoreynolds);
float drag_k = p.DragCubes.AreaDrag * PhysicsGlobals.DragCubeMultiplier * pseudoredragmult;
p.dragScalar = (float)(p.dynamicPressurekPa * drag_k * PhysicsGlobals.DragMultiplier);
drag_force = p.dragScalar * -p.dragVectorDir;
res += Vector3.Cross(drag_force, drag_pos - CoM);
Vector3 sum_force = drag_force;
drag += Vector3.Dot(sum_force, -p.dragVectorDir);
}
if (!p.hasLiftModule)
{
// stock aero lift
if (!p.DragCubes.None)
{
p.bodyLiftScalar = (float)(p.dynamicPressurekPa * p.bodyLiftMultiplier * PhysicsGlobals.BodyLiftMultiplier *
CorrectCoL.CoLMarkerFull.lift_curves.liftMachCurve.Evaluate(mach));
Vector3 lift_force = p.partTransform.rotation * (p.bodyLiftScalar * p.DragCubes.LiftForce);
lift_force = Vector3.ProjectOnPlane(lift_force, -p.dragVectorDir);
res += Vector3.Cross(lift_force, lift_pos - CoM);
Vector3 sum_force = lift_force;
lift += Vector3.Dot(sum_force, Vector3.Cross(p.dragVectorDir, EditorLogic.RootPart.transform.right).normalized);
}
return(res);
}
else
{
double q = 0.5 * qry.refAirDensity * qry.refVector.sqrMagnitude;
for (int i = 0; i < providers.Count; i++)
{
Vector3 dragvect;
Vector3 liftvect;
Vector3 lift_force = Vector3.zero;
Vector3 drag_force = Vector3.zero;
float abs;
ModuleLiftingSurface lsurf = providers[i];
ModuleControlSurface csurf = lsurf as ModuleControlSurface;
lsurf.SetupCoefficients(qry.refVector, out dragvect, out liftvect, out lsurf.liftDot, out abs);
lift_pos = p.partTransform.TransformPoint(p.CoLOffset);
drag_pos = p.partTransform.TransformPoint(p.CoPOffset);
lift_force = lsurf.GetLiftVector(liftvect, lsurf.liftDot, abs, q, mach);
if (lsurf.useInternalDragModel)
{
drag_force = lsurf.GetDragVector(dragvect, abs, q);
}
if (csurf != null)
{
float deflection = (float)deflection_field.GetValue(csurf);
Quaternion incidence = Quaternion.AngleAxis(csurf.ctrlSurfaceRange * deflection, p.partTransform.rotation * Vector3.right);
liftvect = incidence * liftvect;
lsurf.liftDot = Vector3.Dot(dragvect, liftvect);
abs = Mathf.Abs(lsurf.liftDot);
lift_force = lift_force * (1.0f - csurf.ctrlSurfaceArea);
lift_force += lsurf.GetLiftVector(liftvect, lsurf.liftDot, abs, q, mach) * csurf.ctrlSurfaceArea;
if (csurf.useInternalDragModel)
{
drag_force = drag_force * (1.0f - csurf.ctrlSurfaceArea);
drag_force += csurf.GetDragVector(dragvect, abs, q) * csurf.ctrlSurfaceArea;
}
}
res += Vector3.Cross(lift_force, lift_pos - CoM);
res += Vector3.Cross(drag_force, drag_pos - CoM);
Vector3 result_force = lift_force + drag_force;
lift += Vector3.Dot(result_force, Vector3.Cross(qry.refVector, EditorLogic.RootPart.transform.right).normalized);
drag += Vector3.Dot(result_force, -qry.refVector.normalized);
}
return(res);
}
}
return(Vector3.zero);
}
//Retrieve the vector sum of the lift and drag forces on the part when wind is present.
public List <Vector3> GetDragLiftForce(ModularFI.ModularFlightIntegrator fi, Part p, Vector3d vel, Vector3d windVec, double mach, bool isgrounded)
{
//Initialize forces
Vector3 total_drag = Vector3d.zero;
Vector3 total_lift = Vector3d.zero;
List <Vector3> total = new List <Vector3>();
//Set Part drag based on new air velocity
p.dragVector = vel;
p.dragVectorSqrMag = p.dragVector.sqrMagnitude;
//Update dynamic pressure
p.dynamicPressurekPa = 0.0005 * p.atmDensity * p.dragVectorSqrMag;
//If part darg is near zero or part is shielded do not calculate force sum
if (p.dragVectorSqrMag.NearlyEqual(0) || p.ShieldedFromAirstream)
{
p.dragVectorMag = 0f;
p.dragVectorDir = Vector3.zero;
p.dragVectorDirLocal = Vector3.zero;
p.dragScalar = 0f;
}
//Update drag vector
p.dragVectorMag = (float)Math.Sqrt(p.dragVectorSqrMag);
p.dragVectorDir = p.dragVector / p.dragVectorMag;
p.dragVectorDirLocal = -p.transform.InverseTransformDirection(p.dragVectorDir);
if (((!p.DragCubes.None) && (!p.hasLiftModule)) || (p.name.Contains("kerbalEVA")))
{
//Apply drag vector direction to drag cube
Vector3 drag_force = Vector3.zero;
//Update angular drag (adapated from FAR)
p.DragCubes.SetDrag(p.dragVectorDirLocal, (float)mach);
//Calculate drag force and set dragScalar (adapated from Trajectories mod)
double pseudoreynolds = p.atmDensity * Math.Abs(vel.magnitude);
double pseudoReDragMult = PhysicsGlobals.DragCurvePseudoReynolds.Evaluate((float)pseudoreynolds);
double drag = p.DragCubes.AreaDrag * PhysicsGlobals.DragCubeMultiplier * pseudoReDragMult;
p.dragScalar = (float)(p.dynamicPressurekPa * drag * PhysicsGlobals.DragMultiplier);
//Add drag force to total drag
drag_force = -p.dragVectorDir * p.dragScalar;
total_drag += drag_force;
//Estimate lift force from body lift
p.bodyLiftScalar = (float)(p.dynamicPressurekPa * p.bodyLiftMultiplier * PhysicsGlobals.BodyLiftMultiplier) * PhysicsGlobals.GetLiftingSurfaceCurve("BodyLift").liftMachCurve.Evaluate((float)mach);
Vector3 lforce = p.transform.rotation * (p.bodyLiftScalar * p.DragCubes.LiftForce);
lforce = Vector3.ProjectOnPlane(lforce, -p.dragVectorDir);
//Add lift force if vessel is on the ground
total_lift += lforce;
}
for (int j = 0; j < p.Modules.Count; ++j)
{
var m = p.Modules[j];
if (m is ModuleLiftingSurface)
{
//Initialize force vectors
Vector3 lforce3 = Vector3.zero;
Vector3 dforce3 = Vector3.zero;
//Convert kPa to Pa
double liftQ = p.dynamicPressurekPa * 1000;
ModuleLiftingSurface wing = (ModuleLiftingSurface)m;
//Get wing coefficients
Vector3 nVel = Vector3.zero;
Vector3 liftVector = Vector3.zero;
float liftdot;
float absdot;
wing.SetupCoefficients(vel, out nVel, out liftVector, out liftdot, out absdot);
//Get Lift/drag force
lforce3 = wing.GetLiftVector(liftVector, liftdot, absdot, liftQ, (float)mach);
dforce3 = wing.GetDragVector(nVel, absdot, liftQ);
//wing.dragForce = dforce3;
if (p.name.Contains("kerbalEVA"))
{
continue;
}
total_drag += dforce3;
if (isgrounded)
{
total_lift += lforce3;
}
}
}
total.Add(total_drag);
total.Add(total_lift);
return(total);
}
//Update aerodynamics
//Adapted from KSP Trajectories, Kerbal Wind Tunnel, FAR, and AeroGUI
void UpdateAerodynamics(ModularFI.ModularFlightIntegrator fi, Part part)
{
Vessel v = FlightGlobals.ActiveVessel;
wx_enabled = Util.getWindBool(); //Is weather enabled?
use_climo = Util.useCLIM(); //Are we using the climatology
use_point = Util.useWX(); //Are we using MPAS point time-series
//Get main vessel and get reference to Kerbin (i.e. celestial body)
kerbin = Util.getbody();
//Get wind vector (initialize to zero)
Vector3d windVec = Vector3d.zero;
if (use_climo)
{
//Retrieve wind vector from climatology
windVec = KerbalWxClimo.getWSWind(); // .GetWind(FlightGlobals.currentMainBody, part, rb.position);
}
else if (use_point)
{
//Retrieve wind vector from point forecast
windVec = KerbalWxPoint.getWSWind(); // .GetWind(FlightGlobals.currentMainBody, part, rb.position);
}
//Check to see if root part is in list. If not do not perform aero update.
//This avoids updating aerodynamics of parts that have been decoupled and are no longer part of the active vessel.
bool hasPart = false;
for (int i = 0; i < fi.PartThermalDataCount; i++)
{
PartThermalData pttd = fi.partThermalDataList[i];
Part ppart = pttd.part;
if (ppart == v.rootPart)
{
hasPart = true;
}
}
if (!hasPart)
{
return;
}
//Get position of current vessel
double vheight = v.altitude;
double vlat = v.latitude;
double vlng = v.longitude;
double air_pressure; double air_density; double soundSpeed;
//Define gamma (i.e. ratio between specific heat of dry air at constant pressure and specific heat of dry air at constant volume: cp/cv).
double gamma = 1.4;
//Util.Log("MFI wx_enabled: " + wx_enabled+", use_point: "+use_point+", use_climo: "+use_climo);
bool inatmos = false;
if ((FlightGlobals.ActiveVessel.mainBody == kerbin) && (v.altitude <= 70000) && (wx_enabled))
{
if (use_climo)
{
//Retrieve air pressure/density at vessel location
climate_api.wx_aero ptd = _clim_api.getPTD(vlat, vlng, vheight);
air_density = ptd.density;
air_pressure = ptd.pressure;
}
else
{
//Retrieve air pressure/density at vessel location
weather_api.wx_aero ptd = _wx_api.getPTD(vheight);
air_density = ptd.density;
air_pressure = ptd.pressure;
}
//Compute speed of sound based on air pressure and air density.
soundSpeed = Math.Sqrt(gamma * (air_pressure / air_density));
inatmos = true;
}
else
{
soundSpeed = v.speedOfSound;
}
//Get height from surface
double hsfc = v.heightFromTerrain;
//Determine if vessel is grounded (i.e. on the ground)
bool isgrounded = true;
if (hsfc >= 10)
{
isgrounded = false;
}
//If FAR is present FAR will handle aerodynamic updates or if we're not on Kerbin (i.e. in a different atmosphere)
if (part.Modules.Contains <ModuleAeroSurface>() || (part.Modules.Contains("MissileLauncher") && part.vessel.rootPart == part) || (haveFAR) || (v.mainBody != kerbin) || (!inatmos))
{
fi.BaseFIUpdateAerodynamics(part);
if (((!part.DragCubes.None) && (!part.hasLiftModule)) || (part.name.Contains("kerbalEVA")))
{
double drag = part.DragCubes.AreaDrag * PhysicsGlobals.DragCubeMultiplier * fi.pseudoReDragMult;
float pscal = (float)(part.dynamicPressurekPa * drag * PhysicsGlobals.DragMultiplier);
//Estimate lift force from body lift
Vector3 lforce = part.transform.rotation * (part.bodyLiftScalar * part.DragCubes.LiftForce);
lforce = Vector3.ProjectOnPlane(lforce, -part.dragVectorDir);
}
return;
}
else
{
fi.BaseFIUpdateAerodynamics(part); //Run base aerodynamic update first (fix bug where parachutes experience multiplicative acceleration)
// Get rigid body
Rigidbody rb = part.rb;
if (rb)
{
if (part == v.rootPart)
{
v.mach = 0;
}
//Retrieve wind vector at vessel location
//Vector3d windVec = KWPWind.GetWind(FlightGlobals.currentMainBody, part, rb.position);
//Util.Log("MFI windVec: " + windVec + ", use_point: " + use_point + ", use_climo: " + use_climo);
//Retrieve world velocity without wind vector
Vector3 velocity_nowind = rb.velocity + Krakensbane.GetFrameVelocity();
//Compute mach number
double mach_nowind = velocity_nowind.magnitude / soundSpeed;
Vector3 drag_sum = Vector3.zero;
Vector3 lift_sum = Vector3.zero;
if (((!part.DragCubes.None) && (!part.hasLiftModule)) || (part.name.Contains("kerbalEVA")))
{
//Estimate Drag force
double pseudoreynolds = part.atmDensity * Math.Abs(velocity_nowind.magnitude);
double pseudoReDragMult = PhysicsGlobals.DragCurvePseudoReynolds.Evaluate((float)pseudoreynolds);
double drag = part.DragCubes.AreaDrag * PhysicsGlobals.DragCubeMultiplier * pseudoReDragMult;
float pscal = (float)(part.dynamicPressurekPa * drag * PhysicsGlobals.DragMultiplier);
drag_sum += -part.dragVectorDir * pscal;
//Estimate lift force from body lift
float pbodyLiftScalar = (float)(part.dynamicPressurekPa * part.bodyLiftMultiplier * PhysicsGlobals.BodyLiftMultiplier) * PhysicsGlobals.GetLiftingSurfaceCurve("BodyLift").liftMachCurve.Evaluate((float)part.machNumber);
Vector3 lforce2 = part.transform.rotation * (pbodyLiftScalar * part.DragCubes.LiftForce);
lforce2 = Vector3.ProjectOnPlane(lforce2, -part.dragVectorDir);
lift_sum += lforce2;
}
//Util.Log("BodyLift Default: " + part.bodyLiftScalar + ", Lift Force: " + lift_force);
//Loop through each part module to look for lifting surfaces
for (int j = 0; j < part.Modules.Count; ++j)
{
var m = part.Modules[j];
if (m is ModuleLiftingSurface)
{
//Initialize force vectors
Vector3 lforce = Vector3.zero;
Vector3 dforce = Vector3.zero;
//Convert kPa to Pa
double liftQ = part.dynamicPressurekPa * 1000;
ModuleLiftingSurface wing = (ModuleLiftingSurface)m;
//Get wing coefficients
Vector3 nVel = Vector3.zero;
Vector3 liftVector = Vector3.zero;
float liftdot;
float absdot;
wing.SetupCoefficients(velocity_nowind, out nVel, out liftVector, out liftdot, out absdot);
//Get Lift/drag force
lforce = wing.GetLiftVector(liftVector, liftdot, absdot, liftQ, (float)part.machNumber);
dforce = wing.GetDragVector(nVel, absdot, liftQ);
if (part.name.Contains("kerbalEVA"))
{
continue;
}
drag_sum += dforce;
//lift_sum += lforce;
if (isgrounded)
{
lift_sum += lforce;
}
}
}
//Retrieve world velocity and subtract off wind vector
Vector3 velocity_wind = rb.velocity + Krakensbane.GetFrameVelocity() - windVec;
//Compute mach number
double mach_wind = velocity_wind.magnitude / soundSpeed;
//Set mach number
part.machNumber = mach_wind; // *mdiv;
if (part == v.rootPart)
{
fi.mach = mach_wind;
}
//Get drag and lift forces with wind
List <Vector3> total_forces = GetDragLiftForce(fi, part, velocity_wind, windVec, part.machNumber, isgrounded);
//Compute difference in drag/lift with and without wind.
Vector3 total_drag = total_forces[0] - drag_sum;
Vector3 total_lift = total_forces[1] - lift_sum;
//Calculate force due to wind
Vector3 force = total_lift + total_drag;
if (double.IsNaN(force.sqrMagnitude) || (((float)force.magnitude).NearlyEqual(0.0f)))
{
force = Vector3d.zero;
}
else
{
//Adapted from FAR - apply numerical control factor
float numericalControlFactor = (float)(part.rb.mass * velocity_wind.magnitude * 0.67 / (force.magnitude * TimeWarp.fixedDeltaTime));
force *= Math.Min(numericalControlFactor, 1);
part.AddForce(force);
}
v.mach = fi.mach;
}
}
}
//*******************************************************
public static Vector3 SimLiftForce(CelestialBody body, IShipconstruct vessel, Vector3 v_wrld_vel, double altitude, double latitude = 0.0)
{
//Profiler.Start("SimLiftForce");
double pressure = body.GetPressure(altitude);
// Lift and drag for force accumulation.
Vector3d total_lift = 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 = vessel.Parts.Count - 1; i >= 0; i--)
{
// need checks on shielded components
Part part = vessel.Parts[i];
if (part.ShieldedFromAirstream || part.Rigidbody == null)
{
continue;
}
// Get Drag
Vector3 sim_dragVectorDir = v_wrld_vel.normalized;
Vector3 sim_dragVectorDirLocal = -(part.transform.InverseTransformDirection(sim_dragVectorDir));
Vector3 liftForce = new Vector3(0, 0, 0);
//Profiler.Start("SimLiftForce#Body");
switch (part.dragModel)
{
case Part.DragModel.DEFAULT:
case Part.DragModel.CUBE:
DragCubeList cubes = part.DragCubes;
DragCubeList.CubeData p_drag_data = new DragCubeList.CubeData();
if (!cubes.None) // since 1.0.5, some parts don't have drag cubes (for example fuel lines and struts)
{
try
{
cubes.SetDragWeights();
cubes.SetPartOcclusion();
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);
liftForce = p_drag_data.liftForce;
}
break;
}
// If it isn't a wing or lifter, get body lift.
if (!part.hasLiftModule)
{
float simbodyLiftScalar = part.bodyLiftMultiplier * PhysicsGlobals.BodyLiftMultiplier * (float)dyn_pressure;
simbodyLiftScalar *= PhysicsGlobals.GetLiftingSurfaceCurve("BodyLift").liftMachCurve.Evaluate((float)mach);
Vector3 bodyLift = part.transform.rotation * (simbodyLiftScalar * liftForce);
bodyLift = Vector3.ProjectOnPlane(bodyLift, sim_dragVectorDir);
// Only accumulate forces for non-LiftModules
total_lift += bodyLift;
}
//Profiler.Stop("SimLiftForce#Body");
// Find ModuleLifingSurface for wings and liftforce.
// Should catch control surface as it is a subclass
//Profiler.Start("SimLiftForce#LiftModule");
for (int j = part.Modules.Count - 1; j >= 0; j--)
{
if (part.Modules[j] is ModuleLiftingSurface)
{
float mcs_mod = 1.0f;
double liftQ = dyn_pressure * 1000;
ModuleLiftingSurface wing = (ModuleLiftingSurface)part.Modules[j];
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 = part.machNumber;
part.machNumber = mach;
Vector3 local_lift = mcs_mod * wing.GetLiftVector(liftVector, liftdot, absdot, liftQ, (float)mach);
part.machNumber = prevMach;
total_lift += local_lift;
}
}
//Profiler.Stop("SimLiftForce#LiftModule");
}
// RETURN STUFF
//Profiler.Stop("SimLiftForce");
return(total_lift);
}
//*******************************************************
public static Vector3 SimAeroForce(List <Part> parts, CelestialBody body, Vector3 v_wrld_vel, double altitude, double latitude = 0.0)
{
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.
foreach (Part p in parts)
{
// need checks on shielded components
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);
switch (p.dragModel)
{
case Part.DragModel.DEFAULT:
case Part.DragModel.CUBE:
SmartDragCubeList cubes = new SmartDragCubeList(p, parts);
DragCubeList.CubeData p_drag_data;
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
{
p_drag_data = cubes.AddSurfaceDragDirection(-sim_dragVectorDirLocal, (float)mach);
drag = p_drag_data.areaDrag * PhysicsGlobals.DragCubeMultiplier;
liftForce = p_drag_data.liftForce;
}
double sim_dragScalar = dyn_pressure * (double)drag * PhysicsGlobals.DragMultiplier;
total_drag += -(Vector3d)sim_dragVectorDir * sim_dragScalar;
break;
case Part.DragModel.SPHERICAL:
total_drag += -(Vector3d)sim_dragVectorDir * (double)p.maximum_drag;
break;
case Part.DragModel.CYLINDRICAL:
total_drag += -(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:
total_drag += -(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
break;
}
// If it isn't a wing or lifter, get body lift.
if (!p.hasLiftModule)
{
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;
}
// Find ModuleLifingSurface for wings and liftforce.
// Should catch control surface as it is a subclass
foreach (var m in p.Modules.OfType <ModuleLiftingSurface>())
{
float 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);
float simLiftScalar = Mathf.Sign(liftdot) * wing.liftCurve.Evaluate(absdot) * wing.liftMachCurve.Evaluate((float)mach);
simLiftScalar *= wing.deflectionLiftCoeff;
simLiftScalar = (float)(liftQ * (double)(PhysicsGlobals.LiftMultiplier * simLiftScalar));
float simdragScalar = wing.dragCurve.Evaluate(absdot) * wing.dragMachCurve.Evaluate((float)mach);
simdragScalar *= wing.deflectionLiftCoeff;
simdragScalar = (float)(liftQ * (double)(simdragScalar * PhysicsGlobals.LiftDragMultiplier));
Vector3 local_lift = mcs_mod * wing.GetLiftVector(liftVector, liftdot, absdot, liftQ, (float)mach);
Vector3 local_drag = mcs_mod * wing.GetDragVector(nVel, absdot, liftQ);
total_lift += local_lift;
total_drag += local_drag;
}
}
// RETURN STUFF
Vector3 force = total_lift + total_drag;
return(1000 * force);
}