public Vector3 GetVelocity(GameObject body)
{
NBody nbody = body.GetComponent <NBody>();
int i = nbody.engineRef.index;
return(new Vector3((float)v[i, 0], (float)v[i, 1], (float)v[i, 2]));
}
public void AddBody(GameObject gameObject)
{
if (numBodies + 1 >= arraySize)
{
GrowArrays(GROW_ARRAY);
}
bodies[numBodies] = gameObject;
NBody nbody = bodies[numBodies].GetComponent <NBody>();
nbody.engineRef = new GravityEngine.EngineRef(GravityEngine.BodyType.MASSLESS, numBodies);
Vector3 physicsPosition = gameObject.transform.position / GravityEngine.instance.physToWorldFactor;
r[numBodies, 0] = physicsPosition.x;
r[numBodies, 1] = physicsPosition.y;
r[numBodies, 2] = physicsPosition.z;
v[numBodies, 0] = nbody.vel_scaled.x;
v[numBodies, 1] = nbody.vel_scaled.y;
v[numBodies, 2] = nbody.vel_scaled.z;
info[numBodies] = 0;
numBodies++;
#pragma warning disable 162 // disable unreachable code warning
if (GravityEngine.DEBUG)
{
Debug.Log("Added " + gameObject.name);
}
#pragma warning restore 162
}
public void RemoveBody(GameObject gameObject)
{
NBody nbody = gameObject.GetComponent <NBody>();
// shuffle rest of entries in array up
#pragma warning disable 162 // disable unreachable code warning
if (GravityEngine.DEBUG)
{
Debug.Log("Remove body at " + nbody.engineRef.index);
}
#pragma warning restore 162
// shuffle down array
for (int j = nbody.engineRef.index; j < numBodies - 1; j++)
{
for (int k = 0; k < GravityEngine.NDIM; k++)
{
r[j, k] = r[j + 1, k];
v[j, k] = v[j + 1, k];
}
info[j] = info[j + 1];
bodies[j] = bodies[j + 1];
NBody nb = bodies[j].GetComponent <NBody>();
nb.engineRef.index = j;
}
numBodies--;
}
// Create an NBody and check it's mass
public void EccentricityPrediction()
{
const float mass = 1000f;
GameObject star = TestSetupUtils.CreateNBody(mass, new Vector3(0, 0, 0));
NBody starNbody = star.GetComponent <NBody>();
const float orbitRadius = 10f;
GameObject planet = TestSetupUtils.CreatePlanetInOrbitUniversal(starNbody, 1f, orbitRadius);
OrbitUniversal orbitU = planet.GetComponent <OrbitUniversal>();
OrbitPredictor op = TestSetupUtils.AddOrbitPredictor(planet, star);
Assert.NotNull(op);
double[] ecc_values = { 0, 0.1, 0.7, 0.9, 0.99, 1.01, 1.5, 2 };
foreach (double ecc in ecc_values)
{
Debug.LogFormat("#### EccentricityPrediction ecc={0}", ecc);
orbitU.eccentricity = ecc;
TestSetupUtils.SetupGravityEngine(star, planet);
op.TestRunnerSetup();
OrbitUniversal predictedOrbit = op.GetOrbitUniversal();
Assert.NotNull(predictedOrbit);
CompareOrbits(orbitU, predictedOrbit, small);
}
}
// Create an NBody and check it's mass
public void CirclePrediction()
{
const float mass = 1000f;
GameObject star = TestSetupUtils.CreateNBody(mass, new Vector3(0, 0, 0));
NBody starNbody = star.GetComponent <NBody>();
const float orbitRadius = 10f;
GameObject planet = TestSetupUtils.CreatePlanetInOrbitUniversal(starNbody, 1f, orbitRadius);
OrbitUniversal orbitU = planet.GetComponent <OrbitUniversal>();
OrbitPredictor op = TestSetupUtils.AddOrbitPredictor(planet, star);
Assert.NotNull(op);
double[] p_values = { 10, 100, 1000 };
foreach (double p in p_values)
{
Debug.LogFormat("#### CirclePrediction p={0}", p);
orbitU.p = p;
TestSetupUtils.SetupGravityEngine(star, planet);
op.TestRunnerSetup();
OrbitUniversal predictedOrbit = op.GetOrbitUniversal();
Assert.NotNull(predictedOrbit);
CompareOrbits(orbitU, predictedOrbit, small);
}
}
// Create an NBody and check it's mass
public void OmegaUEllipsePrediction()
{
const float mass = 1000f;
GameObject star = TestSetupUtils.CreateNBody(mass, new Vector3(0, 0, 0));
NBody starNbody = star.GetComponent <NBody>();
const float orbitRadius = 10f;
GameObject planet = TestSetupUtils.CreatePlanetInOrbitUniversal(starNbody, 1f, orbitRadius);
OrbitUniversal orbitU = planet.GetComponent <OrbitUniversal>();
// Need a bit of incl, otherwise omegaU comes back as omegaL.
orbitU.inclination = 10;
orbitU.eccentricity = 0.05;
OrbitPredictor op = TestSetupUtils.AddOrbitPredictor(planet, star);
Assert.NotNull(op);
double[] ou_values = { 0, 1, 5, 10, 30, 60, 90, 180, 270, 355, 360 };
foreach (double ou in ou_values)
{
Debug.LogFormat("#### OmegaUPrediction ou={0}", ou);
orbitU.omega_uc = ou;
TestSetupUtils.SetupGravityEngine(star, planet);
op.TestRunnerSetup();
OrbitUniversal predictedOrbit = op.GetOrbitUniversal();
Assert.NotNull(predictedOrbit);
CompareOrbits(orbitU, predictedOrbit, small);
}
}
public Vector3 GetAcceleration(GameObject body)
{
NBody nbody = body.GetComponent <NBody>();
int i = nbody.engineRef.index;
return(new Vector3((float)a[i, 0], (float)a[i, 1], (float)a[i, 2]));
}
public void Init(NBody fromNbody, Vector3 contactPoint)
{
Vector3 zAxis = new Vector3(0, 0, 1f);
this.normal = Vector3.Normalize(contactPoint - fromNbody.transform.position);
rotateToNormal = Quaternion.FromToRotation(zAxis, normal);
this.contactPoint = contactPoint;
float startRadius = Vector3.Distance(contactPoint, fromNbody.transform.position);
// ensure particles start outside the capture radius of the body
// size is the diameter
if (startRadius < fromNbody.size / 2f)
{
startRadius = 1.2f * (float)fromNbody.size / 2f;
this.contactPoint = fromNbody.transform.position + startRadius * normal;
#pragma warning disable 162 // disable unreachable code warning
if (GravityEngine.DEBUG)
{
Debug.Log("DEBUG: setting contact based on capture size. ");
}
#pragma warning restore 162
}
explosionVelocity = ExplosionVelocity(fromNbody, startRadius);
bodyVelocity = GravityEngine.instance.GetScaledVelocity(fromNbody.transform.gameObject);
}
// Vallado Algorithm 10, p118
// (generic, will work for Ellipse as well, provided special cases are handled)
// Not used yet.
private void SetInitialPosition(NBody nbody)
{
// phase is in
float denom = 1 + ecc * Mathf.Cos(phase_nu);
Vector3 r_pqw = new Vector3(p * Mathf.Cos(phase_nu) / denom, p * Mathf.Sin(phase_nu) / denom, 0);
r_initial_phy = r_pqw.magnitude;
Vector3 r = hyper_orientation * r_pqw;
// orbit position is WRT center. Could be adding dynamically to an object in motion, so need current position.
Vector3 centerPos = Vector3.zero;
// used by widgets - so need to get explcitly
centerNbody = OrbitUtils.GetCenterNbody(this.transform, centerObject);
if (centerNbody.engineRef != null)
{
centerPos = GravityEngine.Instance().GetPhysicsPosition(centerNbody);
}
else
{
// setup - not yet added to GE
centerPos = centerNbody.initialPhysPosition;
}
nbody.initialPhysPosition = r + centerPos;
}
public void SetupBodies()
{
if (transform.childCount != 2)
{
// Binary must have 2 NBody children
Debug.LogError("Must have exactly two Nbody objects attached");
return;
}
body1 = transform.GetChild(0).GetComponent <NBody>();
body2 = transform.GetChild(1).GetComponent <NBody>();
if (body1 == null || body2 == null)
{
Debug.LogError("Binary requires children to have NBody scripts attached.");
return;
}
if (transform.parent)
{
binaryNbody = transform.parent.GetComponent <NBody>();
if (binaryNbody)
{
binaryNbody.InitPosition(GravityEngine.Instance());
cmPosition = GravityEngine.Instance().GetPhysicsPosition(binaryNbody);
}
}
// mass is scaled by GE
float m_total = (body1.mass + body2.mass);
float mu1 = body1.mass / m_total;
float mu2 = body2.mass / m_total;
SetupBody(body1, a_scaled * mu2, mu2 * mu2 * body2.mass, false);
SetupBody(body2, a_scaled * mu1, mu1 * mu1 * body1.mass, true);
}
/// <summary>
/// Displays the path of the elliptical orbit when the object is selected in the editor.
/// </summary>
void OnDrawGizmosSelected()
{
// need to have a center to draw gizmo.
body1 = transform.GetChild(0).GetComponent <NBody>();
body2 = transform.GetChild(1).GetComponent <NBody>();
if (body1 == null || body2 == null)
{
return;
}
// only display if this object is directly selected
if (Selection.activeGameObject != transform.gameObject)
{
return;
}
float m_total = (float)(body1.mass + body2.mass);
float mu1 = body1.mass / m_total;
float mu2 = body2.mass / m_total;
UpdateOrbitParams();
CalculateRotation();
DrawEllipse(a_scaled * mu2, transform.position, false);
DrawEllipse(a_scaled * mu1, transform.position, true);
// move bodies to location specified by parameters
SetTransform(a_scaled * mu2, body1, false);
SetTransform(a_scaled * mu1, body2, true);
}
private void SetTransform(float a_n, NBody nbody, bool reflect)
{
float phaseRad = phase * Mathf.Deg2Rad;
float reflect_in_y = 1f;
if (reflect)
{
reflect_in_y = -1f;
phaseRad = 2f * Mathf.PI - phaseRad;
}
// evolution uses true anomoly (angle from center)
float r = a_n * (1f - ecc * ecc) / (1f + ecc * Mathf.Cos(phaseRad));
Vector3 pos = new Vector3(reflect_in_y * r * Mathf.Cos(phaseRad),
r * Mathf.Sin(phaseRad),
0);
// move from XY plane to the orbital plane
Vector3 new_p = ellipse_orientation * pos;
// orbit position is WRT center
if (binaryNbody != null)
{
nbody.transform.position = new_p + binaryNbody.transform.position;
}
else
{
nbody.transform.position = new_p + transform.position;
}
}
// Use this for initialization
// Start() NOT Awake() to ensure that objects created on the fly can have centerBody etc. assigned
// Awake is called from within Object.Instatiate()
void Start()
{
nbody = body.GetComponent <NBody>();
if (nbody == null)
{
Debug.LogWarning("Cannot show orbit - Body requires NBody component");
return;
}
if (centerBody == null)
{
Debug.LogError("Center body is null " + gameObject.name);
}
if (body == null)
{
Debug.LogError("Body is null " + gameObject.name);
}
aroundNBody = centerBody.GetComponent <NBody>();
orbitU.SetNBody(nbody);
orbitU.centerNbody = aroundNBody;
ge = GravityEngine.Instance();
lineR = GetComponent <LineRenderer>();
lineR.positionCount = numPoints + 2 * numPlaneProjections;
}
private void StartExplosion(Vector3 contactPoint, Vector3 normal, GameObject otherNBodyGO)
{
if (!prefabOk)
{
Debug.LogError("prefab not ok");
return;
}
NBody nbody = otherNBodyGO.GetComponent <NBody>();
if (nbody == null)
{
Debug.LogError("Parent of collider does not have NBody - explosion aborted. " + otherNBodyGO.name);
return;
}
// instantiate the prefab and set it's position to that of the current object
GameObject explosionGO = Instantiate(explosionPrefab) as GameObject;
explosionGO.transform.localPosition = Vector3.zero;
explosionGO.transform.position = transform.parent.position;
// find out the velocity of this NBody and provide to dust init routine. (This becomes the CM velocity)
// Tell engine to ignore this body
ExplosionFromNBody explosionFromNBody = explosionGO.GetComponent <ExplosionFromNBody>();
explosionFromNBody.Init(nbody, contactPoint);
// Activate dust (this will inactivate the sphere model, so it will be hidden)
// MUST remove the exploding FIRST or it's mass will accelerate the particles
GravityEngine.instance.InactivateBody(transform.parent.gameObject);
explosionGO.SetActive(true);
}
public void AddNBody(int bodyNum, NBody nbody, Vector3 position, Vector3 velocity)
{
if (numBodies > maxBodies)
{
Debug.LogError("Added more than maximum allocated bodies! max=" + maxBodies);
return;
}
if (bodyNum != numBodies)
{
Debug.LogError("Body numbers are out of sync integrator=" + numBodies + " GE=" + bodyNum);
return;
}
active[numBodies] = true;
// r,m already in GravityEngine
vel[numBodies, 0] = velocity.x;
vel[numBodies, 1] = velocity.y;
vel[numBodies, 2] = velocity.z;
// check for engine
GEExternalAcceleration ext_accel = nbody.GetComponent <GEExternalAcceleration>();
if (ext_accel != null)
{
externalAccel[numBodies] = ext_accel;
#pragma warning disable 162 // disable unreachable code warning
if (GravityEngine.DEBUG)
{
Debug.Log("Added GEExternalAcceleration engine for " + nbody.gameObject);
}
#pragma warning restore 162
}
// Debug.Log ("add object at r=" + gameObject.transform.position + " v=" + ubody.vel + " size2=" + size2[numBodies]
// + " mass=" + ubody.mass);
numBodies++;
}
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));
}
public void OmegaUCircleInclination()
{
const float mass = 1000f;
GameObject star = TestSetupUtils.CreateNBody(mass, new Vector3(0, 0, 0));
NBody starNbody = star.GetComponent <NBody>();
const float orbitRadius = 10f;
GameObject planet = TestSetupUtils.CreatePlanetInOrbitUniversal(starNbody, 1f, orbitRadius);
OrbitUniversal orbitU = planet.GetComponent <OrbitUniversal>();
orbitU.eccentricity = 0.0f;
orbitU.inclination = 5;
orbitU.SetMajorAxisInspector(orbitRadius);
// Try some values of om
float[] omegaValues = { 0f, 30f, 45f, 60f, 90f, 135f, 180f, 210f, 320f };
foreach (float omega in omegaValues)
{
orbitU.omega_uc = omega;
TestSetupUtils.SetupGravityEngine(star, planet);
OrbitData od = new OrbitData();
od.SetOrbitForVelocity(planet.GetComponent <NBody>(), star.GetComponent <NBody>());
Debug.Log("Omega = " + omega + " od.omega_lc=" + od.omega_lc + " od:" + od.LogString());
Assert.IsTrue(GEUnit.FloatEqual(omega, od.omega_uc, 0.1));
}
}
/// <summary>
/// Determine how many parents/grandparents etc. have Kepler mode.
/// GE uses this to ensure evolution starts at the most central body in a heirarchy
/// and works out to the leaves.
/// </summary>
public static int CalcKeplerDepth(IFixedOrbit fixedBody)
{
if (!fixedBody.IsOnRails())
{
return(0);
}
int depth = 0;
bool done = false;
NBody center = fixedBody.GetCenterNBody();
while (!done && (center != null))
{
IFixedOrbit parent = center.GetComponent <IFixedOrbit>();
if ((parent != null) && parent.IsOnRails())
{
depth++;
center = parent.GetCenterNBody();
}
else
{
done = true;
}
}
return(depth);
}
// Check eccentricity and inclination
public void RVtoCOEtoRV()
{
GameObject star = TestSetupUtils.CreateNBody(10, new Vector3(0, 0, 0));
TestSetupUtils.SetupGravityEngine(star, null);
NBody starBody = star.GetComponent <NBody>();
RVpair[] rvp =
{
new RVpair(10, 0, 0, 0, 1, 0),
new RVpair(10, 0, 0, 0, 10, 0),
new RVpair(10, 0, 0, 0, -1, 0),
new RVpair(10, 0, 0, 0, -10, 0),
new RVpair(-10, 10, 0, -4, 3, 0),
new RVpair(-10, 10, 0, 4, -3, 0)
};
for (int i = 0; i < rvp.Length; i++)
{
OrbitUtils.OrbitElements oe = OrbitUtils.RVtoCOE(rvp[i].r, rvp[i].v, starBody, false);
Vector3d r1 = new Vector3d();
Vector3d v1 = new Vector3d();
OrbitUtils.COEtoRV(oe, starBody, ref r1, ref v1, false);
Debug.LogFormat("i={0} r_in={1} r_out={2}\n v_in={3} v_out={4}\n oe: {5}",
i, rvp[i].r, r1, rvp[i].v, v1, oe);
Assert.IsTrue(GEUnit.Vec3dEqual(rvp[i].r, r1, small));
Assert.IsTrue(GEUnit.Vec3dEqual(rvp[i].v, v1, small));
}
}
/// <summary>
/// Determine the SOI radius in internal physics units.
/// </summary>
/// <param name="planet"></param>
/// <param name="moon"></param>
/// <returns></returns>
public static float SoiRadius(NBody planet, NBody moon)
{
// to allow to run before GE is up, use Ellipse component to get radius
OrbitEllipse moonEllipse = moon.gameObject.GetComponent <OrbitEllipse>();
float a;
if (moonEllipse != null)
{
a = moonEllipse.a_scaled;
}
else
{
OrbitUniversal orbitU = moon.GetComponent <OrbitUniversal>();
if (orbitU != null)
{
a = (float)orbitU.GetApogee();
}
else
{
Debug.LogWarning("Could not get moon orbit size");
return(float.NaN);
}
}
// mass scaling will cancel in this ratio
return(Mathf.Pow(moon.mass / planet.mass, 0.4f) * a);
}
/* -----------------------------------------------------------------------------
*
* function rv2coe
*
* this function finds the classical orbital elements given the geocentric
* equatorial position and velocity vectors.
*
* author : david vallado 719-573-2600 21 jun 2002
*
* revisions
* vallado - fix special cases 5 sep 2002
* vallado - delete extra check in inclination code 16 oct 2002
* vallado - add constant file use 29 jun 2003
*
* inputs description range / units
* r - ijk position vector km
* v - ijk velocity vector km / s
*
* outputs :
* p - semilatus rectum km
* a - semimajor axis km
* ecc - eccentricity
* incl - inclination 0.0 to pi rad
* raan - longitude of ascending node 0.0 to 2pi rad
* argp - argument of perigee 0.0 to 2pi rad
* nu - true anomaly 0.0 to 2pi rad
* m - mean anomaly 0.0 to 2pi rad
* eccanom - eccentric, parabolic,
* hyperbolic anomaly rad
* arglat - argument of latitude (ci) 0.0 to 2pi rad
* truelon - true longitude (ce) 0.0 to 2pi rad
* lonper - longitude of periapsis (ee) 0.0 to 2pi rad
*
* locals :
* hbar - angular momentum h vector km2 / s
* ebar - eccentricity e vector
* nbar - line of nodes n vector
* c1 - v**2 - u/r
* rdotv - r dot v
* hk - hk unit vector
* sme - specfic mechanical energy km2 / s2
* i - index
* temp - temporary variable
* typeorbit - type of orbit ee, ei, ce, ci
*
* coupling :
* mag - magnitude of a vector
* cross - cross product of two vectors
* angle - find the angle between two vectors
* newtonnu - find the mean anomaly
*
* references :
* vallado 2013, 113, alg 9, ex 2-5
* --------------------------------------------------------------------------- */
// Vector3 version
public static OrbitElements RVtoCOE(Vector3 r_in,
Vector3 v_in,
NBody centerBody,
bool relativePos)
{
return(RVtoCOE(new Vector3d(r_in), new Vector3d(v_in), centerBody, relativePos));
}
/// <summary>
/// Detach the active stage and enable the next one if present.
/// </summary>
private void DropStage()
{
// automatic staging when a preceeding stage runs out of fuel (engine still on)
if (multiStage.activeStage < multiStage.numStages)
{
// release stage as child, add to GravityEngine
GameObject spentStage = stages[multiStage.activeStage];
spentStage.transform.parent = null;
// add an NBody component,so GE can manage the stage
NBody nbody = spentStage.AddComponent <NBody>();
nbody.initialPos = spentStage.transform.position;
nbody.vel = shipNbody.vel;
// Add an atmosphere model, copy general values from rocket and then adjust
EarthAtmosphere stageAtmosphere = spentStage.AddComponent <EarthAtmosphere>();
stageAtmosphere.InitFrom(atmosphere, nbody);
stageAtmosphere.coeefDrag = multiStage.coeefDrag[multiStage.activeStage];
stageAtmosphere.crossSectionalArea = multiStage.crossSectionalArea[multiStage.activeStage];
stageAtmosphere.inertialMassKg = multiStage.massStageEmpty[multiStage.activeStage];
GravityEngine.Instance().AddBody(spentStage);
// activate the next stage
multiStage.NextStage();
multiStage.SetEngine(true);
GravityEngine.Instance().TrajectoryRestart();
DisplayManager.Instance().DisplayMessage("Staging");
}
else
{
multiStage.SetEngine(false);
DisplayManager.Instance().DisplayMessage("Engine Shutdown");
}
}
// Use this for initialization
void Start()
{
mainCameraBoom.SetActive(false);
shipCameraBoom.SetActive(true);
shipNbody = ship.GetComponent <NBody>();
lineScaler = GetComponent <LineScaler>();
}
/// <summary>
/// Add an element to the sequence that begins at time and evolves based on the orbit data provided.
///
/// Orbit elements must be added in increasing time order.
/// </summary>
/// <param name="time"></param>
/// <param name="orbitData"></param>
/// <param name="body"></param>
/// <param name="centerBody"></param>
/// <param name="callback">(Optional) Method to call when sequence starts</param>
/// <returns></returns>
public OrbitUniversal AppendElementOrbitData(double time,
OrbitData orbitData,
NBody body,
NBody centerBody,
ElementStarted callback)
{
if (BadTime(time))
{
return(null);
}
KeplerElement ke = new KeplerElement
{
timeStart = time,
returnToGE = false,
callback = callback
};
OrbitUniversal orbit = orbitsGO.AddComponent <OrbitUniversal>();
orbit.centerNbody = centerBody;
orbit.SetNBody(body);
orbit.InitFromOrbitData(orbitData, time);
orbit.evolveMode = OrbitUniversal.EvolveMode.KEPLERS_EQN;
ke.orbit = orbit;
keplerElements.Add(ke);
return(orbit);
}
// Check eccentricity and inclination
public void CheckTestRV()
{
const float mass = 1000f;
GameObject star = TestSetupUtils.CreateNBody(mass, new Vector3(0, 0, 0));
NBody starNbody = star.GetComponent <NBody>();
const float orbitRadius = 10f;
GameObject planet = TestSetupUtils.CreatePlanetInOrbitUniversal(starNbody, 1f, orbitRadius);
OrbitUniversal orbitU = planet.GetComponent <OrbitUniversal>();
orbitU.eccentricity = .25f;
orbitU.inclination = 25f;
orbitU.omega_uc = 10f;
orbitU.omega_lc = 20f;
orbitU.phase = 190f;
orbitU.SetMajorAxisInspector(orbitRadius);
OrbitData od = new OrbitData();
od.a = orbitRadius;
od.ecc = 0.25f;
od.inclination = 25f;
od.omega_uc = 10f;
od.omega_lc = 20f;
od.phase = 190f;
od.centralMass = starNbody;
GravityEngine.Instance().UnitTestAwake();
Debug.LogFormat("major-axis: {0} vs {1}", orbitU.GetMajorAxisInspector(), orbitRadius);
Assert.AreEqual(orbitU.GetMajorAxisInspector(), orbitRadius);
TestRV(od, planet, starNbody, orbitRadius);
}
private void RefreshBodyData(BodyFrame frame)
{
cache.Clear();
foreach (var body in (IList <Body>)frame.RawData)
{
if (!body.IsTracked)
{
continue;
}
NBody nbody = frame.Find(body.TrackingId);
if (nbody == null)
{
nbody = new NBody(body.TrackingId, frame.Width, frame.Height);
}
cache.Add(nbody);
RefreshBodyData(body, nbody);
}
lock (frame.Bodies) {
frame.Bodies.Clear();
frame.Bodies.AddRange(cache);
}
}
public void PositionForRadius()
{
const float mass = 1000f;
GameObject star = TestSetupUtils.CreateNBody(mass, new Vector3(0, 0, 0));
NBody starNbody = star.GetComponent <NBody>();
const float orbitRadius = 10f;
GameObject planet = TestSetupUtils.CreatePlanetInOrbitUniversal(starNbody, 1f, orbitRadius);
OrbitUniversal orbitU = planet.GetComponent <OrbitUniversal>();
orbitU.inclination = 5;
orbitU.SetMajorAxisInspector(orbitRadius);
float r = 10.0f;
float[] eccValues = { 0f, 0.1f, 0.9f, 1.1f };
foreach (float ecc in eccValues)
{
Debug.LogFormat("======= ecc={0} =======", ecc);
orbitU.eccentricity = ecc;
orbitU.SetMajorAxisInspector(orbitRadius); // updates p
TestSetupUtils.SetupGravityEngine(star, planet);
Vector3[] positions = orbitU.GetPositionsForRadius(r, new Vector3(0, 0, 0));
Debug.LogFormat("pos[0]={0} pos[1]={1}", positions[0], positions[1]);
foreach (Vector3 p in positions)
{
Debug.LogFormat("Position error={0}", Mathf.Abs(p.magnitude - r));
Assert.IsTrue(GEUnit.FloatEqual(p.magnitude, r, 1E-2));
}
}
}
public void KeplerVsTimeOfFlight()
{
// Need to make sure TOF < 1 period
const float mass = 100f;
GameObject star = TestSetupUtils.CreateNBody(mass, new Vector3(0, 0, 0));
NBody starNbody = star.GetComponent <NBody>();
const float orbitRadius = 10f;
GameObject planet = TestSetupUtils.CreatePlanetInOrbitUniversal(starNbody, 1f, orbitRadius);
OrbitUniversal orbitU = planet.GetComponent <OrbitUniversal>();
// Parabola (ecc=1.0 fails, need to investigate)
float[] ecc_values = { 0.0f, 0.1f, 0.5f, 0.9f, 1.2f, 1.5f };
foreach (float ecc in ecc_values)
{
Debug.LogFormat("======= ecc={0} =======", ecc);
orbitU.eccentricity = ecc;
orbitU.p = 10f;
orbitU.evolveMode = OrbitUniversal.EvolveMode.KEPLERS_EQN;
// Evolve to position r1
double time = 5.0;
TestSetupUtils.SetupGravityEngine(star, planet);
double[] r1 = new double[] { 0, 0, 0 };
// orbitU.PreEvolve(pscale, mscale);
// Ugh. Need to do this before call evolve, since it caches the value.
Vector3d r0_vec = GravityEngine.Instance().GetPositionDoubleV3(planet.GetComponent <NBody>());
orbitU.Evolve(time, ref r1);
Vector3d r1_vec = new Vector3d(ref r1);
// check time to r1
double time_test = orbitU.TimeOfFlight(r0_vec, r1_vec);
Debug.LogFormat("check r0={0} to r1={1} p ={2} after t={3} TOF => {4}",
r0_vec, r1_vec, orbitU.p, time, time_test);
Assert.IsTrue(GEUnit.DoubleEqual(time, time_test, 1E-4));
}
}
/// <summary>
/// Detach the active stage and enable the next one if present.
/// </summary>
private void DropStage()
{
// automatic staging when a preceeding stage runs out of fuel (engine still on)
if (multiStage.activeStage < multiStage.numStages)
{
// release stage as child, add to GravityEngine
GameObject spentStage = stages[multiStage.activeStage];
spentStage.transform.parent = null;
spentStage.AddComponent <NBody>();
NBody nbody = spentStage.GetComponent <NBody>();
nbody.initialPos = spentStage.transform.position;
nbody.vel = shipNbody.vel;
GravityEngine.Instance().AddBody(spentStage);
// activate the next stage
multiStage.NextStage();
multiStage.SetEngine(true);
GravityEngine.Instance().TrajectoryRestart();
DisplayManager.Instance().DisplayMessage("Staging");
}
else
{
multiStage.SetEngine(false);
DisplayManager.Instance().DisplayMessage("Engine Shutdown");
}
}
/// <summary>
/// Add an element to the sequence using r0/v0/t0 initial conditions.
///
/// Position and velocity are with respect to the center body (NOT world/physics space!).
///
/// Orbit segements must be added in increasing time order.
/// </summary>
/// <param name="r0"></param>
/// <param name="v0"></param>
/// <param name="time"></param>
/// <param name="relativePos"></param>
/// <param name="body"></param>
/// <param name="centerBody"></param>
/// <param name="callback">(Optional) Method to call when sequence starts</param>
/// <returns></returns>
public OrbitUniversal AppendElementRVT(Vector3d r0,
Vector3d v0,
double time,
bool relativePos,
NBody body,
NBody centerBody,
ElementStarted callback)
{
if (BadTime(time))
{
return(null);
}
KeplerElement ke = new KeplerElement
{
timeStart = time,
callback = callback,
returnToGE = false
};
OrbitUniversal orbit = orbitsGO.AddComponent <OrbitUniversal>();
orbit.centerNbody = centerBody;
orbit.SetNBody(body);
orbit.InitFromRVT(r0, v0, time, centerBody, relativePos);
orbit.evolveMode = OrbitUniversal.EvolveMode.KEPLERS_EQN;
ke.orbit = orbit;
keplerElements.Add(ke);
return(orbit);
}
private void DrawBone(NBody body, NJointType j0, NJointType j1) {
NJoint joint0 = body.Joints[j0];
NJoint joint1 = body.Joints[j1];
if (j0 == NJointType.Neck && joint0 == null) {
joint0 = body.Joints[NJointType.SpineShoulder];
} else if (j1 == NJointType.Neck && joint1 == null) {
joint1 = body.Joints[NJointType.SpineShoulder];
}
if (joint0 == null || joint1 == null) { return; }
// If we can't find either of these joints, exit
if (joint0.Tracking == NTrackingState.NotTracked ||
joint1.Tracking == NTrackingState.NotTracked) { return; }
// Don't draw if both points are inferred
if (joint0.Tracking == NTrackingState.Inferred &&
joint1.Tracking == NTrackingState.Inferred) { return; }
buffer.DrawPolyline(new System.Drawing.Point[] { joint0.Position2D, joint1.Position2D }, false, brdBlue, 10);
}
private void RefreshBodyData(Skeleton skeleton, NBody nbody) {
cache.Add(nbody);
// Joints
foreach (Joint joint in skeleton.Joints) {
var ntype = ResolveJointType(joint.JointType);
var njoint = nbody.GetJoint(ntype);
var point = Sensor.CoordinateMapper.MapSkeletonPointToColorPoint(joint.Position, ColorFormat);
njoint.Tracking = ResolveTrackingState(joint.TrackingState);
njoint.SetPosition2D(point.X, point.Y);
njoint.SetPosition3D(joint.Position.X, joint.Position.Y, joint.Position.Z);
}
// Misc
nbody.Tracking = NTrackingState.Tracked;
}
private void RefreshBodyData(BodyFrame frame) {
cache.Clear();
foreach (var skeleton in (IList<Skeleton>)frame.RawData) {
if (skeleton.TrackingState != SkeletonTrackingState.Tracked) { continue; }
NBody nbody = frame.Find(Convert.ToUInt64(skeleton.TrackingId));
if (nbody == null) {
nbody = new NBody(Convert.ToUInt64(skeleton.TrackingId), frame.Width, frame.Height);
}
cache.Add(nbody);
RefreshBodyData(skeleton, nbody);
}
lock (frame.Bodies) {
frame.Bodies.Clear();
frame.Bodies.AddRange(cache);
}
}
private void RefreshBodyData(BodyFrame frame){
cache.Clear();
foreach (var body in (IList<Body>) frame.RawData) {
if (!body.IsTracked) { continue; }
NBody nbody = frame.Find(body.TrackingId);
if (nbody == null) {
nbody = new NBody(body.TrackingId, frame.Width, frame.Height);
}
cache.Add(nbody);
RefreshBodyData(body, nbody);
}
lock (frame.Bodies) {
frame.Bodies.Clear();
frame.Bodies.AddRange(cache);
}
}
private void RefreshBodyData(Body body, NBody nbody) {
// Joints
foreach(var joint in body.Joints.Values){
var ntype = ResolveJointType(joint.JointType);
var njoint = nbody.GetJoint(ntype);
var point = coordinateMapper.MapCameraPointToColorSpace(joint.Position);
njoint.Tracking = ResolveTrackingState(joint.TrackingState);
njoint.SetPosition2D(point.X, point.Y);
njoint.SetPosition3D(joint.Position.X, joint.Position.Y, joint.Position.Z);
}
// Misc
nbody.Tracking = NTrackingState.Tracked;
}
public void RepaintColorFrame(Image<Bgra, Byte> buffer, NBody body) {
// Face & Name
var head = body.GetJoint(NJointType.Head).Area;
if (head.Width > 0) {
buffer.Draw(head, brdGreen, 4);
if (body.Name != null) {
var txt = new Rectangle(head.X, head.Y + head.Height + 20, head.Width, head.Height);
DrawText(buffer, txt, body.Name);
}
}
// Hands
var right = body.GetJoint(NJointType.HandRight).Area;
if (right.Width > 0) {
buffer.Draw(right, brdGreen, 4);
}
var left = body.GetJoint(NJointType.HandLeft).Area;
if (left.Width > 0) {
buffer.Draw(left, brdGreen, 4);
}
// Joints
foreach (var joint in body.Joints.Values) {
if (joint == null) { continue; }
if (joint.Tracking == NTrackingState.NotTracked) { continue; }
var color = joint.Tracking == NTrackingState.Inferred ? brdRed : brdGreen;
buffer.Draw(new CircleF(joint.Position2D, 10.0f), color, 10);
}
// Torso
DrawBone(body, NJointType.Head, NJointType.Neck);
DrawBone(body, NJointType.Neck, NJointType.SpineShoulder);
DrawBone(body, NJointType.SpineShoulder, NJointType.SpineMid);
DrawBone(body, NJointType.SpineMid, NJointType.SpineBase);
DrawBone(body, NJointType.SpineShoulder, NJointType.ShoulderRight);
DrawBone(body, NJointType.SpineShoulder, NJointType.ShoulderLeft);
DrawBone(body, NJointType.SpineBase, NJointType.HipRight);
DrawBone(body, NJointType.SpineBase, NJointType.HipLeft);
// Right Arm
DrawBone(body, NJointType.ShoulderRight, NJointType.ElbowRight);
DrawBone(body, NJointType.ElbowRight, NJointType.WristRight);
DrawBone(body, NJointType.WristRight, NJointType.HandRight);
DrawBone(body, NJointType.HandRight, NJointType.HandTipRight);
DrawBone(body, NJointType.WristRight, NJointType.ThumbRight);
// Left Arm
DrawBone(body, NJointType.ShoulderLeft, NJointType.ElbowLeft);
DrawBone(body, NJointType.ElbowLeft, NJointType.WristLeft);
DrawBone(body, NJointType.WristLeft, NJointType.HandLeft);
DrawBone(body, NJointType.HandLeft, NJointType.HandTipLeft);
DrawBone(body, NJointType.WristLeft, NJointType.ThumbLeft);
// Right Leg
DrawBone(body, NJointType.HipRight, NJointType.KneeRight);
DrawBone(body, NJointType.KneeRight, NJointType.AnkleRight);
DrawBone(body, NJointType.AnkleRight, NJointType.FootRight);
// Left Leg
DrawBone(body, NJointType.HipLeft, NJointType.KneeLeft);
DrawBone(body, NJointType.KneeLeft, NJointType.AnkleLeft);
DrawBone(body, NJointType.AnkleLeft, NJointType.FootLeft);
}
