private bool MouseOverVessel(Vessel vessel) { bool result = false; var patchRenderer = vessel.patchedConicRenderer; if (patchRenderer == null || patchRenderer.solver == null) { return(result); } var patches = patchRenderer.solver.maneuverNodes.Any() ? patchRenderer.flightPlanRenders : patchRenderer.patchRenders; if (patches == null) { return(result); } PatchedConics.PatchCastHit hit = default(PatchedConics.PatchCastHit); if (PatchedConics.ScreenCast(Input.mousePosition, patches, out hit)) { result = true; _hitOrbit = hit.pr.patch; _hitScreenPoint = hit.GetScreenSpacePoint(); _hitUT = hit.UTatTA; } return(result); }
private Orbit createOrbitFromState(VesselState state) { var orbit = new Orbit(); orbit.UpdateFromStateVectors(Util.SwapYZ(state.position), Util.SwapYZ(state.velocity), state.referenceBody, state.time); var pars = new PatchedConics.SolverParameters(); pars.FollowManeuvers = false; PatchedConics.CalculatePatch(orbit, new Orbit(), state.time, pars, null); return(orbit); }
private static Orbit CreateOrbitFromState(VesselState state) { Orbit orbit = new Orbit(); orbit.UpdateFromStateVectors(state.Position.SwapYZ(), state.Velocity.SwapYZ(), state.ReferenceBody, state.Time); PatchedConics.SolverParameters pars = new PatchedConics.SolverParameters { FollowManeuvers = false }; PatchedConics.CalculatePatch(orbit, new Orbit(), state.Time, pars, null); return(orbit); }
// calculate the next patch, which makes patchEndTransition be valid // public static Orbit CalculateNextOrbit(this Orbit o, double UT = Double.NegativeInfinity) { PatchedConics.SolverParameters solverParameters = new PatchedConics.SolverParameters(); // hack up a dynamic default value to the current time if (UT == Double.NegativeInfinity) { UT = Planetarium.GetUniversalTime(); } o.StartUT = UT; o.EndUT = o.eccentricity >= 1.0 ? o.period : UT + o.period; Orbit nextOrbit = new Orbit(); PatchedConics.CalculatePatch(o, nextOrbit, UT, solverParameters, null); return(nextOrbit); }
static void DistanceToTarget(double[] x, double[] fi, object obj) { OptimizerData data = (OptimizerData)obj; double t = x[3]; Vector3d DV = new Vector3d(x[0], x[1], x[2]); Orbit orbit = new Orbit(); orbit.UpdateFromStateVectors(data.initial_orbit.getRelativePositionAtUT(t), data.initial_orbit.getOrbitalVelocityAtUT(t) + DV.xzy, data.initial_orbit.referenceBody, t); orbit.StartUT = t; var pars = new PatchedConics.SolverParameters(); Vector3d pos; while (true) { Orbit next_orbit = new Orbit(); PatchedConics.CalculatePatch(orbit, next_orbit, orbit.StartUT, pars, null); if (orbit.EndUT > x[4]) { pos = orbit.getTruePositionAtUT(x[4]); Vector3d err = pos - data.target_orbit.getTruePositionAtUT(x[4]); /* this needs to be components of err and cannot simply be err.magnitude */ fi[0] = err.x; fi[1] = err.y; fi[2] = err.z; /* similarly here, and the 10,000x fudge factor was experimentally determined */ fi[3] = DV.x * 10000.0; fi[4] = DV.y * 10000.0; fi[5] = DV.z * 10000.0; return; } // As of 0.25 CalculatePatch fails if the orbit does not change SoI if (next_orbit.referenceBody == null) { next_orbit.UpdateFromOrbitAtUT(orbit, orbit.StartUT + orbit.period, orbit.referenceBody); } orbit = next_orbit; } }
static void DistanceToTarget(double[] x, double[] fi, object obj) { OptimizerData data = (OptimizerData)obj; double t = data.original_UT + x[3]; Vector3d DV = new Vector3d(x[0], x[1], x[2]); Orbit orbit = new Orbit(); orbit.UpdateFromStateVectors(data.initial_orbit.getRelativePositionAtUT(t), data.initial_orbit.getOrbitalVelocityAtUT(t) + DV.xzy, data.initial_orbit.referenceBody, t); orbit.StartUT = t; var pars = new PatchedConics.SolverParameters(); Vector3d pos; while (true) { Orbit next_orbit = new Orbit(); PatchedConics.CalculatePatch(orbit, next_orbit, orbit.StartUT, pars, null); if (orbit.EndUT > data.UT_arrival) { pos = orbit.getTruePositionAtUT(data.UT_arrival); Vector3d err = pos - data.pos_arrival; fi[0] = err.x; fi[1] = err.y; fi[2] = err.z; return; } // As of 0.25 CalculatePatch fails if the orbit does not change SoI if (next_orbit.referenceBody == null) { next_orbit.UpdateFromOrbitAtUT(orbit, orbit.StartUT + orbit.period, orbit.referenceBody); } orbit = next_orbit; } }
private static bool GetClosestVessel(Orbit refP, Orbit tgtP) { if (refP == null || tgtP == null) { return(false); } if (refP.referenceBody != tgtP.referenceBody) { return(false); } if (!Orbit.PeApIntersects(refP, tgtP, 20000)) { return(false); } double d1 = 0; double d2 = 0; double dT1 = 0; double d4 = 0; double dT2 = 0; double d6 = 0; int i1 = 0; int intersects = Orbit.FindClosestPoints(refP, tgtP, ref d1, ref d2, ref dT1, ref d4, ref dT2, ref d6, 0.001, 10, ref i1); double UT1 = refP.StartUT + refP.GetDTforTrueAnomaly(dT1, 0); double UT2 = refP.StartUT + refP.GetDTforTrueAnomaly(dT2, 0); if (intersects > 1) { double dist1 = double.MaxValue; if (PatchedConics.TAIsWithinPatchBounds(UT1, refP)) { Vector3d refClosest1 = refP.getRelativePositionAtUT(UT1); Vector3d tgtClosest1 = tgtP.getRelativePositionAtUT(UT1); dist1 = (refClosest1 - tgtClosest1).magnitude; } double dist2 = double.MaxValue; if (PatchedConics.TAIsWithinPatchBounds(UT2, refP)) { Vector3d refClosest2 = refP.getRelativePositionAtUT(UT2); Vector3d tgtClosest2 = tgtP.getRelativePositionAtUT(UT2); dist2 = (refClosest2 - tgtClosest2).magnitude; } if (dist1 > double.MaxValue - 1000 && dist2 > double.MaxValue - 1000) { return(false); } bool first = dist1 < dist2; if (first) { _closestDist = dist1; _closestTime = UT1; Vector3d refVel = refP.getOrbitalVelocityAtUT(UT1); Vector3d tgtVel = tgtP.getOrbitalVelocityAtUT(UT1); _closestRelVel = (refVel - tgtVel).magnitude; } else { _closestDist = dist2; _closestTime = UT2; Vector3d refVel = refP.getOrbitalVelocityAtUT(UT2); Vector3d tgtVel = tgtP.getOrbitalVelocityAtUT(UT2); _closestRelVel = (refVel - tgtVel).magnitude; } return(true); } else { if (!PatchedConics.TAIsWithinPatchBounds(UT1, refP)) { UT1 = double.MaxValue; } if (!PatchedConics.TAIsWithinPatchBounds(UT2, refP)) { UT2 = double.MaxValue; } if (UT1 > double.MaxValue - 1000 && UT2 > double.MaxValue - 1000) { return(false); } double useUT = UT1 < UT2 ? UT1 : UT2; Vector3d refClosest = refP.getRelativePositionAtUT(useUT); Vector3d tgtClosest = tgtP.getRelativePositionAtUT(useUT); Vector3d refVel = refP.getOrbitalVelocityAtUT(useUT); Vector3d tgtVel = tgtP.getOrbitalVelocityAtUT(useUT); _closestDist = (refClosest - tgtClosest).magnitude; _closestRelVel = (refVel - tgtVel).magnitude; _closestTime = useUT; return(true); } }
public static bool _CalculatePatch(Orbit p, Orbit nextPatch, double startEpoch, PatchedConics.SolverParameters pars, CelestialBody targetBody) { p.activePatch = true; p.nextPatch = nextPatch; p.patchEndTransition = Orbit.PatchTransitionType.FINAL; p.closestEncounterLevel = Orbit.EncounterSolutionLevel.NONE; p.numClosePoints = 0; log_patches(p, "Patch 0"); int count = Planetarium.Orbits.Count; for (int i = 0; i < count; i++) { OrbitDriver orbitDriver = Planetarium.Orbits[i]; if (orbitDriver.orbit == p) { continue; } if (orbitDriver.celestialBody) { if (targetBody == null) { goto IL_B6; } if (orbitDriver.celestialBody == targetBody) { goto IL_B6; } IL_C5: if (orbitDriver.referenceBody == p.referenceBody) { var enc = PatchedConics.CheckEncounter(p, nextPatch, startEpoch, orbitDriver, targetBody, pars); Utils.Log("Encounter with {}: {}", orbitDriver.celestialBody.bodyName, enc); log_patches(p, "Patch"); goto IL_FA; } goto IL_FA; IL_B6: p.closestTgtApprUT = 0.0; goto IL_C5; } IL_FA :; } log_patches(p, "Patch 1"); if (p.patchEndTransition == Orbit.PatchTransitionType.FINAL) { if (!pars.debug_disableEscapeCheck) { if (p.ApR <= p.referenceBody.sphereOfInfluence) { if (p.eccentricity < 1.0) { p.UTsoi = -1.0; p.StartUT = startEpoch; p.EndUT = startEpoch + p.period; p.patchEndTransition = Orbit.PatchTransitionType.FINAL; goto IL_2C0; } } if (double.IsInfinity(p.referenceBody.sphereOfInfluence)) { p.FEVp = Math.Acos(-(1.0 / p.eccentricity)); p.SEVp = -p.FEVp; p.StartUT = startEpoch; p.EndUT = double.PositiveInfinity; p.UTsoi = double.PositiveInfinity; p.patchEndTransition = Orbit.PatchTransitionType.FINAL; } else { p.FEVp = p.TrueAnomalyAtRadius(p.referenceBody.sphereOfInfluence); p.SEVp = -p.FEVp; p.timeToTransition1 = p.GetDTforTrueAnomaly(p.FEVp, 0.0); p.timeToTransition2 = p.GetDTforTrueAnomaly(p.SEVp, 0.0); p.UTsoi = startEpoch + p.timeToTransition1; nextPatch.UpdateFromOrbitAtUT(p, p.UTsoi, p.referenceBody.referenceBody); p.StartUT = startEpoch; p.EndUT = p.UTsoi; p.patchEndTransition = Orbit.PatchTransitionType.ESCAPE; } } } IL_2C0: nextPatch.StartUT = p.EndUT; double arg_2FD_1; if (nextPatch.eccentricity < 1.0) { arg_2FD_1 = nextPatch.StartUT + nextPatch.period; } else { arg_2FD_1 = nextPatch.period; } nextPatch.EndUT = arg_2FD_1; nextPatch.patchStartTransition = p.patchEndTransition; nextPatch.previousPatch = p; log_patches(p, "Patch 2"); return(p.patchEndTransition != Orbit.PatchTransitionType.FINAL); }
public static double closestVessel(double UT, Orbit o, Orbit tgt, bool closest, double min, double max) { double appUT = 0; if (o.referenceBody != tgt.referenceBody) { return(0); } if (!Orbit.PeApIntersects(o, tgt, 20000)) { return(0); } double d1 = 0; double d2 = 0; double dT1 = 0; double d4 = 0; double dT2 = 0; double d6 = 0; int i1 = 0; int intersects = Orbit.FindClosestPoints(o, tgt, ref d1, ref d2, ref dT1, ref d4, ref dT2, ref d6, 0.001, 10, ref i1); double UT1 = o.StartUT + o.GetDTforTrueAnomaly(dT1, 0); double UT2 = o.StartUT + o.GetDTforTrueAnomaly(dT2, 0); if (intersects > 1) { double dist1 = double.MaxValue; if (PatchedConics.TAIsWithinPatchBounds(UT1, o)) { Vector3d refClosest1 = o.getRelativePositionAtUT(UT1); Vector3d tgtClosest1 = tgt.getRelativePositionAtUT(UT1); dist1 = (refClosest1 - tgtClosest1).magnitude; } double dist2 = double.MaxValue; if (PatchedConics.TAIsWithinPatchBounds(UT2, o)) { Vector3d refClosest2 = o.getRelativePositionAtUT(UT2); Vector3d tgtClosest2 = tgt.getRelativePositionAtUT(UT2); dist2 = (refClosest2 - tgtClosest2).magnitude; } if (dist1 > double.MaxValue - 1000 && dist2 > double.MaxValue - 1000) { return(0); } appUT = dist1 < dist2 ? UT1 : UT2; if (closest) { return(appUT); } if (appUT >= min && appUT <= max) { return(appUT); } appUT = dist1 < dist2 ? UT2 : UT1; if (appUT >= min && appUT <= max) { return(appUT); } } else { if (!PatchedConics.TAIsWithinPatchBounds(UT1, o)) { UT1 = double.MaxValue; } if (!PatchedConics.TAIsWithinPatchBounds(UT2, o)) { UT2 = double.MaxValue; } if (UT1 > double.MaxValue - 1000 && UT2 > double.MaxValue - 1000) { return(0); } appUT = UT1 < UT2 ? UT1 : UT2; return(appUT); } return(0); }
protected BaseTrajectory(VesselWrapper vsl, Vector3d dV, double startUT) { VSL = vsl; var dVm = (float)dV.magnitude; if (dVm > 0) { ManeuverFuel = VSL.Engines.FuelNeeded(dVm); FullManeuver = ManeuverFuel < VSL.Engines.AvailableFuelMass; ManeuverDuration = VSL.Engines.TTB_Precise(dVm); } else { ManeuverFuel = 0; ManeuverDuration = 0; FullManeuver = true; } StartUT = startUT; OrigOrbit = VSL.vessel.orbitDriver.orbit; var obt = StartOrbit; Body = obt.referenceBody; if (dVm > 0) { ManeuverDeltaV = dV; NodeDeltaV = Utils.Orbital2NodeDeltaV(obt, ManeuverDeltaV, StartUT); try { Orbit = TrajectoryCalculator.NewOrbit(obt, ManeuverDeltaV, StartUT); var prev = Orbit; for (int i = 0; i < 3; i++) { if (!PatchedConics.CalculatePatch(prev, prev.nextPatch ?? new Orbit(), prev.epoch, new PatchedConics.SolverParameters(), null)) { break; } prev = prev.nextPatch; } Body = Orbit.referenceBody; // if(Orbit.patchEndTransition != Orbit.PatchTransitionType.FINAL)//debug // { // Utils.Log("**************************************************************************************************");//debug // RendezvousAutopilot.log_patches(Orbit, "Orbit");//deubg // } } catch (ArithmeticException) { Orbit = OrigOrbit; StartUT = VSL.Physics.UT; ManeuverFuel = 0; ManeuverDuration = 0; ManeuverDeltaV = Vector3d.zero; FullManeuver = true; } } else { Orbit = OrigOrbit; } StartPos = obt.getRelativePositionAtUT(StartUT); StartVel = obt.getOrbitalVelocityAtUT(StartUT); AfterStartVel = Orbit.getOrbitalVelocityAtUT(StartUT); }
void DistanceToTarget(double[] x, double[] fi, object obj) { OptimizerData data = (OptimizerData)obj; double t = x[3]; Vector3d DV = new Vector3d(x[0], x[1], x[2]); Orbit orbit = new Orbit(); orbit.UpdateFromStateVectors(data.initial_orbit.getRelativePositionAtUT(t), data.initial_orbit.getOrbitalVelocityAtUT(t) + DV.xzy, data.initial_orbit.referenceBody, t); orbit.StartUT = t; orbit.EndUT = orbit.eccentricity >= 1.0 ? orbit.period : t + orbit.period; Orbit next_orbit = new Orbit(); PatchedConics.CalculatePatch(orbit, next_orbit, t, solverParameters, null); while (true) { if (orbit.referenceBody == data.target_body) { if (orbit.PeR < 100000) { fi[0] = DV.x; fi[1] = DV.y; fi[2] = DV.z; } else { Vector3d err = orbit.getRelativePositionFromTrueAnomaly(0); fi[0] = err.x; fi[1] = err.y; fi[2] = err.z; } data.failed = false; /* we intersected at some point with the target body SOI */ return; } else if (orbit.EndUT > data.UT_arrival) { Vector3d err; if (orbit.referenceBody == data.target_orbit.referenceBody) { err = orbit.getRelativePositionAtUT(data.UT_arrival) - data.target_orbit.getRelativePositionAtUT(data.UT_arrival); } else { err = orbit.getTruePositionAtUT(data.UT_arrival) - data.target_orbit.getTruePositionAtUT(data.UT_arrival); } fi[0] = err.x; fi[1] = err.y; fi[2] = err.z; return; } Orbit temp = orbit; orbit = next_orbit; next_orbit = temp; PatchedConics.CalculatePatch(orbit, next_orbit, orbit.StartUT, solverParameters, null); } }
public static bool CheckEncounterButDontBitchAboutIt(Orbit p, Orbit nextPatch, double startEpoch, OrbitDriver sec, CelestialBody targetBody, PatchedConics.SolverParameters pars, bool logErrors) { Orbit orbit = sec.orbit; double num = 1.1; if (GameSettings.ALWAYS_SHOW_TARGET_APPROACH_MARKERS && sec.celestialBody == targetBody) { num = Math.Sqrt(orbit.semiMajorAxis / sec.celestialBody.sphereOfInfluence); } if (!Orbit.PeApIntersects(p, orbit, sec.celestialBody.sphereOfInfluence * num)) { return(false); } if (p.closestEncounterLevel < Orbit.EncounterSolutionLevel.ORBIT_INTERSECT) { p.closestEncounterLevel = Orbit.EncounterSolutionLevel.ORBIT_INTERSECT; p.closestEncounterBody = sec.celestialBody; } double clEctr = p.ClEctr1; double clEctr2 = p.ClEctr2; double fEVp = p.FEVp; double fEVs = p.FEVs; double sEVp = p.SEVp; double sEVs = p.SEVs; int num2 = FindClosestPointsRevertedCauseNewOneSucks(p, orbit, ref clEctr, ref clEctr2, ref fEVp, ref fEVs, ref sEVp, ref sEVs, 0.0001, pars.maxGeometrySolverIterations, ref pars.GeoSolverIterations); if (num2 < 1) { return(false); } double dTforTrueAnomaly = p.GetDTforTrueAnomaly(fEVp, 0.0); double dTforTrueAnomaly2 = p.GetDTforTrueAnomaly(sEVp, 0.0); double num3 = dTforTrueAnomaly + startEpoch; double num4 = dTforTrueAnomaly2 + startEpoch; // avoid bad numbers if (double.IsInfinity(num3) && !double.IsInfinity(num4)) { num3 = num4; } if (double.IsInfinity(num4) && !double.IsInfinity(num3)) { num4 = num3; } if (double.IsInfinity(num3) && double.IsInfinity(num4)) { UnityEngine.Debug.Log("CheckEncounter: both intercept UTs are infinite"); return(false); } if ((num3 < p.StartUT || num3 > p.EndUT) && (num4 < p.StartUT || num4 > p.EndUT)) { return(false); } if (num4 < num3 || num3 < p.StartUT || num3 > p.EndUT) { UtilMath.SwapValues(ref fEVp, ref sEVp); UtilMath.SwapValues(ref fEVs, ref sEVs); UtilMath.SwapValues(ref clEctr, ref clEctr2); UtilMath.SwapValues(ref dTforTrueAnomaly, ref dTforTrueAnomaly2); UtilMath.SwapValues(ref num3, ref num4); } if (num4 < p.StartUT || num4 > p.EndUT || double.IsInfinity(num4)) { num2 = 1; } p.numClosePoints = num2; p.FEVp = fEVp; p.FEVs = fEVs; p.SEVp = sEVp; p.SEVs = sEVs; p.ClEctr1 = clEctr; p.ClEctr2 = clEctr2; if (Math.Min(p.ClEctr1, p.ClEctr2) > sec.celestialBody.sphereOfInfluence) { if (GameSettings.ALWAYS_SHOW_TARGET_APPROACH_MARKERS && sec.celestialBody == targetBody) { p.UTappr = startEpoch; p.ClAppr = PatchedConics.GetClosestApproach(p, orbit, startEpoch, p.nearestTT * 0.5, pars); p.closestTgtApprUT = p.UTappr; } return(false); } if (p.closestEncounterLevel < Orbit.EncounterSolutionLevel.SOI_INTERSECT_1) { p.closestEncounterLevel = Orbit.EncounterSolutionLevel.SOI_INTERSECT_1; p.closestEncounterBody = sec.celestialBody; } p.timeToTransition1 = dTforTrueAnomaly; p.secondaryPosAtTransition1 = orbit.getPositionAtUTSigma(num3); UnityEngine.Debug.DrawLine(ScaledSpace.LocalToScaledSpace(p.referenceBody.position), ScaledSpace.LocalToScaledSpace(p.secondaryPosAtTransition1), Color.yellow); p.timeToTransition2 = dTforTrueAnomaly2; p.secondaryPosAtTransition2 = orbit.getPositionAtUTSigma(num4); UnityEngine.Debug.DrawLine(ScaledSpace.LocalToScaledSpace(p.referenceBody.position), ScaledSpace.LocalToScaledSpace(p.secondaryPosAtTransition2), Color.red); p.nearestTT = p.timeToTransition1; p.nextTT = p.timeToTransition2; if (double.IsNaN(p.nearestTT)) { UnityEngine.Debug.Log(string.Concat(new object[] { "nearestTT is NaN! t1: ", p.timeToTransition1, ", t2: ", p.timeToTransition2, ", FEVp: ", p.FEVp, ", SEVp: ", p.SEVp })); } p.UTappr = startEpoch; p.ClAppr = PatchedConics.GetClosestApproach(p, orbit, startEpoch, p.nearestTT * 0.5, pars); if (PatchedConics.EncountersBody(p, orbit, nextPatch, sec, startEpoch, pars)) { return(true); } if (num2 > 1) { p.closestEncounterLevel = Orbit.EncounterSolutionLevel.SOI_INTERSECT_2; p.closestEncounterBody = sec.celestialBody; UnityEngine.Debug.DrawLine(ScaledSpace.LocalToScaledSpace(p.getPositionAtUTSigma(p.UTappr)), ScaledSpace.LocalToScaledSpace(orbit.getPositionAtUTSigma(p.UTappr)), XKCDColors.Orange * 0.5f); p.UTappr = startEpoch + p.nearestTT; p.ClAppr = PatchedConics.GetClosestApproach(p, orbit, startEpoch, (p.nextTT - p.nearestTT) * 0.5, pars); if (PatchedConics.EncountersBody(p, orbit, nextPatch, sec, startEpoch, pars)) { return(true); } UnityEngine.Debug.DrawLine(ScaledSpace.LocalToScaledSpace(p.getPositionAtUTSigma(p.UTappr)), ScaledSpace.LocalToScaledSpace(orbit.getPositionAtUTSigma(p.UTappr)), XKCDColors.Orange); } if (sec.celestialBody == targetBody) { p.closestTgtApprUT = p.UTappr; } return(false); }