/// <summary>
/// Sets the mean anomaly and updates all other anomalies.
/// </summary>
/// <param name="m">The m.</param>
public void SetMeanAnomaly(double m)
{
if (!IsValidOrbit)
{
return;
}
MeanAnomaly = m % KeplerOrbitUtils.PI_2;
if (Eccentricity < 1.0)
{
if (MeanAnomaly < 0)
{
MeanAnomaly += KeplerOrbitUtils.PI_2;
}
EccentricAnomaly = KeplerOrbitUtils.KeplerSolver(MeanAnomaly, Eccentricity);
TrueAnomaly = KeplerOrbitUtils.ConvertEccentricToTrueAnomaly(EccentricAnomaly, Eccentricity);
}
else if (Eccentricity > 1.0)
{
EccentricAnomaly = KeplerOrbitUtils.KeplerSolverHyperbolicCase(MeanAnomaly, Eccentricity);
TrueAnomaly = KeplerOrbitUtils.ConvertEccentricToTrueAnomaly(EccentricAnomaly, Eccentricity);
}
else
{
EccentricAnomaly = KeplerOrbitUtils.ConvertMeanToEccentricAnomaly(MeanAnomaly, Eccentricity);
TrueAnomaly = EccentricAnomaly;
}
SetPositionByCurrentAnomaly();
SetVelocityByCurrentAnomaly();
}
/// <summary>
/// Updates the value of orbital anomalies by defined deltatime.
/// </summary>
/// <param name="deltaTime">The delta time.</param>
/// <remarks>
/// Only anomalies values will be changed.
/// Position and velocity states needs to be updated too after this method call.
/// </remarks>
public void UpdateOrbitAnomaliesByTime(double deltaTime)
{
if (Eccentricity < 1.0)
{
MeanAnomaly += MeanMotion * deltaTime;
MeanAnomaly %= KeplerOrbitUtils.PI_2;
if (MeanAnomaly < 0)
{
MeanAnomaly = KeplerOrbitUtils.PI_2 - MeanAnomaly;
}
EccentricAnomaly = KeplerOrbitUtils.KeplerSolver(MeanAnomaly, Eccentricity);
double cosE = Math.Cos(EccentricAnomaly);
TrueAnomaly = Math.Acos((cosE - Eccentricity) / (1 - Eccentricity * cosE));
if (MeanAnomaly > Math.PI)
{
TrueAnomaly = KeplerOrbitUtils.PI_2 - TrueAnomaly;
}
}
else if (Eccentricity > 1.0)
{
MeanAnomaly = MeanAnomaly + MeanMotion * deltaTime;
EccentricAnomaly = KeplerOrbitUtils.KeplerSolverHyperbolicCase(MeanAnomaly, Eccentricity);
TrueAnomaly = Math.Atan2(Math.Sqrt(Eccentricity * Eccentricity - 1.0) * Math.Sinh(EccentricAnomaly), Eccentricity - Math.Cosh(EccentricAnomaly));
}
else
{
MeanAnomaly = MeanAnomaly + MeanMotion * deltaTime;
EccentricAnomaly = KeplerOrbitUtils.ConvertMeanToEccentricAnomaly(MeanAnomaly, Eccentricity);
TrueAnomaly = EccentricAnomaly;
}
}
/// <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;
}
/// <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>
/// Sets the true anomaly and updates all other anomalies.
/// </summary>
/// <param name="t">The t.</param>
public void SetTrueAnomaly(double t)
{
if (!IsValidOrbit)
{
return;
}
t %= KeplerOrbitUtils.PI_2;
if (Eccentricity < 1)
{
if (t < 0)
{
t += KeplerOrbitUtils.PI_2;
}
EccentricAnomaly = KeplerOrbitUtils.ConvertTrueToEccentricAnomaly(t, Eccentricity);
MeanAnomaly = EccentricAnomaly - Eccentricity * Math.Sin(EccentricAnomaly);
}
else
{
EccentricAnomaly = KeplerOrbitUtils.ConvertTrueToEccentricAnomaly(t, Eccentricity);
MeanAnomaly = Math.Sinh(EccentricAnomaly) * Eccentricity - EccentricAnomaly;
}
SetPositionByCurrentAnomaly();
SetVelocityByCurrentAnomaly();
}
/// <summary>
/// Sets the eccentric anomaly and updates all other anomalies.
/// </summary>
/// <param name="e">The e.</param>
public void SetEccentricAnomaly(double e)
{
if (!IsValidOrbit)
{
return;
}
e %= KeplerOrbitUtils.PI_2;
EccentricAnomaly = e;
if (Eccentricity < 1)
{
if (e < 0)
{
e = KeplerOrbitUtils.PI_2 + e;
}
EccentricAnomaly = e;
TrueAnomaly = KeplerOrbitUtils.ConvertEccentricToTrueAnomaly(e, Eccentricity);
MeanAnomaly = EccentricAnomaly - Eccentricity * Math.Sin(EccentricAnomaly);
}
else
{
TrueAnomaly = KeplerOrbitUtils.ConvertEccentricToTrueAnomaly(e, Eccentricity);
MeanAnomaly = Math.Sinh(EccentricAnomaly) * Eccentricity - EccentricAnomaly;
}
SetPositionByCurrentAnomaly();
SetVelocityByCurrentAnomaly();
}
public void SetAutoCircleOrbit()
{
if (IsReferencesAsigned)
{
OrbitData.Velocity = KeplerOrbitUtils.CalcCircleOrbitVelocity(Vector3d.zero, OrbitData.Position, OrbitData.AttractorMass, OrbitData.OrbitNormal, OrbitData.GravConst);
OrbitData.CalculateOrbitStateFromOrbitalVectors();
ForceUpdateVelocityHandleFromInternalState();
}
}
public void SetAutoCircleOrbit()
{
if (IsReferencesAsigned)
{
OrbitData.Velocity = KeplerOrbitUtils.CalcCircleOrbitVelocity(Vector3d.zero, OrbitData.Position, OrbitData.AttractorMass, 1f, OrbitData.OrbitNormal, OrbitData.GravConst);
OrbitData.CalculateNewOrbitData();
if (VelocityHandle != null)
{
VelocityHandle.position = transform.position + (Vector3)OrbitData.Velocity;
}
}
}
/// <summary>
/// Gets the orbit sample points without unnecessary memory alloc for resulting array.
/// However, memory allocation may occur if resulting array has not correct lenght.
/// </summary>
/// <param name="orbitPoints">The orbit points.</param>
/// <param name="pointsCount">The points count.</param>
/// <param name="origin">The origin.</param>
/// <param name="maxDistance">The maximum distance.</param>
public void GetOrbitPointsNoAlloc(ref Vector3d[] orbitPoints, int pointsCount, Vector3d origin, float maxDistance = 1000f)
{
if (pointsCount < 2)
{
orbitPoints = new Vector3d[0];
return;
}
if (Eccentricity < 1)
{
if (orbitPoints == null || orbitPoints.Length != pointsCount)
{
orbitPoints = new Vector3d[pointsCount];
}
if (ApoapsisDistance < maxDistance)
{
for (int i = 0; i < pointsCount; i++)
{
orbitPoints[i] = GetFocalPositionAtEccentricAnomaly(i * KeplerOrbitUtils.PI_2 / (pointsCount - 1d)) + origin;
}
}
else
{
double maxAngle = KeplerOrbitUtils.CalcTrueAnomalyForDistance(maxDistance, Eccentricity, SemiMajorAxis, PeriapsisDistance);
for (int i = 0; i < pointsCount; i++)
{
orbitPoints[i] = GetFocalPositionAtTrueAnomaly(-maxAngle + i * 2d * maxAngle / (pointsCount - 1)) + origin;
}
}
}
else
{
if (maxDistance < PeriapsisDistance)
{
orbitPoints = new Vector3d[0];
return;
}
if (orbitPoints == null || orbitPoints.Length != pointsCount)
{
orbitPoints = new Vector3d[pointsCount];
}
double maxAngle = KeplerOrbitUtils.CalcTrueAnomalyForDistance(maxDistance, Eccentricity, SemiMajorAxis, PeriapsisDistance);
for (int i = 0; i < pointsCount; i++)
{
orbitPoints[i] = GetFocalPositionAtTrueAnomaly(-maxAngle + i * 2d * maxAngle / (pointsCount - 1)) + origin;
}
}
}
/// <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 if (eccentricity > 1.0)
{
this.SemiMinorAxis = SemiMajorAxis * Math.Sqrt(Eccentricity * Eccentricity - 1);
}
else
{
this.SemiMajorAxis = 0;
}
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;
Periapsis = ascendingNode;
Periapsis = KeplerOrbitUtils.RotateVectorByAngle(Periapsis, argOfPerifocusDeg * KeplerOrbitUtils.Deg2Rad, normal).normalized;
this.SemiMajorAxisBasis = Periapsis;
this.SemiMinorAxisBasis = Vector3d.Cross(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();
}
/// <summary>
/// Calculate eccentric anomaly in radians for point.
/// </summary>
/// <param name="point">Point in plane of elliptic shape.</param>
/// <returns>Eccentric anomaly radians.</returns>
public double GetEccentricAnomalyForPoint(Vector3d point)
{
var vector = point - Focus0;
var trueAnomaly = Vector3d.Angle(vector, AxisMain) * KeplerOrbitUtils.Deg2Rad;
if (KeplerOrbitUtils.DotProduct(vector, AxisSecondary) > 0)
{
trueAnomaly = KeplerOrbitUtils.PI_2 - trueAnomaly;
}
var result = KeplerOrbitUtils.ConvertTrueToEccentricAnomaly(trueAnomaly, Eccentricity);
return(result);
}
/// <summary>
/// Gets orbit sample points with defined precision.
/// </summary>
/// <param name="pointsCount">The points count.</param>
/// <param name="origin">The origin.</param>
/// <param name="maxDistance">The maximum distance.</param>
/// <returns>Array of orbit curve points.</returns>
public Vector3d[] GetOrbitPoints(int pointsCount, Vector3d origin, double maxDistance = 1000d)
{
if (pointsCount < 2)
{
return(new Vector3d[0]);
}
Vector3d[] result = new Vector3d[pointsCount];
if (Eccentricity < 1.0)
{
if (ApoapsisDistance < maxDistance)
{
for (int i = 0; i < pointsCount; i++)
{
result[i] = GetFocalPositionAtEccentricAnomaly(i * KeplerOrbitUtils.PI_2 / (pointsCount - 1d)) + origin;
}
}
else if (Eccentricity > 1.0)
{
double maxAngle = KeplerOrbitUtils.CalcTrueAnomalyForDistance(maxDistance, Eccentricity, SemiMajorAxis, PeriapsisDistance);
for (int i = 0; i < pointsCount; i++)
{
result[i] = GetFocalPositionAtTrueAnomaly(-maxAngle + i * 2d * maxAngle / (pointsCount - 1)) + origin;
}
}
else
{
double maxAngle = KeplerOrbitUtils.CalcTrueAnomalyForDistance(maxDistance, Eccentricity, PeriapsisDistance, PeriapsisDistance);
for (int i = 0; i < pointsCount; i++)
{
result[i] = GetFocalPositionAtTrueAnomaly(-maxAngle + i * 2d * maxAngle / (pointsCount - 1)) + origin;
}
}
}
else
{
if (maxDistance < PeriapsisDistance)
{
return(new Vector3d[0]);
}
double maxAngle = KeplerOrbitUtils.CalcTrueAnomalyForDistance(maxDistance, Eccentricity, SemiMajorAxis, PeriapsisDistance);
for (int i = 0; i < pointsCount; i++)
{
result[i] = GetFocalPositionAtTrueAnomaly(-maxAngle + i * 2d * maxAngle / (pointsCount - 1)) + origin;
}
}
return(result);
}
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 and main axis vectors.
/// </summary>
/// <param name="eccentricity">Eccentricity.</param>
/// <param name="semiMajorAxis">Semi major axis vector.</param>
/// <param name="semiMinorAxis">Semi minor axis vector.</param>
/// <param name="meanAnomalyDeg">Mean anomaly in degrees.</param>
/// <param name="attractorMass">Attractor mass.</param>
/// <param name="gConst">Gravitational constant.</param>
public KeplerOrbitData(double eccentricity, Vector3d semiMajorAxis, Vector3d semiMinorAxis, double meanAnomalyDeg, double attractorMass, double gConst)
{
this.Eccentricity = eccentricity;
this.SemiMajorAxisBasis = semiMajorAxis.normalized;
this.SemiMinorAxisBasis = semiMinorAxis.normalized;
this.SemiMajorAxis = semiMajorAxis.magnitude;
this.SemiMinorAxis = semiMinorAxis.magnitude;
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();
}
/// <summary>
/// Updates the value of orbital anomalies by defined deltatime.
/// </summary>
/// <param name="deltaTime">The delta time.</param>
/// <remarks>
/// Only anomalies values will be changed.
/// Position and velocity states needs to be updated too after this method call.
/// </remarks>
public void UpdateOrbitAnomaliesByTime(double deltaTime)
{
if (Eccentricity < 1)
{
if (Period > 1e-5)
{
MeanAnomaly += KeplerOrbitUtils.PI_2 * deltaTime / Period;
}
MeanAnomaly %= KeplerOrbitUtils.PI_2;
if (MeanAnomaly < 0)
{
MeanAnomaly = KeplerOrbitUtils.PI_2 - MeanAnomaly;
}
EccentricAnomaly = KeplerOrbitUtils.KeplerSolver(MeanAnomaly, Eccentricity);
var cosE = Math.Cos(EccentricAnomaly);
TrueAnomaly = Math.Acos((cosE - Eccentricity) / (1 - Eccentricity * cosE));
if (MeanAnomaly > Math.PI)
{
TrueAnomaly = KeplerOrbitUtils.PI_2 - TrueAnomaly;
}
if (double.IsNaN(MeanAnomaly) || double.IsInfinity(MeanAnomaly))
{
Debug.Log("KeplerOrbitData: NaN(INF) MEAN ANOMALY"); //little paranoya
Debug.Break();
}
if (double.IsNaN(EccentricAnomaly) || double.IsInfinity(EccentricAnomaly))
{
Debug.Log("KeplerOrbitData: NaN(INF) ECC ANOMALY");
Debug.Break();
}
if (double.IsNaN(TrueAnomaly) || double.IsInfinity(TrueAnomaly))
{
Debug.Log("KeplerOrbitData: NaN(INF) TRUE ANOMALY");
Debug.Break();
}
}
else
{
double n = Math.Sqrt(AttractorMass * GravConst / Math.Pow(SemiMajorAxis, 3));// * Math.Sign(OrbitNormalDotEclipticNormal);
MeanAnomaly = MeanAnomaly + n * deltaTime;
EccentricAnomaly = KeplerOrbitUtils.KeplerSolverHyperbolicCase(MeanAnomaly, Eccentricity);
TrueAnomaly = Math.Atan2(Math.Sqrt(Eccentricity * Eccentricity - 1.0) * Math.Sinh(EccentricAnomaly), Eccentricity - Math.Cosh(EccentricAnomaly));
}
}
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>
/// Updates the value of orbital anomalies by defined deltatime.
/// </summary>
/// <param name="deltaTime">The delta time.</param>
/// <remarks>
/// Only anomalies values will be changed.
/// Position and velocity states needs to be updated too after this method call.
/// </remarks>
public void UpdateOrbitAnomaliesByTime(double deltaTime)
{
if (Eccentricity < 1)
{
if (Period > KeplerOrbitUtils.Epsilon)
{
MeanAnomaly += MeanMotion * deltaTime;
}
MeanAnomaly %= KeplerOrbitUtils.PI_2;
if (MeanAnomaly < 0)
{
MeanAnomaly = KeplerOrbitUtils.PI_2 - MeanAnomaly;
}
EccentricAnomaly = KeplerOrbitUtils.KeplerSolver(MeanAnomaly, Eccentricity);
double cosE = Math.Cos(EccentricAnomaly);
TrueAnomaly = Math.Acos((cosE - Eccentricity) / (1 - Eccentricity * cosE));
if (MeanAnomaly > Math.PI)
{
TrueAnomaly = KeplerOrbitUtils.PI_2 - TrueAnomaly;
}
if (double.IsNaN(MeanAnomaly) || double.IsInfinity(MeanAnomaly))
{
Debug.Log("KeplerOrbitData: NaN(INF) MEAN ANOMALY"); //little paranoya
Debug.Break();
}
if (double.IsNaN(EccentricAnomaly) || double.IsInfinity(EccentricAnomaly))
{
Debug.Log("KeplerOrbitData: NaN(INF) ECC ANOMALY");
Debug.Break();
}
if (double.IsNaN(TrueAnomaly) || double.IsInfinity(TrueAnomaly))
{
Debug.Log("KeplerOrbitData: NaN(INF) TRUE ANOMALY");
Debug.Break();
}
}
else
{
MeanAnomaly = MeanAnomaly + MeanMotion * deltaTime;
EccentricAnomaly = KeplerOrbitUtils.KeplerSolverHyperbolicCase(MeanAnomaly, Eccentricity);
TrueAnomaly = Math.Atan2(Math.Sqrt(Eccentricity * Eccentricity - 1.0) * Math.Sinh(EccentricAnomaly), Eccentricity - Math.Cosh(EccentricAnomaly));
}
}
/// <summary>
/// Sets the eccentricity and updates all corresponding orbit state values.
/// </summary>
/// <param name="e">The new eccentricity value.</param>
public void SetEccentricity(double e)
{
if (!IsValidOrbit)
{
return;
}
e = Math.Abs(e);
double periapsis = PeriapsisDistance; // Periapsis remains constant
Eccentricity = e;
double compresion = Eccentricity < 1 ? (1 - Eccentricity * Eccentricity) : (Eccentricity * Eccentricity - 1);
SemiMajorAxis = Math.Abs(periapsis / (1 - Eccentricity));
FocalParameter = SemiMajorAxis * compresion;
SemiMinorAxis = SemiMajorAxis * Math.Sqrt(compresion);
CenterPoint = SemiMajorAxis * Math.Abs(Eccentricity) * SemiMajorAxisBasis;
if (Eccentricity < 1.0)
{
EccentricAnomaly = KeplerOrbitUtils.KeplerSolver(MeanAnomaly, Eccentricity);
double cosE = Math.Cos(EccentricAnomaly);
TrueAnomaly = Math.Acos((cosE - Eccentricity) / (1 - Eccentricity * cosE));
if (MeanAnomaly > Math.PI)
{
TrueAnomaly = KeplerOrbitUtils.PI_2 - TrueAnomaly;
}
}
else if (Eccentricity > 1.0)
{
EccentricAnomaly = KeplerOrbitUtils.KeplerSolverHyperbolicCase(MeanAnomaly, Eccentricity);
TrueAnomaly = Math.Atan2(Math.Sqrt(Eccentricity * Eccentricity - 1) * Math.Sinh(EccentricAnomaly), Eccentricity - Math.Cosh(EccentricAnomaly));
}
else
{
EccentricAnomaly = KeplerOrbitUtils.ConvertMeanToEccentricAnomaly(MeanAnomaly, Eccentricity);
TrueAnomaly = EccentricAnomaly;
}
SetVelocityByCurrentAnomaly();
SetPositionByCurrentAnomaly();
CalculateOrbitStateFromOrbitalVectors();
}
private List <Vector3d> CalculateVelocityDifference(KeplerOrbitMover a, KeplerOrbitMover b, KeplerOrbitData transitionOrbit, double departureTime, double duration, double eccAnomalyDeparture, double eccAnomalyArrival)
{
var aMeanAnomalyAtDeparture = a.OrbitData.MeanAnomaly + a.OrbitData.MeanMotion * departureTime;
var aEccAnomAtDeparture = KeplerOrbitUtils.ConvertMeanToEccentricAnomaly(aMeanAnomalyAtDeparture, a.OrbitData.Eccentricity);
var aVelocityAtDeparture = a.OrbitData.GetVelocityAtEccentricAnomaly(aEccAnomAtDeparture);
var bMeanAnomalyAtArrival = b.OrbitData.MeanAnomaly + b.OrbitData.MeanMotion * (departureTime + duration);
var bEccAnomAtArrival = KeplerOrbitUtils.ConvertMeanToEccentricAnomaly(aMeanAnomalyAtDeparture, a.OrbitData.Eccentricity);
var bVelocityAtArrival = a.OrbitData.GetVelocityAtEccentricAnomaly(aEccAnomAtDeparture);
var transitionVeloctyStart = transitionOrbit.GetVelocityAtEccentricAnomaly(eccAnomalyDeparture);
var transitionVelcityEnd = transitionOrbit.GetVelocityAtEccentricAnomaly(eccAnomalyArrival);
var result = new List <Vector3d>();
result.Add(transitionVeloctyStart - aVelocityAtDeparture);
result.Add(bVelocityAtArrival - transitionVelcityEnd);
return(result);
}
/// <summary>
/// Gets calculated orbit points with defined precision.
/// </summary>
/// <param name="pointsCount">The points count.</param>
/// <param name="origin">The origin.</param>
/// <param name="maxDistance">The maximum distance.</param>
/// <returns>Array of orbit curve points.</returns>
public Vector3[] GetOrbitPoints(int pointsCount, Vector3 origin, float maxDistance = 1000f)
{
if (pointsCount < 2)
{
return(new Vector3[0]);
}
var result = new Vector3[pointsCount];
if (Eccentricity < 1)
{
if (ApoapsisDistance < maxDistance)
{
for (var i = 0; i < pointsCount; i++)
{
result[i] = (Vector3)GetFocalPositionAtEccentricAnomaly(i * KeplerOrbitUtils.PI_2 / (pointsCount - 1d)) + origin;
}
}
else
{
var maxAngle = KeplerOrbitUtils.CalcTrueAnomalyForDistance(maxDistance, Eccentricity, SemiMajorAxis);
for (int i = 0; i < pointsCount; i++)
{
result[i] = (Vector3)GetFocalPositionAtTrueAnomaly(-maxAngle + i * 2d * maxAngle / (pointsCount - 1)) + origin;
}
}
}
else
{
if (maxDistance < PeriapsisDistance)
{
return(new Vector3[0]);
}
var maxAngle = KeplerOrbitUtils.CalcTrueAnomalyForDistance(maxDistance, Eccentricity, SemiMajorAxis);
for (int i = 0; i < pointsCount; i++)
{
result[i] = (Vector3)GetFocalPositionAtTrueAnomaly(-maxAngle + i * 2d * maxAngle / (pointsCount - 1)) + origin;
}
}
return(result);
}
/// <summary>
/// Find and return referneces to three bodies: self, target and mutual attractor.
/// Additional attractors chain arrays are used when mutual attractor is not direct parent of target body.
/// If mutual attractor not existent, or target not assigned, then return empty data.
/// </summary>
/// <returns>All references data, or empty data if not found.</returns>
public BodiesReferencesData GetBodiesReferences()
{
if (_orbitMover == null)
{
_orbitMover = GetComponent <KeplerOrbitMover>();
}
if (_orbitMover == null || Target == null || _orbitMover == Target)
{
return(default(BodiesReferencesData));
}
List <KeplerOrbitMover> attractorsA = new List <KeplerOrbitMover>();
List <KeplerOrbitMover> attractorsB = new List <KeplerOrbitMover>();
double mass = 0;
double g = 0;
Transform mutualAttractor = KeplerOrbitUtils.FindMutualAttractor(
a: _orbitMover,
b: Target,
isGetFullChain: false,
attractorsAChain: ref attractorsA,
attractorsBChain: ref attractorsB,
mass: ref mass,
gConst: ref g);
if (mutualAttractor != null)
{
return(new BodiesReferencesData()
{
Origin = _orbitMover,
Target = Target,
Attractor = mutualAttractor,
OriginAttractorsChain = attractorsA.ToArray(),
TargetAttractorsChain = attractorsB.ToArray(),
AttractorMass = mass,
GConst = g,
});
}
return(default(BodiesReferencesData));
}
/// <summary>
/// Gets the ascending node of orbit.
/// </summary>
/// <param name="asc">The asc.</param>
/// <returns><c>true</c> if ascending node exists, otherwise <c>false</c></returns>
public bool GetAscendingNode(out Vector3d asc)
{
Vector3d ascNodeDir = Vector3d.Cross(OrbitNormal, EclipticNormal);
bool s = Vector3d.Dot(Vector3d.Cross(ascNodeDir, SemiMajorAxisBasis), OrbitNormal) >= 0;
double ecc = 0d;
double trueAnom = Vector3d.Angle(ascNodeDir, CenterPoint) * KeplerOrbitUtils.Deg2Rad;
if (Eccentricity < 1.0)
{
double cosT = Math.Cos(trueAnom);
ecc = Math.Acos((Eccentricity + cosT) / (1d + Eccentricity * cosT));
if (!s)
{
ecc = KeplerOrbitUtils.PI_2 - ecc;
}
}
else if (Eccentricity > 1.0)
{
trueAnom = Vector3d.Angle(-ascNodeDir, CenterPoint) * KeplerOrbitUtils.Deg2Rad;
if (trueAnom >= Math.Acos(-1d / Eccentricity))
{
asc = new Vector3d();
return(false);
}
double cosT = Math.Cos(trueAnom);
ecc = KeplerOrbitUtils.Acosh((Eccentricity + cosT) / (1 + Eccentricity * cosT)) * (!s ? -1 : 1);
}
else
{
asc = new Vector3d();
return(false);
}
asc = GetFocalPositionAtEccentricAnomaly(ecc);
return(true);
}
/// <summary>
/// Gets the central position at true anomaly.
/// </summary>
/// <param name="trueAnomaly">The true anomaly.</param>
/// <returns>Position relative to orbit center.</returns>
/// <remarks>
/// Note: central position is not same as focal position.
/// </remarks>
public Vector3d GetCentralPositionAtTrueAnomaly(double trueAnomaly)
{
double ecc = KeplerOrbitUtils.ConvertTrueToEccentricAnomaly(trueAnomaly, Eccentricity);
return(GetCentralPositionAtEccentricAnomaly(ecc));
}
/// <summary>
/// Gets the velocity vector value at eccentric anomaly.
/// </summary>
/// <param name="eccentricAnomaly">The eccentric anomaly.</param>
/// <returns>Velocity vector.</returns>
public Vector3d GetVelocityAtEccentricAnomaly(double eccentricAnomaly)
{
return(GetVelocityAtTrueAnomaly(KeplerOrbitUtils.ConvertEccentricToTrueAnomaly(eccentricAnomaly, Eccentricity)));
}
/// <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;
}
}
/// <summary>
/// Get world space position vector at given time.
/// </summary>
/// <param name="target">Target body.</param>
/// <param name="time">Time, relative to current time.</param>
/// <param name="attractors">Optional chain of attractors. Order of attractors must be from closest to furthest in hierarchy.</param>
/// <returns>Position at given time.</returns>
/// <remarks>
/// Zero time is considered current state.
/// For example, at time 0 result position vector will be equal to current target position.
/// This method allows to progress orbit in time forward (or backward, if passed time is negative) and get position of body at that time.
/// If attractors collection is not null or empty, then evaluation process will propagate through all attractors, which will affect result.
/// </remarks>
public static Vector3 GetPositionAtGivenTime(KeplerOrbitMover target, float time, KeplerOrbitMover[] attractorsChain = null)
{
if (target == null)
{
return(new Vector3());
}
if (!target.OrbitData.IsValidOrbit || target.AttractorSettings.AttractorObject == null)
{
return(target.transform.position);
}
if (attractorsChain == null || attractorsChain.Length == 0)
{
if (!target.enabled || target.TimeScale == 0f)
{
return(target.transform.position);
}
else
{
var finalMeanAnom = target.OrbitData.MeanAnomaly + target.OrbitData.MeanMotion * time;
var finalEccAnom = KeplerOrbitUtils.ConvertMeanToEccentricAnomaly(finalMeanAnom, target.OrbitData.Eccentricity);
var result = target.AttractorSettings.AttractorObject.transform.position + (Vector3)target.OrbitData.GetFocalPositionAtEccentricAnomaly(finalEccAnom);
return(result);
}
}
else
{
var relativePosition = new Vector3();
for (int i = 0; i < attractorsChain.Length; i++)
{
bool isLast = i == attractorsChain.Length - 1;
if (attractorsChain[i].OrbitData.IsValidOrbit && attractorsChain[i].AttractorSettings.AttractorObject != null)
{
if (attractorsChain[i].enabled)
{
var attrMeanAnom = attractorsChain[i].OrbitData.MeanAnomaly + attractorsChain[i].OrbitData.MeanMotion * attractorsChain[i].TimeScale * time;
var attrEccAnom = KeplerOrbitUtils.ConvertMeanToEccentricAnomaly(attrMeanAnom, attractorsChain[i].OrbitData.Eccentricity);
relativePosition += (Vector3)attractorsChain[i].OrbitData.GetFocalPositionAtEccentricAnomaly(attrEccAnom);
}
else
{
relativePosition += attractorsChain[i].transform.position - attractorsChain[i].AttractorSettings.AttractorObject.transform.position;
}
if (isLast)
{
relativePosition += attractorsChain[i].AttractorSettings.AttractorObject.position;
}
}
else
{
if (isLast || attractorsChain[i].AttractorSettings.AttractorObject == null)
{
relativePosition += attractorsChain[i].transform.position;
}
else
{
relativePosition += (Vector3)attractorsChain[i].OrbitData.Position;
}
}
}
if (!target.enabled || target.TimeScale == 0f)
{
relativePosition += target.transform.position - target.AttractorSettings.AttractorObject.position;
}
else
{
var finalMeanAnom = target.OrbitData.MeanAnomaly + target.OrbitData.MeanMotion * time;
var finalEccAnom = KeplerOrbitUtils.ConvertMeanToEccentricAnomaly(finalMeanAnom, target.OrbitData.Eccentricity);
relativePosition += (Vector3)target.OrbitData.GetFocalPositionAtEccentricAnomaly(finalEccAnom);
}
return(relativePosition);
}
}
/// <summary>
/// Caclulate transition tranjectory from point p0 to point p1 around attractor f0.
/// </summary>
/// <param name="p0">First position vector.</param>
/// <param name="p1">Second position vector.</param>
/// <param name="f0">Attractor position vector.</param>
/// <param name="hyperbola">Trajectory hyperbola.</param>
/// <param name="targetDuration">Preferred duration.</param>
/// <param name="isReverseOrbit">Is transfer orbit plane flipped.</param>
/// <param name="attrMass">Attractor mass.</param>
/// <param name="g">Gravity constant.</param>
/// <param name="precision">Calculation precision.</param>
/// <param name="semiMajorAxisUpperLimit">Transition ellipse semi major axis limit.</param>
/// <returns>Calculated trajectory data.</returns>
/// <remarks>
/// Main task of this component is to find elliptic trajectory between any 2 points, orbiting around single attractor.
/// The core problem can be described in more formal form:
/// let's take a body A and body B; this two bodies rotating around attractor F. The Goal is to find an orbit around attractor F, which passes through vector A and B.
/// This problem is equivalent to to problem of finding of an ellipse when only one focus and two points on ellipse are given,
/// which is known as Lambert's problem (but only with one allowed revolution around attractor). The answer to this problem actually is quite simple. In simplified form the solution can be described in these steps:
/// 1. Build a hyperbola, using given points A and B as focuses of hyperbola, and attractor F as point on one branch of hyperbola.
/// 2. Place a new point F2 anywhere on branch opposite to main branch of hyperbola ( where main branch is branch, on which F is located)
/// 3. Build an ellipse using 3 points: F as first focus, new point F2 as second focus, A or B as one point on ellipse.
/// 4. As result, created ellipse will always exactly correspond to transition orbit, and it always will pass through A and B.
/// Eccentricity of ellipse is dependent on where was placed point F2, which is convenient as it can be tweaked for better orbit parameters.
///
/// Each step of this solution is pretty strait forward. With help of hyperbola, given data of 1 focus and 2 points is converting to 2 focuses and 1 point.
///
/// With known ellipse parameters it is easy to construct orbit data of transition, and additionally it is possible to calculate mean anomaly of departure and arrival.
/// Difference between these mean anomalies multiplied by mean motion gives exact duration of transition.
/// Because time of transition is depending from second focus point F2 (which is parametric), it is possible to set some specific transition time value
/// and then adjust F2 to match target time as close as possible.
/// </remarks>
public static TrajectoryData CalcTransitionTrajectory(Vector3d p0, Vector3d p1, Vector3d f0, HyperbolaData hyperbola, double targetDuration, bool isReverseOrbit, double attrMass, double g, double precision, double semiMajorAxisUpperLimit)
{
TrajectoryData result = new TrajectoryData();
double hyperbolaValue = 0.0;
int lastDeltaSign = 0;
int changedDeltaSignCount = 0;
float delta = 0.8f;
double lastDuration = 0;
int tmp = 0;
Vector3d ellipseSecondFocus = new Vector3d();
// Calculate transition multiple times until optimal transition duration not found.
// Usually steps count is not larger than a hundred, so 1000 iterations limit is for fail checking.
while (true && tmp < 1e3)
{
tmp++;
bool isBranch0 = (p0 - f0).magnitude < (p1 - f0).magnitude;
ellipseSecondFocus = hyperbola.GetSamplePointOnBranch(hyperbolicCoordinate: hyperbolaValue, isMainBranch: isBranch0);
var transitionOrbitEllipse = new EllipseData(focus0: f0, focus1: ellipseSecondFocus, p0: p0);
if (KeplerOrbitUtils.DotProduct(transitionOrbitEllipse.Normal, hyperbola.Normal) <= 0)
{
transitionOrbitEllipse.AxisSecondary *= -1;
}
if (transitionOrbitEllipse.A > semiMajorAxisUpperLimit)
{
break;
}
if (isReverseOrbit)
{
transitionOrbitEllipse.AxisSecondary = -transitionOrbitEllipse.AxisSecondary;
}
result.orbit = new KeplerOrbitData(
eccentricity: transitionOrbitEllipse.Eccentricity,
semiMajorAxis: transitionOrbitEllipse.AxisMain * transitionOrbitEllipse.A,
semiMinorAxis: transitionOrbitEllipse.AxisSecondary * transitionOrbitEllipse.B,
meanAnomalyDeg: 0,
attractorMass: attrMass,
gConst: g);
result.EccAnomStart = transitionOrbitEllipse.GetEccentricAnomalyForPoint(p0);
result.EccAnomEnd = transitionOrbitEllipse.GetEccentricAnomalyForPoint(p1);
if (result.EccAnomStart > result.EccAnomEnd)
{
result.EccAnomEnd += KeplerOrbitUtils.PI_2;
}
var meanAnomStart = KeplerOrbitUtils.ConvertEccentricToMeanAnomaly(result.EccAnomStart, eccentricity: result.orbit.Eccentricity);
var meanAnomEnd = KeplerOrbitUtils.ConvertEccentricToMeanAnomaly(result.EccAnomEnd, eccentricity: result.orbit.Eccentricity);
var meanAnomDiff = meanAnomEnd - meanAnomStart;
result.Duration = meanAnomEnd <= meanAnomStart ? 0.0 : meanAnomDiff / result.orbit.MeanMotion;
var diff = result.Duration - targetDuration;
int sign = diff >= 0 ? -1 : 1;
if (KeplerOrbitUtils.Abs(diff) < precision)
{
break;
}
if (sign != lastDeltaSign)
{
lastDeltaSign = sign;
changedDeltaSignCount++;
}
if (changedDeltaSignCount >= 2)
{
delta *= 0.5f;
}
int conicShapeAligmentSign = KeplerOrbitUtils.DotProduct(transitionOrbitEllipse.Normal, hyperbola.Normal) >= 0 ? 1 : -1;
hyperbolaValue += delta * sign * conicShapeAligmentSign;
var stepDurationDiff = result.Duration - lastDuration;
if (KeplerOrbitUtils.Abs(stepDurationDiff) < precision)
{
break;
}
lastDuration = result.Duration;
}
return(result);
}
