public bool GetAscendingNode(out Vector3d asc)
{
var norm = CelestialBodyUtils.CrossProduct(orbitNormal, eclipticNormal);
var s = CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(norm, semiMajorAxisBasis), orbitNormal) < 0;
var ecc = 0d;
var trueAnom = Vector3d.Angle(norm, centerPoint) * Mathd.Deg2Rad;
if (eccentricity < 1)
{
var cosT = System.Math.Cos(trueAnom);
ecc = System.Math.Acos((eccentricity + cosT) / (1d + eccentricity * cosT));
if (!s)
{
ecc = Mathd.PI_2 - ecc;
}
}
else
{
trueAnom = Vector3d.Angle(-norm, centerPoint) * Mathd.Deg2Rad;
if (trueAnom >= Mathd.Acos(-1d / eccentricity))
{
asc = new Vector3d();
return(false);
}
var cosT = System.Math.Cos(trueAnom);
ecc = CelestialBodyUtils.Acosh((eccentricity + cosT) / (1 + eccentricity * cosT)) * (!s ? -1 : 1);
}
asc = GetFocalPositionAtEccentricAnomaly(ecc);
return(true);
}
private void DrawOrbitInEditorFor(CelestialBody body)
{
int pointsCount = _simControl.SceneElementsDisplayParameters.OrbitPointsCount;
if (body.isActiveAndEnabled)
{
if (!Application.isPlaying && body.AttractorRef != null && body.OrbitData.IsDirty)
{
if (body.AttractorRef.Mass <= 0)
{
body.AttractorRef.Mass = 1e-007;//to avoid div by zero
}
body.CalculateNewOrbitData();
}
Handles.color = Color.white;
Vector3d[] points = null;
body.GetOrbitPointsNoAlloc(ref points, pointsCount, false, (float)_simControl.SceneElementsDisplayParameters.MaxOrbitDistance);
for (int i = 1; i < points.Length; i++)
{
Handles.DrawLine((Vector3)points[i - 1], (Vector3)points[i]);
}
if (_simControl.SceneElementsDisplayParameters.DrawOrbitsEclipticProjection && points.Length > 0)
{
var point1 = points[0] - _simControl.EclipticNormal * CelestialBodyUtils.DotProduct(points[0], _simControl.EclipticNormal);
var point2 = Vector3d.zero;
Handles.color = Color.gray;
for (int i = 1; i < points.Length; i++)
{
point2 = points[i] - _simControl.EclipticNormal * CelestialBodyUtils.DotProduct(points[i], _simControl.EclipticNormal);
Handles.DrawLine((Vector3)point1, (Vector3)point2);
point1 = point2;
}
}
}
}
/// <summary>
/// Gets the descending node of 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)
{
var norm = CelestialBodyUtils.CrossProduct(OrbitNormal, EclipticNormal);
var s = CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(norm, SemiMajorAxisBasis), OrbitNormal) < 0;
var ecc = 0d;
var trueAnom = Vector3d.Angle(norm, -CenterPoint) * Mathd.Deg2Rad;
if (Eccentricity < 1)
{
var cosT = Math.Cos(trueAnom);
ecc = Math.Acos((Eccentricity + cosT) / (1d + Eccentricity * cosT));
if (s)
{
ecc = Mathd.PI_2 - ecc;
}
}
else
{
trueAnom = Vector3d.Angle(norm, CenterPoint) * Mathd.Deg2Rad;
if (trueAnom >= Mathd.Acos(-1d / Eccentricity))
{
desc = new Vector3d();
return(false);
}
var cosT = Math.Cos(trueAnom);
ecc = CelestialBodyUtils.Acosh((Eccentricity + cosT) / (1 + Eccentricity * cosT)) * (s ? -1 : 1);
}
desc = GetFocalPositionAtEccentricAnomaly(ecc);
return(true);
}
/// <summary>
/// Orbit plane will be unchanged.
/// </summary>
public void MakeOrbitCircle(bool clockwise) {
if (attractor) {
#if UNITY_EDITOR
if (!Application.isPlaying) {
FindReferences();
attractor.FindReferences();
Undo.RecordObject(this, "Round orbit");
}
#endif
var dotProduct = CelestialBodyUtils.DotProduct(orbitData.orbitNormal, simControlRef.eclipticNormal); //sign of this value determines orbit orientation
if (Mathd.Abs(orbitData.orbitNormal.sqrMagnitude - 1d) > 0.5d) {
orbitData.orbitNormal = simControlRef.eclipticNormal;
}
var v = CelestialBodyUtils.CalcCircleOrbitVelocity(
attractor._position,
_position,
attractor.mass,
mass,
orbitData.orbitNormal * ( clockwise && dotProduct >= 0 || !clockwise && dotProduct < 0 ? 1 : -1 ),
simControlRef.gravitationalConstant
);
if (relativeVelocity != v) {
relativeVelocity = v;
orbitData.isDirty = true;
}
}
}
public static Vector3 GetRayPlaneIntersectionPoint(Vector3 pointOnPlane, Vector3 normal, Vector3 rayOrigin, Vector3 rayDirection)
{
var dotProd = CelestialBodyUtils.DotProduct(rayDirection, normal);
if (Mathd.Abs(dotProd) < 1e-5)
{
return(new Vector3());
}
var p = rayOrigin + rayDirection * CelestialBodyUtils.DotProduct((pointOnPlane - rayOrigin), normal) / dotProd;
p = p - normal * CelestialBodyUtils.DotProduct(p - pointOnPlane, normal); //projection. for better precision
return(p);
}
public void ProjectOntoEclipticPlane() {
#if UNITY_EDITOR
if (!Application.isPlaying) {
Undo.RecordObject(this, "ecliptic projection");
Undo.RecordObject(transformRef, "ecliptic projection");
}
#endif
var projectedPos = _position - simControlRef.eclipticNormal * CelestialBodyUtils.DotProduct(_position, simControlRef.eclipticNormal);
var projectedV = velocity - simControlRef.eclipticNormal * CelestialBodyUtils.DotProduct(velocity, simControlRef.eclipticNormal);
_position = projectedPos;
transformRef.position = (Vector3)projectedPos;
velocity = projectedV;
orbitData.isDirty = true;
#if UNITY_EDITOR
if (!Application.isPlaying) {
EditorUtility.SetDirty(this);
}
#endif
}
private void DrawOrbitElementsAndLabels()
{
var prms = _simControl.SceneElementsDisplayParameters;
for (int i = 0; i < _bodies.Length; i++)
{
if (_bodies[i].IsValidOrbit)
{
if (prms.DrawPeriapsisPoint)
{
// Periapsis.
DrawX((Vector3)_bodies[i].OrbitPeriapsisPoint, prms.CirclesScale, Color.green, (Vector3)_bodies[i].OrbitData.OrbitNormal, (Vector3)_bodies[i].OrbitData.SemiMajorAxisBasis);
}
if (prms.DrawPeriapsisLine)
{
Handles.color = new Color(1, 1, 0);
Handles.DrawLine((Vector3)_bodies[i].AttractorRef.Position, (Vector3)_bodies[i].OrbitPeriapsisPoint);
}
if (prms.DrawPeriapsisLabel)
{
DrawLabelScaled((Vector3)_bodies[i].OrbitPeriapsisPoint, "P", Color.white, 10f);
}
if (prms.DrawApoapsisPoint || prms.DrawApoapsisLabel || prms.DrawApoapsisLine)
{
// Apoapsis.
if (!double.IsInfinity(_bodies[i].OrbitApoapsisPoint.x) && !double.IsNaN(_bodies[i].OrbitApoapsisPoint.x))
{
if (prms.DrawApoapsisPoint)
{
DrawX((Vector3)_bodies[i].OrbitApoapsisPoint, prms.CirclesScale, Color.green, (Vector3)_bodies[i].OrbitData.OrbitNormal, (Vector3)_bodies[i].OrbitData.SemiMajorAxisBasis);
}
if (prms.DrawApoapsisLine)
{
Handles.color = new Color(1, 0.5f, 0);
Handles.DrawLine((Vector3)_bodies[i].AttractorRef.Position, (Vector3)_bodies[i].OrbitApoapsisPoint);
}
if (prms.DrawApoapsisLabel)
{
DrawLabelScaled((Vector3)_bodies[i].OrbitApoapsisPoint, "A", Color.white, 10f);
}
}
}
if (prms.DrawCenterOfMassPoint)
{
// Center of mass.
Handles.color = Color.white;
Handles.DrawWireDisc((Vector3)_bodies[i].CenterOfMass, (Vector3)_bodies[i].OrbitData.OrbitNormal, 1f);
}
if (prms.DrawAscendingNodeLabel || prms.DrawAscendingNodeLine || prms.DrawAscendingNodePoint)
{
Vector3 asc;
if (_bodies[i].GetAscendingNode(out asc))
{
// Ascending node.
asc = asc + (Vector3)_bodies[i].AttractorRef.Position;
if (prms.DrawAscendingNodePoint)
{
DrawX(asc, prms.CirclesScale, Color.blue, (Vector3)_bodies[i].OrbitData.OrbitNormal, (Vector3)_bodies[i].OrbitData.SemiMajorAxisBasis);
}
if (prms.DrawAscendingNodeLine)
{
Handles.color = new Color(0.1f, 0.3f, 0.8f, 0.8f);
Handles.DrawLine((Vector3)_bodies[i].AttractorRef.Position, asc);
}
if (prms.DrawAscendingNodeLabel)
{
DrawLabelScaled(asc, "ASC", Color.white, 10f);
}
}
}
if (prms.DrawDescendingNodeLabel || prms.DrawDescendingNodeLine || prms.DrawDescendingNodePoint)
{
Vector3 desc;
if (_bodies[i].GetDescendingNode(out desc))
{
// Descending node.
desc = desc + (Vector3)_bodies[i].AttractorRef.Position;
if (prms.DrawDescendingNodePoint)
{
DrawX(desc, prms.CirclesScale, Color.blue, (Vector3)_bodies[i].OrbitData.OrbitNormal, (Vector3)_bodies[i].OrbitData.SemiMajorAxisBasis);
}
if (prms.DrawDescendingNodeLine)
{
Handles.color = new Color(0.8f, 0.3f, 0.1f, 0.8f);
Handles.DrawLine((Vector3)_bodies[i].AttractorRef.Position, desc);
}
if (prms.DrawDescendingNodeLabel)
{
DrawLabelScaled(desc, "DESC", Color.white, 10f);
}
}
}
if (prms.DrawInclinationLabel)
{
// Inclination.
DrawInclinationMarkForBody(_bodies[i], prms.NormalAxisScale);
}
if (prms.DrawRadiusVector)
{
// Radius vector.
Handles.color = Color.gray;
Handles.DrawLine((Vector3)_bodies[i].AttractorRef.Position, (Vector3)_bodies[i].Position);
}
if (prms.DrawOrbitsNormal)
{
// Orbit normal.
Handles.color = new Color(0.16f, 0.92f, 0.88f, 0.8f);
Handles.DrawLine((Vector3)_bodies[i].OrbitCenterPoint, (Vector3)_bodies[i].OrbitCenterPoint + (Vector3)_bodies[i].OrbitData.OrbitNormal * prms.NormalAxisScale * 5f);
Handles.DrawWireDisc((Vector3)_bodies[i].OrbitCenterPoint, (Vector3)_bodies[i].OrbitData.OrbitNormal, prms.NormalAxisScale * 2f);
}
if (prms.DrawSemiAxis)
{
// SemiMinor axis normal.
Handles.color = Color.blue;
Handles.DrawLine((Vector3)_bodies[i].OrbitCenterPoint, (Vector3)_bodies[i].OrbitCenterPoint + (Vector3)_bodies[i].OrbitData.SemiMinorAxisBasis * prms.NormalAxisScale * 5f);
// SemiMajor axis normal.
Handles.color = Color.red;
Handles.DrawLine((Vector3)_bodies[i].OrbitCenterPoint, (Vector3)_bodies[i].OrbitCenterPoint + (Vector3)_bodies[i].OrbitData.SemiMajorAxisBasis * prms.NormalAxisScale * 5f);
}
}
if (_simControl.SceneElementsDisplayParameters.DrawBodiesEclipticProjection)
{
Handles.color = Color.yellow;
var ProjectionPos = _bodies[i].Position - _simControl.EclipticNormal * CelestialBodyUtils.DotProduct(_bodies[i].Position, _simControl.EclipticNormal);
if ((ProjectionPos - _bodies[i].Position).sqrMagnitude > 1e-003d)
{
Handles.DrawDottedLine((Vector3)_bodies[i].Position, (Vector3)ProjectionPos, 2f);
Handles.DrawWireDisc((Vector3)ProjectionPos, (Vector3)_simControl.EclipticNormal, _simControl.SceneElementsDisplayParameters.CirclesScale);
}
}
}
}
/// <summary>
/// Calculates the full state of orbit from current body position, attractor position, attractor mass, velocity, and gravConstant.
/// </summary>
public void CalculateNewOrbitData()
{
IsDirty = false;
var MG = AttractorMass * GravitationalConstant;
AttractorDistance = Position.magnitude;
var angularMomentumVector = CelestialBodyUtils.CrossProduct(Position, Velocity);
OrbitNormal = angularMomentumVector.normalized;
Vector3d eccVector;
// Check if zero lenght.
if (OrbitNormal.sqrMagnitude < 0.9 || OrbitNormal.sqrMagnitude > 1.1)
{
OrbitNormal = CelestialBodyUtils.CrossProduct(Position, EclipticUp).normalized;
eccVector = new Vector3d();
}
else
{
eccVector = CelestialBodyUtils.CrossProduct(Velocity, angularMomentumVector) / MG - Position / AttractorDistance;
}
OrbitNormalDotEclipticNormal = CelestialBodyUtils.DotProduct(OrbitNormal, EclipticNormal);
FocalParameter = angularMomentumVector.sqrMagnitude / MG;
Eccentricity = eccVector.magnitude;
EnergyTotal = Velocity.sqrMagnitude - 2 * MG / AttractorDistance;
SemiMinorAxisBasis = CelestialBodyUtils.CrossProduct(angularMomentumVector, eccVector).normalized;
if (SemiMinorAxisBasis.sqrMagnitude < 0.5)
{
SemiMinorAxisBasis = CelestialBodyUtils.CrossProduct(OrbitNormal, Position).normalized;
}
SemiMajorAxisBasis = CelestialBodyUtils.CrossProduct(OrbitNormal, SemiMinorAxisBasis).normalized;
Inclination = Vector3d.Angle(OrbitNormal, EclipticNormal) * Mathd.Deg2Rad;
if (Eccentricity < 1)
{
OrbitCompressionRatio = 1 - Eccentricity * Eccentricity;
SemiMajorAxis = FocalParameter / OrbitCompressionRatio;
SemiMinorAxis = SemiMajorAxis * Math.Sqrt(OrbitCompressionRatio);
CenterPoint = -SemiMajorAxis * eccVector;
Period = Mathd.PI_2 * Mathd.Sqrt(Mathd.Pow(SemiMajorAxis, 3) / MG);
Apoapsis = CenterPoint + SemiMajorAxisBasis * SemiMajorAxis;
Periapsis = CenterPoint - SemiMajorAxisBasis * SemiMajorAxis;
PeriapsisDistance = Periapsis.magnitude;
ApoapsisDistance = Apoapsis.magnitude;
TrueAnomaly = Vector3d.Angle(Position, -SemiMajorAxisBasis) * Mathd.Deg2Rad;
if (CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(Position, SemiMajorAxisBasis), OrbitNormal) < 0)
{
TrueAnomaly = Mathd.PI_2 - TrueAnomaly;
}
EccentricAnomaly = CelestialBodyUtils.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) * Mathd.Deg2Rad;
if (CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(Position, SemiMajorAxisBasis), OrbitNormal) < 0)
{
TrueAnomaly = -TrueAnomaly;
}
EccentricAnomaly = CelestialBodyUtils.ConvertTrueToEccentricAnomaly(TrueAnomaly, Eccentricity);
MeanAnomaly = Math.Sinh(EccentricAnomaly) * Eccentricity - EccentricAnomaly;
}
}
public void CalculateNewOrbitData()
{
isDirty = false;
var MG = attractorMass * gravConst;
attractorDistance = position.magnitude;
var angularMomentumVector = CelestialBodyUtils.CrossProduct(position, velocity);
orbitNormal = angularMomentumVector.normalized;
Vector3d eccVector;
if (orbitNormal.sqrMagnitude < 0.9 || orbitNormal.sqrMagnitude > 1.1) //check if zero lenght
{
orbitNormal = CelestialBodyUtils.CrossProduct(position, eclipticUp).normalized;
eccVector = new Vector3d();
}
else
{
eccVector = CelestialBodyUtils.CrossProduct(velocity, angularMomentumVector) / MG - position / attractorDistance;
}
orbitNormalDotEclipticNormal = CelestialBodyUtils.DotProduct(orbitNormal, eclipticNormal);
focalParameter = angularMomentumVector.sqrMagnitude / MG;
eccentricity = eccVector.magnitude;
//if (debug) {
// string format = "0.0000000000";
// Debug.Log(
// "ECC: " + eccVector.ToString(format) + " LEN: " + eccVector.magnitude.ToString(format) + "\n" +
// "POS: " + position.ToString(format) + " LEN: " + position.magnitude.ToString(format) + "\n" +
// "POSNORM: " + ( position / attractorDistance ).ToString(format) + " LEN: " + ( position / attractorDistance ).magnitude.ToString(format) + "\n" +
// "VEL: " + velocity.ToString(format) + " LEN: " + velocity.magnitude.ToString(format) + "\n" +
// "POScrossVEL: " + angularMomentumVector.ToString(format) + " LEN: " + angularMomentumVector.magnitude.ToString(format) + "\n"
// );
//}
energyTotal = velocity.sqrMagnitude - 2 * MG / attractorDistance;
semiMinorAxisBasis = CelestialBodyUtils.CrossProduct(angularMomentumVector, eccVector).normalized;
if (semiMinorAxisBasis.sqrMagnitude < 0.5)
{
semiMinorAxisBasis = CelestialBodyUtils.CrossProduct(orbitNormal, position).normalized;
}
semiMajorAxisBasis = CelestialBodyUtils.CrossProduct(orbitNormal, semiMinorAxisBasis).normalized;
inclination = Vector3d.Angle(orbitNormal, eclipticNormal) * Mathd.Deg2Rad;
if (eccentricity < 1)
{
orbitCompressionRatio = 1 - eccentricity * eccentricity;
semiMajorAxis = focalParameter / orbitCompressionRatio;
semiMinorAxis = semiMajorAxis * System.Math.Sqrt(orbitCompressionRatio);
centerPoint = -semiMajorAxis * eccVector;
period = Mathd.PI_2 * Mathd.Sqrt(Mathd.Pow(semiMajorAxis, 3) / MG);
apoapsis = centerPoint + semiMajorAxisBasis * semiMajorAxis;
periapsis = centerPoint - semiMajorAxisBasis * semiMajorAxis;
periapsisDistance = periapsis.magnitude;
apoapsisDistance = apoapsis.magnitude;
trueAnomaly = Vector3d.Angle(position, -semiMajorAxisBasis) * Mathd.Deg2Rad;
if (CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(position, semiMajorAxisBasis), orbitNormal) < 0)
{
trueAnomaly = Mathd.PI_2 - trueAnomaly;
}
eccentricAnomaly = CelestialBodyUtils.ConvertTrueToEccentricAnomaly(trueAnomaly, eccentricity);
meanAnomaly = eccentricAnomaly - eccentricity * System.Math.Sin(eccentricAnomaly);
}
else
{
orbitCompressionRatio = eccentricity * eccentricity - 1;
semiMajorAxis = focalParameter / orbitCompressionRatio;
semiMinorAxis = semiMajorAxis * System.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) * Mathd.Deg2Rad;
if (CelestialBodyUtils.DotProduct(CelestialBodyUtils.CrossProduct(position, semiMajorAxisBasis), orbitNormal) < 0)
{
trueAnomaly = -trueAnomaly;
}
eccentricAnomaly = CelestialBodyUtils.ConvertTrueToEccentricAnomaly(trueAnomaly, eccentricity);
meanAnomaly = System.Math.Sinh(eccentricAnomaly) * eccentricity - eccentricAnomaly;
}
}
