| public static GetDensity ( UnityEngine.Vector3d position, CelestialBody body ) : double | ||
| position | UnityEngine.Vector3d | |
| body | CelestialBody | |
| return | double | |
public override Vector3d UnpackForces(Vector2 packedForces, double altitudeAboveSea, double velocity)
{
double rho = StockAeroUtil.GetDensity(altitudeAboveSea, body_);
double scale = velocity * velocity * rho;
return(new Vector3d((double)packedForces.x * scale, (double)packedForces.y * scale, 0.0));
}
private Vector3d getForces_FAR(CelestialBody body, Vector3d bodySpacePosition, Vector3d airVelocity, double angleOfAttack, double dt)
{
if (!isFARInitialized())
{
return(new Vector3d(0, 0, 0));
}
double altitudeAboveSea = bodySpacePosition.magnitude - body.Radius;
double stockRho = StockAeroUtil.GetDensity(altitudeAboveSea, body);
if (stockRho <= 0)
{
return(Vector3d.zero);
}
double rho = stockRho;
rho = useNEAR ? stockRho : (double)FARAeroUtil_GetCurrentDensity.Invoke(null, new object[] { body, altitudeAboveSea, false });
#if !USE_CACHE
return(computeForces_FAR(altitudeAboveSea, airVelocity, bodySpacePosition, angleOfAttack, dt));
#else
//double approxMachNumber = useNEAR ? 0.0 : (double)FARAeroUtil_GetMachNumber.Invoke(null, new object[] { body_, body.maxAtmosphereAltitude * 0.5, new Vector3d((float)airVelocity.magnitude, 0, 0) });
//Util.PostSingleScreenMessage("machNum", "machNumber = " + actualMachNumber + " ; approx machNumber = " + approxMachNumber);
Vector2 force = getCachedForce(airVelocity.magnitude, altitudeAboveSea, angleOfAttack);
Vector3d forward = airVelocity.normalized;
Vector3d right = Vector3d.Cross(forward, bodySpacePosition).normalized;
Vector3d up = Vector3d.Cross(right, forward).normalized;
return(forward * force.x + up * force.y);
#endif
}
public Vector3d getForces_StockAero(CelestialBody body, Vector3d bodySpacePosition, Vector3d airVelocity, double angleOfAttack, double dt)
{
Vector3d position = body.position + bodySpacePosition;
double altitude = (position - body.position).magnitude - body.Radius;
if (altitude > body.atmosphereDepth)
{
return(Vector3d.zero);
}
#if !USE_CACHE
return(computeForces_StockAero(altitude, airVelocity, new Vector3(0, 1, 0), angleOfAttack, dt));
#else
//double approxMachNumber = useNEAR ? 0.0 : (double)FARAeroUtil_GetMachNumber.Invoke(null, new object[] { body_, body.maxAtmosphereAltitude * 0.5, new Vector3d((float)airVelocity.magnitude, 0, 0) });
//Util.PostSingleScreenMessage("machNum", "machNumber = " + actualMachNumber + " ; approx machNumber = " + approxMachNumber);
Vector2 force = getCachedForce(airVelocity.magnitude, altitude, angleOfAttack);
// adjust force using the more accurate air density that we can compute knowing where the vessel is relatively to the sun and body
double preciseRho = StockAeroUtil.GetDensity(position, body);
double approximateRho = StockAeroUtil.GetDensity(altitude, body);
if (approximateRho > 0)
{
force = force * (float)(preciseRho / approximateRho);
}
Vector3d forward = airVelocity.normalized;
Vector3d right = Vector3d.Cross(forward, bodySpacePosition).normalized;
Vector3d up = Vector3d.Cross(right, forward).normalized;
return(forward * force.x + up * force.y);
#endif
}
public void FixedUpdate()
{
if (aerodynamicModel_ != null && vessel_ != null)
{
CelestialBody body = vessel_.orbit.referenceBody;
Vector3d bodySpacePosition = vessel_.GetWorldPos3D() - body.position;
Vector3d bodySpaceVelocity = vessel_.obt_velocity;
double altitudeAboveSea = bodySpacePosition.magnitude - body.Radius;
Vector3d airVelocity = bodySpaceVelocity - body.getRFrmVel(body.position + bodySpacePosition);
#if DEBUG_COMPARE_FORCES
Vector3d FARForce = FARBasicDragModel.debugForceAccumulator + FARWingAerodynamicModel.debugForceAccumulator;
FARBasicDragModel.debugForceAccumulator = new Vector3d(0, 0, 0);
FARWingAerodynamicModel.debugForceAccumulator = new Vector3d(0, 0, 0);
double rho = FARAeroUtil.GetCurrentDensity(body, altitudeAboveSea);
//double rho = vessel_.atmDensity;
//double pressure = FlightGlobals.getStaticPressure(altitudeAboveSea, body);
//double rho = FlightGlobals.getAtmDensity(pressure);
double machNumber = FARAeroUtil.GetMachNumber(body, altitudeAboveSea, airVelocity);
//double machNumber = airVelocity.magnitude / 300.0;
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;
}
Vector3d predictedForce = aerodynamicModel_.computeForces_FAR(rho, machNumber, airVelocity, vesselUp, AoA, 0.05);
aerodynamicModel_.Verbose = true;
Vector3d predictedForceWithCache = aerodynamicModel_.computeForces(body, bodySpacePosition, airVelocity, AoA, 0.05);
aerodynamicModel_.Verbose = false;
Vector3d localFARForce = new Vector3d(Vector3d.Dot(FARForce, vesselRight), Vector3d.Dot(FARForce, vesselUp), Vector3d.Dot(FARForce, 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("FAR/predict comparison", "air vel=" + Math.Floor(airVelocity.magnitude) + ", AoA=" + (AoA * 180.0 / Math.PI) + ", FAR force=" + localFARForce + ", predicted force=" + localPredictedForce);
Util.PostSingleScreenMessage("predict with cache", "predicted force with cache=" + localPredictedForceWithCache);
#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") + " ; approximateRho=" + approximateRho.ToString("0.0000") + " ; actualRho=" + actualRho.ToString("0.0000"));
}
}
public override Vector2 PackForces(Vector3d forces, double altitudeAboveSea, double velocity)
{
double rho = StockAeroUtil.GetDensity(altitudeAboveSea, body_); // would be even better to use FAR method of computing the air density (which also depends on velocity), but this is already better than nothing
if (rho < 0.0000000001)
{
return(new Vector2(0, 0));
}
double invScale = 1.0 / (rho * Math.Max(1.0, velocity * velocity));
forces *= invScale;
return(new Vector2((float)forces.x, (float)forces.y));
}
public override Vector2 PackForces(Vector3d forces, double altitudeAboveSea, double velocity)
{
double rho = StockAeroUtil.GetDensity(altitudeAboveSea, body_);
if (rho < 0.0000000001)
{
return(new Vector2(0, 0));
}
double invScale = 1.0 / (rho * Math.Max(1.0, velocity * velocity)); // divide by v² and rho before storing the force, to increase accuracy (the reverse operation is performed when reading from the cache)
forces *= invScale;
return(new Vector2((float)forces.x, (float)forces.y));
}
private Vector2 getCachedForce(double velocity, double altitudeAboveSea, double angleOfAttack)
{
precomputeCache();
//Util.PostSingleScreenMessage("getFARForce velocity", "velocity = " + velocity);
float vFrac = (float)(velocity / maxFARVelocity * (double)(cachedFARForces.GetLength(0) - 1));
int vFloor = Math.Min(cachedFARForces.GetLength(0) - 2, (int)vFrac);
vFrac = Math.Min(1.0f, vFrac - (float)vFloor);
float aFrac = (float)((angleOfAttack / maxFARAngleOfAttack * 0.5 + 0.5) * (double)(cachedFARForces.GetLength(1) - 1));
int aFloor = Math.Max(0, Math.Min(cachedFARForces.GetLength(1) - 2, (int)aFrac));
aFrac = Math.Max(0.0f, Math.Min(1.0f, aFrac - (float)aFloor));
double maxAltitude = body_.atmosphereDepth;
float mFrac = (float)(altitudeAboveSea / maxAltitude * (double)(cachedFARForces.GetLength(2) - 1));
int mFloor = Math.Max(0, Math.Min(cachedFARForces.GetLength(2) - 2, (int)mFrac));
mFrac = Math.Max(0.0f, Math.Min(1.0f, mFrac - (float)mFloor));
if (Verbose)
{
Util.PostSingleScreenMessage("cache cell", "cache cell: [" + vFloor + ", " + aFloor + ", " + mFloor + "]");
Util.PostSingleScreenMessage("altitude cell", "altitude cell: " + altitudeAboveSea + " / " + maxAltitude + " * " + (double)(cachedFARForces.GetLength(2) - 1));
}
Vector2 res = sample3d(vFloor, vFrac, aFloor, aFrac, mFloor, mFrac);
double stockRho = StockAeroUtil.GetDensity(altitudeAboveSea, body_);
double rho = stockRho;
if (!useStockModel)
{
rho = useNEAR ? stockRho : (double)FARAeroUtil_GetCurrentDensity.Invoke(null, new object[] { body_, altitudeAboveSea, false });
}
res = res * (float)(velocity * velocity * rho);
return(res);
}
private Vector2 computeCacheEntry(int v, int a, int m)
{
double vel = maxFARVelocity * (double)v / (double)(cachedFARForces.GetLength(0) - 1);
double v2 = Math.Max(1.0, vel * vel);
Vector3d velocity = new Vector3d(vel, 0, 0);
double maxAltitude = body_.atmosphereDepth;
double currentAltitude = maxAltitude * (double)m / (double)(cachedFARForces.GetLength(2) - 1);
double stockRho = StockAeroUtil.GetDensity(currentAltitude, body_);
double rho = stockRho;
if (!useStockModel)
{
if (!isFARInitialized())
{
throw new Exception("Internal error");
}
rho = useNEAR ? stockRho : (double)FARAeroUtil_GetCurrentDensity.Invoke(null, new object[] { body_, currentAltitude, false });
}
if (rho < 0.0000000001)
{
return(new Vector2(0, 0));
}
double invScale = 1.0 / (rho * v2); // divide by v² and rho before storing the force, to increase accuracy (the reverse operation is performed when reading from the cache)
double AoA = maxFARAngleOfAttack * ((double)a / (double)(cachedFARForces.GetLength(1) - 1) * 2.0 - 1.0);
Vector3d force;
if (useStockModel)
{
force = computeForces_StockAero(currentAltitude, velocity, new Vector3(0, 1, 0), AoA, 0.25) * invScale;
}
else
{
force = computeForces_FAR(currentAltitude, velocity, new Vector3(0, 1, 0), AoA, 0.25) * invScale;
}
return(cachedFARForces[v, a, m] = new Vector2((float)force.x, (float)force.y));
}
internal static void DebugTelemetry()
{
if (!Util.IsFlight)
{
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 && Trajectories.IsVesselAttached)
{
CelestialBody body = Trajectories.AttachedVessel.orbit.referenceBody;
bodySpacePosition = Trajectories.AttachedVessel.GetWorldPos3D() - body.position;
bodySpaceVelocity = Trajectories.AttachedVessel.obt_velocity;
double altitudeAboveSea = bodySpacePosition.magnitude - body.Radius;
Vector3d airVelocity = bodySpaceVelocity - body.getRFrmVel(body.position + bodySpacePosition);
double R = PreviousFramePos.magnitude;
Vector3d gravityForce = PreviousFramePos * (-body.gravParameter / (R * R * R) * Trajectories.AttachedVessel.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) * (Trajectories.AttachedVessel.totalMass / dt);
TotalForce += bodySpaceVelocity * (dt * 0.000015); // numeric precision fix
Vector3d ActualForce = TotalForce - gravityForce;
Transform vesselTransform = Trajectories.AttachedVessel.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));
Telemetry.Send("ut", now);
Telemetry.Send("altitude", Trajectories.AttachedVessel.altitude);
Telemetry.Send("airspeed", Math.Floor(airVelocity.magnitude));
Telemetry.Send("aoa", (AoA * 180.0 / Math.PI));
Telemetry.Send("force_actual", localActualForce.magnitude);
Telemetry.Send("force_actual.x", localActualForce.x);
Telemetry.Send("force_actual.y", localActualForce.y);
Telemetry.Send("force_actual.z", localActualForce.z);
//Telemetry.Send("force_total", localTotalForce.magnitude);
//Telemetry.Send("force_total.x", localTotalForce.x);
//Telemetry.Send("force_total.y", localTotalForce.y);
//Telemetry.Send("force_total.z", localTotalForce.z);
Telemetry.Send("force_predicted", localPredictedForce.magnitude);
Telemetry.Send("force_predicted.x", localPredictedForce.x);
Telemetry.Send("force_predicted.y", localPredictedForce.y);
Telemetry.Send("force_predicted.z", localPredictedForce.z);
Telemetry.Send("force_predicted_cache", localPredictedForceWithCache.magnitude);
//Telemetry.Send("force_predicted_cache.x", localPredictedForceWithCache.x);
//Telemetry.Send("force_predicted_cache.y", localPredictedForceWithCache.y);
//Telemetry.Send("force_predicted_cache.z", localPredictedForceWithCache.z);
//Telemetry.Send("force_reference", referenceForce.magnitude);
//Telemetry.Send("force_reference.x", referenceForce.x);
//Telemetry.Send("force_reference.y", referenceForce.y);
//Telemetry.Send("force_reference.z", referenceForce.z);
//Telemetry.Send("velocity.x", bodySpaceVelocity.x);
//Telemetry.Send("velocity.y", bodySpaceVelocity.y);
//Telemetry.Send("velocity.z", bodySpaceVelocity.z);
//Vector3d velocity_pos = (bodySpacePosition - PreviousFramePos) / dt;
//Telemetry.Send("velocity_pos.x", velocity_pos.x);
//Telemetry.Send("velocity_pos.y", velocity_pos.y);
//Telemetry.Send("velocity_pos.z", velocity_pos.z);
Telemetry.Send("drag", Trajectories.AttachedVessel.rootPart.rb.drag);
Telemetry.Send("density", Trajectories.AttachedVessel.atmDensity);
Telemetry.Send("density_calc", StockAeroUtil.GetDensity(altitudeAboveSea, body));
Telemetry.Send("density_calc_precise", StockAeroUtil.GetDensity(Trajectories.AttachedVessel.GetWorldPos3D(), body));
Telemetry.Send("temperature", Trajectories.AttachedVessel.atmosphericTemperature);
Telemetry.Send("temperature_calc", StockAeroUtil.GetTemperature(Trajectories.AttachedVessel.GetWorldPos3D(), body));
}
PreviousFrameVelocity = bodySpaceVelocity;
PreviousFramePos = bodySpacePosition;
PreviousFrameTime = now;
}
}
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;
}
}
