public void Solve()
{
SolveKepler();
if (eccentricity < 1)
{
SolveNormal();
}
else
{
SolveHyperbolic();
}
//Convert TrueAnomaly to state vectors
double GM = PlanetInfo.GetGM(referenceBody);
double num = semiMajorAxis * (1.0 - eccentricity * eccentricity);
double cosTA = Math.Cos(trueAnomaly);
double sinTA = Math.Sin(trueAnomaly);
distanceToCB = num / (1.0 + eccentricity * cosTA);
double[] orbitFramePosition = new double[] { cosTA *distanceToCB, sinTA *distanceToCB, 0 };
double velocityScale = Math.Sqrt(GM / num);
double[] orbitFrameVelocity = new double[] { -sinTA * velocityScale, (cosTA + eccentricity) * velocityScale, 0 };
//Rotation from orbital plane to body non-rotating
double sinW = Math.Sin(argumentOfPeriapsis / Constants.DEGREES_IN_RADIANS);
double cosW = Math.Cos(argumentOfPeriapsis / Constants.DEGREES_IN_RADIANS);
double sinO = Math.Sin(LAN / Constants.DEGREES_IN_RADIANS);
double cosO = Math.Cos(LAN / Constants.DEGREES_IN_RADIANS);
double sinI = Math.Sin(inclination / Constants.DEGREES_IN_RADIANS);
double cosI = Math.Cos(inclination / Constants.DEGREES_IN_RADIANS);
double X1 = cosW * cosO - sinW * cosI * sinO;
double X2 = sinW * cosO + cosW * cosI * sinO;
double Y1 = cosW * sinO + sinW * cosI * cosO;
double Y2 = cosW * cosI * cosO - sinW * sinO;
double Z1 = sinW * sinI;
double Z2 = cosW * sinI;
position[0] = orbitFramePosition[0] * X1 - orbitFramePosition[1] * X2;
position[1] = orbitFramePosition[0] * Y1 + orbitFramePosition[1] * Y2;
position[2] = orbitFramePosition[0] * Z1 + orbitFramePosition[1] * Z2;
velocity[0] = orbitFrameVelocity[0] * X1 - orbitFrameVelocity[1] * X2;
velocity[1] = orbitFrameVelocity[0] * Y1 + orbitFrameVelocity[1] * Y2;
velocity[2] = orbitFrameVelocity[0] * Z1 + orbitFrameVelocity[1] * Z2;
}
public void UpdateToUT(double universeTime)
{
double deltaTime = (universeTime - epoch);
if (eccentricity < 1)
{
period = Constants.TAU * Math.Sqrt(Math.Pow(semiMajorAxis, 3) / PlanetInfo.GetGM(referenceBody));
deltaTime = deltaTime % period;
while (deltaTime < 0)
{
deltaTime += period;
}
}
else
{
period = Double.PositiveInfinity;
}
double GM = PlanetInfo.GetGM(referenceBody);
double positiveSMA = Math.Abs(semiMajorAxis);
double meanMotion = Math.Sqrt(GM / (positiveSMA * positiveSMA * positiveSMA));
meanAnomalyAtEpoch = meanAnomalyAtEpoch + deltaTime * meanMotion;
if (eccentricity < 1)
{
//Normalise meanAnomaly
meanAnomalyAtEpoch = meanAnomalyAtEpoch % Constants.TAU;
while (meanAnomalyAtEpoch < -Math.PI)
{
meanAnomalyAtEpoch += Constants.TAU;
}
while (meanAnomalyAtEpoch > Math.PI)
{
meanAnomalyAtEpoch -= Constants.TAU;
}
}
epoch = universeTime;
Solve();
}