public void FixedUpdate()
{
if (HighLogic.LoadedScene != GameScenes.FLIGHT)
return;
double now = Planetarium.GetUniversalTime();
double dt = now - PreviousFrameTime;
if (dt > 0.5 || dt < 0.0)
{
Vector3d bodySpacePosition = new Vector3d();
Vector3d bodySpaceVelocity = new Vector3d();
if (aerodynamicModel_ != null && vessel_ != null)
{
CelestialBody body = vessel_.orbit.referenceBody;
bodySpacePosition = vessel_.GetWorldPos3D() - body.position;
bodySpaceVelocity = vessel_.obt_velocity;
double altitudeAboveSea = bodySpacePosition.magnitude - body.Radius;
Vector3d airVelocity = bodySpaceVelocity - body.getRFrmVel(body.position + bodySpacePosition);
#if DEBUG_COMPARE_FORCES
double R = PreviousFramePos.magnitude;
Vector3d gravityForce = PreviousFramePos * (-body.gravParameter / (R * R * R) * vessel_.totalMass);
Quaternion inverseRotationFix = body.inverseRotation ? Quaternion.AngleAxis((float)(body.angularVelocity.magnitude / Math.PI * 180.0 * dt), Vector3.up) : Quaternion.identity;
Vector3d TotalForce = (bodySpaceVelocity - inverseRotationFix * PreviousFrameVelocity) * (vessel_.totalMass / dt);
TotalForce += bodySpaceVelocity * (dt * 0.000015); // numeric precision fix
Vector3d ActualForce = TotalForce - gravityForce;
Transform vesselTransform = vessel_.ReferenceTransform;
Vector3d vesselBackward = (Vector3d)(-vesselTransform.up.normalized);
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = (Vector3d)(-vesselTransform.forward.normalized);
Vector3d vesselRight = Vector3d.Cross(vesselUp, vesselBackward).normalized;
double AoA = Math.Acos(Vector3d.Dot(airVelocity.normalized, vesselForward.normalized));
if (Vector3d.Dot(airVelocity, vesselUp) > 0)
AoA = -AoA;
VesselAerodynamicModel.DebugParts = true;
Vector3d referenceForce = aerodynamicModel_.ComputeForces(20000, new Vector3d(0, 0, 1500), new Vector3d(0,1,0), 0);
VesselAerodynamicModel.DebugParts = false;
Vector3d predictedForce = aerodynamicModel_.ComputeForces(altitudeAboveSea, airVelocity, vesselUp, AoA);
//VesselAerodynamicModel.Verbose = true;
Vector3d predictedForceWithCache = aerodynamicModel_.GetForces(body, bodySpacePosition, airVelocity, AoA);
//VesselAerodynamicModel.Verbose = false;
Vector3d localTotalForce = new Vector3d(Vector3d.Dot(TotalForce, vesselRight), Vector3d.Dot(TotalForce, vesselUp), Vector3d.Dot(TotalForce, vesselBackward));
Vector3d localActualForce = new Vector3d(Vector3d.Dot(ActualForce, vesselRight), Vector3d.Dot(ActualForce, vesselUp), Vector3d.Dot(ActualForce, vesselBackward));
Vector3d localPredictedForce = new Vector3d(Vector3d.Dot(predictedForce, vesselRight), Vector3d.Dot(predictedForce, vesselUp), Vector3d.Dot(predictedForce, vesselBackward));
Vector3d localPredictedForceWithCache = new Vector3d(Vector3d.Dot(predictedForceWithCache, vesselRight), Vector3d.Dot(predictedForceWithCache, vesselUp), Vector3d.Dot(predictedForceWithCache, vesselBackward));
Util.PostSingleScreenMessage("actual/predict comparison", "air vel=" + Math.Floor(airVelocity.magnitude) + " ; AoA=" + (AoA * 180.0 / Math.PI));
//Util.PostSingleScreenMessage("total force", "actual total force=" + localTotalForce.ToString("0.000"));
Util.PostSingleScreenMessage("actual force", "actual force=" + localActualForce.ToString("0.000"));
Util.PostSingleScreenMessage("predicted force", "predicted force=" + localPredictedForce.ToString("0.000"));
Util.PostSingleScreenMessage("predict with cache", "predicted force with cache=" + localPredictedForceWithCache.ToString("0.000"));
Util.PostSingleScreenMessage("reference force", "reference force=" + referenceForce.ToString("0.000"));
Util.PostSingleScreenMessage("current vel", "current vel=" + bodySpaceVelocity.ToString("0.00") + " (mag=" + bodySpaceVelocity.magnitude.ToString("0.00") + ")");
//Util.PostSingleScreenMessage("vel from pos", "vel from pos=" + ((bodySpacePosition - PreviousFramePos) / dt).ToString("0.000") + " (mag=" + ((bodySpacePosition - PreviousFramePos) / dt).magnitude.ToString("0.00") + ")");
Util.PostSingleScreenMessage("force diff", "force ratio=" + (localActualForce.z / localPredictedForce.z).ToString("0.000"));
Util.PostSingleScreenMessage("drag", "physics drag=" + vessel_.rootPart.rb.drag);
#endif
double approximateRho = StockAeroUtil.GetDensity(altitudeAboveSea, body);
double preciseRho = StockAeroUtil.GetDensity(vessel_.GetWorldPos3D(), body);
double actualRho = vessel_.atmDensity;
Util.PostSingleScreenMessage("rho info", /*"preciseRho=" + preciseRho.ToString("0.0000") + " ; " +*/ "rho=" + approximateRho.ToString("0.0000") + " ; actual=" + actualRho.ToString("0.0000") + " ; ratio=" + (actualRho / approximateRho).ToString("0.00"));
}
PreviousFrameVelocity = bodySpaceVelocity;
PreviousFramePos = bodySpacePosition;
PreviousFrameTime = now;
}
}
/// <summary>Format a Vector3d to bas saved in a ConfigNode</summary>
public static string FormatVector3D(Vector3d vector)
{
string vectorString = vector.ToString();
vectorString = vectorString.Replace("[", "");
vectorString = vectorString.Replace("]", "");
return vectorString;
}
/// <summary>
/// Get ambient conditions at a particular location
/// Density is calculated automatically according to gas law. If the density KSP gives differs by more than 1% from this, then a warning is printed
/// </summary>
/// <param name="position">3D position at which to get conditions</param>
/// <param name="body">Which body to use. Can be null, in which case home body is probably used</param>
public void FromAmbientAtLocation(Vector3d position, CelestialBody body)
{
Zero();
P = FlightGlobals.getStaticPressure(position, body) * 1000d;
_T = FlightGlobals.getExternalTemperature(position, body);
Far = 0d; // Recalculates, so no need to do by hand
if (Math.Abs(FlightGlobals.getAtmDensity(P, T, body) / Rho - 1d) > 0.01d)
if (body != null)
Debug.LogWarning("Ambient density does not obey the gas law on body " + body.name + " at position " + position.ToString());
else
Debug.LogWarning("Ambient density does not obey the gas law on body at position " + position.ToString());
}
