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(); }