/// <summary>
/// Gets the descending node orbit.
/// </summary>
/// <param name="desc">The desc.</param>
/// <returns><c>true</c> if descending node exists, otherwise <c>false</c></returns>
public bool GetDescendingNode(out Vector3d desc)
{
Vector3d norm = KeplerOrbitUtils.CrossProduct(OrbitNormal, EclipticNormal);
bool s = KeplerOrbitUtils.DotProduct(KeplerOrbitUtils.CrossProduct(norm, SemiMajorAxisBasis), OrbitNormal) < 0;
double ecc = 0d;
double trueAnom = Vector3d.Angle(norm, -CenterPoint) * KeplerOrbitUtils.Deg2Rad;
if (Eccentricity < 1)
{
double cosT = Math.Cos(trueAnom);
ecc = Math.Acos((Eccentricity + cosT) / (1d + Eccentricity * cosT));
if (s)
{
ecc = KeplerOrbitUtils.PI_2 - ecc;
}
}
else
{
trueAnom = Vector3d.Angle(norm, CenterPoint) * KeplerOrbitUtils.Deg2Rad;
if (trueAnom >= Math.Acos(-1d / Eccentricity))
{
desc = new Vector3d();
return(false);
}
double cosT = Math.Cos(trueAnom);
ecc = KeplerOrbitUtils.Acosh((Eccentricity + cosT) / (1 + Eccentricity * cosT)) * (s ? -1 : 1);
}
desc = GetFocalPositionAtEccentricAnomaly(ecc);
return(true);
}
/// <summary>
/// Calculates the full state of orbit from current orbital elements: eccentricity, mean anomaly, semi major and semi minor axis.
/// </summary>
/// <remarks>
/// Update orbital state using known main orbital elements and basis axis vectors.
/// Can be used for first initialization of orbit state, in this case initial data must be filled before this method call.
/// Required initial data: eccentricity, mean anomaly, inclination, attractor mass, grav constant, all anomalies, semi minor and semi major axis vectors and magnitudes.
/// Note that semi minor and semi major axis must be fully precalculated from inclination and argument of periapsis or another source data;
/// </remarks>
public void CalculateOrbitStateFromOrbitalElements()
{
MG = AttractorMass * GravConst;
OrbitNormal = -KeplerOrbitUtils.CrossProduct(SemiMajorAxisBasis, SemiMinorAxisBasis).normalized;
OrbitNormalDotEclipticNormal = KeplerOrbitUtils.DotProduct(OrbitNormal, EclipticNormal);
if (Eccentricity < 1.0)
{
OrbitCompressionRatio = 1 - Eccentricity * Eccentricity;
CenterPoint = -SemiMajorAxisBasis * SemiMajorAxis * Eccentricity;
Period = KeplerOrbitUtils.PI_2 * Math.Sqrt(Math.Pow(SemiMajorAxis, 3) / MG);
Apoapsis = CenterPoint - SemiMajorAxisBasis * SemiMajorAxis;
Periapsis = CenterPoint + SemiMajorAxisBasis * SemiMajorAxis;
PeriapsisDistance = Periapsis.magnitude;
ApoapsisDistance = Apoapsis.magnitude;
// All anomalies state already preset.
}
else
{
CenterPoint = SemiMajorAxisBasis * SemiMajorAxis * Eccentricity;
Period = double.PositiveInfinity;
Apoapsis = new Vector3d(double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity);
Periapsis = CenterPoint - SemiMajorAxisBasis * (SemiMajorAxis);
PeriapsisDistance = Periapsis.magnitude;
ApoapsisDistance = double.PositiveInfinity;
}
Position = GetFocalPositionAtEccentricAnomaly(EccentricAnomaly);
double compresion = Eccentricity < 1 ? (1 - Eccentricity * Eccentricity) : (Eccentricity * Eccentricity - 1);
FocalParameter = SemiMajorAxis * compresion;
Velocity = GetVelocityAtTrueAnomaly(this.TrueAnomaly);
AttractorDistance = Position.magnitude;
EnergyTotal = Velocity.sqrMagnitude - 2 * MG / AttractorDistance;
}
private void Initialize(Vector3d focus0, Vector3d focus1, Vector3d p0)
{
Focus0 = focus0;
Focus1 = focus1;
FocusDistance = Focus1 - Focus0;
AxisMain = FocusDistance.normalized;
var tempNormal = KeplerOrbitUtils.CrossProduct(AxisMain, p0 - Focus0).normalized;
AxisSecondary = KeplerOrbitUtils.CrossProduct(AxisMain, tempNormal).normalized;
C = FocusDistance.magnitude * 0.5;
A = System.Math.Abs(((p0 - Focus0).magnitude - (p0 - Focus1).magnitude)) * 0.5;
Eccentricity = C / A;
B = System.Math.Sqrt(C * C - A * A);
Center = focus0 + FocusDistance * 0.5;
}
/// <summary>
/// Create and initialize new orbit state from orbital elements.
/// </summary>
/// <param name="eccentricity">Eccentricity.</param>
/// <param name="semiMajorAxis">Main axis semi width.</param>
/// <param name="meanAnomalyDeg">Mean anomaly in degrees.</param>
/// <param name="inclinationDeg">Orbit inclination in degrees.</param>
/// <param name="argOfPerifocusDeg">Orbit argument of perifocus in degrees.</param>
/// <param name="ascendingNodeDeg">Longitude of ascending node in degrees.</param>
/// <param name="attractorMass">Attractor mass.</param>
/// <param name="gConst">Gravitational constant.</param>
public KeplerOrbitData(double eccentricity, double semiMajorAxis, double meanAnomalyDeg, double inclinationDeg, double argOfPerifocusDeg, double ascendingNodeDeg, double attractorMass, double gConst)
{
this.Eccentricity = eccentricity;
this.SemiMajorAxis = semiMajorAxis;
if (eccentricity < 1.0)
{
this.SemiMinorAxis = SemiMajorAxis * Math.Sqrt(1 - Eccentricity * Eccentricity);
}
else
{
this.SemiMinorAxis = SemiMajorAxis * Math.Sqrt(Eccentricity * Eccentricity - 1);
}
var normal = EclipticNormal.normalized;
var ascendingNode = EclipticRight.normalized;
ascendingNodeDeg %= 360;
if (ascendingNodeDeg > 180)
{
ascendingNodeDeg -= 360;
}
inclinationDeg %= 360;
if (inclinationDeg > 180)
{
inclinationDeg -= 360;
}
argOfPerifocusDeg %= 360;
if (argOfPerifocusDeg > 180)
{
argOfPerifocusDeg -= 360;
}
ascendingNode = KeplerOrbitUtils.RotateVectorByAngle(ascendingNode, ascendingNodeDeg * KeplerOrbitUtils.Deg2Rad, normal).normalized;
normal = KeplerOrbitUtils.RotateVectorByAngle(normal, inclinationDeg * KeplerOrbitUtils.Deg2Rad, ascendingNode).normalized;
var periapsis = ascendingNode;
periapsis = KeplerOrbitUtils.RotateVectorByAngle(periapsis, argOfPerifocusDeg * KeplerOrbitUtils.Deg2Rad, normal).normalized;
this.SemiMajorAxisBasis = periapsis;
this.SemiMinorAxisBasis = KeplerOrbitUtils.CrossProduct(periapsis, normal);
this.MeanAnomaly = meanAnomalyDeg * KeplerOrbitUtils.Deg2Rad;
this.EccentricAnomaly = KeplerOrbitUtils.ConvertMeanToEccentricAnomaly(this.MeanAnomaly, this.Eccentricity);
this.TrueAnomaly = KeplerOrbitUtils.ConvertEccentricToTrueAnomaly(this.EccentricAnomaly, this.Eccentricity);
this.AttractorMass = attractorMass;
this.GravConst = gConst;
CalculateOrbitStateFromOrbitalElements();
}
public EllipseData(Vector3d focus0, Vector3d focus1, Vector3d p0)
{
Focus0 = focus0;
Focus1 = focus1;
FocusDistance = Focus0 - Focus1;
A = ((Focus0 - p0).magnitude + (focus1 - p0).magnitude) * 0.5;
if (A < 0)
{
A = -A;
}
Eccentricity = (FocusDistance.magnitude * 0.5) / A;
B = A * System.Math.Sqrt(1 - Eccentricity * Eccentricity);
AxisMain = FocusDistance.normalized;
var tempNormal = KeplerOrbitUtils.CrossProduct(AxisMain, p0 - Focus0).normalized;
AxisSecondary = KeplerOrbitUtils.CrossProduct(AxisMain, tempNormal).normalized;
Center = Focus1 + FocusDistance * 0.5;
}
/// <summary>
/// Calculates full orbit state from cartesian vectors: current body position, velocity, attractor mass, and gravConstant.
/// </summary>
public void CalculateOrbitStateFromOrbitalVectors()
{
MG = AttractorMass * GravConst;
AttractorDistance = Position.magnitude;
Vector3d angularMomentumVector = KeplerOrbitUtils.CrossProduct(Position, Velocity);
OrbitNormal = angularMomentumVector.normalized;
Vector3d eccVector;
if (OrbitNormal.sqrMagnitude < 0.99)
{
OrbitNormal = KeplerOrbitUtils.CrossProduct(Position, EclipticUp).normalized;
eccVector = new Vector3d();
}
else
{
eccVector = KeplerOrbitUtils.CrossProduct(Velocity, angularMomentumVector) / MG - Position / AttractorDistance;
}
OrbitNormalDotEclipticNormal = KeplerOrbitUtils.DotProduct(OrbitNormal, EclipticNormal);
FocalParameter = angularMomentumVector.sqrMagnitude / MG;
Eccentricity = eccVector.magnitude;
EnergyTotal = Velocity.sqrMagnitude - 2 * MG / AttractorDistance;
SemiMinorAxisBasis = KeplerOrbitUtils.CrossProduct(angularMomentumVector, -eccVector).normalized;
if (SemiMinorAxisBasis.sqrMagnitude < 0.99)
{
SemiMinorAxisBasis = KeplerOrbitUtils.CrossProduct(OrbitNormal, Position).normalized;
}
SemiMajorAxisBasis = KeplerOrbitUtils.CrossProduct(OrbitNormal, SemiMinorAxisBasis).normalized;
if (Eccentricity < 1)
{
OrbitCompressionRatio = 1 - Eccentricity * Eccentricity;
SemiMajorAxis = FocalParameter / OrbitCompressionRatio;
SemiMinorAxis = SemiMajorAxis * Math.Sqrt(OrbitCompressionRatio);
CenterPoint = -SemiMajorAxis * eccVector;
Period = KeplerOrbitUtils.PI_2 * Math.Sqrt(Math.Pow(SemiMajorAxis, 3) / MG);
Apoapsis = CenterPoint - SemiMajorAxisBasis * SemiMajorAxis;
Periapsis = CenterPoint + SemiMajorAxisBasis * SemiMajorAxis;
PeriapsisDistance = Periapsis.magnitude;
ApoapsisDistance = Apoapsis.magnitude;
TrueAnomaly = Vector3d.Angle(Position, SemiMajorAxisBasis) * KeplerOrbitUtils.Deg2Rad;
if (KeplerOrbitUtils.DotProduct(KeplerOrbitUtils.CrossProduct(Position, -SemiMajorAxisBasis), OrbitNormal) < 0)
{
TrueAnomaly = KeplerOrbitUtils.PI_2 - TrueAnomaly;
}
EccentricAnomaly = KeplerOrbitUtils.ConvertTrueToEccentricAnomaly(TrueAnomaly, Eccentricity);
MeanAnomaly = EccentricAnomaly - Eccentricity * Math.Sin(EccentricAnomaly);
}
else
{
OrbitCompressionRatio = Eccentricity * Eccentricity - 1;
SemiMajorAxis = FocalParameter / OrbitCompressionRatio;
SemiMinorAxis = SemiMajorAxis * Math.Sqrt(OrbitCompressionRatio);
CenterPoint = SemiMajorAxis * eccVector;
Period = double.PositiveInfinity;
Apoapsis = new Vector3d(double.PositiveInfinity, double.PositiveInfinity, double.PositiveInfinity);
Periapsis = CenterPoint - SemiMajorAxisBasis * (SemiMajorAxis);
PeriapsisDistance = Periapsis.magnitude;
ApoapsisDistance = double.PositiveInfinity;
TrueAnomaly = Vector3d.Angle(Position, eccVector) * KeplerOrbitUtils.Deg2Rad;
if (KeplerOrbitUtils.DotProduct(KeplerOrbitUtils.CrossProduct(Position, -SemiMajorAxisBasis), OrbitNormal) < 0)
{
TrueAnomaly = -TrueAnomaly;
}
EccentricAnomaly = KeplerOrbitUtils.ConvertTrueToEccentricAnomaly(TrueAnomaly, Eccentricity);
MeanAnomaly = Math.Sinh(EccentricAnomaly) * Eccentricity - EccentricAnomaly;
}
}
