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 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));
}
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;
}
}
protected override Vector3d ComputeForces_Model(Vector3d airVelocity, double altitude)
{
return((Vector3d)StockAeroUtil.SimAeroForce(vessel_, (Vector3)airVelocity, altitude));
}
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;
}
}
public Vector3d computeForces_StockAero(double altitude, Vector3d airVelocity, Vector3d vup, double angleOfAttack, double dt)
{
Transform vesselTransform = vessel_.ReferenceTransform;
//TODO: This is the same code as FAR call, so clearly it can be common in computeForces
// this is weird, but the vessel orientation does not match the reference transform (up is forward), this code fixes it but I don't know if it'll work in all cases
Vector3d vesselBackward = (Vector3d)(-vesselTransform.up.normalized);
Vector3d vesselForward = -vesselBackward;
Vector3d vesselUp = (Vector3d)(-vesselTransform.forward.normalized);
Vector3d vesselRight = Vector3d.Cross(vesselUp, vesselBackward).normalized;
Vector3d airVelocityForFixedAoA = (vesselForward * Math.Cos(-angleOfAttack) + vesselUp * Math.Sin(-angleOfAttack)) * airVelocity.magnitude;
if (altitude >= body_.atmosphereDepth)
{
return(Vector3d.zero);
}
Vector3 aero_force = StockAeroUtil.SimAeroForce(vessel_, (Vector3)airVelocityForFixedAoA, altitude);
//Util.PostSingleScreenMessage("aeroforce", String.Format("aero_force = {0}",aero_force));
Vector3d totalForce = (Vector3d)aero_force;
if (Double.IsNaN(totalForce.x) || Double.IsNaN(totalForce.y) || Double.IsNaN(totalForce.z))
{
Debug.Log("Trajectories: WARNING: StockAero totalForce is NAN (altitude=" + altitude + ", airVelocity=" + airVelocity.magnitude + ", angleOfAttack=" + angleOfAttack);
return(new Vector3d(0, 0, 0)); // Don't send NaN into the simulation as it would cause bad things (infinite loops, crash, etc.). I think this case only happens at the atmosphere edge, so the total force should be 0 anyway.
}
// convert the force computed by FAR (depends on the current vessel orientation, which is irrelevant for the prediction) to the predicted vessel orientation (which depends on the predicted velocity)
Vector3d localForce = new Vector3d(Vector3d.Dot(vesselRight, totalForce), Vector3d.Dot(vesselUp, totalForce), Vector3d.Dot(vesselBackward, totalForce));
//if (Double.IsNaN(localForce.x) || Double.IsNaN(localForce.y) || Double.IsNaN(localForce.z))
// throw new Exception("localForce is NAN");
Vector3d velForward = airVelocity.normalized;
Vector3d velBackward = -velForward;
Vector3d velRight = Vector3d.Cross(vup, velBackward);
if (velRight.sqrMagnitude < 0.001)
{
velRight = Vector3d.Cross(vesselUp, velBackward);
if (velRight.sqrMagnitude < 0.001)
{
velRight = Vector3d.Cross(vesselBackward, velBackward).normalized;
}
else
{
velRight = velRight.normalized;
}
}
else
{
velRight = velRight.normalized;
}
Vector3d velUp = Vector3d.Cross(velBackward, velRight).normalized;
Vector3d predictedVesselForward = velForward * Math.Cos(angleOfAttack) + velUp * Math.Sin(angleOfAttack);
Vector3d predictedVesselBackward = -predictedVesselForward;
Vector3d predictedVesselRight = velRight;
Vector3d predictedVesselUp = Vector3d.Cross(predictedVesselBackward, predictedVesselRight).normalized;
Vector3d res = predictedVesselRight * localForce.x + predictedVesselUp * localForce.y + predictedVesselBackward * localForce.z;
if (Double.IsNaN(res.x) || Double.IsNaN(res.y) || Double.IsNaN(res.z))
{
Debug.Log("Trajectories: res is NaN (altitude=" + altitude + ", airVelocity=" + airVelocity.magnitude + ", angleOfAttack=" + angleOfAttack);
return(new Vector3d(0, 0, 0)); // Don't send NaN into the simulation as it would cause bad things (infinite loops, crash, etc.). I think this case only happens at the atmosphere edge, so the total force should be 0 anyway.
}
return(res);
}
protected override Vector3d ComputeForces_Model(Vector3d airVelocity, double altitude)
{
return(StockAeroUtil.SimAeroForce(trajectory_.AttachedVessel, airVelocity, altitude));
}
