private void TestRV(OrbitData od, GameObject planet, GameObject star)
{
GameObject testPlanet = TestSetupUtils.CreatePlanetInHyper(star, 1f);
testPlanet.name = "TestPlanet";
OrbitHyper testHyper = testPlanet.GetComponent <OrbitHyper>();
testHyper.InitFromOrbitData(od);
planet.name = "Planet";
// Awkward but cannot add a new object to GE when it is stopped, so re-add all three
GravityEngine ge = GravityEngine.Instance();
ge.Clear();
ge.AddBody(star);
ge.AddBody(planet);
ge.AddBody(testPlanet);
ge.Setup();
ge.LogDump();
Vector3 r_od = ge.GetPhysicsPosition(testPlanet.GetComponent <NBody>());
Vector3 v_od = ge.GetVelocity(testPlanet);
Vector3 r_i = ge.GetPhysicsPosition(planet.GetComponent <NBody>());
Vector3 v_i = ge.GetVelocity(planet);
Debug.Log(" r_i=" + r_i + " r_od=" + r_od + " delta=" + Vector3.Distance(r_i, r_od));
Debug.Log(" v_i=" + v_i + " v_od=" + v_od + " delta=" + Vector3.Distance(v_i, v_od));
Assert.IsTrue(FloatEqual(Vector3.Distance(r_i, r_od), 0f, 5E-2));
Assert.IsTrue(FloatEqual(Vector3.Distance(v_i, v_od), 0f, 5E-2));
}
private void TestRV(OrbitData od, GameObject planet, NBody starNbody, float orbitRadius)
{
GameObject testPlanet = TestSetupUtils.CreatePlanetInOrbitUniversal(starNbody, 1f, orbitRadius);
OrbitUniversal orbitU = testPlanet.GetComponent <OrbitUniversal>();
// Run init explicitly to update transform details
orbitU.InitFromOrbitData(od, 0);
// Awkward but previously could not add a new object to GE when it is stopped, so re-add all three
// Leave as is, since it works!
GravityEngine ge = GravityEngine.Instance();
ge.Clear();
ge.AddBody(starNbody.gameObject);
ge.AddBody(planet);
ge.AddBody(testPlanet);
ge.Setup();
ge.LogDump();
Vector3 r_od = ge.GetPhysicsPosition(testPlanet.GetComponent <NBody>());
Vector3 v_od = ge.GetVelocity(testPlanet);
Vector3 r_i = ge.GetPhysicsPosition(planet.GetComponent <NBody>());
Vector3 v_i = ge.GetVelocity(planet);
Debug.Log(" r_i=" + r_i + " r_od=" + r_od + " delta=" + Vector3.Distance(r_i, r_od));
Debug.Log(" v_i=" + v_i + " v_od=" + v_od + " delta=" + Vector3.Distance(v_i, v_od));
Assert.IsTrue(GEUnit.FloatEqual(Vector3.Distance(r_i, r_od), 0f, 1E-2));
Assert.IsTrue(GEUnit.FloatEqual(Vector3.Distance(v_i, v_od), 0f, 1E-2));
}
private void TestRV(OrbitData od, GameObject planet, GameObject star, float orbitRadius)
{
GameObject testPlanet = TestSetupUtils.CreatePlanetInOrbit(star, 1f, orbitRadius);
OrbitEllipse testEllipse = testPlanet.GetComponent <OrbitEllipse>();
// Run init explicitly to update transform details
testEllipse.InitFromOrbitData(od);
// Awkward but cannot add a new object to GE when it is stopped, so re-add all three
GravityEngine ge = GravityEngine.Instance();
ge.Clear();
ge.AddBody(star);
ge.AddBody(planet);
ge.AddBody(testPlanet);
ge.Setup();
ge.LogDump();
Vector3 r_od = ge.GetPhysicsPosition(testPlanet.GetComponent <NBody>());
Vector3 v_od = ge.GetVelocity(testPlanet);
Vector3 r_i = ge.GetPhysicsPosition(planet.GetComponent <NBody>());
Vector3 v_i = ge.GetVelocity(planet);
Debug.Log(" r_i=" + r_i + " r_od=" + r_od + " delta=" + Vector3.Distance(r_i, r_od));
Debug.Log(" v_i=" + v_i + " v_od=" + v_od + " delta=" + Vector3.Distance(v_i, v_od));
Assert.IsTrue(FloatEqual(Vector3.Distance(r_i, r_od), 0f, 1E-2));
Assert.IsTrue(FloatEqual(Vector3.Distance(v_i, v_od), 0f, 1E-2));
}
// Update is called once per frame
void FixedUpdate()
{
// monitor for engine start (not ideal, but avoids linking to LaunchUI script)
if ((timeStarted < 0) && engine.engineOn)
{
timeStarted = ge.GetPhysicalTime();
float pitch0 = pitchCurve.Evaluate(0);
Vector3 localVertical = Vector3.Normalize(ge.GetPhysicsPosition(ship) - ge.GetPhysicsPosition(earth));
lastAttitude = Vector3.Cross(localVertical, Vector3.forward); // forward = (0,0,1)
lastAttitude = Quaternion.AngleAxis(pitch0, Vector3.forward) * lastAttitude;
}
if (timeStarted > 0)
{
float t = (float)(ge.GetTimeWorldSeconds() - timeStarted) / timeRangePhysical;
t = Mathf.Clamp(t, 0f, 1f);
// pitch in range 0..90 as curve goes 0..1
float pitch = pitchCurve.Evaluate(t) * 90.0f;
// find the local vertical
Vector3 localVertical = Vector3.Normalize(ge.GetPhysicsPosition(ship) - ge.GetPhysicsPosition(earth));
// First get local horizontal (this ordering of cross product assumes we want to go East)
Vector3 attitude = Vector3.Cross(localVertical, Vector3.forward); // forward = (0,0,1)
attitude = Quaternion.AngleAxis(pitch, Vector3.forward) * attitude;
engine.SetThrustAxis(-attitude);
shipModel.RotateToVector(attitude);
// when attitude changes by 1 degree, re-compute trajectories
if (Vector3.Angle(attitude, lastAttitude) > 1f)
{
ge.TrajectoryRestart();
lastAttitude = attitude;
}
// thrust
float thrust = thrustCurve.Evaluate(t);
engine.SetThrottlePercent(100f * thrust);
}
}
/// <summary>
/// Used to determine the initial physics position of an NBody (initialPhysPosition).
///
/// Used in two contexts:
/// (1) When a body is added to GE (either at setup or when a body is dynamically added at run-time).
/// (2) In the editor DrawGizmo calls when the orbit path is being show in a scene that is not running.
/// In the case of orbit gizmos the orbit parameters will MOVE the object to the correct position in the
/// orbit based on the position calculated here.
///
/// If the NBody game object has an INBodyInit component (e.g from an OrbitEllipse) then
/// this is used to determine it's position. There is a potential for recursion here, since that
/// orbit ellipse may be on a NBody that is in turn positioned by an orbit ellipse e.g. moon/planet/sun.
///
/// If the NBody has an engine ref (i.e. GE has taken charge) then update with the position from GE.
/// /// </summary>
/// <param name="ge"></param>
public void InitPosition(GravityEngine ge)
{
// Not ideal that NBody knows so much about methods to run setup. Be better to delegate this eventually.
// check for initial condition setup (e.g. initial conditions for elliptical orbit/binary pair)
if (engineRef != null)
{
initialPhysPosition = ge.GetPhysicsPosition(this);
}
else
{
// If there is a Kepler sequence, use that instead of e.g. the first OrbitU
INbodyInit initNbody;
KeplerSequence keplerSeq = GetComponent <KeplerSequence>();
if (keplerSeq != null)
{
initNbody = (INbodyInit)keplerSeq;
}
else
{
initNbody = gameObject.GetComponent <INbodyInit>();
}
if (initNbody != null)
{
initNbody.InitNBody(ge.physToWorldFactor, ge.massScale);
}
else
{
// binary pair
if (transform.parent != null)
{
BinaryPair bp = transform.parent.GetComponent <BinaryPair>();
if (bp != null)
{
bp.SetupBodies();
return;
}
}
if (ge.units != GravityScaler.Units.DIMENSIONLESS)
{
// value in scaled systems is taken from the NBody scaled position and not the transform
initialPhysPosition = initialPos;
}
else
{
// value is taken from the transform object
initialPhysPosition = transform.position;
}
}
}
}
// Update is called once per fixed frame by GE
public void FixedUpdate()
{
// need to get physics positions for everything
GravityEngine ge = GravityEngine.Instance();
Vector3 shipPos = ge.GetPhysicsPosition(spaceship);
Vector3 body1Pos = ge.GetPhysicsPosition(body1);
Vector3 body2Pos = ge.GetPhysicsPosition(body2);
float D = Vector3.Distance(body1Pos, body2Pos);
NBody influencer = lastInfluencer;
float d = Vector3.Distance(shipPos, body2Pos);
// add a bit of hysteresis
if (d < ENTER_DERATE * D * massRatio25)
{
influencer = body2;
}
else if (d > D * massRatio25)
{
influencer = body1;
}
if (influencer != lastInfluencer)
{
if (patchChanged != null)
{
patchChanged.OnNewInfluencer(influencer, lastInfluencer);
}
#pragma warning disable 162 // disable unreachable code warning
if (GravityEngine.DEBUG)
{
Debug.LogFormat("Influencer changed from {0} to {1} at d={2} t={3}",
lastInfluencer, influencer, d, ge.GetPhysicalTime());
}
#pragma warning restore 162
lastInfluencer = influencer;
}
}
void FixedUpdate()
{
Vector3 pos = ge.GetPhysicsPosition(referenceObject);
if (pos.magnitude > distanceTrigger)
{
Vector3 unityPos = referenceObject.transform.position;
ge.MoveAll(referenceObject);
Debug.Log("====================================================================");
Debug.LogFormat("Moving physics position={0} D={1} ", pos, pos.magnitude);
// moving the camera need to move in Unity space (not physics space)
if (cameraObject != null)
{
cameraObject.transform.position -= unityPos;
}
// ge.singleStep = true;
}
}
/// <summary>
/// Apply scale to the orbit. This is used by the inspector scripts during
/// scene setup. Do not use at run-time.
/// </summary>
/// <param name="scale">Scale.</param>
public void ApplyScale(float scale)
{
if (paramBy == ParamBy.AXIS_A)
{
a_scaled = a * scale;
}
else if (paramBy == ParamBy.CLOSEST_P)
{
p_scaled = p * scale;
}
if (binaryNbody != null)
{
// this binary will have orbit w.r.t something. Make sure that update has been done
// check for initial condition setup (e.g. initial conditions for elliptical orbit)
GravityEngine ge = GravityEngine.Instance();
if (binaryNbody.engineRef == null)
{
INbodyInit initNbody = GetComponent <INbodyInit>();
if (initNbody != null)
{
initNbody.InitNBody(ge.physToWorldFactor, ge.massScale);
}
cmPosition = binaryNbody.initialPhysPosition;
}
else
{
cmPosition = ge.GetPhysicsPosition(binaryNbody);
}
}
else
{
cmPosition = transform.position * scale / GravityEngine.instance.physToWorldFactor;
}
UpdateOrbitParams();
// if there is a binaryNbody, then need to determine the CM position and velocity before the
SetupBodies();
}
/// <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);
}
}
}
/// <summary>
/// Computes the transfer with the moon on the +X axis without accounting for the moon motion during
/// transit. (That is accounted for in the ExecuteTransfer routine).
///
/// This allows a co-rotating visualization of the orbit.
/// </summary>
/// <returns></returns>
private Vector3 ComputeTransfer()
{
OrbitData shipOrbit = new OrbitData();
shipOrbit.SetOrbitForVelocity(spaceship, planet);
// compute the min energy path (this will be in the short path direction)
lambertU = new LambertUniversal(shipOrbit, startPoint, targetPoint, shortPath);
// apply any time of flight change
double t_flight = tflightFactor * lambertU.GetTMin();
bool reverse = !shortPath;
const bool df = false;
const int nrev = 0;
int error = lambertU.ComputeXfer(reverse, df, nrev, t_flight);
if (error != 0)
{
Debug.LogWarning("Lambert failed to find solution.");
aroundMoonSegment.gameObject.SetActive(false);
return(Vector3.zero);
}
// Check Lambert is going in the correct direction
Vector3 shipOrbitAxis = Vector3.Cross(ge.GetVelocity(spaceship), ge.GetPhysicsPosition(spaceship)).normalized;
Vector3 tliOrbitAxis = Vector3.Cross(lambertU.GetTransferVelocity(), startPoint.ToVector3());
if (Vector3.Dot(shipOrbitAxis, tliOrbitAxis) < 0)
{
error = lambertU.ComputeXfer(!reverse, df, nrev, t_flight);
if (error != 0)
{
Debug.LogWarning("Lambert failed to find solution for reverse path. error=" + error);
return(Vector3.zero);
}
}
Vector3 tliVelocity = lambertU.GetTransferVelocity();
toMoonOrbit.SetVelocity(tliVelocity);
toMoonSegment.SetVelocity(tliVelocity);
aroundMoonSegment.gameObject.SetActive(true);
// Set velocity for orbit around moon
Vector3 soiEnterVel = lambertU.GetFinalVelocity();
aroundMoonSegment.SetVelocity(soiEnterVel);
// update shipEnterSOI object
ge.UpdatePositionAndVelocity(shipEnterSOI, targetPoint.ToVector3(), soiEnterVel);
// Find the orbit around the moon. By using the mirror position we're assuming it's
// a hyperbola (since there is no course correction at SOI this is true).
// (Moon is in correct position for these calcs so can use world positions, relativePos=false)
Vector3d soiEnterV = new Vector3d(lambertU.GetFinalVelocity());
OrbitUtils.OrbitElements oe = OrbitUtils.RVtoCOE(targetPoint, soiEnterV, moonBody, false);
Vector3d soiExitR = new Vector3d();
Vector3d soiExitV = new Vector3d();
OrbitUtils.COEtoRVMirror(oe, moonBody, ref soiExitR, ref soiExitV, false);
// Set position and vel for exit ship, so exit orbit predictor can run. Moon offset/vel already added.
ge.SetPositionDoubleV3(shipExitSOI, soiExitR);
ge.SetVelocityDoubleV3(shipExitSOI, soiExitV);
aroundMoonSegment.UpdateOrbit();
return(tliVelocity);
}
