public Vector3_ Check2D(Vector3_ tp, Creature target)
{
var n = new Vector2_(creature.x, creature.cy);
var t = new Vector2_(target.x, target.cy);
var c = creature.colliderSize + target.colliderSize;
if (t.y > n.y + creature.colliderHeight || Mathd.Abs(n.x - t.x) < c * 0.025)
{
return(tp);
}
var nx = n.x;
var tx = tp.x;
if (tx > nx && t.x < n.x || tx < nx && t.x > n.x)
{
return(tp);
}
var max = tx > nx ? t.x - c : t.x + c;
nx = tx > nx && tx > max || tx < nx && tx < max ? max : tx;
tp.x = nx;
return(tp);
}
public bool ContainsLatLon(Vector2d coord)
{
////first check tile
var coordinateTileId = Conversions.LatitudeLongitudeToTileId(
coord.x, coord.y, Tile.CurrentZoom);
if (Points.Count > 0)
{
var from = Conversions.LatLonToMeters(coord.x, coord.y);
var to = new Vector2d((Points[0][0].x / Tile.TileScale) + Tile.Rect.Center.x, (Points[0][0].z / Tile.TileScale) + Tile.Rect.Center.y);
var dist = Vector2d.Distance(from, to);
if (Mathd.Abs(dist) < 50)
{
return(true);
}
}
//Debug.Log("Distance -> " + dist);
{
if ((!coordinateTileId.Canonical.Equals(Tile.CanonicalTileId)))
{
return(false);
}
//then check polygon
var point = Conversions.LatitudeLongitudeToVectorTilePosition(coord, Tile.CurrentZoom);
var output = PolygonUtils.PointInPolygon(new Point2d <float>(point.x, point.y), _geom);
return(output);
}
}
public RectD(Vector2d min, Vector2d size)
{
Min = min;
Max = min + size;
Center = new Vector2d(Min.x + size.x / 2, Min.y + size.y / 2);
Size = new Vector2d(Mathd.Abs(size.x), Mathd.Abs(size.y));
}
public static double Gamma(double value, double absmax, double gamma)
{
bool flag = false;
if (value < 0.0)
{
flag = true;
}
double num1 = Mathd.Abs(value);
if (num1 > absmax)
{
if (flag)
{
return(-num1);
}
else
{
return(num1);
}
}
else
{
double num2 = Mathd.Pow(num1 / absmax, gamma) * absmax;
if (flag)
{
return(-num2);
}
else
{
return(num2);
}
}
}
public OrbitPoint[] GenerateOrbit(BodyInfo body)
{
List <OrbitPoint> orbitPoints = new List <OrbitPoint>();
OrbitalElements orbit = body.orbit;
double semiMinorAxis = orbit.SemiMinorAxis();
//double moveOffset = (orbit.apoapsis + orbit.periapsis) / 2 - orbit.periapsis;
float timer = Mathf.PI * 2;
for (float t = 0; t < timer; t++)
{
double posX = orbit.semiMajorAxis * Mathd.Cos(timer) /* - moveOffset*/;
double posZ = semiMinorAxis * Mathd.Sin(timer);
double posY = 0; // ?
double nPosX = posX * Mathd.Cos(orbit.inclination) - posY * Mathd.Sin(orbit.inclination);
double nPosY = posY * Mathd.Cos(orbit.inclination) + posX * Mathd.Sin(orbit.inclination);
double nPosX2 = nPosX * Mathd.Cos(orbit.inclination) - posZ * Mathd.Sin(orbit.inclination);
double nPosZ = posZ * Mathd.Cos(orbit.inclination) + nPosX * Mathd.Sin(orbit.inclination);
PrecisePosition pointPosition = new PrecisePosition(nPosX, nPosY, nPosZ);
double pointVelocity = Mathd.Sqrt(
Mathd.Abs((g * body.mass) * ((2 / body.radius) - (1 / orbit.semiMajorAxis)))
);
orbitPoints.Add(new OrbitPoint(pointPosition, pointVelocity));
}
return(orbitPoints.ToArray());
}
public bool Intersect(Box2D other)
{
if (m_radius == 0 || other.size_ == Vector2_.zero)
{
return(false);
}
var dx = Mathd.Abs(m_x - other.x);
var dy = Mathd.Abs(m_y - other.y);
if (dx > other.width * 0.5 + m_radius || dy > other.height * 0.5 + m_radius)
{
return(false);
}
if (dx <= other.width * 0.5 || dy <= other.height * 0.5)
{
return(true);
}
var hdx = (dx - other.width * 0.5);
var hdy = (dy - other.height * 0.5);
var dsq = hdx * hdx + hdy * hdy;
return(dsq <= m_radius * m_radius);
}
/// <summary>
/// Orbit plane will be unchanged.
/// </summary>
public void MakeOrbitCircle(bool clockwise) {
if (attractor) {
#if UNITY_EDITOR
if (!Application.isPlaying) {
FindReferences();
attractor.FindReferences();
Undo.RecordObject(this, "Round orbit");
}
#endif
var dotProduct = CelestialBodyUtils.DotProduct(orbitData.orbitNormal, simControlRef.eclipticNormal); //sign of this value determines orbit orientation
if (Mathd.Abs(orbitData.orbitNormal.sqrMagnitude - 1d) > 0.5d) {
orbitData.orbitNormal = simControlRef.eclipticNormal;
}
var v = CelestialBodyUtils.CalcCircleOrbitVelocity(
attractor._position,
_position,
attractor.mass,
mass,
orbitData.orbitNormal * ( clockwise && dotProduct >= 0 || !clockwise && dotProduct < 0 ? 1 : -1 ),
simControlRef.gravitationalConstant
);
if (relativeVelocity != v) {
relativeVelocity = v;
orbitData.isDirty = true;
}
}
}
private void Update()
{
if (Mathd.Abs(SimControl.TimeScale) > 1e-6)
{
Calc();
}
ShowPredictOrbit();
}
double Abs(double x, double y, double z)
{
if (modules == null || modules.Count < 1)
{
return(0.0);
}
return(Mathd.Abs(planet.noiseModules[modules[0]].Get(x, y, z)));
}
} // findc2c3
/// <summary>
/// Determine the time of flight in physics time units (GE internal time) that it takes for the body
/// in orbit to go from position r0 to position r1 in an orbit with parameter p.
///
/// The angle between two 3D vectors cannot be greater than 180 degrees unless the orientation of the
/// plane they define is also specified. The normal parameter is used for this. If an angle less than
/// 180 is desired then the cross product of r0 and v0 can be used as the normal.
/// Calling TOF via the OrbitUniversal wrapper will handle all that automatically.
///
/// </summary>
/// <param name="r0">from point (with respect to center)</param>
/// <param name="r1">to point (with respect to center)</param>
/// <param name="p">orbit semi-parameter</param>
/// <param name="mu">centerbody mass</param>
/// <param name="normal">normal to orital plane</param>
/// <returns>time to travel from r0 to r1 in GE time</returns>
public static double TimeOfFlight(Vector3d r0, Vector3d r1, double p, double mu, Vector3d normal)
{
// Vallado, Algorithm 11, p126
double tof = 0;
double r0r1 = r0.magnitude * r1.magnitude;
double cos_dnu = Vector3d.Dot(r0, r1) / r0r1;
double sin_dnu = Vector3d.Cross(r0, r1).magnitude / r0r1;
// use the normal to determine if angle is > 180
if (Vector3d.Dot(Vector3d.Cross(r0, r1), normal) < 0.0)
{
sin_dnu *= -1.0;
}
// GE - precision issue at 180 degrees. Simply return 1/2 the orbit period.
if (Mathd.Abs(1.0 + cos_dnu) < 1E-5)
{
double a180 = 0.5f * (r0.magnitude + r1.magnitude);
return(Mathd.Sqrt(a180 * a180 * a180 / mu) * Mathd.PI);
}
// sin_nu: Need to use direction of flight to pick sign per Algorithm 53
double k = r0r1 * (1.0 - cos_dnu);
double l = r0.magnitude + r1.magnitude;
double m = r0r1 * (1 + cos_dnu);
double a = (m * k * p) / ((2.0 * m - l * l) * p * p + 2.0 * k * l * p - k * k);
double f = 1.0 - (r1.magnitude / p) * (1.0 - cos_dnu);
double g = r0r1 * sin_dnu / (Mathd.Sqrt(mu * p));
double alpha = 1 / a;
if (alpha > 1E-7)
{
// ellipse
double delta_nu = Mathd.Atan2(sin_dnu, cos_dnu);
double fdot = Mathd.Sqrt(mu / p) * Mathd.Tan(0.5 * delta_nu) *
((1 - cos_dnu) / p - (1 / r0.magnitude) - (1.0 / r1.magnitude));
double cos_deltaE = 1 - r0.magnitude / a * (1.0 - f);
double sin_deltaE = -r0r1 * fdot / (Mathd.Sqrt(mu * a));
double deltaE = Mathd.Atan2(sin_deltaE, cos_deltaE);
tof = g + Mathd.Sqrt(a * a * a / mu) * (deltaE - sin_deltaE);
}
else if (alpha < -1E-7)
{
// hyperbola
double cosh_deltaH = 1.0 + (f - 1.0) * r0.magnitude / a;
double deltaH = GEMath.Acosh(cosh_deltaH);
tof = g + Mathd.Sqrt(-a * a * a / mu) * (GEMath.Sinh(deltaH) - deltaH);
}
else
{
// parabola
double c = Mathd.Sqrt(r0.magnitude * r0.magnitude + r1.magnitude * r1.magnitude - 2.0 * r0r1 * cos_dnu);
double s = 0.5 * (r0.magnitude + r1.magnitude + c);
tof = 2 / 3 * Mathd.Sqrt(s * s * s / (2.0 * mu)) * (1 - Mathd.Pow(((s - c) / s), 1.5));
}
return(tof);
}
public static double MoveTowards(double current, double target, double maxDelta)
{
if (Mathd.Abs(target - current) <= maxDelta)
{
return(target);
}
else
{
return(current + Mathd.Sign(target - current) * maxDelta);
}
}
public static Vector3 GetRayPlaneIntersectionPoint(Vector3 pointOnPlane, Vector3 normal, Vector3 rayOrigin, Vector3 rayDirection)
{
var dotProd = CelestialBodyUtils.DotProduct(rayDirection, normal);
if (Mathd.Abs(dotProd) < 1e-5)
{
return(new Vector3());
}
var p = rayOrigin + rayDirection * CelestialBodyUtils.DotProduct((pointOnPlane - rayOrigin), normal) / dotProd;
p = p - normal * CelestialBodyUtils.DotProduct(p - pointOnPlane, normal); //projection. for better precision
return(p);
}
/// <summary>
/// Cotangent function.
///
/// Adapted from Vallado's astmath CPP functions.
///
/// </summary>
public static double Cot(double x)
{
double temp = Mathd.Tan(x);
if (Mathd.Abs(temp) < 1E-8)
{
return(double.NaN);
}
else
{
return(1.0 / temp);
}
}
} // lambhodograph
private static double KBattin
(
double v
)
{
double[] d = new double[21]
{
1.0 / 3.0, 4.0 / 27.0,
8.0 / 27.0, 2.0 / 9.0,
22.0 / 81.0, 208.0 / 891.0,
340.0 / 1287.0, 418.0 / 1755.0,
598.0 / 2295.0, 700.0 / 2907.0,
928.0 / 3591.0, 1054.0 / 4347.0,
1330.0 / 5175.0, 1480.0 / 6075.0,
1804.0 / 7047.0, 1978.0 / 8091.0,
2350.0 / 9207.0, 2548.0 / 10395.0,
2968.0 / 11655.0, 3190.0 / 12987.0,
3658.0 / 14391.0
};
double del, delold, term, termold, sum1;
int i;
/* ---- process forwards ---- */
sum1 = d[0];
delold = 1.0;
termold = d[0];
i = 1;
while ((i <= 20) && (Mathd.Abs(termold) > 0.00000001))
{
del = 1.0 / (1.0 - d[i] * v * delold);
term = termold * (del - 1.0);
sum1 = sum1 + term;
i++;
delold = del;
termold = term;
}
//return sum1;
int ktr = 20;
double sum2 = 0.0;
double term2 = 1.0 + d[ktr] * v;
for (i = 1; i <= ktr - 1; i++)
{
sum2 = d[ktr - i] * v / term2;
term2 = 1.0 + sum2;
}
return(d[0] / term2);
} // double kbattin
public override void OnPointerUp(PointerEventData eventData)
{
base.OnPointerUp(eventData);
if (SnapInMiddle)
{
if (Mathd.Abs(m_Value) < SnapToZeroTreshold)
{
value = 0f;
RefreshFillAreas();
}
}
if (SimulationControl.Instance != null)
{
SimulationControl.Instance.TimeScale = value;
}
}
public override void OnPointerUp(PointerEventData eventData)
{
base.OnPointerUp(eventData);
if (snapInMiddle)
{
if (Mathd.Abs(m_Value) < 0.1f)
{
value = 0f;
RefreshFillAreas();
}
}
if (simControl != null)
{
simControl.timeScale = value;
}
}
public void GenerateObjects()
{
List <string> curCollisions = player.GetComponent <Collision>().getCurrentCollisions();
Vector2d playerLoc = locman.GetComponent <PositionWithLocationProvider>().getCurrentLoc();
foreach (var name in curCollisions)
{
Vector2d objLoc = obman.GetComponent <ObjectManager>().getObjectCoords(name);
Debug.Log("objloc: " + objLoc);
Debug.Log("playerloc: " + playerLoc);
double LatDistance = Mathd.Abs(Mathd.Abs(objLoc.x) - Mathd.Abs(playerLoc.x)) * 111000;
double LongDistance = Mathd.Abs(Mathd.Abs(objLoc.y) - Mathd.Abs(playerLoc.y)) * (Mathd.Cos(objLoc.x) * 111000);
Debug.Log("LR: " + LongDistance + " UD: " + LatDistance);
double newPosX, newPosY;
if (objLoc.x < playerLoc.x)
{
Debug.Log("Spawning South");
Debug.Log("Obj Loc x: " + objLoc.x + " Player Loc x: " + playerLoc.x);
newPosY = ARCam.transform.position.z - (LatDistance);
}
else
{
Debug.Log("Spawning North");
Debug.Log("Obj Loc x: " + objLoc.x + " Player Loc x: " + playerLoc.x);
newPosY = ARCam.transform.position.z + (LatDistance);
}
if (objLoc.y < playerLoc.y)
{
Debug.Log("Spawning West");
Debug.Log("Obj Loc x: " + objLoc.y + " Player Loc x: " + playerLoc.y);
newPosX = ARCam.transform.position.x - (LongDistance);
}
else
{
Debug.Log("Spawning East");
Debug.Log("Obj Loc x: " + objLoc.y + " Player Loc x: " + playerLoc.y);
newPosX = ARCam.transform.position.x + (LongDistance);
}
GameObject plsWork = Instantiate(prefab, new Vector3((float)newPosX, ARCam.transform.position.y, (float)newPosY), Quaternion.identity);
plsWork.name = name;
plsWork.transform.parent = this.transform;
}
var newRotation = Quaternion.Euler(0, -Input.compass.trueHeading, 0);
this.transform.rotation = newRotation;
}
} // coe2rvMirror
public static double GetPhaseFromOE(OrbitUtils.OrbitElements oe)
{
if (oe.ecc < small)
{
// ---------------- circular equatorial ------------------
if ((oe.incl < small) | (Mathd.Abs(oe.incl - Mathd.PI) < small))
{
return(oe.truelon);
}
else
{
// -------------- circular inclined ------------------
return(oe.arglat);
}
}
return(oe.nu);
}
// Update is called once per frame
void Update()
{
if (updateLocationTry == false)
{
StartCoroutine(UpdateLocation());
}
//text onscreen
logLabel.GetComponent <Text>().text = log;
gpstext = "Lat: " + latitude.ToString() + "\n" + "Lon: " + longtitude.ToString() + "\n" + "Acc: " + horizontalAcc.ToString() + "\n" + "Alt: " + altitude.ToString() + "\n" + "Time: " + timeStamp.ToString() + "\n" + "DistBtw: " + distanceTraveledBetw.ToString() + "\n" + "Angle: " + angleInBetween.ToString() + "\n" + "DistTarget: " + distanceToTarget.ToString() + "\n" + "Throttle: " + throttle.ToString();
gpsLabel.GetComponent <Text> ().text = gpstext;
//rotating cursor for navigation
cursor.GetComponent <RectTransform>().localRotation = Quaternion.Euler(0.0F, 0.0F, -1.0F * (float)angleInBetween + 180.0F);
//showing big angle
angleText.GetComponent <Text> ().text = Mathd.RoundToInt(Mathd.Abs(angleInBetween)).ToString();
//throttlebar
throttleBar.GetComponent <Image>().fillAmount = throttle;
}
public static string DisplayDistanceMeasure(double measure, int decimals = 2)
{
double m = Mathd.Abs(measure);
int u = 0;
if (m < 1e3d)
{
u = 0;
}
else if (m < 1e6d)
{
measure *= 1e-3d;
u = 1;
}
else if (m < 1e9d)
{
measure *= 1e-6d;
u = 2;
}
else if (m < LightSpeed)
{
measure *= 1e-9d;
u = 3;
}
else if (m < LightDay)
{
measure *= InvLightSpeed;
u = 4;
}
else if (m < LightYear)
{
measure *= InvLightDay;
u = 5;
}
else
{
measure *= InvLightYear;
u = 6;
}
return(measure.ToString("f" + decimals) + distUnits[u]);
}
private void CompareOrbits(OrbitUniversal orbit1, OrbitUniversal orbit2, double precision)
{
if (Mathd.Abs(orbit1.p - orbit2.p) > precision)
{
Debug.LogFormat("** FAIL ** p compare failed {0} vs {1}", orbit1.p, orbit2.p);
Debug.LogFormat("** orbit1={0}\n** orbit2={1}", orbit1.DumpInfo(), orbit2.DumpInfo());
Assert.Fail();
}
if (Mathd.Abs(orbit1.eccentricity - orbit2.eccentricity) > precision)
{
Debug.LogFormat("** FAIL ** ecc compare failed {0} vs {1}", orbit1.eccentricity, orbit2.eccentricity);
Debug.LogFormat("** orbit1={0}\n** orbit2={1}", orbit1.DumpInfo(), orbit2.DumpInfo());
Assert.Fail();
}
if (Mathd.Abs(orbit1.inclination - orbit2.inclination) > precision)
{
Debug.LogFormat("** FAIL ** incl compare failed {0} vs {1}", orbit1.inclination, orbit2.inclination);
Debug.LogFormat("** orbit1={0}\n** orbit2={1}", orbit1.DumpInfo(), orbit2.DumpInfo());
Assert.Fail();
}
double diff = orbit1.omega_lc - orbit2.omega_lc;
if ((Mathd.Abs(diff) > precision) &&
(Mathd.Abs(diff - 360.0) > precision) &&
(Mathd.Abs(diff + 360.0) > precision))
{
Debug.LogFormat("** FAIL ** Omega LC compare failed {0} vs {1}", orbit1.omega_lc, orbit2.omega_lc);
Debug.LogFormat("** orbit1={0}\n** orbit2={1}", orbit1.DumpInfo(), orbit2.DumpInfo());
Assert.Fail();
}
diff = orbit1.omega_uc - orbit2.omega_uc;
if ((Mathd.Abs(diff) > precision) &&
(Mathd.Abs(diff - 360.0) > precision) &&
(Mathd.Abs(diff + 360.0) > precision))
{
Debug.LogFormat("** FAIL ** omega UC compare failed {0} vs {1}", orbit1.omega_uc, orbit2.omega_uc);
Debug.LogFormat("** orbit1={0}\n** orbit2={1}", orbit1.DumpInfo(), orbit2.DumpInfo());
Assert.Fail();
}
}
double Billow(double x, double y, double z)
{
double value = 0.0, signal;
x *= frequency;
y *= frequency;
z *= frequency;
for (uint i = 0; i < octaves; ++i)
{
signal = sources[i].Get(x, y, z);
signal = 2.0 * Mathd.Abs(signal) - 1.0;
value += signal * exparray[i];
x *= lacunarity;
y *= lacunarity;
z *= lacunarity;
}
value += 0.5;
return(value * correction[octaves - 1, 0] + correction[octaves - 1, 1]);
}
public static string DisplaySpeedMeasure(double measure, int decimals = 2)
{
double m = Mathd.Abs(measure);
int u = 0;
if (m < 1e3d)
{
u = 0; // m
}
else if (m < 1e6d)
{
measure *= 1e-3d;
u = 1; // km
}
else
{
measure *= InvLightSpeed;
u = 2; // c
}
return(measure.ToString("f" + decimals) + spdUnits[u]);
}
double Ridged(double x, double y, double z)
{
double result = 0.0, signal;
x *= frequency;
y *= frequency;
z *= frequency;
for (uint i = 0; i < octaves; ++i)
{
signal = sources[i].Get(x, y, z);
signal = offst - Mathd.Abs(signal);
signal *= signal;
result += signal * exparray[i];
x *= lacunarity;
y *= lacunarity;
z *= lacunarity;
}
return(result * correction[octaves - 1, 0] + correction[octaves - 1, 1]);
}
/// <summary>
/// Sets the eccentricity and updates all corresponding orbit state values.
/// </summary>
/// <param name="e">The new eccentricity value.</param>
/// <remarks>
/// Mean anomaly will try to preserve.
/// </remarks>
public void SetEccentricity(double e)
{
if (!IsValidOrbit)
{
return;
}
e = Mathd.Abs(e);
var _periapsis = PeriapsisDistance; // Periapsis remains constant
Eccentricity = e;
var compresion = Eccentricity < 1 ? (1 - Eccentricity * Eccentricity) : (Eccentricity * Eccentricity - 1);
SemiMajorAxis = Math.Abs(_periapsis / (1 - Eccentricity));
FocalParameter = SemiMajorAxis * compresion;
SemiMinorAxis = SemiMajorAxis * Mathd.Sqrt(compresion);
CenterPoint = SemiMajorAxis * Math.Abs(Eccentricity) * SemiMajorAxisBasis;
if (Eccentricity < 1)
{
EccentricAnomaly = CelestialBodyUtils.KeplerSolver(MeanAnomaly, Eccentricity);
var cosE = Math.Cos(EccentricAnomaly);
TrueAnomaly = Math.Acos((cosE - Eccentricity) / (1 - Eccentricity * cosE));
if (MeanAnomaly > Mathd.PI)
{
TrueAnomaly = Mathd.PI_2 - TrueAnomaly;
}
}
else
{
EccentricAnomaly = CelestialBodyUtils.KeplerSolverHyperbolicCase(MeanAnomaly, Eccentricity);
TrueAnomaly = Math.Atan2(Math.Sqrt(Eccentricity * Eccentricity - 1) * Math.Sinh(EccentricAnomaly), Eccentricity - Math.Cosh(EccentricAnomaly));
}
SetVelocityByCurrentAnomaly();
SetPositionByCurrentAnomaly();
CalculateNewOrbitData();
}
public void SetEccentricity(double e)
{
if (!isValidOrbit)
{
return;
}
e = Mathd.Abs(e);
var _periapsis = periapsisDistance; // Periapsis remains constant
eccentricity = e;
var compresion = eccentricity < 1 ? (1 - eccentricity * eccentricity) : (eccentricity * eccentricity - 1);
semiMajorAxis = System.Math.Abs(_periapsis / (1 - eccentricity));
focalParameter = semiMajorAxis * compresion;
semiMinorAxis = semiMajorAxis * Mathd.Sqrt(compresion);
centerPoint = semiMajorAxis * System.Math.Abs(eccentricity) * semiMajorAxisBasis;
if (eccentricity < 1)
{
eccentricAnomaly = CelestialBodyUtils.KeplerSolver(meanAnomaly, eccentricity);
var cosE = System.Math.Cos(eccentricAnomaly);
trueAnomaly = System.Math.Acos((cosE - eccentricity) / (1 - eccentricity * cosE));
if (meanAnomaly > Mathd.PI)
{
trueAnomaly = Mathd.PI_2 - trueAnomaly;
}
}
else
{
eccentricAnomaly = CelestialBodyUtils.KeplerSolverHyperbolicCase(meanAnomaly, eccentricity);
trueAnomaly = System.Math.Atan2(System.Math.Sqrt(eccentricity * eccentricity - 1) * System.Math.Sinh(eccentricAnomaly), eccentricity - System.Math.Cosh(eccentricAnomaly));
}
SetVelocityByCurrentAnomaly();
SetPositionByCurrentAnomaly();
CalculateNewOrbitData();
}
public CircularInclinationAndAN(OrbitData fromOrbit, OrbitData toOrbit) : base(fromOrbit, toOrbit)
{
name = "Circular Change Inclination and Ascending Node";
// check the orbits are circular and have the same radius
if (fromOrbit.ecc > GEConst.small)
{
Debug.LogWarning("fromOrbit is not circular. ecc=" + fromOrbit.ecc);
return;
}
if (toOrbit.ecc > GEConst.small)
{
Debug.LogWarning("toOrbit is not circular. ecc=" + toOrbit.ecc);
return;
}
if (Mathf.Abs(fromOrbit.a - toOrbit.a) > GEConst.small)
{
Debug.LogWarning("Orbits do not have the same radius delta=" + Mathf.Abs(fromOrbit.a - toOrbit.a));
return;
}
double dOmega = (toOrbit.omega_uc - fromOrbit.omega_uc) * Mathd.Deg2Rad;
double i_initial = fromOrbit.inclination * Mathd.Deg2Rad;
double i_final = toOrbit.inclination * Mathd.Deg2Rad;
// u_initial = omega_lc + nu (i.e. phase of circular orbit)
// eqn (6-25)
double cos_theta = Mathd.Cos(i_initial) * Mathd.Cos(i_final) +
Mathd.Sin(i_initial) * Mathd.Sin(i_final) * Mathd.Cos(dOmega);
// Quadrant check
double theta = Mathd.Acos(Mathd.Clamp(cos_theta, -1.0, 1.0));
if (dOmega < 0)
{
theta = -theta;
}
// u_initial: phase of intersection in the initial orbit
double numer = Mathd.Sin(i_final) * Mathd.Cos(dOmega) - cos_theta * Mathd.Sin(i_initial);
double denom = Mathd.Sin(theta) * Mathd.Cos(i_initial);
if (Mathd.Abs(denom) < 1E-6)
{
Debug.LogError("u_initial: about to divide by zero (small theta)");
// return;
}
double u_initial = Mathd.Acos(Mathd.Clamp(numer / denom, -1.0, 1.0));
// u_final: phase of intersection in the final orbit
numer = Mathd.Cos(i_initial) * Mathd.Sin(i_final) - Mathd.Sin(i_initial) * Mathd.Cos(i_final) * Mathd.Cos(dOmega);
if (Mathd.Abs(Mathd.Sin(theta)) < 1E-6)
{
Debug.LogError("u_final: about to divide by zero (small theta)");
return;
}
double u_final = Mathd.Acos(Mathd.Clamp(numer / Mathd.Sin(theta), -1.0, 1.0));
double u_initialDeg = u_initial * Mathd.Rad2Deg;
double u_finalDeg = u_final * Mathd.Rad2Deg;
// Orbits cross at two places, pick the location closest to the current position of the fromOrbit
double time_to_crossing = fromOrbit.period * (u_initialDeg - fromOrbit.phase) / 360f;
if (time_to_crossing < 0)
{
u_initialDeg += 180f;
u_finalDeg += 180f;
time_to_crossing += 0.5f * fromOrbit.period;
}
// Determine velocity change required
Vector3 dV = toOrbit.GetPhysicsVelocityForEllipse((float)u_finalDeg) -
fromOrbit.GetPhysicsVelocityForEllipse((float)u_initialDeg);
// Create a maneuver object
Maneuver m = new Maneuver();
m.physPosition = new Vector3d(fromOrbit.GetPhysicsPositionforEllipse((float)(u_initialDeg)));
m.mtype = Maneuver.Mtype.vector;
m.dV = dV.magnitude;
m.velChange = dV;
m.worldTime = GravityEngine.instance.GetPhysicalTime() + (float)time_to_crossing;
m.nbody = fromOrbit.nbody;
maneuvers.Add(m);
//Debug.LogFormat("u_initial = {0} u_final={1} (deg) dOmega={2} (deg) timeToCrossing={3} fromPhase={4} cos_theta={5} theta={6}",
// u_initialDeg,
// u_finalDeg,
// dOmega * Mathd.Rad2Deg,
// time_to_crossing,
// fromOrbit.phase,
// cos_theta,
// theta);
}
public static bool Cross(Box2D box, Box2D other, Vector2_ tp, ref Vector2_ hit)
{
if (box.Intersect(other))
{
return(true);
}
var o = box.center_;
tp -= o;
other.Set(other.center_ - o, other.size_ + box.size_);
box.SetCenter(Vector2_.zero);
double minx = Mathd.Min(0, tp.x), maxx = Mathd.Max(0, tp.x), miny = Mathd.Min(0, tp.y), maxy = Mathd.Max(0, tp.y);
var l1 = Mathd.Abs(other.leftEdge) < Mathd.Abs(other.rightEdge) ? other.leftEdge : other.rightEdge;
var l2 = Mathd.Abs(other.topEdge) < Mathd.Abs(other.bottomEdge) ? other.topEdge : other.bottomEdge;
bool b1 = false, b2 = false;
if (other.leftEdge < 0 && other.rightEdge > 0)
{
goto _l2;
}
_l1:
b1 = true;
if (l1 > minx && l1 < maxx)
{
var yy = tp.y / tp.x * l1;
if (yy > other.bottomEdge && yy < other.topEdge)
{
hit.Set(l1, yy);
hit += o;
return(true);
}
}
if (b2)
{
return(false);
}
_l2:
b2 = true;
if (l2 > miny && l2 < maxy)
{
var xx = l2 * tp.x / tp.y;
if (xx > other.leftEdge && xx < other.rightEdge)
{
hit.Set(xx, l2);
hit += o;
return(true);
}
}
if (!b1)
{
goto _l1;
}
return(false);
}
private void OnInputChanged(InputKey[] changedKeys, int count)
{
if (!current)
{
return;
}
// Inputs bitmask:The first 20 bits contains 4 input id. every 5 bit stored one input id, 20-29 contains hold input, last 2 bits: left and right movement state
var ns = m_state;
//var n = $"({ns.BitMask(24)},{ns >> 20 & 0x0F}|{ns.BitMask(29)},{ns >> 25 & 0x0F}),";
//for (var pp = 0; pp < MAX_KEY_CACHE_COUNT; ++pp)
//{
// var k = (m_state >> KEY_BITS * pp) & 0x1F;
// n += "[" + k + "], ";
//}
//Logger.LogDetail("nidx: {0}, keys: {1}", m_inputIndex, n);
for (var i = 0; i < count; ++i)
{
var key = changedKeys[i];
if (key.value == KEY_LEFT || key.value == KEY_RIGHT) // Movement key
{
ns = ns.BitMask(key.value, key.fired);
continue;
}
if (m_disabledKeys.BitMask(key.ID))
{
continue; // Key disabled
}
if (key.ID < 1 || key.ID > 0x1F)
{
continue; // Valid input id: [1 - 31]
}
if (Mathd.Abs(key.value) < 1 || Mathd.Abs(key.value) > 0x1D)
{
continue; // Valid key value: [1 - 29]
}
if (key.isInstant && (m_inputIndex >= MAX_KEY_CACHE_COUNT || !key.fired))
{
continue;
}
if (key.keyFired && !OnValidateKeyboard(key))
{
continue;
}
var val = key.ID;
if (!key.isInstant)
{
val = val.BitMask(4, true);
var hold = ns >> INSTANT_KEY_BITS & 0x3FF;
int h1 = hold & 0x1F, h2 = hold >> 5 & 0x1F;
if (!key.fired)
{
if (val != h1 && val != h2)
{
continue;
}
h1 = val == h1 ? h2 : h1;
h2 = 0;
}
else
{
h2 = h1;
h1 = val;
}
hold = h1 == h2 ? h1 : h1 | h2 << 5;
ns = ns & ~0x3FF00000 | hold << INSTANT_KEY_BITS;
}
else
{
ns |= val << m_inputIndex * KEY_BITS;
++m_inputIndex;
}
}
//n = $"({ns.BitMask(24)},{ns >> 20 & 0x0F}|{ns.BitMask(29)},{ns >> 25 & 0x0F}),";
//for (var pp = 0; pp < MAX_KEY_CACHE_COUNT; ++pp)
//{
// var k = (m_state >> KEY_BITS * pp) & 0x0F;
// n += "[" + k + "], ";
//}
//Logger.LogDetail("nidx: {0}, keys: {1}", m_inputIndex, n);
UpdateState(ns);
}
/* -----------------------------------------------------------------------------
*
* function newtonnu
*
* this function solves keplers equation when the true anomaly is known.
* the mean and eccentric, parabolic, or hyperbolic anomaly is also found.
* the parabolic limit at 168ø is arbitrary. the hyperbolic anomaly is also
* limited. the hyperbolic sine is used because it's not double valued.
*
* author : david vallado 719-573-2600 27 may 2002
*
* revisions
* vallado - fix small 24 sep 2002
*
* inputs description range / units
* ecc - eccentricity 0.0 to
* nu - true anomaly -2pi to 2pi rad
*
* outputs :
* e0 - eccentric anomaly 0.0 to 2pi rad 153.02 deg
* m - mean anomaly 0.0 to 2pi rad 151.7425 deg
*
* locals :
* e1 - eccentric anomaly, next value rad
* sine - sine of e
* cose - cosine of e
* ktr - index
*
* coupling :
* arcsinh - arc hyperbolic sine
* sinh - hyperbolic sine
*
* references :
* vallado 2013, 77, alg 5
* --------------------------------------------------------------------------- */
private static void NewtonNu(OrbitElements oe)
{
double small, sine, cose, cosnu, temp;
double ecc = oe.ecc;
double nu = oe.nu;
double e0, m;
// --------------------- implementation ---------------------
e0 = 999999.9;
m = 999999.9;
small = 0.00000001;
// --------------------------- circular ------------------------
if (Mathd.Abs(ecc) < small)
{
m = nu;
e0 = nu;
}
else
// ---------------------- elliptical -----------------------
if (ecc < 1.0 - small)
{
cosnu = Mathd.Cos(nu);
temp = 1.0 / (1.0 + ecc * cosnu);
sine = (Mathd.Sqrt(1.0 - ecc * ecc) * Mathd.Sin(nu)) * temp;
cose = (ecc + cosnu) * temp;
e0 = Mathd.Atan2(sine, cose);
m = e0 - ecc * Mathd.Sin(e0);
}
else
// -------------------- hyperbolic --------------------
if (ecc > 1.0 + small)
{
if ((ecc > 1.0) && (Mathd.Abs(nu) + 0.00001 < Mathd.PI - Mathd.Acos(Mathd.Clamp(1.0 / ecc, -1.0, 1.0))))
{
sine = (Mathd.Sqrt(ecc * ecc - 1.0) * Mathd.Sin(nu)) / (1.0 + ecc * Mathd.Cos(nu));
e0 = GEMath.Asinh(sine);
m = ecc * GEMath.Sinh(e0) - e0;
}
}
else
// ----------------- parabolic ---------------------
if (Mathd.Acos(Mathd.Clamp(nu, -1.0, 1.0)) < 168.0 * Mathd.PI / 180.0)
{
e0 = Mathd.Tan(nu * 0.5);
m = e0 + (e0 * e0 * e0) / 3.0;
}
if (ecc < 1.0)
{
m = System.Math.Truncate(m / (2.0 * Mathd.PI));
if (m < 0.0)
{
m = m + 2.0 * Mathd.PI;
}
e0 = System.Math.Truncate(e0 / (2.0 * Mathd.PI));
}
oe.m = m;
oe.eccanom = e0;
} // newtonnu
