public static double GetTrueAnomaly(KeplerElements orbit, DateTime time)
{
TimeSpan timeSinceEpoch = time - orbit.Epoch;
// Don't attempt to calculate large timeframes.
while (timeSinceEpoch.TotalSeconds > orbit.OrbitalPeriod && orbit.OrbitalPeriod != 0)
{
double years = timeSinceEpoch.TotalSeconds / orbit.OrbitalPeriod;
timeSinceEpoch -= TimeSpan.FromSeconds(years * orbit.OrbitalPeriod);
orbit.Epoch += TimeSpan.FromSeconds(years * orbit.OrbitalPeriod);
}
double m0 = orbit.MeanAnomalyAtEpoch;
double n = orbit.MeanMotion;
double currentMeanAnomaly = OrbitMath.GetMeanAnomalyFromTime(m0, n, timeSinceEpoch.TotalSeconds);
double eccentricAnomaly = GetEccentricAnomaly(orbit, currentMeanAnomaly);
return(OrbitMath.TrueAnomalyFromEccentricAnomaly(orbit.Eccentricity, eccentricAnomaly));
/*
* var x = Math.Cos(eccentricAnomaly) - orbit.Eccentricity;
* var y = Math.Sqrt(1 - orbit.Eccentricity * orbit.Eccentricity) * Math.Sin(eccentricAnomaly);
* return Math.Atan2(y, x);
*/
}
public static Vector3 GetPosition_m(KeplerElements orbit, double trueAnomaly)
{
// http://en.wikipedia.org/wiki/True_anomaly#Radius_from_true_anomaly
double radius = orbit.SemiMajorAxis * (1 - orbit.Eccentricity * orbit.Eccentricity) / (1 + orbit.Eccentricity * Math.Cos(trueAnomaly));
double incl = orbit.Inclination;
//https://downloads.rene-schwarz.com/download/M001-Keplerian_Orbit_Elements_to_Cartesian_State_Vectors.pdf
double lofAN = orbit.LoAN;
//double aofP = Angle.ToRadians(orbit.ArgumentOfPeriapsis);
double angleFromLoAN = trueAnomaly + orbit.AoP;
double x = Math.Cos(lofAN) * Math.Cos(angleFromLoAN) - Math.Sin(lofAN) * Math.Sin(angleFromLoAN) * Math.Cos(incl);
double y = Math.Sin(lofAN) * Math.Cos(angleFromLoAN) + Math.Cos(lofAN) * Math.Sin(angleFromLoAN) * Math.Cos(incl);
double z = Math.Sin(incl) * Math.Sin(angleFromLoAN);
return(new Vector3(x, y, z) * radius);
}
/// <summary>
/// In Meters
/// </summary>
/// <param name="parent"></param>
/// <param name="myMass"></param>
/// <param name="ke">in m</param>
/// <param name="atDateTime"></param>
/// <returns></returns>
public static OrbitDB FromKeplerElements(Entity parent, double myMass, KeplerElements ke, DateTime atDateTime)
{
OrbitDB orbit = new OrbitDB(parent)
{
SemiMajorAxis = ke.SemiMajorAxis,
Eccentricity = ke.Eccentricity,
Inclination = ke.Inclination,
LongitudeOfAscendingNode = ke.LoAN,
ArgumentOfPeriapsis = ke.AoP,
MeanAnomalyAtEpoch = ke.MeanAnomalyAtEpoch,
Epoch = atDateTime,
_parentMass = parent.GetDataBlob <MassVolumeDB>().Mass,
_myMass = myMass
};
orbit.IsStationary = false;
orbit.CalculateExtendedParameters();
return(orbit);
}
/// <summary>
/// Calculates the current Eccentric Anomaly given certain orbital parameters.
/// </summary>
public static double GetEccentricAnomaly(KeplerElements orbit, double currentMeanAnomaly)
{
//Kepler's Equation
const int numIterations = 1000;
var e = new double[numIterations];
const double epsilon = 1E-12; // Plenty of accuracy.
int i = 0;
if (orbit.Eccentricity > 0.8)
{
e[i] = Math.PI;
}
else
{
e[i] = currentMeanAnomaly;
}
do
{
// Newton's Method.
/* E(n) - e sin(E(n)) - M(t)
* E(n+1) = E(n) - ( ------------------------- )
* 1 - e cos(E(n)
*
* E == EccentricAnomaly, e == Eccentricity, M == MeanAnomaly.
* http://en.wikipedia.org/wiki/Eccentric_anomaly#From_the_mean_anomaly
*/
e[i + 1] = e[i] - (e[i] - orbit.Eccentricity * Math.Sin(e[i]) - currentMeanAnomaly) / (1 - orbit.Eccentricity * Math.Cos(e[i]));
i++;
} while (Math.Abs(e[i] - e[i - 1]) > epsilon && i + 1 < numIterations);
if (i + 1 >= numIterations)
{
Event gameEvent = new Event("Non-convergence of Newton's method while calculating Eccentric Anomaly from kepler Elements.");
gameEvent.EventType = EventType.Opps;
StaticRefLib.EventLog.AddEvent(gameEvent);
}
return(e[i - 1]);
}
public KeplerElements GetElements()
{
KeplerElements ke = new KeplerElements();
ke.SemiMajorAxis = SemiMajorAxis; //a
ke.SemiMinorAxis = SemiMajorAxis * Math.Sqrt(1 - Eccentricity * Eccentricity); //b
ke.Eccentricity = Eccentricity; //e
ke.Periapsis = Periapsis; //q
ke.Apoapsis = Apoapsis; //Q
ke.LoAN = LongitudeOfAscendingNode; //Ω (upper case Omega)
ke.AoP = ArgumentOfPeriapsis; //ω (lower case omega)
ke.Inclination = Inclination; //i
ke.MeanMotion = MeanMotion; //n
ke.MeanAnomalyAtEpoch = MeanAnomalyAtEpoch; //M0
ke.Epoch = Epoch;
ke.LinearEccentricity = Eccentricity * SemiMajorAxis; //ae
ke.OrbitalPeriod = OrbitalPeriod.TotalSeconds;
//ke.TrueAnomalyAtEpoch ; //ν or f or θ
return(ke);
}
/// <summary>
/// This was designed so that fast moving objects will get interpolated a lot more than slow moving objects
/// so fast moving objects shouldn't loose positional acuracy when close to a planet,
/// and slow moving objects won't have processor time wasted on them by calulcating too often.
/// However this seems to be unstable and looses energy, unsure why. currently set it to just itterate/interpolate every second.
/// so currently will be using more time to get through this than neccisary.
/// </summary>
/// <param name="entity">Entity.</param>
/// <param name="deltaSeconds">Delta seconds.</param>
public static void NewtonMove(Entity entity, int deltaSeconds)
{
NewtonMoveDB newtonMoveDB = entity.GetDataBlob <NewtonMoveDB>();
NewtonThrustAbilityDB newtonThrust = entity.GetDataBlob <NewtonThrustAbilityDB>();
PositionDB positionDB = entity.GetDataBlob <PositionDB>();
double mass_Kg = entity.GetDataBlob <MassVolumeDB>().Mass;
double parentMass_kg = newtonMoveDB.ParentMass;
var manager = entity.Manager;
DateTime dateTimeFrom = newtonMoveDB.LastProcessDateTime;
DateTime dateTimeNow = manager.ManagerSubpulses.StarSysDateTime;
DateTime dateTimeFuture = dateTimeNow + TimeSpan.FromSeconds(deltaSeconds);
double deltaT = (dateTimeFuture - dateTimeFrom).TotalSeconds;
double secondsToItterate = deltaT;
while (secondsToItterate > 0)
{
//double timeStep = Math.Max(secondsToItterate / speed_kms, 1);
//timeStep = Math.Min(timeStep, secondsToItterate);
double timeStepInSeconds = 1;//because the above seems unstable and looses energy.
double distanceToParent_m = positionDB.GetDistanceTo_m(newtonMoveDB.SOIParent.GetDataBlob <PositionDB>());
distanceToParent_m = Math.Max(distanceToParent_m, 0.1); //don't let the distance be 0 (once collision is in this will likely never happen anyway)
double gravForce = GameConstants.Science.GravitationalConstant * (mass_Kg * parentMass_kg / Math.Pow(distanceToParent_m, 2));
Vector3 gravForceVector = gravForce * -Vector3.Normalise(positionDB.RelativePosition_m);
Vector3 totalDVFromGrav = (gravForceVector / mass_Kg) * timeStepInSeconds;
double maxAccelFromThrust1 = newtonThrust.ExhaustVelocity * Math.Log(mass_Kg / (mass_Kg - newtonThrust.FuelBurnRate)); //per second
double maxAccelFromThrust = newtonThrust.ThrustInNewtons / mass_Kg; //per second
Vector3 manuverDV = newtonMoveDB.DeltaVForManuver_m; //how much dv needed to complete the manuver.
double dryMass = mass_Kg - newtonThrust.FuelBurnRate * timeStepInSeconds; //how much our ship weighs after a timestep of fuel is used.
//how much dv can we get in this timestep.
double deltaVThisStep = OrbitMath.TsiolkovskyRocketEquation(mass_Kg, dryMass, newtonThrust.ExhaustVelocity);
deltaVThisStep = Math.Min(manuverDV.Length(), deltaVThisStep); //don't use more Dv than what is called for.
deltaVThisStep = Math.Min(newtonThrust.DeltaV, deltaVThisStep); //check we've got the deltaV to spend.
Vector3 totalDVFromThrust = Vector3.Normalise(manuverDV) * deltaVThisStep;
//remove the deltaV we're expending from the max (TODO: Remove fuel from cargo, change mass of ship)
newtonThrust.DeltaV -= deltaVThisStep;
//remove the vectorDV from the amount needed to fully complete the manuver.
newtonMoveDB.DeltaVForManuver_m -= totalDVFromThrust;
Vector3 totalDV = totalDVFromGrav + totalDVFromThrust;
Vector3 newVelocity = totalDV + newtonMoveDB.CurrentVector_ms;
newtonMoveDB.CurrentVector_ms = newVelocity;
Vector3 deltaPos = (newtonMoveDB.CurrentVector_ms + newVelocity) / 2 * timeStepInSeconds;
positionDB.RelativePosition_m += deltaPos;
double sOIRadius = OrbitProcessor.GetSOI_m(newtonMoveDB.SOIParent);
if (positionDB.RelativePosition_m.Length() >= sOIRadius)
{
Entity newParent;
Vector3 parentRalitiveVector;
//if our parent is a regular kepler object (normaly this is the case)
if (newtonMoveDB.SOIParent.HasDataBlob <OrbitDB>())
{
var orbitDB = newtonMoveDB.SOIParent.GetDataBlob <OrbitDB>();
newParent = orbitDB.Parent;
var parentVelocity = OrbitProcessor.InstantaneousOrbitalVelocityVector_m(orbitDB, entity.StarSysDateTime);
parentRalitiveVector = newtonMoveDB.CurrentVector_ms + parentVelocity;
}
else //if (newtonMoveDB.SOIParent.HasDataBlob<NewtonMoveDB>())
{ //this will pretty much never happen.
newParent = newtonMoveDB.SOIParent.GetDataBlob <NewtonMoveDB>().SOIParent;
var parentVelocity = newtonMoveDB.SOIParent.GetDataBlob <NewtonMoveDB>().CurrentVector_ms;
parentRalitiveVector = newtonMoveDB.CurrentVector_ms + parentVelocity;
}
parentMass_kg = newParent.GetDataBlob <MassVolumeDB>().Mass;
Vector3 posRalitiveToNewParent = positionDB.AbsolutePosition_m - newParent.GetDataBlob <PositionDB>().AbsolutePosition_m;
var dateTime = dateTimeNow + TimeSpan.FromSeconds(deltaSeconds - secondsToItterate);
double sgp = GMath.StandardGravitationalParameter(parentMass_kg + mass_Kg);
var kE = OrbitMath.KeplerFromPositionAndVelocity(sgp, posRalitiveToNewParent, parentRalitiveVector, dateTime);
positionDB.SetParent(newParent);
newtonMoveDB.ParentMass = parentMass_kg;
newtonMoveDB.SOIParent = newParent;
newtonMoveDB.CurrentVector_ms = parentRalitiveVector;
}
if (newtonMoveDB.DeltaVForManuver_m.Length() <= 0) //if we've completed the manuver.
{
var dateTime = dateTimeNow + TimeSpan.FromSeconds(deltaSeconds - secondsToItterate);
double sgp = GMath.StandardGravitationalParameter(parentMass_kg + mass_Kg);
KeplerElements kE = OrbitMath.KeplerFromPositionAndVelocity(sgp, positionDB.RelativePosition_m, newtonMoveDB.CurrentVector_ms, dateTime);
var parentEntity = Entity.GetSOIParentEntity(entity, positionDB);
if (kE.Eccentricity < 1) //if we're going to end up in a regular orbit around our new parent
{
var newOrbit = OrbitDB.FromKeplerElements(
parentEntity,
mass_Kg,
kE,
dateTime);
entity.RemoveDataBlob <NewtonMoveDB>();
entity.SetDataBlob(newOrbit);
positionDB.SetParent(parentEntity);
var newPos = OrbitProcessor.GetPosition_m(newOrbit, dateTime);
positionDB.RelativePosition_m = newPos;
}
break;
}
secondsToItterate -= timeStepInSeconds;
}
newtonMoveDB.LastProcessDateTime = dateTimeFuture;
}
public static OrbitDB FromKeplerElements(Entity parent, double parentMass, double myMass, KeplerElements ke, DateTime atDateTime)
{
OrbitDB orbit = new OrbitDB(parent, parentMass, myMass,
ke.SemiMajorAxis,
ke.Eccentricity,
Angle.ToDegrees(ke.Inclination),
Angle.ToDegrees(ke.LoAN),
Angle.ToDegrees(ke.AoP),
Angle.ToDegrees(ke.MeanAnomalyAtEpoch),
ke.Epoch);// - TimeSpan.FromSeconds(ke.Epoch));
//var pos = OrbitProcessor.GetAbsolutePosition_AU(orbit, atDateTime);
return(orbit);
}
private const double Epsilon = 1.0e-15; //TODO: test how low we can go
/// <summary>
/// Kepler elements from velocity and position.
/// Note, to get correct results ensure all Sgp, position, and velocity values are all in the same type (ie meters, km, or AU)
/// </summary>
/// <returns>a struct of Kepler elements.</returns>
/// <param name="standardGravParam">Standard grav parameter.</param>
/// <param name="position">Position ralitive to parent</param>
/// <param name="velocity">Velocity ralitive to parent</param>
public static KeplerElements KeplerFromPositionAndVelocity(double standardGravParam, Vector3 position, Vector3 velocity, DateTime epoch)
{
KeplerElements ke = new KeplerElements();
Vector3 angularVelocity = Vector3.Cross(position, velocity);
Vector3 nodeVector = Vector3.Cross(new Vector3(0, 0, 1), angularVelocity);
Vector3 eccentVector = EccentricityVector(standardGravParam, position, velocity);
double eccentricity = eccentVector.Length();
double specificOrbitalEnergy = Math.Pow(velocity.Length(), 2) * 0.5 - standardGravParam / position.Length();
double semiMajorAxis;
double p; //p is where the ellipse or hypobola crosses a line from the focal point 90 degrees from the sma
if (Math.Abs(eccentricity) > 1) //hypobola
{
semiMajorAxis = -(-standardGravParam / (2 * specificOrbitalEnergy)); //in this case the sma is negitive
p = semiMajorAxis * (1 - eccentricity * eccentricity);
}
else if (Math.Abs(eccentricity) < 1) //ellipse
{
semiMajorAxis = -standardGravParam / (2 * specificOrbitalEnergy);
p = semiMajorAxis * (1 - eccentricity * eccentricity);
}
else //parabola
{
p = angularVelocity.Length() * angularVelocity.Length() / standardGravParam;
semiMajorAxis = double.MaxValue;
}
double semiMinorAxis = EllipseMath.SemiMinorAxis(semiMajorAxis, eccentricity);
double linierEccentricity = eccentricity * semiMajorAxis;
double inclination = Math.Acos(angularVelocity.Z / angularVelocity.Length()); //should be 0 in 2d. or pi if counter clockwise orbit.
if (double.IsNaN(inclination))
{
inclination = 0;
}
double longdOfAN = CalculateLongitudeOfAscendingNode(nodeVector);
double trueAnomaly = TrueAnomaly(eccentVector, position, velocity);
double argOfPeriaps = GetArgumentOfPeriapsis2(position, inclination, longdOfAN, trueAnomaly);
var meanMotion = Math.Sqrt(standardGravParam / Math.Pow(semiMajorAxis, 3));
double eccentricAnomoly = GetEccentricAnomalyFromTrueAnomaly(trueAnomaly, eccentricity);
var meanAnomaly = GetMeanAnomaly(eccentricity, eccentricAnomoly);
ke.SemiMajorAxis = semiMajorAxis;
ke.SemiMinorAxis = semiMinorAxis;
ke.Eccentricity = eccentricity;
ke.Apoapsis = EllipseMath.Apoapsis(eccentricity, semiMajorAxis);
ke.Periapsis = EllipseMath.Periapsis(eccentricity, semiMajorAxis);
ke.LinierEccentricity = EllipseMath.LinierEccentricity(ke.Apoapsis, semiMajorAxis);
ke.LoAN = longdOfAN;
ke.AoP = argOfPeriaps;
ke.Inclination = inclination;
ke.MeanMotion = meanMotion;
ke.MeanAnomalyAtEpoch = meanAnomaly;
ke.TrueAnomalyAtEpoch = trueAnomaly;
ke.Epoch = epoch; //TimeFromPeriapsis(semiMajorAxis, standardGravParam, meanAnomaly);
//Epoch(semiMajorAxis, semiMinorAxis, eccentricAnomoly, OrbitalPeriod(standardGravParam, semiMajorAxis));
return(ke);
}
public static Vector3 GetPosition_m(KeplerElements orbit, DateTime time)
{
return(GetPosition_m(orbit, GetTrueAnomaly(orbit, time)));
}
