public void GetCoMAndSize(out Vector3d CoM, out double mass, out double area, out double MAC, out double b)
{
CoM = Vector3d.zero;
mass = 0; area = 0; MAC = 0; b = 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;
FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
if (w != null && !w.isShielded)
{
area += w.S;
MAC += w.GetMAC() * w.S;
b += w.Getb_2() * w.S;
}
}
if (area > 0)
{
MAC /= area;
b /= area;
}
else
{
area = _maxCrossSectionFromBody;
MAC = _bodyLength;
b = 1;
}
CoM /= mass;
mass *= 1000;
}
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, 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;
}
private bool FindPartsWithoutFARModel(List <Part> editorShip)
{
bool returnValue = false;
for (int i = 0; i < editorShip.Count; i++)
{
Part p = editorShip[i];
if (p == null)
{
continue;
}
if (p != null && FARAeroUtil.IsNonphysical(p) &&
p.physicalSignificance != Part.PhysicalSignificance.NONE)
{
MonoBehaviour.print(p + ": FAR correcting physical significance to fix CoM in editor");
p.physicalSignificance = Part.PhysicalSignificance.NONE;
}
string title = p.partInfo.title.ToLowerInvariant();
if (p.Modules.Contains("FARBasicDragModel"))
{
List <PartModule> modulesToRemove = new List <PartModule>();
for (int j = 0; j < p.Modules.Count; j++)
{
PartModule m = p.Modules[j];
if (!(m is FARBasicDragModel))
{
continue;
}
FARBasicDragModel d = m as FARBasicDragModel;
if (d.CdCurve == null || d.ClPotentialCurve == null || d.ClViscousCurve == null || d.CmCurve == null)
{
modulesToRemove.Add(m);
}
}
if (modulesToRemove.Count > 0)
{
for (int j = 0; j < modulesToRemove.Count; j++)
{
PartModule m = modulesToRemove[j];
p.RemoveModule(m);
Debug.Log("Removing Incomplete FAR Drag Module");
}
if (p.Modules.Contains("FARPayloadFairingModule"))
{
p.RemoveModule(p.Modules["FARPayloadFairingModule"]);
}
if (p.Modules.Contains("FARCargoBayModule"))
{
p.RemoveModule(p.Modules["FARCargoBayModule"]);
}
if (p.Modules.Contains("FARControlSys"))
{
p.RemoveModule(p.Modules["FARControlSys"]);
}
}
}
if (p is StrutConnector || p is FuelLine || p is ControlSurface || p is Winglet || FARPartClassification.ExemptPartFromGettingDragModel(p, title))
{
continue;
}
FARPartModule q = p.GetComponent <FARPartModule>();
if (q != null && !(q is FARControlSys))
{
continue;
}
bool updatedModules = false;
if (FARPartClassification.PartIsCargoBay(p, title))
{
if (!p.Modules.Contains("FARCargoBayModule"))
{
p.AddModule("FARCargoBayModule");
p.Modules["FARCargoBayModule"].OnStart(PartModule.StartState.Editor);
FARAeroUtil.AddBasicDragModuleWithoutDragPropertySetup(p);
p.Modules["FARBasicDragModel"].OnStart(PartModule.StartState.Editor);
updatedModules = true;
}
}
if (!updatedModules)
{
if (FARPartClassification.PartIsPayloadFairing(p, title))
{
if (!p.Modules.Contains("FARPayloadFairingModule"))
{
p.AddModule("FARPayloadFairingModule");
p.Modules["FARPayloadFairingModule"].OnStart(PartModule.StartState.Editor);
FARAeroUtil.AddBasicDragModuleWithoutDragPropertySetup(p);
p.Modules["FARBasicDragModel"].OnStart(PartModule.StartState.Editor);
updatedModules = true;
}
}
if (!updatedModules && !p.Modules.Contains("FARBasicDragModel"))
{
FARAeroUtil.AddBasicDragModuleWithoutDragPropertySetup(p);
p.Modules["FARBasicDragModel"].OnStart(PartModule.StartState.Editor);
updatedModules = true;
}
}
returnValue |= updatedModules;
FARPartModule b = p.GetComponent <FARPartModule>();
if (b != null)
{
b.VesselPartList = editorShip; //This prevents every single part in the ship running this due to VesselPartsList not being initialized
}
}
for (int i = 0; i < editorShip.Count; i++)
{
Part p = editorShip[i];
FARBasicDragModel d = p.GetComponent <FARBasicDragModel>();
if (d != null)
{
d.UpdatePropertiesWithShapeChange();
}
}
return(returnValue);
}
private void UpdateAerodynamicCenter()
{
var aeroSection = new FARCenterQuery();
var dummy = new FARCenterQuery();
if (EditorLogic.RootPart is 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;
foreach (FARAeroSection section in _currentAeroSections)
{
section.PredictionCalculateAeroForces(1, 0.5f, 100000, 0, 0.005f, vel, aeroSection);
}
FARBaseAerodynamics.PrecomputeGlobalCenterOfLift(aeroSection, dummy, vel, 1);
Vector3 pos = Vector3.zero;
float mass = 0;
foreach (Part p in EditorLogic.SortedShipList)
{
if (FARAeroUtil.IsNonphysical(p))
{
continue;
}
float tmpMass = p.mass + p.GetResourceMass();
mass += tmpMass;
pos += p.partTransform.position * tmpMass;
}
pos /= mass;
Vector3 force0 = aeroSection.force;
Vector3 moment0 = aeroSection.TorqueAt(pos);
aeroSection.ClearAll();
vel = (vel_base + vel_fuzz).normalized;
foreach (FARAeroSection section in _currentAeroSections)
{
section.PredictionCalculateAeroForces(1, 0.5f, 100000, 0, 0.005f, vel, aeroSection);
}
FARBaseAerodynamics.PrecomputeGlobalCenterOfLift(aeroSection, dummy, vel, 1);
Vector3 force1 = aeroSection.force;
Vector3 moment1 = aeroSection.TorqueAt(pos);
aeroSection.ClearAll();
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);
vesselRootLocalAeroCenter = deltaMomentPerp.magnitude /
deltaForcePerpMag *
Math.Sign(Vector3.Dot(Vector3.Cross(deltaForce, deltaMoment), vel_base)) *
vel_base;
vesselRootLocalAeroCenter += pos;
vesselRootLocalAeroCenter = rootPartTrans.worldToLocalMatrix.MultiplyPoint3x4(vesselRootLocalAeroCenter);
}
public StabilityDerivOutput CalculateStabilityDerivs(double u0, double q, double machNumber, double alpha, double beta, double phi, int flapSetting, bool spoilers, CelestialBody body, double alt)
{
StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput();
stabDerivOutput.nominalVelocity = u0;
stabDerivOutput.altitude = alt;
stabDerivOutput.body = body;
Vector3d CoM = Vector3d.zero;
double mass = 0;
double MAC = 0;
double b = 0;
double area = 0;
double Ix = 0;
double Iy = 0;
double Iz = 0;
double Ixy = 0;
double Iyz = 0;
double Ixz = 0;
InstantConditionSimInput input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
InstantConditionSimOutput nominalOutput;
InstantConditionSimOutput pertOutput = new InstantConditionSimOutput();
_instantCondition.GetClCdCmSteady(input, out nominalOutput, true);
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;
FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
if (w != null)
{
if (w.isShielded)
{
continue;
}
area += w.S;
MAC += w.GetMAC() * w.S;
b += w.Getb_2() * w.S;
if (w is FARControllableSurface)
{
(w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers);
}
}
}
if (area == 0)
{
area = _instantCondition._maxCrossSectionFromBody;
MAC = _instantCondition._bodyLength;
b = 1;
}
MAC /= area;
b /= area;
CoM /= mass;
mass *= 1000;
stabDerivOutput.b = b;
stabDerivOutput.MAC = MAC;
stabDerivOutput.area = area;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (p == null || FARAeroUtil.IsNonphysical(p))
{
continue;
}
//This section handles the parallel axis theorem
Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
double x2, y2, z2, x, y, z;
x2 = relPos.z * relPos.z;
y2 = relPos.x * relPos.x;
z2 = relPos.y * relPos.y;
x = relPos.z;
y = relPos.x;
z = relPos.y;
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
Ix += (y2 + z2) * partMass;
Iy += (x2 + z2) * partMass;
Iz += (x2 + y2) * partMass;
Ixy += -x * y * partMass;
Iyz += -z * y * partMass;
Ixz += -x * z * partMass;
//And this handles the part's own moment of inertia
Vector3 principalInertia = p.Rigidbody.inertiaTensor;
Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;
//The rows of the direction cosine matrix for a quaternion
Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));
Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
-prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));
Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
-prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);
//And converting the principal moments of inertia into the coordinate system used by the system
Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;
Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
}
Ix *= 1000;
Iy *= 1000;
Iz *= 1000;
stabDerivOutput.stabDerivs[0] = Ix;
stabDerivOutput.stabDerivs[1] = Iy;
stabDerivOutput.stabDerivs[2] = Iz;
stabDerivOutput.stabDerivs[24] = Ixy;
stabDerivOutput.stabDerivs[25] = Iyz;
stabDerivOutput.stabDerivs[26] = Ixz;
double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt); //This is the effect of gravity
effectiveG -= u0 * u0 / (alt + body.Radius); //This is the effective reduction of gravity due to high velocity
double neededCl = mass * effectiveG / (q * area);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
//Longitudinal Mess
_instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);
alpha = FARMathUtil.BrentsMethod(_instantCondition.FunctionIterateForAlpha, -30d, 30d, 0.001, 500);
input.alpha = alpha;
nominalOutput = _instantCondition.iterationOutput;
//alpha_str = (alpha * Mathf.PI / 180).ToString();
input.alpha = (alpha + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
stabDerivOutput.stableCl = neededCl;
stabDerivOutput.stableCd = nominalOutput.Cd;
stabDerivOutput.stableAoA = alpha;
stabDerivOutput.stableAoAState = "";
if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
{
stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">");
}
Debug.Log("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad); //vert vel derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);
pertOutput.Cl += nominalOutput.Cd;
pertOutput.Cd -= nominalOutput.Cl;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (Iy * u0);
stabDerivOutput.stabDerivs[3] = pertOutput.Cl; //Zw
stabDerivOutput.stabDerivs[4] = pertOutput.Cd; //Xw
stabDerivOutput.stabDerivs[5] = pertOutput.Cm; //Mw
input.alpha = alpha;
input.machNumber = machNumber + 0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber; //fwd vel derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;
pertOutput.Cl += 2 * nominalOutput.Cl;
pertOutput.Cd += 2 * nominalOutput.Cd;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (u0 * Iy);
stabDerivOutput.stabDerivs[6] = pertOutput.Cl; //Zu
stabDerivOutput.stabDerivs[7] = pertOutput.Cd; //Xu
stabDerivOutput.stabDerivs[8] = pertOutput.Cm; //Mu
input.machNumber = machNumber;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.alphaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05; //pitch rate derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;
pertOutput.Cl *= q * area * MAC / (2 * u0 * mass);
pertOutput.Cd *= q * area * MAC / (2 * u0 * mass);
pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);
stabDerivOutput.stabDerivs[9] = pertOutput.Cl; //Zq
stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq
input.alphaDot = 0;
input.pitchValue = 0.1;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1; //elevator derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;
pertOutput.Cl *= q * area / mass;
pertOutput.Cd *= q * area / mass;
pertOutput.Cm *= q * area * MAC / Iy;
stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me
//Lateral Mess
input.pitchValue = 0;
input.beta = (beta + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad); //sideslip angle derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);
pertOutput.Cy *= q * area / mass;
pertOutput.Cn *= q * area * b / Iz;
pertOutput.C_roll *= q * area * b / Ix;
stabDerivOutput.stabDerivs[15] = pertOutput.Cy; //Yb
stabDerivOutput.stabDerivs[17] = pertOutput.Cn; //Nb
stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb
input.beta = beta;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.phiDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05; //roll rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[18] = pertOutput.Cy; //Yp
stabDerivOutput.stabDerivs[20] = pertOutput.Cn; //Np
stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp
input.phiDot = 0;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.betaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f; //yaw rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[21] = pertOutput.Cy; //Yr
stabDerivOutput.stabDerivs[23] = pertOutput.Cn; //Nr
stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr
return(stabDerivOutput);
}
public StabilityDerivExportOutput CalculateStabilityDerivs(CelestialBody body, double alt, double machNumber, int flapSetting, bool spoilers, double alpha, double beta, double phi)
{
double pressure = body.GetPressure(alt);
double temperature = body.GetTemperature(alt);
double density = body.GetDensity(pressure, temperature);
double sspeed = body.GetSpeedOfSound(pressure, density);
double u0 = sspeed * machNumber;
double q = u0 * u0 * density * 0.5f;
StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput();
StabilityDerivExportVariables stabDerivExport = new StabilityDerivExportVariables();
stabDerivOutput.nominalVelocity = u0;
stabDerivOutput.altitude = alt;
stabDerivOutput.body = body;
Vector3d CoM = Vector3d.zero;
double mass = 0;
double MAC = 0;
double b = 0;
double area = 0;
double Ix = 0;
double Iy = 0;
double Iz = 0;
double Ixy = 0;
double Iyz = 0;
double Ixz = 0;
InstantConditionSimInput input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
InstantConditionSimOutput nominalOutput;
InstantConditionSimOutput pertOutput = new InstantConditionSimOutput();
_instantCondition.GetClCdCmSteady(input, out nominalOutput, true);
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;
FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
if (w != null)
{
if (w.isShielded)
{
continue;
}
area += w.S;
MAC += w.GetMAC() * w.S;
b += w.Getb_2() * w.S;
if (w is FARControllableSurface)
{
(w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers);
}
}
}
if (area.NearlyEqual(0))
{
area = _instantCondition._maxCrossSectionFromBody;
MAC = _instantCondition._bodyLength;
b = 1;
}
MAC /= area;
b /= area;
CoM /= mass;
mass *= 1000;
stabDerivOutput.b = b;
stabDerivOutput.MAC = MAC;
stabDerivOutput.area = area;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (p == null || FARAeroUtil.IsNonphysical(p))
{
continue;
}
//This section handles the parallel axis theorem
Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
double x2, y2, z2, x, y, z;
x2 = relPos.z * relPos.z;
y2 = relPos.x * relPos.x;
z2 = relPos.y * relPos.y;
x = relPos.z;
y = relPos.x;
z = relPos.y;
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
Ix += (y2 + z2) * partMass;
Iy += (x2 + z2) * partMass;
Iz += (x2 + y2) * partMass;
Ixy += -x * y * partMass;
Iyz += -z * y * partMass;
Ixz += -x * z * partMass;
//And this handles the part's own moment of inertia
Vector3 principalInertia = p.Rigidbody.inertiaTensor;
Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;
//The rows of the direction cosine matrix for a quaternion
Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));
Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
-prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));
Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
-prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);
//And converting the principal moments of inertia into the coordinate system used by the system
Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;
Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
}
Ix *= 1000;
Iy *= 1000;
Iz *= 1000;
stabDerivOutput.stabDerivs[0] = Ix;
stabDerivOutput.stabDerivs[1] = Iy;
stabDerivOutput.stabDerivs[2] = Iz;
stabDerivOutput.stabDerivs[24] = Ixy;
stabDerivOutput.stabDerivs[25] = Iyz;
stabDerivOutput.stabDerivs[26] = Ixz;
double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt); //This is the effect of gravity
effectiveG -= u0 * u0 / (alt + body.Radius); //This is the effective reduction of gravity due to high velocity
double neededCl = mass * effectiveG / (q * area);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
//Longitudinal Mess
_instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);
alpha = FARMathUtil.SelectedSearchMethod(machNumber, _instantCondition.FunctionIterateForAlpha);
input.alpha = alpha;
nominalOutput = _instantCondition.iterationOutput;
//alpha_str = (alpha * Mathf.PI / 180).ToString();
input.alpha = (alpha + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
stabDerivOutput.stableCl = neededCl;
stabDerivOutput.stableCd = nominalOutput.Cd;
stabDerivOutput.stableAoA = alpha;
stabDerivOutput.stableAoAState = "";
if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
{
stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">");
}
FARLogger.Info("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad); //vert vel derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);
pertOutput.Cl += nominalOutput.Cd;
pertOutput.Cd -= nominalOutput.Cl;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (Iy * u0);
stabDerivOutput.stabDerivs[3] = pertOutput.Cl; //Zw
stabDerivOutput.stabDerivs[4] = pertOutput.Cd; //Xw
stabDerivOutput.stabDerivs[5] = pertOutput.Cm; //Mw
// Rodhern: The motivation for the revised stability derivatives sign interpretations of Zq, Xq, Ze and Xe
// is to align the sign conventions used for Zu, Zq, Ze, Xu, Xq and Xe. Further explanation can be found
// here: https://forum.kerbalspaceprogram.com/index.php?/topic/109098-official-far-craft-repository/&do=findComment&comment=2425057
input.alpha = alpha;
input.machNumber = machNumber + 0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber; //fwd vel derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;
pertOutput.Cl += 2 * nominalOutput.Cl;
pertOutput.Cd += 2 * nominalOutput.Cd;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (u0 * Iy);
stabDerivOutput.stabDerivs[6] = pertOutput.Cl; //Zu
stabDerivOutput.stabDerivs[7] = pertOutput.Cd; //Xu
stabDerivOutput.stabDerivs[8] = pertOutput.Cm; //Mu
input.machNumber = machNumber;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.alphaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05; //pitch rate derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;
pertOutput.Cl *= -q * area * MAC / (2 * u0 * mass); // Rodhern: Replaced 'q' by '-q', so that formulas
pertOutput.Cd *= -q * area * MAC / (2 * u0 * mass); // for Zq and Xq match those for Zu and Xu.
pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);
stabDerivOutput.stabDerivs[9] = pertOutput.Cl; //Zq
stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq
input.alphaDot = 0;
input.pitchValue = 0.1;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1; //elevator derivs
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;
pertOutput.Cl *= -q * area / mass; // Rodhern: Replaced 'q' by '-q', so that formulas
pertOutput.Cd *= -q * area / mass; // for Ze and Xe match those for Zu and Xu.
pertOutput.Cm *= q * area * MAC / Iy;
stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me
//Lateral Mess
input.pitchValue = 0;
input.beta = (beta + 2);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad); //sideslip angle derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);
pertOutput.Cy *= q * area / mass;
pertOutput.Cn *= q * area * b / Iz;
pertOutput.C_roll *= q * area * b / Ix;
stabDerivOutput.stabDerivs[15] = pertOutput.Cy; //Yb
stabDerivOutput.stabDerivs[17] = pertOutput.Cn; //Nb
stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb
input.beta = beta;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.phiDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05; //roll rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[18] = pertOutput.Cy; //Yp
stabDerivOutput.stabDerivs[20] = pertOutput.Cn; //Np
stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp
input.phiDot = 0;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.betaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f; //yaw rate derivs
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[21] = pertOutput.Cy; //Yr
stabDerivOutput.stabDerivs[23] = pertOutput.Cn; //Nr
stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr
// Assign values to export variables
stabDerivExport.craftmass = mass;
stabDerivExport.envpressure = pressure;
stabDerivExport.envtemperature = temperature;
stabDerivExport.envdensity = density;
stabDerivExport.envsoundspeed = sspeed;
stabDerivExport.envg = _instantCondition.CalculateAccelerationDueToGravity(body, alt);
stabDerivExport.sitmach = machNumber;
stabDerivExport.sitdynpres = q;
stabDerivExport.siteffg = effectiveG;
return(new StabilityDerivExportOutput(stabDerivOutput, stabDerivExport));
}
public void GetClCdCmSteady(Vector3d CoM, double alpha, double beta, double phi, double alphaDot, double betaDot, double phiDot, double M, double pitch, out double Cl, out double Cd, out double Cm, out double Cy, out double Cn, out double C_roll, bool clear, bool reset_stall = false, int flap_setting = 0, bool spoilersDeployed = false, bool vehicleFueled = true)
{
Cl = 0;
Cd = 0;
Cm = 0;
Cy = 0;
Cn = 0;
C_roll = 0;
double area = 0;
double MAC = 0;
double b_2 = 0;
alpha *= FARMathUtil.deg2rad;
beta *= FARMathUtil.deg2rad;
phi *= FARMathUtil.deg2rad;
Vector3d forward = Vector3.forward;
Vector3d up = Vector3.up;
Vector3d right = Vector3.right;
if (EditorDriver.editorFacility == EditorFacility.VAB)
{
forward = Vector3.up;
up = -Vector3.forward;
}
Vector3d AngVel = (phiDot - Math.Sin(alpha) * betaDot) * forward + (Math.Cos(phi) * alphaDot + Math.Cos(alpha) * Math.Sin(phi) * betaDot) * right + (Math.Sin(phi) * alphaDot - Math.Cos(alpha) * Math.Cos(phi) * betaDot) * up;
Vector3d velocity = forward * Math.Cos(alpha) * Math.Cos(beta) + right * (Math.Sin(phi) * Math.Sin(alpha) * Math.Cos(beta) - Math.Cos(phi) * Math.Sin(beta)) - up * (Math.Cos(phi) * Math.Sin(alpha) * Math.Cos(beta) - Math.Cos(phi) * Math.Sin(beta));
velocity.Normalize();
Vector3d liftVector = -forward *Math.Sin(alpha) + right * Math.Sin(phi) * Math.Cos(alpha) - up * Math.Cos(phi) * Math.Cos(alpha);
Vector3d sideways = Vector3.Cross(velocity, liftVector);
for (int i = 0; i < FARAeroUtil.CurEditorWings.Count; i++)
{
FARWingAerodynamicModel w = FARAeroUtil.CurEditorWings[i];
if (w.isShielded)
{
continue;
}
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)pitch, 0, 0, flap_setting, spoilersDeployed);
}
w.ComputeClCdEditor(vel, M);
double tmpCl = w.GetCl() * w.S;
Cl += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), liftVector);
Cy += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), sideways);
double tmpCd = w.GetCd() * w.S;
Cd += tmpCd;
Cm += tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), liftVector) + tmpCd * -Vector3d.Dot((relPos), liftVector);
Cn += tmpCd * Vector3d.Dot((relPos), sideways) + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), sideways);
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;
}
for (int i = 0; i < FARAeroUtil.CurEditorOtherDrag.Count; i++)
{
FARBasicDragModel d = FARAeroUtil.CurEditorOtherDrag[i];
if (d.isShielded)
{
continue;
}
Vector3d relPos = d.part.transform.position - CoM;
Vector3d vel = velocity + Vector3d.Cross(AngVel, relPos);
double tmpCd = d.GetDragEditor(vel, M);
Cd += tmpCd;
double tmpCl = d.GetLiftEditor();
Cl += tmpCl * -Vector3d.Dot(d.GetLiftDirection(), liftVector);
Cy += tmpCl * -Vector3d.Dot(d.GetLiftDirection(), sideways);
relPos = d.GetCoDWithoutMomentShift() - CoM;
Cm += d.GetMomentEditor() + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(d.GetLiftDirection(), liftVector) + tmpCd * -Vector3d.Dot((relPos), liftVector);
Cn += tmpCd * Vector3d.Dot((relPos), sideways) + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(d.GetLiftDirection(), sideways);
C_roll += tmpCl * Vector3d.Dot((relPos), sideways) * -Vector3d.Dot(d.GetLiftDirection(), liftVector);
}
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 (vehicleFueled && p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
double stock_drag = partMass * p.maximum_drag * FlightGlobals.DragMultiplier * 1000;
Cd += stock_drag;
Cm += stock_drag * -Vector3d.Dot(part_pos, liftVector);
Cn += stock_drag * Vector3d.Dot(part_pos, sideways);
}
if (area == 0)
{
area = 1;
b_2 = 1;
MAC = 1;
}
double recipArea = 1 / area;
MAC *= recipArea;
b_2 *= recipArea;
Cl *= recipArea;
Cd *= recipArea;
Cm *= recipArea / MAC;
Cy *= recipArea;
Cn *= recipArea / b_2;
C_roll *= recipArea / b_2;
}
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;
if (EditorDriver.editorFacility == EditorFacility.VAB)
{
forward = Vector3.up;
up = -Vector3.forward;
}
double mass = 0;
List <Part> partsList = EditorLogic.SortedShipList;
foreach (Part p in partsList)
{
if (FARAeroUtil.IsNonphysical(p))
{
continue;
}
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
// 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;
// Rodhern: The original reference directions (velocity, liftVector, sideways) did not form an orthonormal
// basis. That in turn produced some counterintuitive calculation results, such as coupled yaw and pitch
// derivatives. A more thorough discussion of the topic can be found on the KSP forums:
// https://forum.kerbalspaceprogram.com/index.php?/topic/19321-131-ferram-aerospace-research-v01591-liepmann-4218/&do=findComment&comment=2781270
// The reference directions have been replaced by new ones that are orthonormal by construction.
// In dkavolis branch Vector3.Cross() and Vector3d.Normalize() are used explicitly. There is no apparent
// benefit to this other than possibly improved readability.
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 velocity = forward * cosAlpha * cosBeta;
velocity += right * (sinPhi * sinAlpha * cosBeta + cosPhi * sinBeta);
velocity += -up * (cosPhi * sinAlpha * cosBeta - sinPhi * sinBeta);
velocity.Normalize();
Vector3d liftDown = -forward * sinAlpha;
liftDown += right * sinPhi * cosAlpha;
liftDown += -up * cosPhi * cosAlpha;
liftDown.Normalize();
Vector3d sideways = Vector3.Cross(velocity, liftDown);
sideways.Normalize();
Vector3d angVel = forward * (phiDot - sinAlpha * betaDot);
angVel += right * (cosPhi * alphaDot + cosAlpha * sinPhi * betaDot);
angVel += up * (sinPhi * alphaDot - cosAlpha * cosPhi * betaDot);
foreach (FARWingAerodynamicModel w in _wingAerodynamicModel)
{
if (!(w && w.part))
{
continue;
}
w.ComputeForceEditor(velocity, 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 controllableSurface)
{
controllableSurface.SetControlStateEditor(CoM,
vel,
(float)input.pitchValue,
0,
0,
input.flaps,
input.spoilers);
}
else if (w.isShielded)
{
continue;
}
Vector3d force = w.ComputeForceEditor(vel.normalized, input.machNumber, 2) * 1000;
output.Cl += -Vector3d.Dot(force, liftDown);
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, liftDown);
output.C_roll += Vector3d.Dot(moment, velocity);
area += w.S;
MAC += w.GetMAC() * w.S;
b_2 += w.Getb_2() * w.S;
}
var center = new FARCenterQuery();
foreach (FARAeroSection aeroSection in _currentAeroSections)
{
aeroSection.PredictionCalculateAeroForces(2,
(float)input.machNumber,
10000,
0,
0.005f,
velocity.normalized,
center);
}
Vector3d centerForce = center.force * 1000;
output.Cl += -Vector3d.Dot(centerForce, liftDown);
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, liftDown);
output.C_roll += Vector3d.Dot(centerMoment, velocity);
if (area.NearlyEqual(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;
}
public StabilityDerivOutput CalculateStabilityDerivs(
CelestialBody body,
double alt,
double machNumber,
int flapSetting,
bool spoilers,
double alpha,
double beta,
double phi
)
{
GasProperties properties = FARAtmosphere.GetGasProperties(body,
new Vector3d(0, 0, alt),
Planetarium.GetUniversalTime());
double pressure = properties.Pressure;
double temperature = properties.Temperature;
double density = properties.Density;
double sspeed = properties.SpeedOfSound;
double u0 = sspeed * machNumber;
double q = u0 * u0 * density * 0.5f;
var stabDerivOutput = new StabilityDerivOutput
{
nominalVelocity = u0,
altitude = alt,
body = body
};
Vector3d CoM = Vector3d.zero;
double mass = 0;
double MAC = 0;
double b = 0;
double area = 0;
double Ix = 0;
double Iy = 0;
double Iz = 0;
double Ixy = 0;
double Iyz = 0;
double Ixz = 0;
var input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
var pertOutput = new InstantConditionSimOutput();
_instantCondition.GetClCdCmSteady(input, out InstantConditionSimOutput nominalOutput, true);
List <Part> partsList = EditorLogic.SortedShipList;
foreach (Part p in partsList)
{
if (FARAeroUtil.IsNonphysical(p))
{
continue;
}
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
// 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;
FARWingAerodynamicModel w = p.GetComponent <FARWingAerodynamicModel>();
if (w == null)
{
continue;
}
if (w.isShielded)
{
continue;
}
area += w.S;
MAC += w.GetMAC() * w.S;
b += w.Getb_2() * w.S;
if (w is FARControllableSurface controllableSurface)
{
controllableSurface.SetControlStateEditor(CoM,
p.transform.up,
0,
0,
0,
input.flaps,
input.spoilers);
}
}
if (area.NearlyEqual(0))
{
area = _instantCondition._maxCrossSectionFromBody;
MAC = _instantCondition._bodyLength;
b = 1;
}
MAC /= area;
b /= area;
CoM /= mass;
mass *= 1000;
stabDerivOutput.b = b;
stabDerivOutput.MAC = MAC;
stabDerivOutput.area = area;
foreach (Part p in partsList)
{
if (p == null || FARAeroUtil.IsNonphysical(p))
{
continue;
}
//This section handles the parallel axis theorem
Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
double x2 = relPos.z * relPos.z;
double y2 = relPos.x * relPos.x;
double z2 = relPos.y * relPos.y;
double x = relPos.z;
double y = relPos.x;
double z = relPos.y;
double partMass = p.mass;
if (p.Resources.Count > 0)
{
partMass += p.GetResourceMass();
}
// If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
Ix += (y2 + z2) * partMass;
Iy += (x2 + z2) * partMass;
Iz += (x2 + y2) * partMass;
Ixy += -x * y * partMass;
Iyz += -z * y * partMass;
Ixz += -x * z * partMass;
//And this handles the part's own moment of inertia
Vector3 principalInertia = p.Rigidbody.inertiaTensor;
Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;
//The rows of the direction cosine matrix for a quaternion
var Row1 =
new Vector3(prncInertRot.x * prncInertRot.x -
prncInertRot.y * prncInertRot.y -
prncInertRot.z * prncInertRot.z +
prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));
var Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
-prncInertRot.x * prncInertRot.x +
prncInertRot.y * prncInertRot.y -
prncInertRot.z * prncInertRot.z +
prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));
var Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
-prncInertRot.x * prncInertRot.x -
prncInertRot.y * prncInertRot.y +
prncInertRot.z * prncInertRot.z +
prncInertRot.w * prncInertRot.w);
//And converting the principal moments of inertia into the coordinate system used by the system
Ix += principalInertia.x * Row1.x * Row1.x +
principalInertia.y * Row1.y * Row1.y +
principalInertia.z * Row1.z * Row1.z;
Iy += principalInertia.x * Row2.x * Row2.x +
principalInertia.y * Row2.y * Row2.y +
principalInertia.z * Row2.z * Row2.z;
Iz += principalInertia.x * Row3.x * Row3.x +
principalInertia.y * Row3.y * Row3.y +
principalInertia.z * Row3.z * Row3.z;
Ixy += principalInertia.x * Row1.x * Row2.x +
principalInertia.y * Row1.y * Row2.y +
principalInertia.z * Row1.z * Row2.z;
Ixz += principalInertia.x * Row1.x * Row3.x +
principalInertia.y * Row1.y * Row3.y +
principalInertia.z * Row1.z * Row3.z;
Iyz += principalInertia.x * Row2.x * Row3.x +
principalInertia.y * Row2.y * Row3.y +
principalInertia.z * Row2.z * Row3.z;
}
Ix *= 1000;
Iy *= 1000;
Iz *= 1000;
stabDerivOutput.stabDerivs[0] = Ix;
stabDerivOutput.stabDerivs[1] = Iy;
stabDerivOutput.stabDerivs[2] = Iz;
stabDerivOutput.stabDerivs[24] = Ixy;
stabDerivOutput.stabDerivs[25] = Iyz;
stabDerivOutput.stabDerivs[26] = Ixz;
//This is the effect of gravity
double effectiveG = InstantConditionSim.CalculateAccelerationDueToGravity(body, alt);
//This is the effective reduction of gravity due to high velocity
effectiveG -= u0 * u0 / (alt + body.Radius);
double neededCl = mass * effectiveG / (q * area);
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
//Longitudinal Mess
_instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);
FARMathUtil.OptimizationResult optResult =
FARMathUtil.Secant(_instantCondition.FunctionIterateForAlpha,
0,
10,
1e-4,
1e-4,
minLimit: -90,
maxLimit: 90);
int calls = optResult.FunctionCalls;
// if stable AoA doesn't exist, calculate derivatives at 0 incidence
if (!optResult.Converged)
{
FARLogger.Info("Stable angle of attack not found, calculating derivatives at 0 incidence instead");
alpha = 0;
_instantCondition.FunctionIterateForAlpha(alpha);
calls += 1;
}
else
{
alpha = optResult.Result;
}
input.alpha = alpha;
nominalOutput = _instantCondition.iterationOutput;
input.alpha = alpha + 2;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
stabDerivOutput.stableCl = neededCl;
stabDerivOutput.stableCd = nominalOutput.Cd;
stabDerivOutput.stableAoA = alpha;
stabDerivOutput.stableAoAState = "";
if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
{
stabDerivOutput.stableAoAState = nominalOutput.Cl > neededCl ? "<" : ">";
}
FARLogger.Info("Cl needed: " +
neededCl.ToString(CultureInfo.InvariantCulture) +
", AoA: " +
stabDerivOutput.stableAoA.ToString(CultureInfo.InvariantCulture) +
", Cl: " +
nominalOutput.Cl.ToString(CultureInfo.InvariantCulture) +
", Cd: " +
nominalOutput.Cd.ToString(CultureInfo.InvariantCulture) +
", function calls: " +
calls.ToString());
//vert vel derivs
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad);
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);
pertOutput.Cl += nominalOutput.Cd;
pertOutput.Cd -= nominalOutput.Cl;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (Iy * u0);
stabDerivOutput.stabDerivs[3] = pertOutput.Cl; //Zw
stabDerivOutput.stabDerivs[4] = pertOutput.Cd; //Xw
stabDerivOutput.stabDerivs[5] = pertOutput.Cm; //Mw
// Rodhern: The motivation for the revised stability derivatives sign interpretations of Zq, Xq, Ze and Xe
// is to align the sign conventions used for Zu, Zq, Ze, Xu, Xq and Xe. Further explanation can be found
// here: https://forum.kerbalspaceprogram.com/index.php?/topic/109098-official-far-craft-repository/&do=findComment&comment=2425057
input.alpha = alpha;
input.machNumber = machNumber + 0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//fwd vel derivs
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber;
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;
pertOutput.Cl += 2 * nominalOutput.Cl;
pertOutput.Cd += 2 * nominalOutput.Cd;
pertOutput.Cl *= -q * area / (mass * u0);
pertOutput.Cd *= -q * area / (mass * u0);
pertOutput.Cm *= q * area * MAC / (u0 * Iy);
stabDerivOutput.stabDerivs[6] = pertOutput.Cl; //Zu
stabDerivOutput.stabDerivs[7] = pertOutput.Cd; //Xu
stabDerivOutput.stabDerivs[8] = pertOutput.Cm; //Mu
input.machNumber = machNumber;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.alphaDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//pitch rate derivs
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05;
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;
pertOutput.Cl *= -q * area * MAC / (2 * u0 * mass); // Rodhern: Replaced 'q' by '-q', so that formulas
pertOutput.Cd *= -q * area * MAC / (2 * u0 * mass); // for Zq and Xq match those for Zu and Xu.
pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);
stabDerivOutput.stabDerivs[9] = pertOutput.Cl; //Zq
stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq
input.alphaDot = 0;
input.pitchValue = 0.1;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//elevator derivs
pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1;
pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;
pertOutput.Cl *= -q * area / mass; // Rodhern: Replaced 'q' by '-q', so that formulas
pertOutput.Cd *= -q * area / mass; // for Ze and Xe match those for Zu and Xu.
pertOutput.Cm *= q * area * MAC / Iy;
stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me
//Lateral Mess
input.pitchValue = 0;
input.beta = beta + 2;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//sideslip angle derivs
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad);
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);
pertOutput.Cy *= q * area / mass;
pertOutput.Cn *= q * area * b / Iz;
pertOutput.C_roll *= q * area * b / Ix;
stabDerivOutput.stabDerivs[15] = pertOutput.Cy; //Yb
stabDerivOutput.stabDerivs[17] = pertOutput.Cn; //Nb
stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb
input.beta = beta;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.phiDot = -0.05;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
//roll rate derivs
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05;
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[18] = pertOutput.Cy; //Yp
stabDerivOutput.stabDerivs[20] = pertOutput.Cn; //Np
stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp
input.phiDot = 0;
_instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
input.betaDot = -0.05;
//yaw rate derivs
_instantCondition.GetClCdCmSteady(input, out pertOutput, true);
pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f;
pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;
pertOutput.Cy *= q * area * b / (2 * mass * u0);
pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);
stabDerivOutput.stabDerivs[21] = pertOutput.Cy; //Yr
stabDerivOutput.stabDerivs[23] = pertOutput.Cn; //Nr
stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr
return(stabDerivOutput);
}
public void GetInertia(Vector3d CoM, out double Ix, out double Iy, out double Iz, out double Ixy, out double Iyz, out double Ixz)
{
Ix = 0; Iy = 0; Iz = 0; Ixy = 0; Iyz = 0; Ixz = 0;
List <Part> partsList = EditorLogic.SortedShipList;
for (int i = 0; i < partsList.Count; i++)
{
Part p = partsList[i];
if (p == null || FARAeroUtil.IsNonphysical(p))
{
continue;
}
//This section handles the parallel axis theorem
Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
double x2, y2, z2, x, y, z;
x2 = relPos.z * relPos.z;
y2 = relPos.x * relPos.x;
z2 = relPos.y * relPos.y;
x = relPos.z;
y = relPos.x;
z = relPos.y;
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
Ix += (y2 + z2) * partMass;
Iy += (x2 + z2) * partMass;
Iz += (x2 + y2) * partMass;
Ixy += -x * y * partMass;
Iyz += -z * y * partMass;
Ixz += -x * z * partMass;
//And this handles the part's own moment of inertia
Vector3 principalInertia = p.Rigidbody.inertiaTensor;
Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;
//The rows of the direction cosine matrix for a quaternion
Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));
Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
-prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));
Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
-prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);
//And converting the principal moments of inertia into the coordinate system used by the system
Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;
Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
}
Ix *= 1000;
Iy *= 1000;
Iz *= 1000;
}
private bool FindPartsWithoutFARModel(List <Part> editorShip)
{
bool returnValue = false;
foreach (Part p in editorShip)
{
if (p == null)
{
continue;
}
if (p != null && FARAeroUtil.IsNonphysical(p) &&
p.physicalSignificance != Part.PhysicalSignificance.NONE)
{
MonoBehaviour.print(p + ": FAR correcting physical significance to fix CoM in editor");
p.physicalSignificance = Part.PhysicalSignificance.NONE;
}
string title = p.partInfo.title.ToLowerInvariant();
if (p is StrutConnector || p is FuelLine || p is ControlSurface || p is Winglet || FARPartClassification.ExemptPartFromGettingDragModel(p, title))
{
continue;
}
FARPartModule q = p.GetComponent <FARPartModule>();
if (q != null && !(q is FARControlSys))
{
continue;
}
bool updatedModules = false;
if (FARPartClassification.PartIsCargoBay(p, title))
{
if (!p.Modules.Contains("FARCargoBayModule"))
{
p.AddModule("FARCargoBayModule");
p.Modules["FARCargoBayModule"].OnStart(PartModule.StartState.Editor);
FARAeroUtil.AddBasicDragModule(p);
p.Modules["FARBasicDragModel"].OnStart(PartModule.StartState.Editor);
updatedModules = true;
}
}
if (!updatedModules)
{
if (FARPartClassification.PartIsPayloadFairing(p, title))
{
if (!p.Modules.Contains("FARPayloadFairingModule"))
{
p.AddModule("FARPayloadFairingModule");
p.Modules["FARPayloadFairingModule"].OnStart(PartModule.StartState.Editor);
FARAeroUtil.AddBasicDragModule(p);
p.Modules["FARBasicDragModel"].OnStart(PartModule.StartState.Editor);
updatedModules = true;
}
}
if (!updatedModules && !p.Modules.Contains("FARBasicDragModel"))
{
FARAeroUtil.AddBasicDragModule(p);
p.Modules["FARBasicDragModel"].OnStart(PartModule.StartState.Editor);
updatedModules = true;
}
}
returnValue |= updatedModules;
FARPartModule b = p.GetComponent <FARPartModule>();
if (b != null)
{
b.VesselPartList = editorShip; //This prevents every single part in the ship running this due to VesselPartsList not being initialized
}
}
return(returnValue);
}