| public GetForces ( CelestialBody body, UnityEngine.Vector3d bodySpacePosition, UnityEngine.Vector3d airVelocity, double angleOfAttack ) : UnityEngine.Vector3d | ||
| body | CelestialBody | |
| bodySpacePosition | UnityEngine.Vector3d | |
| airVelocity | UnityEngine.Vector3d | |
| angleOfAttack | double | |
| return | UnityEngine.Vector3d | |
private IEnumerable <bool> AddPatch(VesselState startingState, DescentProfile profile)
{
if (null == vessel_.patchedConicSolver)
{
UnityEngine.Debug.LogWarning("Trajectories: AddPatch() attempted when patchedConicsSolver is null; Skipping.");
yield break;
}
CelestialBody body = startingState.referenceBody;
var patch = new Patch();
patch.startingState = startingState;
patch.isAtmospheric = false;
patch.spaceOrbit = startingState.stockPatch ?? createOrbitFromState(startingState);
patch.endTime = patch.startingState.time + patch.spaceOrbit.period;
// the flight plan does not always contain the first patches (before the first maneuver node), so we populate it with the current orbit and associated encounters etc.
var flightPlan = new List <Orbit>();
for (var orbit = vessel_.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
{
if (vessel_.patchedConicSolver.flightPlan.Contains(orbit))
{
break;
}
flightPlan.Add(orbit);
}
foreach (var orbit in vessel_.patchedConicSolver.flightPlan)
{
flightPlan.Add(orbit);
}
Orbit nextStockPatch = null;
if (startingState.stockPatch != null)
{
int planIdx = flightPlan.IndexOf(startingState.stockPatch);
if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
{
nextStockPatch = flightPlan[planIdx + 1];
}
}
if (nextStockPatch != null)
{
patch.endTime = nextStockPatch.StartUT;
}
double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);
double minAltitude = patch.spaceOrbit.PeA;
if (patch.endTime < startingState.time + patch.spaceOrbit.timeToPe)
{
minAltitude = patch.spaceOrbit.getRelativePositionAtUT(patch.endTime).magnitude;
}
if (minAltitude < maxAtmosphereAltitude)
{
double entryTime;
if (startingState.position.magnitude <= body.Radius + maxAtmosphereAltitude)
{
// whole orbit is inside the atmosphere
entryTime = startingState.time;
}
else
{
// binary search of entry time in atmosphere
// I guess an analytic solution could be found, but I'm too lazy to search it
double from = startingState.time;
double to = from + patch.spaceOrbit.timeToPe;
int loopCount = 0;
while (to - from > 0.1)
{
++loopCount;
if (loopCount > 1000)
{
UnityEngine.Debug.Log("WARNING: infinite loop? (Trajectories.Trajectory.AddPatch, atmosphere limit search)");
++errorCount_;
break;
}
double middle = (from + to) * 0.5;
if (patch.spaceOrbit.getRelativePositionAtUT(middle).magnitude < body.Radius + maxAtmosphereAltitude)
{
to = middle;
}
else
{
from = middle;
}
}
entryTime = to;
}
if (entryTime > startingState.time + 0.1)
{
// add the space patch before atmospheric entry
patch.endTime = entryTime;
if (body.atmosphere)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)),
referenceBody = body,
time = entryTime,
velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime))
};
yield break;
}
else
{
// the body has no atmosphere, so what we actually computed is the impact on the body surface
// now, go back in time until the impact point is above the ground to take ground height in account
// we assume the ground is horizontal around the impact position
double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)), entryTime)) + body.Radius;
double iterationSize = 1.0;
while (entryTime > startingState.time + iterationSize && patch.spaceOrbit.getRelativePositionAtUT(entryTime).magnitude < groundAltitude)
{
entryTime -= iterationSize;
}
patch.endTime = entryTime;
patch.rawImpactPosition = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, entryTime);
patch.impactVelocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
}
else
{
if (patch.startingState.referenceBody != vessel_.mainBody)
{
// in current aerodynamic prediction code, we can't handle predictions for another body, so we stop here
AddPatch_outState = null;
yield break;
}
// simulate atmospheric flight (drag and lift), until landing (more likely to be a crash as we don't predict user piloting) or atmosphere exit (typically for an aerobraking maneuver)
// the simulation assumes a constant angle of attack
patch.isAtmospheric = true;
patch.startingState.stockPatch = null;
double dt = 0.1; // lower dt would be more accurate, but a tradeoff has to be found between performances and accuracy
int maxIterations = (int)(30.0 * 60.0 / dt); // some shallow entries can result in very long flight, for performances reasons, we limit the prediction duration
int chunkSize = 128;
double trajectoryInterval = 10.0; // time between two consecutive stored positions (more intermediate positions are computed for better accuracy), also used for ground collision checks
var buffer = new List <Point[]>();
buffer.Add(new Point[chunkSize]);
int nextPosIdx = 0;
Vector3d pos = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
Vector3d vel = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
Vector3d prevPos = pos - vel * dt;
//Util.PostSingleScreenMessage("initial vel", "initial vel = " + vel);
double currentTime = entryTime;
double lastPositionStoredUT = 0;
Vector3d lastPositionStored = new Vector3d();
bool hitGround = false;
int iteration = 0;
int incrementIterations = 0;
int minIterationsPerIncrement = maxIterations / Settings.fetch.MaxFramesPerPatch;
while (true)
{
++iteration;
++incrementIterations;
if (incrementIterations > minIterationsPerIncrement && incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
{
yield return(false);
incrementIterations = 0;
}
double R = pos.magnitude;
double altitude = R - body.Radius;
double atmosphereCoeff = altitude / maxAtmosphereAltitude;
if (hitGround || atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 || iteration == maxIterations || currentTime > patch.endTime)
{
if (hitGround || atmosphereCoeff <= 0.0)
{
patch.rawImpactPosition = pos;
patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, currentTime);
patch.impactVelocity = vel;
}
patch.endTime = Math.Min(currentTime, patch.endTime);
int totalCount = (buffer.Count - 1) * chunkSize + nextPosIdx;
patch.atmosphericTrajectory = new Point[totalCount];
int outIdx = 0;
foreach (var chunk in buffer)
{
foreach (var p in chunk)
{
if (outIdx == totalCount)
{
break;
}
patch.atmosphericTrajectory[outIdx++] = p;
}
}
if (iteration == maxIterations)
{
ScreenMessages.PostScreenMessage("WARNING: trajectory prediction stopped, too many iterations");
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
else if (atmosphereCoeff <= 0.0 || hitGround)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
else
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
position = pos,
velocity = vel,
referenceBody = body,
time = patch.endTime
};
yield break;
}
}
Vector3d gravityAccel = pos * (-body.gravParameter / (R * R * R));
//Util.PostSingleScreenMessage("prediction vel", "prediction vel = " + vel);
Vector3d airVelocity = vel - body.getRFrmVel(body.position + pos);
double angleOfAttack = profile.GetAngleOfAttack(body, pos, airVelocity);
Vector3d aerodynamicForce = aerodynamicModel_.GetForces(body, pos, airVelocity, angleOfAttack);
Vector3d acceleration = gravityAccel + aerodynamicForce / aerodynamicModel_.mass;
// acceleration in the vessel reference frame is acceleration - gravityAccel
maxAccelBackBuffer_ = Math.Max((float)(aerodynamicForce.magnitude / aerodynamicModel_.mass), maxAccelBackBuffer_);
//vel += acceleration * dt;
//pos += vel * dt;
// Verlet integration (more precise than using the velocity)
Vector3d ppos = prevPos;
prevPos = pos;
pos = pos + pos - ppos + acceleration * (dt * dt);
vel = (pos - prevPos) / dt;
currentTime += dt;
double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
if (currentTime >= lastPositionStoredUT + interval)
{
double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, pos, currentTime));
if (lastPositionStoredUT > 0)
{
// check terrain collision, to detect impact on mountains etc.
Vector3 rayOrigin = lastPositionStored;
Vector3 rayEnd = pos;
double absGroundAltitude = groundAltitude + body.Radius;
if (absGroundAltitude > rayEnd.magnitude)
{
hitGround = true;
float coeff = Math.Max(0.01f, (float)((absGroundAltitude - rayOrigin.magnitude) / (rayEnd.magnitude - rayOrigin.magnitude)));
pos = rayEnd * coeff + rayOrigin * (1.0f - coeff);
currentTime = currentTime * coeff + lastPositionStoredUT * (1.0f - coeff);
}
}
lastPositionStoredUT = currentTime;
if (nextPosIdx == chunkSize)
{
buffer.Add(new Point[chunkSize]);
nextPosIdx = 0;
}
Vector3d nextPos = pos;
if (Settings.fetch.BodyFixedMode)
{
nextPos = calculateRotatedPosition(body, nextPos, currentTime);
}
buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
buffer.Last()[nextPosIdx].orbitalVelocity = vel;
buffer.Last()[nextPosIdx].groundAltitude = (float)groundAltitude;
buffer.Last()[nextPosIdx].time = currentTime;
buffer.Last()[nextPosIdx++].pos = nextPos;
lastPositionStored = pos;
}
}
}
}
else
{
// no atmospheric entry, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextStockPatch != null)
{
AddPatch_outState = new VesselState
{
position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(patch.endTime)),
velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(patch.endTime)),
referenceBody = nextStockPatch == null ? body : nextStockPatch.referenceBody,
time = patch.endTime,
stockPatch = nextStockPatch
};
yield break;
}
else
{
AddPatch_outState = null;
yield break;
}
}
}
private IEnumerable <bool> AddPatch(VesselState startingState)
{
if (null == AttachedVessel.patchedConicSolver)
{
AttachedVessel.AttachPatchedConicsSolver();
}
CelestialBody body = startingState.ReferenceBody;
Patch patch = new Patch
{
StartingState = startingState,
IsAtmospheric = false,
SpaceOrbit = startingState.StockPatch ?? CreateOrbitFromState(startingState)
};
patch.EndTime = patch.StartingState.Time + patch.SpaceOrbit.period;
// the flight plan does not always contain the first patches (before the first maneuver node),
// so we populate it with the current orbit and associated encounters etc.
List <Orbit> flightPlan = new List <Orbit>();
for (Orbit orbit = AttachedVessel.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
{
if (AttachedVessel.patchedConicSolver.flightPlan.Contains(orbit))
{
break;
}
flightPlan.Add(orbit);
}
foreach (Orbit orbit in AttachedVessel.patchedConicSolver.flightPlan)
{
flightPlan.Add(orbit);
}
Orbit nextPatch = null;
if (startingState.StockPatch == null)
{
nextPatch = patch.SpaceOrbit.nextPatch;
}
else
{
int planIdx = flightPlan.IndexOf(startingState.StockPatch);
if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
{
nextPatch = flightPlan[planIdx + 1];
}
}
if (nextPatch != null)
{
patch.EndTime = nextPatch.StartUT;
}
double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);
if (!body.atmosphere)
{
maxAtmosphereAltitude = body.pqsController.mapMaxHeight;
}
double minAltitude = patch.SpaceOrbit.PeA;
if (patch.SpaceOrbit.timeToPe < 0 || patch.EndTime < startingState.Time + patch.SpaceOrbit.timeToPe)
{
minAltitude = Math.Min(
patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).magnitude,
patch.SpaceOrbit.getRelativePositionAtUT(patch.StartingState.Time + 1.0).magnitude
) - body.Radius;
}
if (minAltitude < maxAtmosphereAltitude)
{
double entryTime;
if (startingState.Position.magnitude <= body.Radius + maxAtmosphereAltitude)
{
// whole orbit is inside the atmosphere
entryTime = startingState.Time;
}
else
{
entryTime = FindOrbitBodyIntersection(
patch.SpaceOrbit,
startingState.Time, startingState.Time + patch.SpaceOrbit.timeToPe,
body.Radius + maxAtmosphereAltitude);
}
if (entryTime > startingState.Time + 0.1 || !body.atmosphere)
{
if (body.atmosphere)
{
// add the space patch before atmospheric entry
patch.EndTime = entryTime;
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
Position = patch.SpaceOrbit.getRelativePositionAtUT(entryTime).SwapYZ(),
ReferenceBody = body,
Time = entryTime,
Velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime).SwapYZ()
};
}
else
{
// the body has no atmosphere, so what we actually computed is the entry
// inside the "ground sphere" (defined by the maximal ground altitude)
// now we iterate until the inner ground sphere (minimal altitude), and
// check if we hit the ground along the way
double groundRangeExit = FindOrbitBodyIntersection(
patch.SpaceOrbit,
startingState.Time, startingState.Time + patch.SpaceOrbit.timeToPe,
body.Radius - maxAtmosphereAltitude);
if (groundRangeExit <= entryTime)
{
groundRangeExit = startingState.Time + patch.SpaceOrbit.timeToPe;
}
double iterationSize = (groundRangeExit - entryTime) / 100.0;
double t;
bool groundImpact = false;
for (t = entryTime; t < groundRangeExit; t += iterationSize)
{
Vector3d pos = patch.SpaceOrbit.getRelativePositionAtUT(t);
double groundAltitude = GetGroundAltitude(body, CalculateRotatedPosition(body, pos.SwapYZ(), t))
+ body.Radius;
if (pos.magnitude < groundAltitude)
{
t -= iterationSize;
groundImpact = true;
break;
}
}
if (groundImpact)
{
patch.EndTime = t;
patch.RawImpactPosition = patch.SpaceOrbit.getRelativePositionAtUT(t).SwapYZ();
patch.ImpactPosition = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, t);
patch.ImpactVelocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(t).SwapYZ();
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
}
else
{
// no impact, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextPatch != null)
{
AddPatch_outState = new VesselState
{
Position = patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).SwapYZ(),
Velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime).SwapYZ(),
ReferenceBody = nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
}
else
{
AddPatch_outState = null;
}
}
}
}
else
{
if (patch.StartingState.ReferenceBody != AttachedVessel.mainBody)
{
// currently, we can't handle predictions for another body, so we stop
AddPatch_outState = null;
yield break;
}
// simulate atmospheric flight (drag and lift), until impact or atmosphere exit
// (typically for an aerobraking maneuver) assuming a constant angle of attack
patch.IsAtmospheric = true;
patch.StartingState.StockPatch = null;
// lower dt would be more accurate, but a trade-off has to be found between performances and accuracy
double dt = Settings.IntegrationStepSize;
// some shallow entries can result in very long flight. For performances reasons,
// we limit the prediction duration
int maxIterations = (int)(60.0 * 60.0 / dt);
int chunkSize = 128;
// time between two consecutive stored positions (more intermediate positions are computed for better accuracy),
// also used for ground collision checks
double trajectoryInterval = 10.0;
List <Point[]> buffer = new List <Point[]>
{
new Point[chunkSize]
};
int nextPosIdx = 0;
SimulationState state;
state.position = patch.SpaceOrbit.getRelativePositionAtUT(entryTime).SwapYZ();
state.velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime).SwapYZ();
// Initialize a patch with zero acceleration
Vector3d currentAccel = new Vector3d(0.0, 0.0, 0.0);
double currentTime = entryTime;
double lastPositionStoredUT = 0;
Vector3d lastPositionStored = new Vector3d();
bool hitGround = false;
int iteration = 0;
int incrementIterations = 0;
int minIterationsPerIncrement = maxIterations / Settings.MaxFramesPerPatch;
#region Acceleration Functor
// function that calculates the acceleration under current parameters
Vector3d AccelerationFunc(Vector3d position, Vector3d velocity)
{
Profiler.Start("accelerationFunc inside");
// gravity acceleration
double R_ = position.magnitude;
Vector3d accel_g = position * (-body.gravParameter / (R_ * R_ * R_));
// aero force
Vector3d vel_air = velocity - body.getRFrmVel(body.position + position);
double aoa = DescentProfile.GetAngleOfAttack(body, position, vel_air) ?? 0d;
Profiler.Start("GetForces");
Vector3d force_aero = aerodynamicModel_.GetForces(body, position, vel_air, aoa);
Profiler.Stop("GetForces");
Vector3d accel = accel_g + force_aero / aerodynamicModel_.Mass;
accel += API.HandleAccel(AttachedVessel, body, position, velocity, aoa);
Profiler.Stop("accelerationFunc inside");
return(accel);
}
#endregion
#region Integration Loop
while (true)
{
++iteration;
++incrementIterations;
if (incrementIterations > minIterationsPerIncrement && Util.ElapsedMilliseconds(increment_time) > MAX_INCREMENT_TIME)
{
yield return(false);
incrementIterations = 0;
}
double R = state.position.magnitude;
double altitude = R - body.Radius;
double atmosphereCoeff = altitude / maxAtmosphereAltitude;
if (hitGround ||
atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 ||
iteration == maxIterations || currentTime > patch.EndTime)
{
//Util.PostSingleScreenMessage("atmo force", "Atmospheric accumulated force: " + accumulatedForces.ToString("0.00"));
if (hitGround || atmosphereCoeff <= 0.0)
{
patch.RawImpactPosition = state.position;
patch.ImpactPosition = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, currentTime);
patch.ImpactVelocity = state.velocity;
}
patch.EndTime = Math.Min(currentTime, patch.EndTime);
int totalCount = (buffer.Count - 1) * chunkSize + nextPosIdx;
patch.AtmosphericTrajectory = new Point[totalCount];
int outIdx = 0;
foreach (Point[] chunk in buffer)
{
foreach (Point p in chunk)
{
if (outIdx == totalCount)
{
break;
}
patch.AtmosphericTrajectory[outIdx++] = p;
}
}
if (iteration == maxIterations)
{
ScreenMessages.PostScreenMessage("WARNING: trajectory prediction stopped, too many iterations");
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
if (atmosphereCoeff <= 0.0 || hitGround)
{
patchesBackBuffer_.Add(patch);
AddPatch_outState = null;
yield break;
}
patchesBackBuffer_.Add(patch);
AddPatch_outState = new VesselState
{
Position = state.position,
Velocity = state.velocity,
ReferenceBody = body,
Time = patch.EndTime
};
yield break;
}
Vector3d lastAccel = currentAccel;
SimulationState lastState = state;
Profiler.Start("IntegrationStep");
// Verlet integration (more precise than using the velocity)
// state = VerletStep(state, accelerationFunc, dt);
state = RK4Step(state, AccelerationFunc, dt, out currentAccel);
currentTime += dt;
// KSP presumably uses Euler integration for position updates. Since RK4 is actually more precise than that,
// we try to reintroduce an approximation of the error.
// The local truncation error for euler integration is:
// LTE = 1/2 * h^2 * y''(t)
// https://en.wikipedia.org/wiki/Euler_method#Local_truncation_error
//
// For us,
// h is the time step of the outer simulation (KSP), which is the physics time step
// y''(t) is the difference of the velocity/acceleration divided by the physics time step
state.position += 0.5 * TimeWarp.fixedDeltaTime * currentAccel * dt;
state.velocity += 0.5 * TimeWarp.fixedDeltaTime * (currentAccel - lastAccel);
Profiler.Stop("IntegrationStep");
// calculate gravity and aerodynamic force
Vector3d gravityAccel = lastState.position * (-body.gravParameter / (R * R * R));
Vector3d aerodynamicForce = (currentAccel - gravityAccel) / aerodynamicModel_.Mass;
// acceleration in the vessel reference frame is acceleration - gravityAccel
maxAccelBackBuffer_ = Math.Max(
(float)(aerodynamicForce.magnitude / aerodynamicModel_.Mass),
maxAccelBackBuffer_);
#region Impact Calculation
Profiler.Start("AddPatch#impact");
double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
if (currentTime >= lastPositionStoredUT + interval)
{
double groundAltitude = GetGroundAltitude(body, CalculateRotatedPosition(body, state.position, currentTime));
if (lastPositionStoredUT > 0)
{
// check terrain collision, to detect impact on mountains etc.
Vector3 rayOrigin = lastPositionStored;
Vector3 rayEnd = state.position;
double absGroundAltitude = groundAltitude + body.Radius;
if (absGroundAltitude > rayEnd.magnitude)
{
hitGround = true;
float coeff = Math.Max(0.01f, (float)((absGroundAltitude - rayOrigin.magnitude)
/ (rayEnd.magnitude - rayOrigin.magnitude)));
state.position = rayEnd * coeff + rayOrigin * (1.0f - coeff);
currentTime = currentTime * coeff + lastPositionStoredUT * (1.0f - coeff);
}
}
lastPositionStoredUT = currentTime;
if (nextPosIdx == chunkSize)
{
buffer.Add(new Point[chunkSize]);
nextPosIdx = 0;
}
Vector3d nextPos = state.position;
if (Settings.BodyFixedMode)
{
nextPos = CalculateRotatedPosition(body, nextPos, currentTime);
}
buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
buffer.Last()[nextPosIdx].orbitalVelocity = state.velocity;
buffer.Last()[nextPosIdx].groundAltitude = (float)groundAltitude;
buffer.Last()[nextPosIdx].time = currentTime;
buffer.Last()[nextPosIdx++].pos = nextPos;
lastPositionStored = state.position;
}
Profiler.Stop("AddPatch#impact");
#endregion
}
#endregion
}
}
else
{
// no atmospheric entry, just add the space orbit
patchesBackBuffer_.Add(patch);
if (nextPatch != null)
{
AddPatch_outState = new VesselState
{
Position = patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).SwapYZ(),
Velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime).SwapYZ(),
ReferenceBody = nextPatch.referenceBody,
Time = patch.EndTime,
StockPatch = nextPatch
};
}
else
{
AddPatch_outState = null;
}
}
}
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;
}
}
