public void Init()
{
eventManager = GameObject.Find("EventManager").GetComponent<EventManager>();
rv = new VectorD();
rv.Resize(6);
Debug.Log("position " + transform.localPosition);
rv[0] = transform.localPosition.x * HoloManager.SolScale;
rv[1] = transform.localPosition.y * HoloManager.SolScale;
rv[2] = transform.localPosition.z * HoloManager.SolScale;
Debug.Log(name + " real dist: " + OVTools.FormatDistance(getRFloat().magnitude));
//comput velocity assuming pos is in the origin
//TODO use double operations?
var vel = Vector3.Cross(transform.localPosition, Vector3.up).normalized;
vel *= Mathf.Sqrt((float)parentGM / (HoloManager.SolScale * transform.localPosition.magnitude));
rv[3] = vel.x;
rv[4] = vel.y;
rv[5] = vel.z;
//set oe
var tempoe = Util.rv2oe(parentGM, rv);
SetOE(tempoe);
params_ = new VectorD();
params_.Resize(7);
params_[0] = 0;
params_[1] = 0;
params_[2] = 0;
params_[3] = parentGM;
params_[4] = 0;
params_[5] = 0;
params_[6] = 0;
}
public static VectorD dynamics(float t, VectorD x, VectorD params_)
{
Debug.Assert(x.Count == 6);
Debug.Assert(params_.Count == 7);
VectorD dx = new VectorD();
dx.Resize(6);
//the derivative of position is velocity
dx[0] = x[3];
dx[1] = x[4];
dx[2] = x[5];
//distance between the two bodies
float distance = 0;
distance += (x[0] - params_[0]) * (x[0] - params_[0]);
distance += (x[1] - params_[1]) * (x[1] - params_[1]);
distance += (x[2] - params_[2]) * (x[2] - params_[2]);
distance = Mathf.Sqrt(distance);
//acceleration due to gravity
dx[3] = -params_[3] * (x[0] - params_[0]) / Mathf.Pow(distance, 3);
dx[4] = -params_[3] * (x[1] - params_[1]) / Mathf.Pow(distance, 3);
dx[5] = -params_[3] * (x[2] - params_[2]) / Mathf.Pow(distance, 3);
//perturbing acceleration
dx[3] += params_[4];
dx[4] += params_[5];
dx[5] += params_[6];
return(dx);
}
public static Vector3 rk4(float dt, Vector3 pos, Vector3 vel, Vector3 parentPos, float gm, Vector3 accel)
{
var t0 = (float)dt;
VectorD x0 = new VectorD();
x0.Resize(6);
//pos,vel
x0[0] = pos.x;
x0[1] = pos.y;
x0[2] = pos.z;
x0[3] = vel.x;
x0[4] = vel.y;
x0[5] = vel.z;
VectorD params_ = new VectorD();
params_.Resize(7);
//parent pos, gm, accel
params_[0] = parentPos.x;
params_[1] = parentPos.y;
params_[2] = parentPos.z;
params_[3] = gm;
params_[4] = accel.x;
params_[5] = accel.y;
params_[6] = accel.z;
var result = rungeKutta4(0, t0, x0, params_);
var ret = new Vector3((float)result[3], (float)result[4], (float)result[5]);
return(ret);
}
void Start()
{
rv = new VectorD();
rv.Resize(6);
Debug.Log("position " + transform.position);
rv[0] = transform.position.x;
rv[1] = transform.position.y;
rv[2] = transform.position.z;
//comput velocity assuming pos is in the origin
var vel = Vector3.Cross(transform.position, Vector3.up).normalized;
vel *= Mathf.Sqrt((float)parentGM / transform.position.magnitude);
rv[3] = vel.x;
rv[4] = vel.y;
rv[5] = vel.z;
params_ = new VectorD();
params_.Resize(7);
params_[0] = 0;
params_[1] = 0;
params_[2] = 0;
params_[3] = parentGM;
params_[4] = 0;
params_[5] = 0;
params_[6] = 0;
}
public static VectorD convertToParams(float[] parentPos, float gm, float[] accel)
{
VectorD params_ = new VectorD();
params_.Resize(7);
params_[0] = parentPos[0];
params_[1] = parentPos[1];
params_[2] = parentPos[2];
params_[3] = gm;
params_[4] = accel[0];
params_[5] = accel[1];
params_[6] = accel[2];
return(params_);
}
public static VectorD oe2rv(float grav_param, OrbitalElements oe)
{
// rotation matrix
float R11 =
Mathf.Cos(oe.aop) * Mathf.Cos(oe.lan) - Mathf.Cos(oe.inc) * Mathf.Sin(oe.aop) * Mathf.Sin(oe.lan);
float R12 =
-Mathf.Cos(oe.lan) * Mathf.Sin(oe.aop) - Mathf.Cos(oe.inc) * Mathf.Cos(oe.aop) * Mathf.Sin(oe.lan);
//float R13 = Mathf.Sin(oe.inc)*Mathf.Sin(oe.lan);
float R21 =
Mathf.Cos(oe.inc) * Mathf.Cos(oe.lan) * Mathf.Sin(oe.aop) + Mathf.Cos(oe.aop) * Mathf.Sin(oe.lan);
float R22 =
Mathf.Cos(oe.inc) * Mathf.Cos(oe.aop) * Mathf.Cos(oe.lan) - Mathf.Sin(oe.aop) * Mathf.Sin(oe.lan);
//float R23 = -Mathf.Cos(oe.lan)*Mathf.Sin(oe.inc);
float R31 = Mathf.Sin(oe.inc) * Mathf.Sin(oe.aop);
float R32 = Mathf.Cos(oe.aop) * Mathf.Sin(oe.inc);
//float R33 = Mathf.Cos(oe.inc);
// semi-latus rectum
float p = oe.sma * (1 - oe.ecc * oe.ecc);
// position in the perifocal frame
//std::vector<float> r_pf(3);
float[] r_pf = new float[3];
float r_norm = p / (1 + oe.ecc * Mathf.Cos(oe.tra));
r_pf[0] = r_norm * Mathf.Cos(oe.tra);
r_pf[1] = r_norm * Mathf.Sin(oe.tra);
r_pf[2] = 0;
// velocity in the perifocal frame
//std::vector<float> v_pf(3);
float[] v_pf = new float[3];
v_pf[0] = Mathf.Sqrt(grav_param / p) * -Mathf.Sin(oe.tra);
v_pf[1] = Mathf.Sqrt(grav_param / p) * (oe.ecc + Mathf.Cos(oe.tra));
v_pf[2] = 0;
// rotate the position and velocity into the body-fixed inertial frame
//std::vector<float> rv(6);
VectorD rv = new VectorD();
rv.Resize(6);
rv[0] = R11 * r_pf[0] + R12 * r_pf[1] /*+R13*r_pf[2]*/;
rv[1] = R21 * r_pf[0] + R22 * r_pf[1] /*+R23*r_pf[2]*/;
rv[2] = R31 * r_pf[0] + R32 * r_pf[1] /*+R33*r_pf[2]*/;
rv[3] = R11 * v_pf[0] + R12 * v_pf[1] /*+R13*v_pf[2]*/;
rv[4] = R21 * v_pf[0] + R22 * v_pf[1] /*+R23*v_pf[2]*/;
rv[5] = R31 * v_pf[0] + R32 * v_pf[1] /*+R33*v_pf[2]*/;
return(rv);
}
public static Vector3d oe2rd(double grav_param, OrbitalElements oe)
{
// rotation matrix
double R11 =
Math.Cos(oe.aop) * Math.Cos(oe.lan) - Math.Cos(oe.inc) * Math.Sin(oe.aop) * Math.Sin(oe.lan);
double R12 =
-Math.Cos(oe.lan) * Math.Sin(oe.aop) - Math.Cos(oe.inc) * Math.Cos(oe.aop) * Math.Sin(oe.lan);
//double R13 = Math.Sin(oe.inc)*Math.Sin(oe.lan);
double R21 =
Math.Cos(oe.inc) * Math.Cos(oe.lan) * Math.Sin(oe.aop) + Math.Cos(oe.aop) * Math.Sin(oe.lan);
double R22 =
Math.Cos(oe.inc) * Math.Cos(oe.aop) * Math.Cos(oe.lan) - Math.Sin(oe.aop) * Math.Sin(oe.lan);
//double R23 = -Math.Cos(oe.lan)*Math.Sin(oe.inc);
double R31 = Math.Sin(oe.inc) * Math.Sin(oe.aop);
double R32 = Math.Cos(oe.aop) * Math.Sin(oe.inc);
//double R33 = Math.Cos(oe.inc);
// semi-latus rectum
double p = oe.sma * (1 - oe.ecc * oe.ecc);
// position in the perifocal frame
//std::vector<double> r_pf(3);
double[] r_pf = new double[3];
double r_norm = p / (1 + oe.ecc * Math.Cos(oe.tra));
r_pf[0] = r_norm * Math.Cos(oe.tra);
r_pf[1] = r_norm * Math.Sin(oe.tra);
r_pf[2] = 0.0;
// velocity in the perifocal frame
//std::vector<double> v_pf(3);
double[] v_pf = new double[3];
v_pf[0] = Math.Sqrt(grav_param / p) * -Math.Sin(oe.tra);
v_pf[1] = Math.Sqrt(grav_param / p) * (oe.ecc + Math.Cos(oe.tra));
v_pf[2] = 0.0;
// rotate the position and velocity into the body-fixed inertial frame
//std::vector<double> rv(6);
VectorD r = new VectorD();
r.Resize(3);
r[0] = R11 * r_pf[0] + R12 * r_pf[1] /*+R13*r_pf[2]*/;
r[1] = R21 * r_pf[0] + R22 * r_pf[1] /*+R23*r_pf[2]*/;
r[2] = R31 * r_pf[0] + R32 * r_pf[1] /*+R33*r_pf[2]*/;
Vector3d pos = new Vector3d(r[0], r[1], r[2]);
return(pos);
}
public static VectorD convertToParams(Vector3 parentPos, float gm, Vector3 accel)
{
VectorD params_ = new VectorD();
params_.Resize(7);
//parent pos, gm, accel
params_[0] = parentPos.x;
params_[1] = parentPos.y;
params_[2] = parentPos.z;
params_[3] = gm;
params_[4] = accel.x;
params_[5] = accel.y;
params_[6] = accel.z;
return(params_);
}
public static VectorD convertToRv(ref Vector3 pos, ref Vector3 vel)
{
VectorD params_ = new VectorD();
params_.Resize(6);
Debug.Assert(params_.Count == 6);
//2013 Orbital Vector was written with z being up
//unity y is up, so must swap axis
params_[0] = pos.x;
params_[1] = pos.y;
params_[2] = pos.z;
params_[3] = vel.x;
params_[4] = vel.y;
params_[5] = vel.z;
return(params_);
}
public static VectorD convertToRv(ref float[] pos, ref float[] vel)
{
Debug.Assert(vel.Length == 3);
Debug.Assert(pos.Length == 3);
VectorD params_ = new VectorD();
params_.Resize(6);
Debug.Assert(params_.Count == 6);
//2013 Orbital Vector was written with z being up
//unity y is up, so must swap axis
params_[0] = pos[0];
params_[1] = pos[1];
params_[2] = pos[2];
params_[3] = vel[0];
params_[4] = vel[1];
params_[5] = vel[2];
return(params_);
}
