public void InitNewParticles(int fromParticle, int toParticle, ref double [,] r, ref double[,] v)
{
#pragma warning disable 162 // disable unreachable code warning
if (GravityEngine.DEBUG)
{
Debug.Log("Create explosion: contact=" + contactPoint + " normal=" + normal);
}
#pragma warning restore 162
for (int i = fromParticle; i < toParticle; i++)
{
r[i, 0] = (float)contactPoint.x;
r[i, 1] = (float)contactPoint.y;
r[i, 2] = (float)contactPoint.z;
// to generate the velocity for cone around z-axis then rotate into place
float offset = Mathf.Sin(Mathf.Deg2Rad * coneWidth) * Random.value;
float angle = 2f * Mathf.PI * Random.value;
Vector3 velocity = new Vector3(offset * Mathf.Sin(angle), offset * Mathf.Cos(angle), 1f);
float velocityScale = NUtils.GaussianValue(explosionVelocity, velocitySpread);
Vector3 scaledVelocity = velocityScale * Vector3.Normalize(velocity);
Vector3 finalVelocity = rotateToNormal * scaledVelocity + bodyVelocity;
v[i, 0] = (float)finalVelocity.x;
v[i, 1] = (float)finalVelocity.y;
v[i, 2] = (float)finalVelocity.z;
}
#pragma warning disable 162
if (GravityEngine.DEBUG)
{
Debug.Log(string.Format("Explosion particle 0: v=({0} ,{1}, {2}) x=({3}, {4}, {5} vesc={6} vmag={7})",
v[0, 0], v[0, 1], v[0, 2], r[0, 0], r[0, 1], r[0, 2], explosionVelocity,
System.Math.Sqrt(v[0, 0] * v[0, 0] + v[0, 1] * v[0, 1] + v[0, 2] * v[0, 2])));
}
#pragma warning restore 162
}
/// <summary>
/// Convert Mean Anomoly to True Anomoly for an ellipse with eccentricity e.
/// </summary>
/// <returns>True Anomoly in degrees.</returns>
/// <param name="m">Mean Anomoly. (degrees)</param>
/// <param name="e">Eccentricty.</param>
public static float MeanToTrueAnomoly(float m, float e)
{
int loopCount = 0;
float u = m * Mathf.Deg2Rad; // seed with mean anomoly
float u_next = 0;
// some extreme comet orbits (e.g. Halley) need a LOT of iterations
const int LOOP_LIMIT = 200;
while (loopCount++ < LOOP_LIMIT)
{
// this should always converge in a small number of iterations - but be paranoid
u_next = u + (m - (u - e * Mathf.Sin(u))) / (1 - e * Mathf.Cos(u));
if (Mathf.Abs(u_next - u) < 1E-6)
{
break;
}
u = u_next;
}
if (loopCount >= LOOP_LIMIT)
{
Debug.LogError("Failed to converge u_n=" + u_next); // keep going anyway
}
// 2) eccentric anomoly is angle from center of ellipse, not focus (where centerObject is). Convert
// to true anomoly, f - the angle measured from the focus. (see Fig 3.2 in Gravity)
float cos_f = (Mathf.Cos(u) - e) / (1f - e * Mathf.Cos(u));
float sin_f = (Mathf.Sqrt(1 - e * e) * Mathf.Sin(u)) / (1f - e * Mathf.Cos(u));
float f_deg = NUtils.AngleFromSinCos(sin_f, cos_f) * Mathf.Rad2Deg;
//Debug.Log("m=" + m + " E=" + u * Mathf.Deg2Rad + " f=" + f_deg);
return(f_deg);
}
/// <summary>
/// Calculate an array of orbit positions. Used by the OrbitPredictor, OrbitRenderer and Editor
/// Gimzo to illustrate the hyperbola.
/// </summary>
/// <returns>The positions.</returns>
/// <param name="numPoints">Number points.</param>
public Vector3[] OrbitPositions(int numPoints, Vector3 centerPos, bool doSceneMapping)
{
CalculateRotation();
Vector3[] emptyArray = { new Vector3(0, 0, 0), new Vector3(0, 0, 0) };
// need to have a center to create positions.
if (centerObject == null)
{
centerObject = transform.parent.gameObject;
if (centerObject == null)
{
return(emptyArray);
}
}
Vector3[] points = new Vector3[numPoints];
float theta = -1f * branchDisplayFactor * Mathf.PI;
float dTheta = 2f * Mathf.Abs(theta) / (float)numPoints;
GravityEngine ge = GravityEngine.Instance();
for (int i = 0; i < numPoints; i++)
{
points[i] = PositionForThetaLeftBranch(theta, centerPos);
if (NUtils.VectorNaN(points[i]))
{
points[i] = Vector3.zero;
}
else if (doSceneMapping && ge.mapToScene)
{
points[i] = ge.MapToScene(points[i]);
}
theta += dTheta;
}
return(points);
}
/// <summary>
/// Use AD/KL to position the arrival symbol on the SOI.
///
/// </summary>
private bool UpdateManeuverSymbols()
{
bool keyPressed = false;
// ship
if (Input.GetKey(KeyCode.K))
{
soiInclination = (float)NUtils.DegreesPM180(soiInclination + dAngleDegrees);
keyPressed = true;
}
else if (Input.GetKey(KeyCode.L))
{
soiInclination = (float)NUtils.DegreesPM180(soiInclination - dAngleDegrees);
keyPressed = true;
}
else if (Input.GetKey(KeyCode.A))
{
soiAngleDeg = (float)NUtils.DegreesMod360(soiAngleDeg + dAngleDegrees);
keyPressed = true;
}
else if (Input.GetKey(KeyCode.D))
{
soiAngleDeg = (float)NUtils.DegreesMod360(soiAngleDeg - dAngleDegrees);
keyPressed = true;
}
// update degree values so can see result in inspector
return(keyPressed);
}
/// <summary>
/// Determine the angle for a position on the hyperbola given the position.
/// </summary>
/// <param name="position"></param>
/// <returns></returns>
public float ThetaForPosition(Vector3 position, Vector3 cPos)
{
Vector3 ellipseAxis = PositionForThetaLeftBranch(0f, cPos);
Vector3 normal = hyper_orientation * Vector3.forward;
return(NUtils.AngleFullCircleRadians(ellipseAxis, position - centerPos, normal));
}
public void PreEvolve(GravityState gravityState, ref byte[] info)
{
double[] mass = gravityState.m;
double[,] pos = gravityState.r;
Get_acc_jerk_pot_coll(ref mass, ref pos, ref info);
initialEnergy = NUtils.GetEnergy(numBodies, ref mass, ref pos, ref vel);
}
/// <summary>
/// Calculate an array of points that describe the specified orbit
/// </summary>
/// <returns>The positions.</returns>
/// <param name="numPoints">Number points.</param>
public Vector3[] OrbitPositions(int numPoints, Vector3 centerPos, bool doSceneMapping)
{
GravityEngine ge = GravityEngine.Instance();
Vector3[] points = new Vector3[numPoints];
UpdateOrbitParams();
CalculateRotation();
float dtheta = 2f * Mathf.PI / numPoints;
float theta = 0;
// add a fudge factor to ensure we go all the way around the circle
for (int i = 0; i < numPoints; i++)
{
points[i] = PositionForTheta(theta, centerPos);
if (NUtils.VectorNaN(points[i]))
{
Debug.LogError("Vector NaN + " + points[i]);
points[i] = Vector3.zero;
}
else if (doSceneMapping && ge.mapToScene)
{
points[i] = ge.MapToScene(points[i]);
}
theta += dtheta;
}
// close the path (credit for fix to R. Vincent)
points[numPoints - 1] = points[0];
return(points);
}
public static void SetEllipse(float epoch, EllipseBase ellipseBase, SolarBody sb) {
// orbital element variation is not provided - only phase to determine
// What is the delat to the reference epoch?
float deltaYears = epoch - sb.epoch;
float period = SolarUtils.GetPeriodYears(sb);
float numPeriod = deltaYears/period;
ellipseBase.phase = OrbitEllipse.MeanToTrueAnomoly(NUtils.DegressMod360( sb.longitude + 360f*numPeriod), ellipseBase.ecc);
}
/// <summary>
/// Determine points from body through closest approach the same distance on the other side
/// </summary>
/// <param name="numPoints"></param>
/// <param name="centerPos"></param>
/// <param name="startPos"></param>
/// <returns></returns>
public Vector3[] OrbitSegmentSymmetricPositions(int numPoints, Vector3 centerPos,
Vector3 startPos)
{
GravityEngine ge = GravityEngine.Instance();
CalculateRotation();
// map points into xy plane
Vector3 start_xy = Quaternion.Inverse(hyper_orientation) * (startPos - centerPos);
// symmetric around origin
float start_y = -start_xy.y;
float end_y = start_xy.y;
if (start_y > end_y)
{
float temp = start_y;
start_y = end_y;
end_y = temp;
}
float dy = Mathf.Abs(end_y - start_y) / (float)numPoints;
float y = start_y;
int i = 0;
Vector3[] points = new Vector3[numPoints];
while (y < end_y)
{
points[i] = PositionForY(y, centerPos);
if (NUtils.VectorNaN(points[i]))
{
Debug.LogError(string.Format("Vector NaN = {0} y={1} ecc={2} ", points[i], y, ecc));
points[i] = Vector3.zero;
}
else if (ge.mapToScene)
{
points[i] = ge.MapToScene(points[i]);
}
y += dy;
i++;
if (i > numPoints - 1)
{
break;
}
}
// fill to end with last point
int last = i - 1;
if (last < 0)
{
last = 0;
}
while (i < numPoints)
{
points[i++] = points[last];
}
return(points);
}
public void AngleFromSinCos()
{
// check each quadrant
float[] angles = { 0, 0.3f * Mathf.PI, 0.7f * Mathf.PI, 1.3f * Mathf.PI, 1.7f * Mathf.PI };
foreach (float angle in angles)
{
float a = NUtils.AngleFromSinCos(Mathf.Sin(angle), Mathf.Cos(angle));
Debug.Log("angle=" + angle + " a=" + a);
Assert.IsTrue(Mathf.Abs(angle - a) < 1E-2);
}
}
public void Mod360()
{
// check each quadrant
float[] angles = { -90f, 20f, 355f, 270f + 2 * 360f, 14f * 360f + 5f };
float[] answer = { 270f, 20f, 355f, 270f, 5f };
for (int i = 0; i < angles.Length; i++)
{
float a = NUtils.DegreesMod360(angles[i]);
Debug.Log("angle=" + angles[i] + " a=" + a);
Assert.IsTrue(Mathf.Abs(a - answer[i]) < 1E-2);
}
}
public void PreEvolve(GravityState gravityState, ref byte[] info)
{
double[] m = gravityState.m;
double[,] r = gravityState.r;
// Precalc initial acceleration
double[] rji = new double[GravityEngine.NDIM];
double r2;
double r3;
double r_sep;
double accel;
for (int i = 0; i < numBodies; i++)
{
for (int k = 0; k < GravityEngine.NDIM; k++)
{
a[i, k] = 0.0;
}
}
for (int i = 0; i < numBodies; i++)
{
for (int j = i + 1; j < numBodies; j++)
{
r2 = 0;
for (int k = 0; k < GravityEngine.NDIM; k++)
{
rji[k] = r[j, k] - r[i, k];
r2 += rji[k] * rji[k];
}
if (forceDelegate == null)
{
r3 = r2 * System.Math.Sqrt(r2) + EPSILON;
for (int k = 0; k < GravityEngine.NDIM; k++)
{
a[i, k] += m[j] * rji[k] / r3;
a[j, k] -= m[i] * rji[k] / r3;
}
}
else
{
r_sep = System.Math.Sqrt(r2) + EPSILON;
accel = forceDelegate.CalcF(r_sep);
for (int k = 0; k < GravityEngine.NDIM; k++)
{
a[i, k] += m[j] * accel * (rji[k] / r_sep);
a[j, k] -= m[i] * accel * (rji[k] / r_sep);
}
}
}
}
initialEnergy = NUtils.GetEnergy(numBodies, ref m, ref r, ref v);
}
public static void SetEllipse(float epoch, EllipseBase ellipseBase, SolarBody sb)
{
// orbital element variation is not provided - only phase to determine
// What is the delta to the reference epoch?
float deltaYears = epoch - sb.epoch;
float period = SolarUtils.GetPeriodYears(sb);
float numPeriod = deltaYears / period;
// epoch was for time at perihelion (i.e. longitude = 0)
// w = w_bar - Omega,
ellipseBase.phase = OrbitEllipse.MeanToTrueAnomoly(NUtils.DegressMod360(360f * numPeriod), ellipseBase.ecc);
// Debug.Log(" numP=" + numPeriod + " M=" + NUtils.DegressMod360(360f*numPeriod) +
// " phase=" + ellipseBase.phase);
}
// Update is called once per frame
void Update()
{
if (Input.GetKey(KeyCode.A))
{
fromPhase += PHASE_PER_KEY;
}
else if (Input.GetKey(KeyCode.S))
{
fromPhase -= PHASE_PER_KEY;
}
else if (Input.GetKey(KeyCode.Q))
{
toPhase += PHASE_PER_KEY;
}
else if (Input.GetKey(KeyCode.W))
{
toPhase -= PHASE_PER_KEY;
}
fromPhase = NUtils.DegreesMod360(fromPhase);
toPhase = NUtils.DegreesMod360(toPhase);
SetMarkers();
shipOrbitData.SetOrbitForVelocity(spaceshipNBody, shipOrbit.centerNbody);
// Determine TOF
Vector3d from = new Vector3d(shipOrbitData.GetPhysicsPositionforEllipse(fromPhase));
Vector3d to = new Vector3d(shipOrbitData.GetPhysicsPositionforEllipse(toPhase));
Vector3d shipPos = GravityEngine.instance.GetPositionDoubleV3(spaceshipNBody);
double tPeri = shipOrbit.TimeOfFlight(shipPos, new Vector3d(shipOrbitData.GetPhysicsPositionforEllipse(0f)));
double tApo = shipOrbit.TimeOfFlight(shipPos, new Vector3d(shipOrbitData.GetPhysicsPositionforEllipse(180f)));
double tof = shipOrbit.TimeOfFlight(from, to);
// Scale to game time
//tApo = GravityScaler.ScaleToGameSeconds((float) tApo);
//tPeri = GravityScaler.ScaleToGameSeconds((float)tPeri);
//tof = GravityScaler.ScaleToGameSeconds((float)tof);
//tofText.text = string.Format("Time of Flight = {0:#.#}\nTime to Apoapsis = {1:#.#}\nTime to Periapsis = {2:#.#}\ntau = {3}",
// tof, tApo, tPeri, shipOrbitData.tau);
GravityScaler.Units units = GravityEngine.instance.units;
tofText.text = string.Format("Time of Flight = {0}\nTime to Apoapsis = {1}\nTime to Periapsis = {2}\ntau = {3}",
GravityScaler.GetWorldTimeFormatted(tof, units),
GravityScaler.GetWorldTimeFormatted(tApo, units),
GravityScaler.GetWorldTimeFormatted(tPeri, units),
GravityScaler.GetWorldTimeFormatted(shipOrbitData.tau, units));
}
/// <summary>
/// Sets the ellipse parameters for planet.
/// </summary>
/// <param name="epoch">Epoch.</param>
/// <param name="ellipseBase">Ellipse base.</param>
public static void SetEllipse(float epoch, EllipseBase ellipseBase, SolarBody sb)
{
// Based on JPL data, apply the per century drift to the orbital parameters
// T = number of centuries past J2000
float T = (epoch - 2000f) / 100f;
ellipseBase.a = sb.a + sb.a_dot * T;
ellipseBase.ecc = sb.ecc + sb.ecc_dot * T;
ellipseBase.inclination = sb.inclination + sb.inclination_dot * T;
ellipseBase.omega_uc = NUtils.DegreesMod360(sb.omega_uc + sb.omega_uc_dot * T);
// following JPL doc (http://ssd.jpl.nasa.gov/txt/aprx_pos_planets.pdf)
float current_omega_bar = sb.omega_lc_bar + sb.omega_lc_bar_dot * T;
ellipseBase.omega_lc = NUtils.DegreesMod360(current_omega_bar - ellipseBase.omega_uc);
float L = sb.longitude + sb.longitudeDot * T;
float mean_anomoly = NUtils.DegreesMod360(L - current_omega_bar);
ellipseBase.phase = mean_anomoly;
}
/// <summary>
/// Calculate an array of points that describe the specified orbit
/// </summary>
/// <returns>The positions.</returns>
/// <param name="numPoints">Number points.</param>
public Vector3[] OrbitPositions(int numPoints)
{
Vector3[] emptyArray = { new Vector3(0, 0, 0), new Vector3(0, 0, 0) };
// need to have a center to create positions.
if (centerObject == null && transform.parent != null)
{
centerObject = transform.parent.gameObject;
}
if (centerObject == null)
{
return(emptyArray);
}
Vector3[] points = new Vector3[numPoints];
UpdateOrbitParams();
CalculateRotation();
// start at theta=0 on the ellipse and transform to world space
float r = a_scaled * (1f - ecc * ecc) / (1f + ecc);
Vector3 oldPosition = new Vector3(r, 0, 0);
oldPosition = ellipse_orientation * oldPosition;
oldPosition += centerObject.transform.position;
float dtheta = 2f * Mathf.PI / numPoints;
float theta = 0;
// add a fudge factor to ensure we go all the way around the circle
for (int i = 0; i < numPoints; i++)
{
points[i] = PositionForTheta(theta);
if (NUtils.VectorNaN(points[i]))
{
points[i] = Vector3.zero;
}
theta += dtheta;
}
// close the path (credit for fix to R. Vincent)
points[numPoints - 1] = points[0];
return(points);
}
/// <summary>
/// Use the A/D to position the maneuver symbol on the start orbit.
/// Use KL to position the symbol on the SOI.
///
/// As move to a new maneuver destination the transfer time will reset to the
/// minimum energy value. At a given maneuver position, can use W/S to increase/decrease
/// the transfer time.
/// </summary>
private bool UpdateManeuverSymbols()
{
bool keyPressed = false;
// ship
if (Input.GetKeyDown(KeyCode.K))
{
soiAngle = (float)NUtils.AngleMod2Pi(soiAngle + dAngleDegrees * Mathf.Deg2Rad);
UpdateSoiPosition();
keyPressed = true;
}
else if (Input.GetKeyDown(KeyCode.L))
{
soiAngle = (float)NUtils.AngleMod2Pi(soiAngle - dAngleDegrees * Mathf.Deg2Rad);
UpdateSoiPosition();
keyPressed = true;
}
// update degree values so can see result in inspector
shipAngleDeg = shipAngle * Mathf.Rad2Deg;
soiAngleDeg = soiAngle * Mathf.Rad2Deg;
return(keyPressed);
}
public void PreEvolve(ref double[] mass, ref double[,] pos, ref byte[] info)
{
Get_acc_jerk_pot_coll(ref mass, ref pos, ref info);
initialEnergy = NUtils.GetEnergy(numBodies, ref mass, ref pos, ref vel);
}
public float GetEnergy(ref double[] m, ref double[,] r)
{
return((float)NUtils.GetEnergy(numBodies, ref m, ref r, ref vel));
}
public float GetEnergy(GravityState gravityState)
{
return((float)NUtils.GetEnergy(numBodies, ref gravityState.m, ref gravityState.r, ref v));
}
/// <summary>
/// Called from the GravityEngine on FixedUpdate cycles to determine current position of body given
/// the physics time evolution only when mode=KEPLERS_EQN.
///
/// This routine updates the game object position in game space and physics space.
///
/// Do not call this method directly.
/// </summary>
/// <param name="physicsTime">Physics time.</param>
/// <param name="physicalScale">Physical scale.</param>
/// <param name="r">Reference to array into which new position is placed.</param>
public void Evolve(double physicsTime, ref double[] r_new)
{
// There is no simple expression for the position in an Elliptical orbit as a function of time.
// (Expressions exist that describe position as a function of the angle in the orbit, but determining
// angle as a function of time results in an expression that is not elementary - Kepler's equation).
//
// Here we follow the excellent development of the equations in "Gravity" (Poisson and Will, 2014).
//
// following Gravity (Poisson & Will) Box 3.1
// 1) First use Newton's root-finding method to determine eccentic anomoly u for a given t
// The mean anomoly (angle from center of ellipse if motion was circular and uniform) is determined
// from the orbit period and time evolved so far.
int loopCount = 0;
// mean_anomoly is the angle around the circle of radius a if the body moved uniformly
// (which it does not)
float mean_anomoly = 2f * Mathf.PI * (float)physicsTime / orbitPeriod;
mean_anomoly += mean_anomoly_phase;
float u = mean_anomoly; // seed with mean anomoly
float u_next = 0;
const int LOOP_LIMIT = 20;
while (loopCount++ < LOOP_LIMIT)
{
// this should always converge in a small number of iterations - but be paranoid
u_next = u + (mean_anomoly - (u - ecc * Mathf.Sin(u))) / (1 - ecc * Mathf.Cos(u));
if (Mathf.Abs(u_next - u) < 1E-5)
{
break;
}
u = u_next;
}
if (loopCount >= LOOP_LIMIT)
{
u_next = u + (mean_anomoly - (u - ecc * Mathf.Sin(u))) / (1 - ecc * Mathf.Cos(u));
Debug.LogWarning(string.Format("Failed to converge (use best estimate) err={0:E5}", Mathf.Abs(u_next - u)));
// keep going anyway
}
// 2) eccentric anomoly is angle from center of ellipse, not focus (where centerObject is). Convert
// to true anomoly, f - the angle measured from the focus. (see Fig 3.2 in Gravity)
float cos_f = (Mathf.Cos(u) - ecc) / (1f - ecc * Mathf.Cos(u));
float sin_f = (Mathf.Sqrt(1 - ecc * ecc) * Mathf.Sin(u)) / (1f - ecc * Mathf.Cos(u));
float r = a_phy * (1f - ecc * ecc) / (1f + ecc * cos_f);
position = new Vector3(r * cos_f, r * sin_f, 0);
// Need a precise (right now) value for centerBody for best results, get from the engine
// move from XY plane to the orbital plane and scale to world space
// orbit position is WRT center
position = ellipse_orientation * position + GravityEngine.Instance().GetPhysicsPosition(centerNbody);
// fill in r. GE will use this position.
r_new[0] = position.x;
r_new[1] = position.y;
r_new[2] = position.z;
// determine velocity
// (C&P from above for performance)
// Murray and Dermot
// (2.26)
// This should really be (M+m), but assume m << M
// (massScale is added in at the GE level)
float n = Mathf.Sqrt((float)(centerNbody.mass * GravityEngine.Instance().massScale) / (a_phy * a_phy * a_phy));
// (2.36)
float denom = Mathf.Sqrt(1f - ecc * ecc);
float xdot = -1f * n * a_phy * sin_f / denom;
float ydot = n * a_phy * (ecc + cos_f) / denom;
Vector3 v_xy = new Vector3(xdot, ydot, 0);
velocityScaled = ellipse_orientation * v_xy + GravityEngine.Instance().GetScaledVelocity(centerNbody);
phase = NUtils.AngleFromSinCos(sin_f, cos_f) * Mathf.Rad2Deg;
}
private void HyperOrbitForVelocity(NBody forNbody, NBody aroundNBody)
{
// Murray and Dermott Ch2.8 (2.134) - (2.140)
float velSq = forNbody.vel_scaled.sqrMagnitude;
Vector3 r_vec = forNbody.transform.position - aroundNBody.transform.position;
float r = Vector3.Magnitude(r_vec);
r_initial = r;
float mu = aroundNBody.mass;
// semi-major axis (2.134)
// (reversed for HB)
a = 1f / (velSq / mu - 2 / r);
// Determine angular momentum, h
Vector3 h_vec = Vector3.Cross(r_vec, forNbody.vel_scaled);
float h = Vector3.Magnitude(h_vec);
//Debug.Log("h_vec = " + h_vec + " r_vec=" + r_vec + " v=" + forNbody.vel + " planet pos=" + forNbody.transform.position);
// eccentricity (2.135)
ecc = Mathf.Sqrt(h * h / (mu * a) + 1f);
perihelion = a * (ecc - 1);
// inclination (0..180 so Acos is unambiguous) (2.136)
float inclRad = Mathf.Acos(h_vec.z / h);
// Omega_uc - only relevant if there is non-zero inclination
float omega_ucRad = 0f;
float sinOmega = 0f;
float cosOmega = 1f;
bool inclNoneZero = Mathf.Abs(Mathf.Sin(inclRad)) > 1E-5;
if (inclNoneZero)
{
sinOmega = h_vec.x / (h * Mathf.Sin(inclRad));
cosOmega = -h_vec.y / (h * Mathf.Sin(inclRad));
}
else if (h_vec.z < 0)
{
// if incl = 180
sinOmega *= -1f;
cosOmega *= -1f;
}
omega_ucRad = NUtils.AngleFromSinCos(sinOmega, cosOmega);
// Debug.Log("sinOmega=" + sinOmega + " cosOmega=" + cosOmega + " omega_ucRad=" + omega_ucRad + " inclRad=" + inclRad
// + " inclNonZero=" + inclNoneZero + " h_vec=" + h_vec);
float f = 0;
if (ecc > 1E-3)
{
// Like M&D 2.31 but for e^2-1 not 1-e^2
float rdot = Mathf.Sqrt(velSq - h * h / (r * r));
if (Vector3.Dot(r_vec, forNbody.vel_scaled) < 0)
{
rdot *= -1f;
}
float cos_f = (1 / ecc) * (a * (ecc * ecc - 1) / r - 1f);
float sin_f = rdot * a * (ecc * ecc - 1f) / (h * ecc);
f = NUtils.AngleFromSinCos(sin_f, cos_f);
}
// (2.6.17)
// sin(w+f) = Z/(r sin(i))
// cos(w+f) = [X cos(Omega) + Y sin(Omega)]/r
// when sin(i) = 0, will have Z=0 and w + f = 0
float sin_of = 0f;
float cos_of = 1f;
float omega_lcRad = 0f;
if (inclNoneZero)
{
sin_of = r_vec.z / (r * Mathf.Sin(inclRad));
cos_of = (r_vec.x * cosOmega + r_vec.y * sinOmega) / r;
omega_lcRad = (NUtils.AngleFromSinCos(sin_of, cos_of) - f);
}
else
{
float sin_theta = r_vec.y / r;
float cos_theta = r_vec.x / r;
float theta = NUtils.AngleFromSinCos(sin_theta, cos_theta);
// Debug.Log("r_vec=" + r_vec + " theta=" + theta + " f=" + f + " (theta - f)=" + (theta-f));
if (inclRad < Mathf.PI / 2f)
{
omega_lcRad = theta - f;
}
else
{
if (h_vec.z > 0)
{
omega_ucRad = f - theta;
}
else
{
// Rotated by 180 around x, so flip sign of y
sin_theta = -r_vec.y / r;
cos_theta = r_vec.x / r;
theta = NUtils.AngleFromSinCos(sin_theta, cos_theta);
omega_ucRad = f - theta;
}
}
}
// Ellipse wants degrees
omega_lc = Mathf.Rad2Deg * omega_lcRad;
if (omega_lc < 0)
{
omega_lc += 360f;
}
omega_uc = Mathf.Rad2Deg * omega_ucRad;
if (omega_uc < 0)
{
omega_uc += 360f;
}
phase = Mathf.Rad2Deg * f;
inclination = Mathf.Rad2Deg * inclRad;
// Debug.Log("cosof=" + cos_of + " sinof=" + sin_of + " of=" + NUtils.AngleFromSinCos(sin_of, cos_of) + " f=" + f);
}
/// <summary>
/// Generate the points for an orbit segment given the start and end positions. If shortPath then
/// the short path between the points will be shown, otherwise the long way around.
///
/// The points are used to determine an angle from the main axis of the ellipse and although they
/// should be on the ellipse for best results, the code will do it's best if they are not.
/// </summary>
/// <param name="numPoints"></param>
/// <param name="centerPos"></param>
/// <param name="startPos"></param>
/// <param name="endPos"></param>
/// <param name="shortPath"></param>
/// <returns></returns>
public Vector3[] OrbitSegmentPositions(int numPoints,
Vector3 centerPos,
Vector3 startPos,
Vector3 endPos,
bool shortPath)
{
GravityEngine ge = GravityEngine.Instance();
Vector3[] points = new Vector3[numPoints];
UpdateOrbitParams();
CalculateRotation();
float dtheta = 2f * Mathf.PI / numPoints;
float theta = 0;
// find the vector to theta=0 on the ellipse, with no offset
Vector3 ellipseAxis = PositionForTheta(0f, Vector3.zero);
Vector3 normal = ellipse_orientation * Vector3.forward;
float theta1 = NUtils.AngleFullCircleRadians(ellipseAxis, startPos - centerPos, normal);
float theta2 = NUtils.AngleFullCircleRadians(ellipseAxis, endPos - centerPos, normal);
if (inclination > 90)
{
float temp = theta1;
theta1 = theta2;
theta2 = temp;
}
if (theta1 > theta2)
{
float temp = theta1;
theta1 = theta2;
theta2 = temp;
}
if (!shortPath)
{
float temp = theta1;
theta1 = theta2;
// ok to go beyond 2 Pi, since will increment to theta2
theta2 = temp + 2f * Mathf.PI;
}
// Debug.LogFormat("theta1={0} theta2={1} start={2} end={3} axis={4}", theta1, theta2, startPos, endPos, ellipseAxis);
int i = 0;
for (theta = theta1; theta < theta2; theta += dtheta)
{
points[i] = PositionForTheta(theta, centerPos);
if (NUtils.VectorNaN(points[i]))
{
Debug.LogError("Vector NaN + " + points[i]);
points[i] = Vector3.zero;
}
else if (ge.mapToScene)
{
points[i] = ge.MapToScene(points[i]);
}
i++;
if (i > numPoints - 1)
{
break;
}
}
// fill to end with last point
int last = i - 1;
if (last < 0)
{
last = 0;
}
while (i < numPoints)
{
points[i++] = points[last];
}
return(points);
}
private void EllipseOrbitForVelocity(NBody forNbody, NBody aroundNBody)
{
// Murray and Dermott Ch2.8 (2.134) - (2.140)
Vector3 vel = forNbody.vel_scaled - aroundNBody.vel_scaled;
float velSq = vel.sqrMagnitude;
Vector3 r_vec = forNbody.transform.position - aroundNBody.transform.position;
float r = Vector3.Magnitude(r_vec);
float mu = aroundNBody.mass;
// semi-major axis (2.134)
a = 1f / (2 / r - velSq / mu);
// Determine angular momentum, h
Vector3 h_vec = Vector3.Cross(r_vec, forNbody.vel_scaled);
float h = Vector3.Magnitude(h_vec);
//Debug.Log("h_vec = " + h_vec + " r_vec=" + r_vec + " v=" + forNbody.vel + " planet pos=" + forNbody.transform.position);
// eccentricity (2.135)
float eqn1 = h * h / (mu * a);
if (eqn1 > 1f)
{
// Can be slightly bigger than 1 due to numerical error
ecc = 0f;
}
else
{
ecc = Mathf.Sqrt(1f - eqn1);
}
// inclination (0..180 so Acos is unambiguous) (2.136)
float inclRad = Mathf.Acos(h_vec.z / h);
// Omega_uc - only relevant if there is non-zero inclination
float omega_ucRad = 0f;
float sinOmega = 0f;
float cosOmega = 1f;
bool inclNoneZero = Mathf.Abs(Mathf.Sin(inclRad)) > 1E-5;
if (inclNoneZero)
{
sinOmega = h_vec.x / (h * Mathf.Sin(inclRad));
cosOmega = -h_vec.y / (h * Mathf.Sin(inclRad));
}
else if (h_vec.z < 0)
{
// if incl = 180
sinOmega *= -1f;
cosOmega *= -1f;
}
omega_ucRad = NUtils.AngleFromSinCos(sinOmega, cosOmega);
// Debug.Log("sinOmega=" + sinOmega + " cosOmega=" + cosOmega + " omega_ucRad=" + omega_ucRad + " inclRad=" + inclRad
// + " inclNonZero=" + inclNoneZero + " h_vec=" + h_vec);
float f = 0;
if (ecc > 1E-3)
{
// Ellipse (from Sidi)
float cosPhi = (a - r) / (a * ecc);
float sinPhi = Vector3.Dot(r_vec, forNbody.vel_scaled) / (ecc * Mathf.Sqrt(mu * a));
float cosf = (cosPhi - ecc) / (1 - ecc * cosPhi);
float sinf = sinPhi * Mathf.Sqrt(1 - ecc * ecc) / (1 - ecc * cosPhi);
f = NUtils.AngleFromSinCos(sinf, cosf);
}
// (2.6.17)
// sin(w+f) = Z/(r sin(i))
// cos(w+f) = [X cos(Omega) + Y sin(Omega)]/r
// when sin(i) = 0, will have Z=0 and w + f = 0
float sin_of = 0f;
float cos_of = 1f;
float omega_lcRad = 0f;
if (inclNoneZero)
{
sin_of = r_vec.z / (r * Mathf.Sin(inclRad));
cos_of = (r_vec.x * cosOmega + r_vec.y * sinOmega) / r;
omega_lcRad = (NUtils.AngleFromSinCos(sin_of, cos_of) - f);
}
else
{
float sin_theta = r_vec.y / r;
float cos_theta = r_vec.x / r;
float theta = NUtils.AngleFromSinCos(sin_theta, cos_theta);
if (inclRad < Mathf.PI / 2f)
{
omega_lcRad = theta - f;
}
else
{
if (h_vec.z > 0)
{
omega_ucRad = f - theta;
}
else
{
// Rotated by 180 around x, so flip sign of y
sin_theta = -r_vec.y / r;
cos_theta = r_vec.x / r;
theta = NUtils.AngleFromSinCos(sin_theta, cos_theta);
omega_ucRad = f - theta;
}
}
}
// Ellipse wants degrees
omega_lc = Mathf.Rad2Deg * omega_lcRad;
if (omega_lc < 0)
{
omega_lc += 360f;
}
omega_uc = Mathf.Rad2Deg * omega_ucRad;
if (omega_uc < 0)
{
omega_uc += 360f;
}
phase = Mathf.Rad2Deg * f;
inclination = Mathf.Rad2Deg * inclRad;
// Debug.Log("cosof=" + cos_of + " sinof=" + sin_of + " of=" + NUtils.AngleFromSinCos(sin_of, cos_of) + " f=" + f);
// Determine time to periapsis (2.140)
// Eqn assumes E=0 when t=tau
float E = Mathf.Acos((1f - r / a) / ecc);
// this equation has a G but we set G=1
period = 2 * Mathf.PI * Mathf.Sqrt(a * a * a) / Mathf.Sqrt(aroundNBody.mass);
float M = (E - ecc * Mathf.Sin(E));
float tau = M * Mathf.Sqrt(a * a * a) / Mathf.Sqrt(aroundNBody.mass);
// tau is giving time to/from apoapsis, need to find out which
float vdotr = Vector3.Dot(vel, r_vec);
if (vdotr > 0)
{
tau = period - tau;
}
}
