// Merge two force sets
public void AddAll(FARCenterQuery q2)
{
force += q2.force;
torque += q2.torque;
pos += q2.pos;
amount += q2.amount;
}
public void SimulateAeroProperties(out Vector3 aeroForce, out Vector3 aeroTorque, Vector3 velocityWorldVector, double altitude)
{
FARCenterQuery center = new FARCenterQuery();
float pressure;
float density;
float temperature;
float speedOfSound;
CelestialBody body = _vessel.mainBody; //Calculate main gas properties
pressure = (float)body.GetPressure(altitude);
temperature = (float)body.GetTemperature(altitude);
density = (float)body.GetDensity(pressure, temperature);
speedOfSound = (float)body.GetSpeedOfSound(pressure, density);
float velocityMag = velocityWorldVector.magnitude;
float machNumber = velocityMag / speedOfSound;
float reynoldsNumber = (float)FARAeroUtil.CalculateReynoldsNumber(density, Length, velocityMag, machNumber, temperature, body.atmosphereAdiabaticIndex);
float reynoldsPerLength = reynoldsNumber / (float)Length;
float skinFriction = (float)FARAeroUtil.SkinFrictionDrag(reynoldsNumber, machNumber);
for(int i = 0; i < _currentAeroSections.Count; i++)
_currentAeroSections[i].PredictionCalculateAeroForces(density, machNumber, reynoldsPerLength, skinFriction, velocityWorldVector, center);
for (int i = 0; i < _legacyWingModels.Count; i++)
_legacyWingModels[i].PrecomputeCenterOfLift(velocityWorldVector, machNumber, density, center);
aeroForce = center.force;
aeroTorque = center.TorqueAt(_vessel.CoM);
}
public static void PrecomputeGlobalCenterOfLift(FARCenterQuery lift, FARCenterQuery dummy, Vector3 vel, double density)
{
/* Center of lift is the location where the derivative of
the total torque provided by aerodynamic forces relative to
AoA is zero (or at least minimal). This approximates the
derivative by a simple subtraction, like before. */
var parts = GetAllEditorModules();
for(int i = 0; i < parts.Count; i++)
{
FARBaseAerodynamics ba = parts[i];
ba.velocityEditor = vel;
ba.ResetCenterOfLift();
}
// run computations twice to let things like flap interactions settle
for (int i = 0; i < parts.Count; i++)
{
FARBaseAerodynamics ba = parts[i];
ba.PrecomputeCenterOfLift(vel, 0, density, dummy);
}
for (int i = 0; i < parts.Count; i++)
{
FARBaseAerodynamics ba = parts[i];
ba.PrecomputeCenterOfLift(vel, 0, density, lift);
}
}
public virtual Vector3d PrecomputeCenterOfLift(Vector3d velocity, double MachNumber, double density, FARCenterQuery center)
{
return Vector3d.zero;
}
public void GetClCdCmSteady(InstantConditionSimInput input, out InstantConditionSimOutput output, bool clear, bool reset_stall = false)
{
output = new InstantConditionSimOutput();
double area = 0;
double MAC = 0;
double b_2 = 0;
Vector3d forward = Vector3.forward;
Vector3d up = Vector3.up;
Vector3d right = Vector3.right;
Vector3d CoM = Vector3d.zero;
double mass = 0;
List<Part> partsList = EditorLogic.SortedShipList;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (FARAeroUtil.IsNonphysical(p))
continue;
double partMass = p.mass;
if (p.Resources.Count > 0)
partMass += p.GetResourceMass();
//partMass += p.GetModuleMass(p.mass);
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
mass += partMass;
}
CoM /= mass;
if (EditorDriver.editorFacility == EditorFacility.VAB)
{
forward = Vector3.up;
up = -Vector3.forward;
}
double sinAlpha = Math.Sin(input.alpha * Math.PI / 180);
double cosAlpha = Math.Sqrt(Math.Max(1 - sinAlpha * sinAlpha, 0));
double sinBeta = Math.Sin(input.beta * Math.PI / 180);
double cosBeta = Math.Sqrt(Math.Max(1 - sinBeta * sinBeta, 0));
double sinPhi = Math.Sin(input.phi * Math.PI / 180);
double cosPhi = Math.Sqrt(Math.Max(1 - sinPhi * sinPhi, 0));
double alphaDot = input.alphaDot * Math.PI / 180;
double betaDot = input.betaDot * Math.PI / 180;
double phiDot = input.phiDot * Math.PI / 180;
Vector3d AngVel = (phiDot - sinAlpha * betaDot) * forward;
AngVel += (cosPhi * alphaDot + cosAlpha * sinPhi * betaDot) * right;
AngVel += (sinPhi * alphaDot - cosAlpha * cosPhi * betaDot) * up;
Vector3d velocity = forward * cosAlpha * cosBeta;
velocity += right * (sinPhi * cosAlpha * cosBeta + cosPhi * sinBeta);
velocity += -up * cosPhi * (sinAlpha * cosBeta + sinBeta);
velocity.Normalize();
//this is negative wrt the ground
Vector3d liftVector = -forward * sinAlpha + right * sinPhi * cosAlpha - up * cosPhi * cosAlpha;
Vector3d sideways = Vector3.Cross(velocity, liftVector).normalized;
for (int i = 0; i < _wingAerodynamicModel.Count; i++)
{
FARWingAerodynamicModel w = _wingAerodynamicModel[i];
if (!(w && w.part))
continue;
w.ComputeForceEditor(velocity.normalized, input.machNumber, 2);
if (clear)
w.EditorClClear(reset_stall);
Vector3d relPos = w.GetAerodynamicCenter() - CoM;
Vector3d vel = velocity + Vector3d.Cross(AngVel, relPos);
if (w is FARControllableSurface)
(w as FARControllableSurface).SetControlStateEditor(CoM, vel, (float)input.pitchValue, 0, 0, input.flaps, input.spoilers);
else if (w.isShielded)
continue;
//w.ComputeForceEditor(velocity, input.machNumber); //do this just to get the AC right
Vector3d force = w.ComputeForceEditor(vel.normalized, input.machNumber, 2) * 1000;
output.Cl += -Vector3d.Dot(force, liftVector);
output.Cy += Vector3d.Dot(force, sideways);
output.Cd += -Vector3d.Dot(force, velocity);
Vector3d moment = -Vector3d.Cross(relPos, force);
output.Cm += Vector3d.Dot(moment, sideways);
output.Cn += Vector3d.Dot(moment, liftVector);
output.C_roll += Vector3d.Dot(moment, velocity);
//w.ComputeClCdEditor(vel.normalized, input.machNumber);
/*double tmpCl = w.GetCl() * w.S;
output.Cl += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), liftVector);
output.Cy += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), sideways);
double tmpCd = w.GetCd() * w.S;
output.Cd += tmpCd;
output.Cm += tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), liftVector) + tmpCd * -Vector3d.Dot((relPos), liftVector);
output.Cn += tmpCd * Vector3d.Dot((relPos), sideways) + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), sideways);
output.C_roll += tmpCl * Vector3d.Dot((relPos), sideways) * -Vector3d.Dot(w.GetLiftDirection(), liftVector);*/
area += w.S;
MAC += w.GetMAC() * w.S;
b_2 += w.Getb_2() * w.S;
}
FARCenterQuery center = new FARCenterQuery();
for (int i = 0; i < _currentAeroSections.Count; i++)
{
_currentAeroSections[i].PredictionCalculateAeroForces(2, (float)input.machNumber, 10000, 0.005f, velocity.normalized, center);
}
Vector3d centerForce = center.force * 1000;
output.Cl += -Vector3d.Dot(centerForce, liftVector);
output.Cy += Vector3d.Dot(centerForce, sideways);
output.Cd += -Vector3d.Dot(centerForce, velocity);
Vector3d centerMoment = -center.TorqueAt(CoM) * 1000;
output.Cm += Vector3d.Dot(centerMoment, sideways);
output.Cn += Vector3d.Dot(centerMoment, liftVector);
output.C_roll += Vector3d.Dot(centerMoment, velocity);
/*for (int i = 0; i < FARAeroUtil.CurEditorParts.Count; i++)
{
Part p = FARAeroUtil.CurEditorParts[i];
if (FARAeroUtil.IsNonphysical(p))
continue;
Vector3 part_pos = p.transform.TransformPoint(p.CoMOffset) - CoM;
double partMass = p.mass;
if (p.Resources.Count > 0)
partMass += p.GetResourceMass();
double stock_drag = partMass * p.maximum_drag * FlightGlobals.DragMultiplier * 1000;
output.Cd += stock_drag;
output.Cm += stock_drag * -Vector3d.Dot(part_pos, liftVector);
output.Cn += stock_drag * Vector3d.Dot(part_pos, sideways);
}*/
if (area == 0)
{
area = _maxCrossSectionFromBody;
b_2 = 1;
MAC = _bodyLength;
}
double recipArea = 1 / area;
MAC *= recipArea;
b_2 *= recipArea;
output.Cl *= recipArea;
output.Cd *= recipArea;
output.Cm *= recipArea / MAC;
output.Cy *= recipArea;
output.Cn *= recipArea / b_2;
output.C_roll *= recipArea / b_2;
}
protected override Vector3d PrecomputeCenterOfLift(Vector3d velocity, double MachNumber, FARCenterQuery center)
{
Vector3d force = RunDragCalculation(velocity, MachNumber, 1);
Vector3d pos = GetCoD();
center.AddForce(pos, force);
return(force);
}
public virtual Vector3d PrecomputeCenterOfLift(Vector3d velocity, FlightEnv fltenv, FARCenterQuery center)
{
return(Vector3d.zero);
}
public void UpdateSimulationContext(Vector4 worldVel, FARCenterQuery center, float atmDensity)
{
this.worldVel = worldVel;
this.center = center;
this.atmDensity = atmDensity;
}
protected override Vector3d PrecomputeCenterOfLift(Vector3d velocity, double MachNumber, FARCenterQuery center)
{
double AoA = CalculateAoA(velocity);
Vector3d force = CalculateForces(velocity, MachNumber, AoA);
center.AddForce(AerodynamicCenter, force);
return force;
}
public virtual Vector3d PrecomputeCenterOfLift(Vector3d velocity, double MachNumber, double density, FARCenterQuery center)
{
return(Vector3d.zero);
}
protected override Vector3d PrecomputeCenterOfLift(Vector3d velocity, double MachNumber, FARCenterQuery center)
{
Vector3d force = RunDragCalculation(velocity, MachNumber, 1);
Vector3d pos = GetCoD();
center.AddForce(pos, force);
return force;
}
private static void PrecomputeGlobalCenterOfLift()
{
/* Center of lift is the location where the derivative of
* the total torque provided by aerodynamic forces relative to
* AoA is zero (or at least minimal). This approximates the
* derivative by a simple subtraction, like before. */
Vector3 vel_base, vel_fuzz;
if (EditorLogic.fetch.editorType == EditorLogic.EditorMode.SPH)
{
vel_base = Vector3.forward;
vel_fuzz = 0.02f * Vector3.up;
}
else
{
vel_base = Vector3.up;
vel_fuzz = -0.02f * Vector3.forward;
}
FARCenterQuery lift = new FARCenterQuery();
FARCenterQuery dummy = new FARCenterQuery();
var parts = GetAllEditorModules();
// Pass 1
Vector3 vel = (vel_base - vel_fuzz).normalized;
foreach (var ba in parts)
{
ba.velocityEditor = vel;
ba.ResetCenterOfLift();
}
// run computations twice to let things like flap interactions settle
foreach (var ba in parts)
{
ba.PrecomputeCenterOfLift(vel, 0, dummy);
}
foreach (var ba in parts)
{
ba.CoLForce = ba.PrecomputeCenterOfLift(vel, 0, lift);
}
// flip sign of data in the accumulator to indirectly subtract passes
lift.force = -lift.force;
lift.torque = -lift.torque;
// Pass 2
vel = (vel_base + vel_fuzz).normalized;
foreach (var ba in parts)
{
ba.velocityEditor = vel;
ba.ResetCenterOfLift();
}
foreach (var ba in parts)
{
ba.PrecomputeCenterOfLift(vel, 0, dummy);
}
foreach (var ba in parts)
{
ba.CoLForce -= ba.PrecomputeCenterOfLift(vel, 0, lift);
}
// Choose the center location
GlobalCoL = lift.GetMinTorquePos();
GlobalCoLReady = true;
}
public void PredictionCalculateAeroForces(float atmDensity, float machNumber, float reynoldsPerUnitLength, float pseudoKnudsenNumber, float skinFrictionDrag, Vector3 vel, ferram4.FARCenterQuery center)
{
simContext.UpdateSimulationContext(vel, center, atmDensity);
CalculateAeroForces(atmDensity, machNumber, reynoldsPerUnitLength, pseudoKnudsenNumber, skinFrictionDrag, simContext);
}
protected override Vector3d PrecomputeCenterOfLift(Vector3d velocity, double MachNumber, FARCenterQuery center)
{
try
{
Vector3d force = RunDragCalculation(velocity, MachNumber, 1);
Vector3d pos = GetCoD();
center.AddForce(pos, force);
return(force);
}
catch //FIX ME!!!
{ //Yell at KSP devs so that I don't have to engage in bad code practice
//Debug.Log("The expected exception from the symmetry counterpart part transform internals was caught and suppressed");
return(Vector3.zero);
}
}
private static void PrecomputeGlobalCenterOfLift()
{
/* Center of lift is the location where the derivative of
the total torque provided by aerodynamic forces relative to
AoA is zero (or at least minimal). This approximates the
derivative by a simple subtraction, like before. */
Vector3 vel_base, vel_fuzz;
if (EditorLogic.fetch.editorType == EditorLogic.EditorMode.SPH)
{
vel_base = Vector3.forward;
vel_fuzz = 0.02f * Vector3.up;
}
else
{
vel_base = Vector3.up;
vel_fuzz = -0.02f * Vector3.forward;
}
FARCenterQuery lift = new FARCenterQuery();
FARCenterQuery dummy = new FARCenterQuery();
var parts = GetAllEditorModules();
// Pass 1
Vector3 vel = (vel_base - vel_fuzz).normalized;
foreach (var ba in parts)
{
ba.velocityEditor = vel;
ba.ResetCenterOfLift();
}
// run computations twice to let things like flap interactions settle
foreach (var ba in parts)
ba.PrecomputeCenterOfLift(vel, 0, dummy);
foreach (var ba in parts)
ba.CoLForce = ba.PrecomputeCenterOfLift(vel, 0, lift);
// flip sign of data in the accumulator to indirectly subtract passes
lift.force = -lift.force;
lift.torque = - lift.torque;
// Pass 2
vel = (vel_base + vel_fuzz).normalized;
foreach (var ba in parts)
{
ba.velocityEditor = vel;
ba.ResetCenterOfLift();
}
foreach (var ba in parts)
ba.PrecomputeCenterOfLift(vel, 0, dummy);
foreach (var ba in parts)
ba.CoLForce -= ba.PrecomputeCenterOfLift(vel, 0, lift);
// Choose the center location
GlobalCoL = lift.GetMinTorquePos();
GlobalCoLReady = true;
}
void UpdateAerodynamicCenter()
{
FARCenterQuery aeroSection, dummy;
aeroSection = new FARCenterQuery();
dummy = new FARCenterQuery();
if((object)EditorLogic.RootPart == null)
return;
Vector3 vel_base, vel_fuzz;
Transform rootPartTrans = EditorLogic.RootPart.partTransform;
if (EditorDriver.editorFacility == EditorFacility.SPH)
{
vel_base = Vector3.forward;
vel_fuzz = 0.02f * Vector3.up;
}
else
{
vel_base = Vector3.up;
vel_fuzz = -0.02f * Vector3.forward;
}
Vector3 vel = (vel_base - vel_fuzz).normalized;
for(int i = 0; i < _currentAeroSections.Count; i++)
{
FARAeroSection section = _currentAeroSections[i];
section.PredictionCalculateAeroForces(1, 0.5f, 100000, 0.005f, vel, aeroSection);
}
FARBaseAerodynamics.PrecomputeGlobalCenterOfLift(aeroSection, dummy, vel, 1);
Vector3 pos = Vector3.zero;//rootPartTrans.position;
float mass = 0;
for(int i = 0; i < EditorLogic.SortedShipList.Count; i++)
{
Part p = EditorLogic.SortedShipList[i];
float tmpMass = p.mass + p.GetResourceMass();
mass += tmpMass;
pos += p.partTransform.position * tmpMass;
}
pos /= mass;
Vector3 avgForcePos = Vector3.zero;
Vector3 force0, moment0;
force0 = aeroSection.force;
moment0 = aeroSection.TorqueAt(pos);
avgForcePos += aeroSection.GetPos();
//aeroSection.force = -aeroSection.force;
//aeroSection.torque = -aeroSection.torque;
aeroSection.ClearAll();
vel = (vel_base + vel_fuzz).normalized;
for (int i = 0; i < _currentAeroSections.Count; i++)
{
FARAeroSection section = _currentAeroSections[i];
section.PredictionCalculateAeroForces(1, 0.5f, 100000, 0.005f, vel, aeroSection);
}
FARBaseAerodynamics.PrecomputeGlobalCenterOfLift(aeroSection, dummy, vel, 1);
Vector3 force1, moment1;
force1 = aeroSection.force;
moment1 = aeroSection.TorqueAt(pos);
avgForcePos += aeroSection.GetPos();
aeroSection.ClearAll();
avgForcePos *= 0.5f;
Vector3 deltaForce = force1 - force0;
Vector3 deltaMoment = moment1 - moment0;
Vector3 deltaForcePerp = Vector3.ProjectOnPlane(deltaForce, vel_base);
float deltaForcePerpMag = deltaForcePerp.magnitude;
Vector3 deltaForcePerpNorm = deltaForcePerp / deltaForcePerpMag;
Vector3 deltaMomentPerp = deltaMoment - Vector3.Dot(deltaMoment, deltaForcePerpNorm) * deltaForcePerpNorm - Vector3.Project(deltaMoment, vel_base);
//float dist = deltaMomentPerp.magnitude / deltaForcePerpMag;
//vesselRootLocalAeroCenter = vel_base * dist;
vesselRootLocalAeroCenter = deltaMomentPerp.magnitude / deltaForcePerpMag * vel_base * Math.Sign(Vector3.Dot(Vector3.Cross(deltaForce, deltaMoment), vel_base));
//Debug.Log(dist + " " + deltaMomentPerp.magnitude + " " + deltaForcePerpMag);
//vesselRootLocalAeroCenter += avgForcePos;
//avgForcePos = rootPartTrans.worldToLocalMatrix.MultiplyPoint3x4(avgForcePos);
//vesselRootLocalAeroCenter += Vector3.ProjectOnPlane(avgForcePos, Vector3.up);
//vesselRootLocalAeroCenter = aeroSection.GetPos();
vesselRootLocalAeroCenter += pos;//Vector3.ProjectOnPlane(avgForcePos - pos, vesselRootLocalAeroCenter) + pos;
vesselRootLocalAeroCenter = rootPartTrans.worldToLocalMatrix.MultiplyPoint3x4(vesselRootLocalAeroCenter);
}
public SimulatedForceContext(Vector3 worldVel, FARCenterQuery center, float atmDensity)
{
this.worldVel = worldVel;
this.center = center;
this.atmDensity = atmDensity;
}
