/// <summary>
/// Compute the axis of the orbit by taking r x v and normalizing.
/// </summary>
/// <returns></returns>
public Vector3d GetAxis()
{
GravityEngine ge = GravityEngine.Instance();
Vector3d r = ge.GetPositionDoubleV3(nbody) - ge.GetPositionDoubleV3(centralMass);
Vector3d v = GravityEngine.Instance().GetVelocityDoubleV3(nbody) - ge.GetVelocityDoubleV3(centralMass);
return(Vector3d.Normalize(Vector3d.Cross(r, v)));
}
public LambertBattin(OrbitData _fromOrbit, OrbitData _toOrbit) : base(_fromOrbit, _toOrbit)
{
name = "LambertBattin";
// Fundamentals of Astrodynamics and Applications, Vallado, 4th Ed., Algorithm 56 p475
// Take r0, r => a_min, e_min, t_min, v0
GravityEngine ge = GravityEngine.Instance();
// If Nbody is available get position directly. If not (target marker) then
// use orbit phase to compute position
center3d = ge.GetPositionDoubleV3(_fromOrbit.centralMass);
if (fromOrbit.nbody != null)
{
r1 = ge.GetPositionDoubleV3(fromOrbit.nbody);
r1crossv1 = _fromOrbit.GetAxis();
}
else
{
r1 = new Vector3d(toOrbit.GetPhysicsPositionforEllipse(fromOrbit.phase));
Debug.LogError("Code incomplete need to get v1");
}
if (toOrbit.nbody != null)
{
r2 = ge.GetPositionDoubleV3(toOrbit.nbody);
}
else
{
r2 = new Vector3d(toOrbit.GetPhysicsPositionforEllipse(toOrbit.phase));
}
r1 = r1 - center3d;
r2 = r2 - center3d;
if (fromOrbit.a < toOrbit.a)
{
innerOrbit = fromOrbit;
outerOrbit = toOrbit;
innerToOuter = true;
}
else
{
innerOrbit = toOrbit;
outerOrbit = fromOrbit;
innerToOuter = false;
}
fromNBody = fromOrbit.nbody;
mu = ge.GetPhysicsMass(fromOrbit.centralMass);
}
public CircularizeXfer(OrbitData fromOrbit) : base(fromOrbit)
{
name = "Circularize";
GravityEngine ge = GravityEngine.Instance();
// find velocity vector perpendicular to r for circular orbit
Vector3d r_ship = ge.GetPositionDoubleV3(fromOrbit.nbody);
Vector3d v_ship = ge.GetVelocityDoubleV3(fromOrbit.nbody);
Vector3d r_center = ge.GetPositionDoubleV3(fromOrbit.centralMass);
Vector3d v_center = ge.GetVelocityDoubleV3(fromOrbit.centralMass);
Vector3d r = r_ship - r_center;
// want velocity relative to central mass (it could be moving)
Vector3d v = v_ship - v_center;
// to get axis of orbit, can take r x v
Vector3d axis = Vector3d.Cross(r, v).normalized;
// vis visa for circular orbit
double mu = GravityEngine.Instance().GetMass(centerBody);
double v_mag = Mathd.Sqrt(mu / r.magnitude);
// positive v is counter-clockwise
Vector3d v_dir = Vector3d.Cross(axis, r).normalized;
Vector3d v_circular = v_mag * v_dir;
Maneuver m1;
m1 = new Maneuver();
m1.nbody = fromOrbit.nbody;
m1.mtype = Maneuver.Mtype.vector;
m1.velChange = (v_circular - v).ToVector3();
m1.dV = Vector3.Magnitude(m1.velChange);
// maneuver positions and info for KeplerSeq conversion and velocity directions
Vector3d h_unit = fromOrbit.GetAxis();
m1.physPosition = r_ship;
m1.relativePos = r_ship - r_center;
m1.relativeVel = v_circular.magnitude * Vector3d.Cross(h_unit, m1.relativePos).normalized;
m1.relativeTo = fromOrbit.centralMass;
//Debug.LogFormat("v_ship={0} v_center={1} v_c.x={2}", vel_ship, vel_center, v_center[0]);
//Debug.Log(string.Format("v_ship={0} v_circular={1} axis={2} v_dir={3} velChange={4}",
// vel_ship, v_circular, axis, v_dir, m1.velChange));
m1.worldTime = ge.GetPhysicalTime();
maneuvers.Add(m1);
}
// Update is called once per frame
void Update()
{
// on a deferred add (while running) may get an OP update before actually added to GE. This would be bad.
if (nbody.engineRef == null)
{
return;
}
// TODO: optimize/accuracy: if on rails could use orbitU (or KS) directly
// Now there is MapToRender MUST use physics and not transform position
Vector3 centerPos = GravityEngine.Instance().GetPhysicsPosition(aroundNBody);
// Is the resulting orbit and ellipse or hyperbola?
bool mapToScene = true;
Vector3d pos = ge.GetPositionDoubleV3(nbody);
Vector3d vel;
if (velocityFromScript)
{
vel = new Vector3d(velocity);
}
else
{
vel = ge.GetVelocityDoubleV3(nbody);
}
orbitU.InitFromRVT(pos, vel, ge.GetPhysicalTimeDouble(), aroundNBody, false);
// Add lines to the inclination=0 plane of the orbit
Vector3[] points = orbitU.OrbitPositions(numPoints, centerPos, mapToScene, hyperDisplayRadius);
int totalPoints = numPoints + 2 * numPlaneProjections;
if (numPlaneProjections > 0)
{
Vector3[] pointsWithProj = new Vector3[totalPoints];
int projEvery = numPoints / numPlaneProjections;
int p = 0;
int orbitP = 0;
while (p < totalPoints)
{
pointsWithProj[p++] = points[orbitP];
if ((orbitP % projEvery) == 0)
{
// add a line to plane and back
pointsWithProj[p++] = Vector3.ProjectOnPlane(points[orbitP], planeNormal);
pointsWithProj[p++] = points[orbitP];
}
orbitP++;
}
lineR.SetPositions(pointsWithProj);
}
else
{
lineR.SetPositions(points);
}
}
public LambertBattin(NBody fromBody, NBody centerNBody, Vector3d r_from, Vector3d r_to, Vector3d fromAxis) : base()
{
name = "LambertBattin";
fromNBody = fromBody;
GravityEngine ge = GravityEngine.Instance();
centerBody = centerNBody;
center3d = ge.GetPositionDoubleV3(centerNBody);
mu = ge.GetPhysicsMass(centerNBody);
r1 = r_from - center3d;
r1crossv1 = fromAxis;
r2 = r_to - center3d;
innerOrbit = fromOrbit;
innerToOuter = true;
}
/// <summary>
/// Create a Lambert transfer from a given NBody orbit to a position in physics space.
///
/// The constructor will compute the minimum energy transfer to the point taking the shortest
/// path setting into account. The time for the transfer can then be retreived.
/// </summary>
/// <param name="_fromOrbit"></param>
/// <param name="r_from"></param>
/// <param name="r_to"></param>
/// <param name="shortPath"></param>
public LambertUniversal(OrbitData _fromOrbit, Vector3d r_from, Vector3d r_to, bool shortPath) : base(_fromOrbit)
{
name = "LambertUniversal";
GravityEngine ge = GravityEngine.Instance();
center3d = ge.GetPositionDoubleV3(centerBody);
r1 = r_from - center3d;
r2 = r_to - center3d;
fromOrbit = _fromOrbit;
innerOrbit = fromOrbit;
innerToOuter = true;
mu = ge.GetPhysicsMass(fromOrbit.centralMass);
// determine t_min.
double[] r0 = new double[] { r1.x, r1.y, r1.z };
double[] r = new double[] { r2.x, r2.y, r2.z };
ComputeMinTime(ref r0, ref r, _fromOrbit.centralMass, shortPath);
}
// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(KeyCode.Space))
{
// toggle the engine
if (rocketEngine.engineOn)
{
rocketEngine.SetEngine(false);
ge.BodyOnRails(shipNbody, centerNbody);
}
else
{
rocketEngine.SetEngine(true);
Vector3d vel = ge.GetVelocityDoubleV3(shipNbody);
rocketEngine.SetThrustAxis(vel.ToVector3().normalized);
ge.BodyOffRails(shipNbody, ge.GetPositionDoubleV3(shipNbody), vel);
}
}
}
private void Deactivate()
{
// Get CM object pos & vel from GE
GravityEngine ge = GravityEngine.Instance();
Vector3d cmPos = ge.GetPositionDoubleV3(cmNbody);
Vector3d cmVel = ge.GetVelocityDoubleV3(cmNbody);
int i = 0;
foreach (NBody nbody in nbodies)
{
Rigidbody rb = nbody.GetComponentInChildren <Rigidbody>();
if (rb == null)
{
Debug.LogWarning("could not find rigidbody for " + nbody.gameObject.name);
continue;
}
// rb.isKinematic = true;
// set position and velocity
Vector3d nbodyVel = new Vector3d(GravityScaler.ScaleVelSceneToPhys(rb.velocity));
ge.SetVelocityDoubleV3(nbody, cmVel + nbodyVel);
Vector3d nbodyPos = new Vector3d(GravityScaler.ScalePositionSceneToPhys(nbody.transform.localPosition));
ge.SetPositionDoubleV3(nbody, cmPos + nbodyPos);
ge.ActivateBody(nbody.gameObject);
// restore parent
nbody.transform.parent = priorParents[i];
i++;
}
ge.RemoveBody(cmObject);
Destroy(cmObject);
// de-activate any RCS elements
foreach (ReactionControlSystem r in rcs)
{
if (r != null)
{
r.SetRigidBodyEnabled(false);
}
}
}
// Update is called once per frame
void Update()
{
// Now there is MapToRender MUST use physics and not transform position
Vector3 centerPos = GravityEngine.Instance().GetPhysicsPosition(aroundNBody);
// Is the resulting orbit and ellipse or hyperbola?
bool mapToScene = true;
Vector3d pos = ge.GetPositionDoubleV3(nbody);
Vector3d vel;
if (velocityFromScript)
{
vel = new Vector3d(velocity);
}
else
{
vel = ge.GetVelocityDoubleV3(nbody);
}
if (destination != null)
{
// since the segment code just uses this for the angle, the scale does not matter
destPoint = destination.transform.position;
}
orbitU.InitFromRVT(pos, vel, ge.GetPhysicalTimeDouble(), aroundNBody, false);
// TODO: Decide on best segment approach. Common for hyperbola vs ellipse ??
Vector3[] positions;
if (orbitU.eccentricity < 1.0)
{
positions = orbitU.EllipseSegment(numPoints, centerPos, pos.ToVector3(), destPoint, shortPath);
}
else
{
float radius = (pos.ToVector3() - centerPos).magnitude;
positions = orbitU.HyperSegmentSymmetric(numPoints, centerPos, radius, mapToScene);
}
lineR.SetPositions(positions);
}
/// <summary>
/// Computes the transfer and updates all the ghost bodies.
/// </summary>
/// <returns></returns>
private void ComputeTransfer()
{
double timeNow = ge.GetPhysicalTimeDouble();
// First using the transfer time, move the ghost Moon to position at SOI arrival.
// Call evolve via LockAtTime on the ghostMoon to move it. Set position based on this.
double t_flight = tflightFactor * timeHohmann;
double timeatSoi = timeNow + t_flight;
ghostMoonOrbit[MOON_SOI_ENTER].LockAtTime(timeatSoi);
// Determine the moon phase angle
double moonPhase = ghostMoonSoiEnterOrbitPredictor.GetOrbitUniversal().phase;
// Place the TLI ship at the user-requested angle wrt planet-moon line
// Put ghost ship in same orbit geometry as the moon, assuming it is circular. Then
// can use same phase value.
// (Ship needs to reach this departure point, it may not even be on the ship orbit
// in general).
ghostShipOrbit[TLI].phase = shipTLIAngleDeg + (moonPhase + 180f);
ghostShipOrbit[TLI].inclination = ghostMoonOrbit[MOON_SOI_ENTER].inclination;
ghostShipOrbit[TLI].omega_lc = ghostMoonOrbit[MOON_SOI_ENTER].omega_lc;
ghostShipOrbit[TLI].omega_uc = ghostMoonOrbit[MOON_SOI_ENTER].omega_uc;
ghostShipOrbit[TLI].p_inspector = shipOrbit.p;
ghostShipOrbit[TLI].Init();
ghostShipOrbit[TLI].LockAtTime(0);
// Place the SOI enter ship at the user-requested angle in an SOI orbit. Lock at time 0 so the phase
// is held per the user input.
ghostShipOrbit[ENTER_SOI].phase = soiAngleDeg + moonPhase;
ghostShipOrbit[ENTER_SOI].inclination = soiInclination + shipOrbit.inclination;
ghostShipOrbit[ENTER_SOI].omega_lc = ghostMoonOrbit[MOON_SOI_ENTER].omega_lc;
ghostShipOrbit[ENTER_SOI].omega_uc = ghostMoonOrbit[MOON_SOI_ENTER].omega_uc;
ghostShipOrbit[ENTER_SOI].Init();
ghostShipOrbit[ENTER_SOI].LockAtTime(0);
// Find the line to the ENTER_SOI position. Ship departs from that line continued through planet
// at the shipRadius distance (assumes circular ship orbit)
// TODO: Handle planet not at (0,0,0)
Vector3d soiEntryPos = ge.GetPositionDoubleV3(ghostShip[ENTER_SOI]);
Vector3d planetPos = ge.GetPositionDoubleV3(planet);
Vector3d departurePoint = ge.GetPositionDoubleV3(ghostShip[TLI]);
// Use Lambert to find the departure velocity to get from departure to soiEntry
// Since we need 180 degrees from departure to arrival, use LambertBattin
lambertB = new LambertBattin(ghostShip[TO_MOON], planet, departurePoint, soiEntryPos, shipOrbit.GetAxis());
// apply any time of flight change
bool reverse = !shortPath;
const bool df = false;
const int nrev = 0;
int error = lambertB.ComputeXfer(reverse, df, nrev, t_flight);
if (error != 0)
{
Debug.LogWarning("Lambert failed to find solution. error=" + error);
return;
}
// Check Lambert is going in the correct direction
//Vector3 shipOrbitAxis = Vector3.Cross(ge.GetPhysicsPosition(spaceship), ge.GetVelocity(spaceship) ).normalized;
//Vector3 tliOrbitAxis = Vector3.Cross(departurePoint.ToVector3(), lambertB.GetTransferVelocity().ToVector3()).normalized;
Vector3 shipOrbitAxis = Vector3.Cross(ge.GetVelocity(spaceship), ge.GetPhysicsPosition(spaceship)).normalized;
Vector3 tliOrbitAxis = Vector3.Cross(lambertB.GetTransferVelocity().ToVector3(), departurePoint.ToVector3()).normalized;
if (Vector3.Dot(shipOrbitAxis, tliOrbitAxis) < 0)
{
error = lambertB.ComputeXfer(!reverse, df, nrev, t_flight);
if (error != 0)
{
Debug.LogWarning("Lambert failed to find solution for reverse path. error=" + error);
return;
}
}
Debug.LogFormat("tli_vel={0}", lambertB.GetTransferVelocity());
ghostShipOrbit[TO_MOON].InitFromRVT(departurePoint, lambertB.GetTransferVelocity(), timeNow, planet, false);
// Set velocity for orbit around moon. Will be updated every frame
ghostShipOrbit[SOI_HYPER].InitFromRVT(soiEntryPos, lambertB.GetFinalVelocity(), timeNow, ghostMoon[MOON_SOI_ENTER], false);
// Find the exit point of the hyperbola in the SOI
OrbitUtils.OrbitElements oe = OrbitUtils.RVtoCOE(soiEntryPos, lambertB.GetFinalVelocity(), ghostMoon[MOON_SOI_ENTER], false);
Vector3d soiExitR = new Vector3d();
Vector3d soiExitV = new Vector3d();
OrbitUtils.COEtoRVMirror(oe, ghostMoon[MOON_SOI_ENTER], ref soiExitR, ref soiExitV, false);
// Find time to go around the moon. TOF requires relative positions!!
Vector3d ghostSoiEnterPos = ge.GetPositionDoubleV3(ghostMoon[MOON_SOI_ENTER]);
Vector3d soiEnterRelative = soiEntryPos - ghostSoiEnterPos;
Vector3d soiExitRelative = soiExitR - ghostSoiEnterPos;
Vector3d soiExitVelRelative = soiExitV - ge.GetVelocityDoubleV3(ghostMoon[MOON_SOI_ENTER]);
double hyperTOF = ghostShipOrbit[SOI_HYPER].TimeOfFlight(soiEnterRelative, soiExitRelative);
// Position the ghost moon for SOI exit (timeAtSoi includes timeNow)
t_soiExit = timeatSoi + hyperTOF;
ghostMoonOrbit[MOON_SOI_EXIT].LockAtTime(t_soiExit);
// Set position and vel for exit ship, so exit orbit predictor can run.
Vector3d ghostMoonSoiExitPos = ge.GetPositionDoubleV3(ghostMoon[MOON_SOI_EXIT]);
Vector3d ghostMoonSoiExitVel = ge.GetVelocityDoubleV3(ghostMoon[MOON_SOI_EXIT]);
ghostShipOrbit[EXIT_SOI].InitFromRVT(soiExitRelative + ghostMoonSoiExitPos,
soiExitVelRelative + ghostMoonSoiExitVel,
timeNow, planet, false);
}
/// <summary>
/// Take the Nbody objects in the nbodies list set them inactive and make them children of a new
/// NBody object moving as the CM of the nbodies. This allows RigidBody mechanics during close
/// encounters.
/// </summary>
private void Activate()
{
if (nbodies.Length < 2)
{
Debug.LogError("Need two or more nbodies");
return;
}
GravityEngine ge = GravityEngine.Instance();
// Step 1: calculate CM position and velocity
Vector3d cmPos = new Vector3d(0, 0, 0);
Vector3d cmVel = new Vector3d(0, 0, 0);
float mass = 0f;
rigidBodies = new Rigidbody[nbodies.Length];
// RigidBody is assumed to be attached to one of the children (to keep model scale independent)
int i = 0;
foreach (NBody nbody in nbodies)
{
rigidBodies[i] = nbody.GetComponentInChildren <Rigidbody>();
//rigidBodies[i] = nbody.GetComponent<Rigidbody>();
if (rigidBodies[i] == null)
{
Debug.LogError("Abort - No rigidbody detected on " + nbody.gameObject.name);
return;
}
mass += rigidBodies[i].mass;
cmPos += rigidBodies[i].mass * ge.GetPositionDoubleV3(nbody);
cmVel += rigidBodies[i].mass * ge.GetVelocityDoubleV3(nbody);
i++;
}
cmPos /= mass;
cmVel /= mass;
Debug.LogFormat("CM p={0} v={1} mass={2}", cmPos.ToVector3(), cmVel.ToVector3(), mass);
// Step2: Inactivate the NBodies and make children of a new NBody object
priorParents = new Transform[nbodies.Length];
cmObject = new GameObject("DockingGroupCM");
cmNbody = cmObject.AddComponent <NBody>();
// Set cm pos/vel
// NBody InitPosition will use transform or initialPos base on units. Set both.
cmNbody.initialPos = cmPos.ToVector3() * ge.physToWorldFactor;
cmNbody.transform.position = cmNbody.initialPos;
ge.AddBody(cmObject);
ge.SetVelocity(cmNbody, cmVel.ToVector3());
Debug.LogFormat("set pos={0} actual={1}", cmPos.ToVector3(), ge.GetPhysicsPosition(cmNbody));
i = 0;
foreach (NBody nbody in nbodies)
{
Vector3d pos = ge.GetPositionDoubleV3(nbody);
Vector3d vel = ge.GetVelocityDoubleV3(nbody);
priorParents[i] = nbody.gameObject.transform.parent;
ge.InactivateBody(nbody.gameObject);
nbody.gameObject.transform.parent = cmObject.transform;
// position wrt to CM. Need to convert to Unity scene units from GE Internal
pos = (pos - cmPos) * ge.physToWorldFactor;
vel = GravityScaler.ScaleVelPhysToScene(vel - cmVel);
nbody.transform.localPosition = pos.ToVector3();
rigidBodies[i].velocity = vel.ToVector3();
// rigidBodies[i].isKinematic = false;
i++;
Debug.LogFormat("body {0} p={1} v={2}", nbody.gameObject.name, pos.ToVector3(), vel.ToVector3());
}
// activate any RCS elements
foreach (ReactionControlSystem r in rcs)
{
if (r != null)
{
r.SetRigidBodyEnabled(true);
}
}
}
