561 lines
21 KiB
C#
561 lines
21 KiB
C#
|
/*
|
|||
|
|
* 2022-4-8
|
|||
|
|
*
|
|||
|
|
* Originally written by Nerdu#7492
|
|||
|
|
*
|
|||
|
|
* 2D orbital mechanics
|
|||
|
|
* solves for both Keplerian elements and orbital state vectors
|
|||
|
|
* Supports hyperbolic orbits
|
|||
|
|
*
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
using System;
|
|||
|
|
using System.Collections;
|
|||
|
|
using System.Collections.Generic;
|
|||
|
|
using UnityEngine;
|
|||
|
|
|
|||
|
|
public class spaceshipScript : MonoBehaviour
|
|||
|
|
{
|
|||
|
|
private Camera cam;
|
|||
|
|
private LineRenderer orbitLR;
|
|||
|
|
private LineRenderer shipToPlanetLR;
|
|||
|
|
private LineRenderer velocityLR;
|
|||
|
|
private LineRenderer perpRLR;
|
|||
|
|
|
|||
|
|
public float mouseScrollMultiplier;
|
|||
|
|
public AnimationCurve curveForMouseZoom;
|
|||
|
|
public float maxMouseZoomMultiplier;
|
|||
|
|
|
|||
|
|
public float minZoom;
|
|||
|
|
|
|||
|
|
public float maxZoom;
|
|||
|
|
|
|||
|
|
public Transform orbitedPlanet;
|
|||
|
|
public celestialBodyScript orbitedPlanetScript;
|
|||
|
|
|
|||
|
|
|
|||
|
|
private Rigidbody2D rb;
|
|||
|
|
|
|||
|
|
public Vector2 initVelocity;
|
|||
|
|
public OrbitalParameters currentOrbitalParams;
|
|||
|
|
|
|||
|
|
private orbitSimulationManager simManager;
|
|||
|
|
|
|||
|
|
public float orbitDrawnMaxWidth;
|
|||
|
|
public AnimationCurve orbitDrawnWidthCurve;
|
|||
|
|
|
|||
|
|
public float thrustVelocity;
|
|||
|
|
|
|||
|
|
public float rotationSpeed;
|
|||
|
|
|
|||
|
|
//km
|
|||
|
|
public float physicsDistance;
|
|||
|
|
// Start is called before the first frame update
|
|||
|
|
void Start()
|
|||
|
|
{
|
|||
|
|
orbitLR = transform.GetChild(0).GetComponent<LineRenderer>();
|
|||
|
|
shipToPlanetLR = transform.GetChild(1).GetComponent<LineRenderer>();
|
|||
|
|
velocityLR = transform.GetChild(2).GetComponent<LineRenderer>();
|
|||
|
|
perpRLR = transform.GetChild(3).GetComponent<LineRenderer>();
|
|||
|
|
simManager = GameObject.FindGameObjectWithTag("simManager").GetComponent<orbitSimulationManager>();
|
|||
|
|
cam = Camera.main;
|
|||
|
|
rb = GetComponent<Rigidbody2D>();
|
|||
|
|
rb.velocity = initVelocity / simManager.scale;
|
|||
|
|
Vector2 posFromPlanet = (Vector2)transform.position - (Vector2) orbitedPlanet.position;
|
|||
|
|
posFromPlanet *= simManager.scale;
|
|||
|
|
CalculateOrbitalParams(new Vector2d((double)initVelocity.x, (double)initVelocity.y), new Vector2d(posFromPlanet.x, posFromPlanet.y), orbitedPlanetScript.mass, true);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Update is called once per frame
|
|||
|
|
void Update()
|
|||
|
|
{
|
|||
|
|
if (Input.GetKey(KeyCode.A))
|
|||
|
|
{
|
|||
|
|
transform.Rotate(0f,0f,rotationSpeed * Time.deltaTime * simManager.timeScale);
|
|||
|
|
}
|
|||
|
|
if (Input.GetKey(KeyCode.D))
|
|||
|
|
{
|
|||
|
|
transform.Rotate(0f,0f,-rotationSpeed * Time.deltaTime * simManager.timeScale);
|
|||
|
|
}
|
|||
|
|
ZoomOrthoCamera();
|
|||
|
|
float orbitWidthVal = orbitDrawnMaxWidth * orbitDrawnWidthCurve.Evaluate(cam.orthographicSize / maxZoom);
|
|||
|
|
orbitLR.startWidth = orbitWidthVal;
|
|||
|
|
orbitLR.endWidth = orbitWidthVal;
|
|||
|
|
shipToPlanetLR.startWidth = orbitWidthVal;
|
|||
|
|
shipToPlanetLR.endWidth = orbitWidthVal;
|
|||
|
|
velocityLR.startWidth = orbitWidthVal;
|
|||
|
|
velocityLR.endWidth = orbitWidthVal;
|
|||
|
|
perpRLR.startWidth = orbitWidthVal;
|
|||
|
|
perpRLR.endWidth = orbitWidthVal;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void FixedUpdate()
|
|||
|
|
{
|
|||
|
|
Vector2d dir = new Vector2d(Mathd.Cos((double)transform.rotation.eulerAngles.z * Mathd.Deg2Rad), Mathd.Sin((double)transform.rotation.eulerAngles.z * Mathd.Deg2Rad));
|
|||
|
|
if (Input.GetKey(KeyCode.Space))
|
|||
|
|
{
|
|||
|
|
if (Vector2.Distance(transform.position, orbitedPlanet.position) > (orbitedPlanetScript.radius + physicsDistance)/simManager.scale)
|
|||
|
|
{
|
|||
|
|
Vector2 posFromPlanet = (Vector2)transform.position - (Vector2) orbitedPlanet.position;
|
|||
|
|
if (simManager.timeScale > 8)
|
|||
|
|
{
|
|||
|
|
simManager.timeScale = 8;
|
|||
|
|
}
|
|||
|
|
posFromPlanet *= simManager.scale;
|
|||
|
|
Vector2d currentVel = new Vector2d(currentOrbitalParams.v * Mathd.Cos(currentOrbitalParams.theta), currentOrbitalParams.v * Mathd.Sin(currentOrbitalParams.theta));
|
|||
|
|
currentVel += thrustVelocity * dir * Time.fixedDeltaTime * simManager.timeScale;
|
|||
|
|
CalculateOrbitalParams(currentVel, new Vector2d(posFromPlanet.x, posFromPlanet.y), orbitedPlanetScript.mass, false);
|
|||
|
|
rb.velocity = new Vector2((float)currentVel.x, (float)currentVel.y) / simManager.scale;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//print((new Vector2((float)dir.x, (float)dir.y) * (thrustVelocity * simManager.timeScale * Time.fixedDeltaTime)));
|
|||
|
|
rb.AddForce(new Vector2((float)dir.x, (float)dir.y).normalized * (50f * Time.deltaTime));
|
|||
|
|
//rb.velocity = rb.velocity + (new Vector2((float)dir.x, (float)dir.y) * (thrustVelocity * simManager.timeScale * Time.fixedDeltaTime))/simManager.scale;
|
|||
|
|
Vector2d currentVel = new Vector2d(rb.velocity.x, rb.velocity.y) * simManager.scale;
|
|||
|
|
Vector2 posFromPlanet = (Vector2)transform.position - (Vector2) orbitedPlanet.position;
|
|||
|
|
posFromPlanet *= simManager.scale;
|
|||
|
|
CalculateOrbitalParams(currentVel, new Vector2d(posFromPlanet.x, posFromPlanet.y), orbitedPlanetScript.mass, false);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (Vector2.Distance(transform.position, orbitedPlanet.position) > (orbitedPlanetScript.radius + physicsDistance)/simManager.scale)
|
|||
|
|
{
|
|||
|
|
rb.isKinematic = true;
|
|||
|
|
Vector2d pos = currentOrbitalParams.RecalculateAtTime(simManager.time);
|
|||
|
|
Vector3 relative = new Vector3((float) pos.x / simManager.scale, (float) pos.y / simManager.scale, 0f);
|
|||
|
|
shipToPlanetLR.positionCount = 2;
|
|||
|
|
perpRLR.positionCount = 2;
|
|||
|
|
shipToPlanetLR.SetPositions(new Vector3[]{new Vector3(0f,0f,0f), relative});
|
|||
|
|
perpRLR.SetPositions(new Vector3[]{relative, relative + new Vector3((float)dir.x, (float)dir.y) * 500f});
|
|||
|
|
float velAngle = (float) currentOrbitalParams.theta;
|
|||
|
|
float vel = (float) currentOrbitalParams.v;
|
|||
|
|
Vector3 velocityVector = new Vector3(vel * Mathf.Cos(velAngle), vel * Mathf.Sin(velAngle));
|
|||
|
|
velocityLR.SetPositions(new Vector3[]{relative, velocityVector + relative});
|
|||
|
|
transform.position = orbitedPlanet.position + relative;
|
|||
|
|
Vector2d currentVel = new Vector2d(currentOrbitalParams.v * Mathd.Cos(currentOrbitalParams.theta), currentOrbitalParams.v * Mathd.Sin(currentOrbitalParams.theta));
|
|||
|
|
rb.velocity = new Vector2((float)currentVel.x, (float)currentVel.y) / simManager.scale;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if (simManager.timeScale > 4)
|
|||
|
|
{
|
|||
|
|
simManager.timeScale = 4;
|
|||
|
|
}
|
|||
|
|
rb.isKinematic = false;
|
|||
|
|
double gravVal = currentOrbitalParams.G *
|
|||
|
|
(orbitedPlanetScript.mass /
|
|||
|
|
Mathf.Pow(Vector2.Distance(orbitedPlanet.position, transform.position) * simManager.scale,
|
|||
|
|
2));
|
|||
|
|
print(gravVal);
|
|||
|
|
print((float)gravVal * Time.fixedDeltaTime * (10/simManager.scale));
|
|||
|
|
rb.velocity = rb.velocity + (float) gravVal * (1 / simManager.scale) * Time.fixedDeltaTime * simManager.timeScale *
|
|||
|
|
(Vector2)(orbitedPlanet.position - transform.position).normalized;
|
|||
|
|
//rb.AddForce(((float)gravVal * rb.mass * (10/simManager.scale) * Time.fixedDeltaTime * simManager.timeScale * (orbitedPlanet.position - transform.position).normalized) );
|
|||
|
|
currentOrbitalParams.v = ((double) rb.velocity.magnitude * Mathd.Sign((double) rb.velocity.magnitude)) *
|
|||
|
|
simManager.scale;
|
|||
|
|
currentOrbitalParams.theta = Mathd.Atan2(rb.velocity.normalized.y, rb.velocity.normalized.x);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private void LateUpdate()
|
|||
|
|
{
|
|||
|
|
orbitLR.transform.position = orbitedPlanet.position;
|
|||
|
|
orbitLR.transform.rotation = Quaternion.identity;
|
|||
|
|
perpRLR.transform.position = orbitedPlanet.position;
|
|||
|
|
perpRLR.transform.rotation = Quaternion.identity;
|
|||
|
|
velocityLR.transform.position = orbitedPlanet.position;
|
|||
|
|
velocityLR.transform.rotation = Quaternion.identity;
|
|||
|
|
shipToPlanetLR.transform.position = orbitedPlanet.position + new Vector3(0f, 0f, 1f);
|
|||
|
|
shipToPlanetLR.transform.rotation = Quaternion.identity;
|
|||
|
|
Camera.main.transform.rotation = Quaternion.identity;
|
|||
|
|
//RotateCam();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void CalculateOrbitalParams(Vector2d initialVel, Vector2d positionFromPlanet, double planetMass, bool doPrints)
|
|||
|
|
{
|
|||
|
|
currentOrbitalParams = new OrbitalParameters();
|
|||
|
|
currentOrbitalParams.CalculateAllInitialParams(initialVel, positionFromPlanet, planetMass, simManager.time - Time.fixedDeltaTime * simManager.timeScale, doPrints);
|
|||
|
|
DrawOrbit(100);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void DrawOrbit(int posCount)
|
|||
|
|
{
|
|||
|
|
orbitLR.positionCount = posCount;
|
|||
|
|
Vector3[] positions = new Vector3[posCount];
|
|||
|
|
for (var i = 0; i < posCount; i++)
|
|||
|
|
{
|
|||
|
|
double value;
|
|||
|
|
if (currentOrbitalParams.e < 1)
|
|||
|
|
{
|
|||
|
|
value = ((currentOrbitalParams.T / posCount) * i) + currentOrbitalParams.t0;;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
value = ((2 * Mathd.PI / posCount) * i) + currentOrbitalParams.w;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//double dist = currentOrbitalParams.EvaluateEllipticalOrbit(currentOrbitalParams.a, currentOrbitalParams.e,
|
|||
|
|
// currentOrbitalParams.w, angle);
|
|||
|
|
//Vector2d pos = new Vector2d(dist * Mathd.Cos(angle), dist * Mathd.Sin(angle));
|
|||
|
|
Vector2d pos = currentOrbitalParams.GetPositionAtTime(value);
|
|||
|
|
Vector2 adjustedPos = new Vector2((float)pos.x/simManager.scale, (float)pos.y/simManager.scale);
|
|||
|
|
positions[i] = adjustedPos;
|
|||
|
|
}
|
|||
|
|
orbitLR.SetPositions(positions);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void ZoomOrthoCamera()
|
|||
|
|
{
|
|||
|
|
// Calculate how much we will have to move towards the zoomTowards position
|
|||
|
|
|
|||
|
|
mouseScrollMultiplier = curveForMouseZoom.Evaluate(cam.orthographicSize/maxZoom) * maxMouseZoomMultiplier;
|
|||
|
|
// Zoom camera
|
|||
|
|
cam.orthographicSize -= Input.mouseScrollDelta.y * mouseScrollMultiplier;
|
|||
|
|
|
|||
|
|
// Limit zoom
|
|||
|
|
cam.orthographicSize = Mathf.Clamp(cam.orthographicSize, minZoom, maxZoom);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void RotateCam()
|
|||
|
|
{
|
|||
|
|
Vector2 dir = transform.position - orbitedPlanet.position;
|
|||
|
|
float angle = Mathf.Atan2(dir.y, dir.x) * Mathf.Rad2Deg;
|
|||
|
|
angle -= 90f;
|
|||
|
|
cam.transform.rotation = Quaternion.Euler(0f, 0f, angle);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
[System.Serializable]
|
|||
|
|
public class OrbitalParameters
|
|||
|
|
{
|
|||
|
|
[Header("Semi-major axis")] public double a;
|
|||
|
|
[Header("Semi-minor axis")] public double b;
|
|||
|
|
[Header("Eccentricity")] public double e;
|
|||
|
|
[Header("True Anomaly")] public double f;
|
|||
|
|
[Header("Angle Of Perigee")] public double w;
|
|||
|
|
[Header("Eccentric Anomaly")] public double E;
|
|||
|
|
[Header("Initial Mean Anomaly")] public double M0;
|
|||
|
|
[Header("Initial Time")] public double t0;
|
|||
|
|
[Header("Current Mean Anomaly")] public double M;
|
|||
|
|
[Header("Orbital Period")] public double T;
|
|||
|
|
[Header("Mean Motion")] public double n;
|
|||
|
|
[Header("Flight Path Angle")] public double phi;
|
|||
|
|
[Header("Velocity")] public double v;
|
|||
|
|
[Header("VelAngle")] public double theta;
|
|||
|
|
|
|||
|
|
private Vector2d eVec;
|
|||
|
|
[HideInInspector]
|
|||
|
|
public double h;
|
|||
|
|
private double u;
|
|||
|
|
|
|||
|
|
[HideInInspector]
|
|||
|
|
public double G = 6.67408 * Mathd.Pow(10, -11);
|
|||
|
|
|
|||
|
|
public void CalculateAllInitialParams(Vector2d velocity, Vector2d positionFromPlanet, double planetMass, double _t0, bool doPrints)
|
|||
|
|
{
|
|||
|
|
u = G * planetMass;
|
|||
|
|
t0 = _t0;
|
|||
|
|
if (doPrints)
|
|||
|
|
{
|
|||
|
|
Debug.Log("circleVel: " + Mathd.Sqrt(u/positionFromPlanet.magnitude) + ", escape Vel: " + Mathd.Sqrt(2 * u/positionFromPlanet.magnitude));
|
|||
|
|
}
|
|||
|
|
//circular vel
|
|||
|
|
//velocity = new Vector2d(-Mathd.Sqrt(u/positionFromPlanet.magnitude), 0);
|
|||
|
|
//parabolic vel - escape vel
|
|||
|
|
//velocity = new Vector2d(Mathd.Sqrt(2 * u/positionFromPlanet.magnitude), 0);
|
|||
|
|
a = CalculateSemiMajorAxis(velocity.magnitude, positionFromPlanet.magnitude, u);
|
|||
|
|
h = Vector2d.CrossProduct(positionFromPlanet, velocity);
|
|||
|
|
eVec = CalculateEccentricityVector(velocity, positionFromPlanet, u);
|
|||
|
|
e = eVec.magnitude;
|
|||
|
|
T = CalculateOrbitalPeriod(u, a, e);
|
|||
|
|
n = CalculateMeanMotion(T, h, e, u, a);
|
|||
|
|
f = TrueAnomalyFromStateVectors(eVec, positionFromPlanet, velocity);
|
|||
|
|
w = AngleOfPerigee(positionFromPlanet, f);
|
|||
|
|
|
|||
|
|
if (doPrints)
|
|||
|
|
{
|
|||
|
|
Debug.Log("(" + eVec.x + ", " + eVec.y + ")");
|
|||
|
|
}
|
|||
|
|
if (e < 1)
|
|||
|
|
{
|
|||
|
|
E = CalculateEccentricAnomalyFromTrueAnomaly(f, e);
|
|||
|
|
M0 = MeanAnomalyFromEccentricAnomaly(E, e);
|
|||
|
|
M = M0;
|
|||
|
|
if (doPrints)
|
|||
|
|
{
|
|||
|
|
Debug.Log(f + " " + E);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if(e >= 1)
|
|||
|
|
{
|
|||
|
|
E = CalculateHyperbolicAnomalyFromTrueAnomaly(f, e);
|
|||
|
|
M0 = MeanAnomalyFromHyperbolicAnomaly(E, e);
|
|||
|
|
M = M0;
|
|||
|
|
if (doPrints)
|
|||
|
|
{
|
|||
|
|
Debug.Log(f + " " + E + " " + (e * Mathd.Sinh(E) - E));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
v = CalculateVelocityVisViva(a, u, positionFromPlanet.magnitude);
|
|||
|
|
phi = CalculateFlightPathAngle(e, f);
|
|||
|
|
theta = GetCurrentDirectionOfVelocity(positionFromPlanet, phi, h);
|
|||
|
|
|
|||
|
|
//E = CalculateEccentricAnomalyFromMeanAnomaly(e, M, 5);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//not context based
|
|||
|
|
public Vector2d GetPositionAtTime(double t)
|
|||
|
|
{
|
|||
|
|
if (e < 1)
|
|||
|
|
{
|
|||
|
|
double _M = M0 + n * (t - t0);
|
|||
|
|
double _E = CalculateEccentricAnomalyFromMeanAnomaly(e, _M, 7);
|
|||
|
|
double _f = CalculateTrueAnomalyFromEccentricAnomaly(_E, e, 7);
|
|||
|
|
double angle = _f + w;
|
|||
|
|
double r = EvaluateOrbit(a, e, w, angle);
|
|||
|
|
return new Vector2d(r * Mathd.Cos(angle),r * Mathd.Sin(angle));
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
/*double _M = M0 + n * (t - t0);
|
|||
|
|
double _E = CalculateHyperbolicAnomalyFromMeanAnomaly(e, _M, 7);
|
|||
|
|
double _f = CalculateTrueAnomalyFromHyperbolicAnomaly(_E, e);
|
|||
|
|
double angle = _f + w;
|
|||
|
|
double r = EvaluateOrbit(a, e, w, angle);
|
|||
|
|
return new Vector2d(r * Mathd.Cos(angle),r * Mathd.Sin(angle));*/
|
|||
|
|
double r = EvaluateOrbit(a, e, w, t);
|
|||
|
|
return new Vector2d(r * Mathd.Cos(t), r * Mathd.Sin(t));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//context based
|
|||
|
|
public Vector2d RecalculateAtTime(double t)
|
|||
|
|
{
|
|||
|
|
if (e < 1)
|
|||
|
|
{
|
|||
|
|
M = M0 + n * (t - t0);
|
|||
|
|
E = CalculateEccentricAnomalyFromMeanAnomaly(e, M, 7);
|
|||
|
|
f = CalculateTrueAnomalyFromEccentricAnomaly(E, e, 7);
|
|||
|
|
double angle = f + w;
|
|||
|
|
double r = EvaluateOrbit(a, e, w, angle);
|
|||
|
|
Vector2d pos = new Vector2d(r * Mathd.Cos(angle),r * Mathd.Sin(angle));
|
|||
|
|
v = CalculateVelocityVisViva(a, u, pos.magnitude);
|
|||
|
|
phi = CalculateFlightPathAngle(e, f);
|
|||
|
|
theta = GetCurrentDirectionOfVelocity(pos, phi,h);
|
|||
|
|
return pos;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
M = M0 + n * (t - t0);
|
|||
|
|
E = CalculateHyperbolicAnomalyFromMeanAnomaly(e, M, 7);
|
|||
|
|
f = CalculateTrueAnomalyFromHyperbolicAnomaly(E, e);
|
|||
|
|
double angle = f + w;
|
|||
|
|
double r = EvaluateOrbit(a, e, w, angle);
|
|||
|
|
Vector2d pos = new Vector2d(r * Mathd.Cos(angle),r * Mathd.Sin(angle));
|
|||
|
|
v = CalculateVelocityVisViva(a, u, pos.magnitude);
|
|||
|
|
phi = CalculateFlightPathAngle(e, f);
|
|||
|
|
theta = GetCurrentDirectionOfVelocity(pos, phi, h);
|
|||
|
|
return pos;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double EvaluateOrbit(double _a, double _e, double _w, double angle)
|
|||
|
|
{
|
|||
|
|
double top = _a * (1 - Mathd.Pow(_e, 2));
|
|||
|
|
double bot = 1 + e * Mathd.Cos(angle - _w);
|
|||
|
|
return top / bot;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateVelocityVisViva(double _a, double _u, double _r)
|
|||
|
|
{
|
|||
|
|
return Mathd.Sqrt(_u * ((2.0 / _r) - (1.0 / _a)));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateFlightPathAngle(double _e, double _f)
|
|||
|
|
{
|
|||
|
|
return Mathd.Atan2(_e * Mathd.Sin(_f), 1 + _e * Mathd.Cos(_f));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double GetCurrentDirectionOfVelocity(Vector2d pos, double _phi, double _h)
|
|||
|
|
{
|
|||
|
|
Vector2d perp = new Vector2d(pos.y, -pos.x) * 0.5f;
|
|||
|
|
Vector2d perp2 = new Vector2d(-pos.y, pos.x) * 0.5f;
|
|||
|
|
double perpAngle = Mathd.Atan2(perp2.y - perp.y, perp2.x - perp.x);
|
|||
|
|
double velAngle;
|
|||
|
|
if (_h < 0)
|
|||
|
|
{
|
|||
|
|
velAngle = perpAngle - _phi + Mathd.PI;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
velAngle = perpAngle - _phi;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return velAngle;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double AngleOfPerigee(Vector2d r, double _f)
|
|||
|
|
{
|
|||
|
|
return Mathd.Atan2(r.y, r.x) - _f;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateSemiMajorAxis(double v, double r, double u)
|
|||
|
|
{
|
|||
|
|
return (u * r) / (2 * u - (Mathd.Pow(v, 2) * r));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateOrbitalPeriod(double u, double _a, double _e)
|
|||
|
|
{
|
|||
|
|
if (_e <= 1)
|
|||
|
|
{
|
|||
|
|
return 2 * Mathd.PI * Mathd.Sqrt(Mathd.Pow(_a, 3) / u);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//WRONG GIVES MEAN MOTION U IDIOT MF.
|
|||
|
|
public double CalculateMeanMotion(double _T, double _h, double _e, double _u, double _a)
|
|||
|
|
{
|
|||
|
|
if (_e < 1)
|
|||
|
|
{
|
|||
|
|
return (2 * Mathd.PI) / _T * Mathd.Sign(_h);
|
|||
|
|
}
|
|||
|
|
else if (_e > 1)
|
|||
|
|
{
|
|||
|
|
return Mathd.Sqrt(_u / -Mathd.Pow(_a, 3)) * Mathd.Sign(_h);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
return Mathd.Sqrt(_u / Mathd.Pow(_a, 3)) * Mathd.Sign(_h);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public Vector2d CalculateEccentricityVector(Vector2d v, Vector2d r, double u)
|
|||
|
|
{
|
|||
|
|
double _h = Vector2d.CrossProduct(r, v);
|
|||
|
|
return ((Mathd.Pow(v.magnitude, 2) - (u / r.magnitude)) * r - Vector2d.Dot(r, v) * v) / u;
|
|||
|
|
//return new Vector2d(((v.y * _h)/u) - (r.x/r.magnitude), ((v.x * _h)/u) - (r.y/r.magnitude));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateEccentricAnomalyFromMeanAnomaly(double _e, double _M, int dp)
|
|||
|
|
{
|
|||
|
|
//dp = number of decimal places
|
|||
|
|
|
|||
|
|
int maxIter=30, i=0;
|
|||
|
|
|
|||
|
|
double delta=Mathd.Pow(10,-dp);
|
|||
|
|
|
|||
|
|
double _E, F;
|
|||
|
|
|
|||
|
|
_E=_M;
|
|||
|
|
|
|||
|
|
F = _E - _e*Mathd.Sin(_M) - _M;
|
|||
|
|
|
|||
|
|
while ((Mathd.Abs(F)>delta) && (i<maxIter)) {
|
|||
|
|
|
|||
|
|
_E = _E - F/(1.0-_e*Mathd.Cos(_E));
|
|||
|
|
F = _E - _e*Math.Sin(_E) - _M;
|
|||
|
|
|
|||
|
|
i = i + 1;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return _E;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateHyperbolicAnomalyFromMeanAnomaly(double _e, double _M, int dp)
|
|||
|
|
{
|
|||
|
|
//dp = number of decimal places
|
|||
|
|
|
|||
|
|
int maxIter=30, i=0;
|
|||
|
|
|
|||
|
|
double delta=Mathd.Pow(10,-dp);
|
|||
|
|
|
|||
|
|
double _E, F;
|
|||
|
|
|
|||
|
|
_E=_M;
|
|||
|
|
|
|||
|
|
F = _e * Mathd.Sinh(_E) - _E - _M;
|
|||
|
|
|
|||
|
|
while ((Mathd.Abs(F)>delta) && (i<maxIter)) {
|
|||
|
|
|
|||
|
|
_E = _E + F/(1.0 - _e * Mathd.Cosh(_E));
|
|||
|
|
F = _e * Mathd.Sinh(_E) - _E - _M;
|
|||
|
|
|
|||
|
|
i = i + 1;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return _E;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateEccentricAnomalyFromTrueAnomaly(double _f, double _e)
|
|||
|
|
{
|
|||
|
|
return Mathd.Atan2(Mathd.Sqrt(1 - Mathd.Pow(_e, 2)) * Mathd.Sin(_f), _e + Mathd.Cos(_f));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateHyperbolicAnomalyFromTrueAnomaly(double _f, double _e)
|
|||
|
|
{
|
|||
|
|
return 2 * Mathd.Atanh(Mathd.Sqrt((_e - 1)/(_e + 1)) * Mathd.Tan(_f/2));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double MeanAnomalyFromEccentricAnomaly(double _E, double _e)
|
|||
|
|
{
|
|||
|
|
return _E - _e * Mathd.Sin(_E);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double MeanAnomalyFromHyperbolicAnomaly(double _H, double _e)
|
|||
|
|
{
|
|||
|
|
return _e * Mathd.Sinh(_H) - _H;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateTrueAnomalyFromEccentricAnomaly(double _E, double _e, int dp)
|
|||
|
|
{
|
|||
|
|
return Mathd.Atan2(Mathd.Sqrt(1 - Mathd.Pow(_e, 2)) * Mathd.Sin(_E), Mathd.Cos(_E) - _e);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double CalculateTrueAnomalyFromHyperbolicAnomaly(double _H, double _e)
|
|||
|
|
{
|
|||
|
|
return 2 * Mathd.Atan2(Mathd.Sqrt(_e + 1) * Mathd.Sinh(_H/2), Mathd.Sqrt(_e - 1) * Mathd.Cosh(_H/2));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public double TrueAnomalyFromStateVectors(Vector2d _e, Vector2d r, Vector2d v)
|
|||
|
|
{
|
|||
|
|
if (_e.magnitude > 0)
|
|||
|
|
{
|
|||
|
|
double h = Vector2d.CrossProduct(r, v);
|
|||
|
|
double val = Mathd.Acos(Vector2d.Dot(_e, r) / (_e.magnitude * r.magnitude));
|
|||
|
|
if (!Mathd.Approximately(Mathd.Sign(Vector2d.Dot(r, v)), Mathd.Sign(h)))
|
|||
|
|
{
|
|||
|
|
return (2 * Mathd.PI) - val;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
return val;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
return Mathd.Atan2(r.y, r.x);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|