Пример #1
0
        public override void OnLoad(ConfigNode node)
        {
            if (node == null)
            {
                return;
            }

            Util.DebugLog("");

            //version = Util.ConfigValue(node, "version", Version);     // get saved version, defaults to current version if none

            Settings ??= new Settings();                          // get trajectories settings from the config.xml file if it exists or create a new one
            if (Settings != null)
            {
                Settings.Load();

                DescentProfile.Start();
                Trajectory.Start();
                MapOverlay.Start();
                FlightOverlay.Start();
                NavBallOverlay.Start();
                MainGUI.Start();
                AppLauncherButton.Start();
            }
            else
            {
                Util.LogError("There was a problem with the config.xml settings file");
            }
        }
Пример #2
0
        private void AttachVessel()
        {
            Util.DebugLog("Loading profiles for vessel");

            AttachedVessel = FlightGlobals.ActiveVessel;

            if (AttachedVessel == null)
            {
                Util.DebugLog("No vessel");
                DescentProfile.Clear();
                TargetProfile.Clear();
                TargetProfile.ManualText = "";
            }
            else
            {
                TrajectoriesVesselSettings module = AttachedVessel.Parts.SelectMany(p => p.Modules.OfType <TrajectoriesVesselSettings>()).FirstOrDefault();
                if (module == null)
                {
                    Util.DebugLog("No TrajectoriesVesselSettings module");
                    DescentProfile.Clear();
                    TargetProfile.Clear();
                    TargetProfile.ManualText = "";
                }
                else if (!module.Initialized)
                {
                    Util.DebugLog("Initializing TrajectoriesVesselSettings module");
                    DescentProfile.Clear();
                    DescentProfile.Save(module);
                    TargetProfile.Clear();
                    TargetProfile.ManualText = "";
                    TargetProfile.Save(module);
                    module.Initialized = true;
                    Util.Log("New vessel, profiles created");
                }
                else
                {
                    Util.DebugLog("Reading profile settings...");
                    // descent profile
                    if (DescentProfile.Ready)
                    {
                        DescentProfile.RetrogradeEntry        = module.RetrogradeEntry;
                        DescentProfile.AtmosEntry.AngleRad    = module.EntryAngle;
                        DescentProfile.AtmosEntry.Horizon     = module.EntryHorizon;
                        DescentProfile.HighAltitude.AngleRad  = module.HighAngle;
                        DescentProfile.HighAltitude.Horizon   = module.HighHorizon;
                        DescentProfile.LowAltitude.AngleRad   = module.LowAngle;
                        DescentProfile.LowAltitude.Horizon    = module.LowHorizon;
                        DescentProfile.FinalApproach.AngleRad = module.GroundAngle;
                        DescentProfile.FinalApproach.Horizon  = module.GroundHorizon;
                        DescentProfile.RefreshGui();
                    }

                    // target profile
                    TargetProfile.SetFromLocalPos(FlightGlobals.Bodies.FirstOrDefault(b => b.name == module.TargetBody),
                                                  new Vector3d(module.TargetPosition_x, module.TargetPosition_y, module.TargetPosition_z));
                    TargetProfile.ManualText = module.ManualTargetTxt;
                    Util.Log("Profiles loaded");
                }
            }
        }
 internal void Destroy()
 {
     Util.DebugLog("");
     DescentProfile.Destroy();
     FlightOverlay.Destroy();
     NavBallOverlay.Destroy();
     MapOverlay.Destroy();
 }
 internal Trajectory(Vessel vessel)
 {
     AttachedVessel = vessel;
     TargetProfile  = new TargetProfile(this);
     DescentProfile = new DescentProfile(this);
     FlightOverlay  = new FlightOverlay(this);
     MapOverlay     = new MapOverlay(this);
     NavBallOverlay = new NavBallOverlay(this);
     Util.DebugLog("Constructing");
 }
Пример #5
0
        private static Vector3d CalcReference()
        {
            if (!Trajectories.IsVesselAttached || TargetProfile.Body == null)
            {
                return(Vector3d.zero);
            }

            double plannedAngleOfAttack = (double)DescentProfile.GetAngleOfAttack(TargetProfile.Body, position, velocity);

            return(velocity.normalized * Math.Cos(plannedAngleOfAttack) + Vector3d.Cross(vel_right, velocity).normalized *Math.Sin(plannedAngleOfAttack));
        }
Пример #6
0
 internal void OnDestroy()
 {
     Util.DebugLog("");
     AttachedVessel = null;
     AppLauncherButton.DestroyToolbarButton();
     MainGUI.DeSpawn();
     NavBallOverlay.DestroyTransforms();
     FlightOverlay.Destroy();
     MapOverlay.DestroyRenderer();
     Trajectory.Destroy();
     DescentProfile.Clear();
 }
Пример #7
0
        public void ComputeTrajectory(Vessel vessel, DescentProfile profile, bool incremental)
        {
            try
            {
                incrementTime_ = Stopwatch.StartNew();

                if (partialComputation_ == null || vessel != vessel_)
                {
                    patchesBackBuffer_.Clear();
                    maxAccelBackBuffer_ = 0;

                    vessel_ = vessel;

                    if (vessel == null)
                    {
                        patches_.Clear();
                        return;
                    }

                    if (partialComputation_ != null)
                    {
                        partialComputation_.Dispose();
                    }
                    partialComputation_ = computeTrajectoryIncrement(vessel, profile).GetEnumerator();
                }

                bool finished = !partialComputation_.MoveNext();

                if (finished)
                {
                    var tmp = patches_;
                    patches_           = patchesBackBuffer_;
                    patchesBackBuffer_ = tmp;

                    maxAccel_ = maxAccelBackBuffer_;

                    partialComputation_.Dispose();
                    partialComputation_ = null;
                }

                frameTime_ += (float)incrementTime_.ElapsedMilliseconds;
            }
            catch (Exception)
            {
                ++errorCount_;
                throw;
            }
        }
Пример #8
0
 internal void OnApplicationQuit()
 {
     Util.Log("Ending after {0} seconds", Time.time);
     AttachedVessel = null;
     AppLauncherButton.Destroy();
     MainGUI.Destroy();
     NavBallOverlay.Destroy();
     FlightOverlay.Destroy();
     MapOverlay.Destroy();
     Trajectory.Destroy();
     DescentProfile.Destroy();
     if (Settings != null)
     {
         Settings.Destroy();
     }
     Settings = null;
 }
Пример #9
0
        private IEnumerable <bool> computeTrajectoryIncrement(Vessel vessel, DescentProfile profile)
        {
            if (aerodynamicModel_ == null || !aerodynamicModel_.isValidFor(vessel, vessel.mainBody))
            {
                aerodynamicModel_ = AerodynamicModelFactory.GetModel(vessel, vessel.mainBody);
            }
            else
            {
                aerodynamicModel_.IncrementalUpdate();
            }

            var state = vessel.LandedOrSplashed ? null : new VesselState(vessel);

            for (int patchIdx = 0; patchIdx < Settings.fetch.MaxPatchCount; ++patchIdx)
            {
                if (state == null)
                {
                    break;
                }

                if (incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
                {
                    yield return(false);
                }

                if (null != vessel_.patchedConicSolver)
                {
                    var maneuverNodes = vessel_.patchedConicSolver.maneuverNodes;
                    foreach (var node in maneuverNodes)
                    {
                        if (node.UT == state.time)
                        {
                            state.velocity += node.GetBurnVector(createOrbitFromState(state));
                            break;
                        }
                    }
                    foreach (var result in AddPatch(state, profile))
                    {
                        yield return(false);
                    }
                }

                state = AddPatch_outState;
            }
        }
Пример #10
0
 public void Start()
 {
     fetch_ = this;
 }
Пример #11
0
        private static Vector2d GetCorrection()
        {
            if (!Trajectories.IsVesselAttached)
            {
                return(Vector2d.zero);
            }

            Vector3d?     targetPosition = TargetProfile.WorldPosition;
            CelestialBody body           = TargetProfile.Body;

            if (!targetPosition.HasValue || patch == null || !patch.ImpactPosition.HasValue || patch.StartingState.ReferenceBody != body || !patch.IsAtmospheric)
            {
                return(Vector2d.zero);
            }

            // Get impact position, or, if some point over the trajectory has not enough clearance, smoothly interpolate to that point depending on how much clearance is missing
            Vector3d impactPosition = patch.ImpactPosition.Value;

            foreach (Trajectory.Point p in patch.AtmosphericTrajectory)
            {
                double neededClearance  = 600.0d;
                double missingClearance = neededClearance - (p.pos.magnitude - body.Radius - p.groundAltitude);
                if (missingClearance > 0.0d)
                {
                    if (Vector3d.Distance(p.pos, patch.RawImpactPosition.Value) > 3000.0d)
                    {
                        double   coeff      = missingClearance / neededClearance;
                        Vector3d rotatedPos = p.pos;
                        if (!Settings.BodyFixedMode)
                        {
                            rotatedPos = Trajectory.CalculateRotatedPosition(body, p.pos, p.time);
                        }
                        impactPosition = impactPosition * (1.0d - coeff) + rotatedPos * coeff;
                    }
                    break;
                }
            }

            Vector3d right  = Vector3d.Cross(patch.ImpactVelocity.Value, impactPosition).normalized;
            Vector3d behind = Vector3d.Cross(right, impactPosition).normalized;

            Vector3d offset    = targetPosition.Value - impactPosition;
            Vector2d offsetDir = new Vector2d(Vector3d.Dot(right, offset), Vector3d.Dot(behind, offset));

            offsetDir *= 0.00005d; // 20km <-> 1 <-> 45° (this is purely indicative, no physical meaning, it would be very complicated to compute an actual correction angle as it depends on the spacecraft behavior in the atmosphere ; a small angle will suffice for a plane, but even a big angle might do almost nothing for a rocket)

            Vector3d pos = Trajectories.AttachedVessel.GetWorldPos3D() - body.position;
            Vector3d vel = Trajectories.AttachedVessel.obt_velocity - body.getRFrmVel(body.position + pos); // air velocity

            double plannedAngleOfAttack = (double)DescentProfile.GetAngleOfAttack(body, pos, vel);

            if (plannedAngleOfAttack < Util.HALF_PI)
            {
                offsetDir.y = -offsetDir.y; // behavior is different for prograde or retrograde entry
            }
            double maxCorrection = 1.0d;

            offsetDir.x = Util.Clamp(offsetDir.x, -maxCorrection, maxCorrection);
            offsetDir.y = Util.Clamp(offsetDir.y, -maxCorrection, maxCorrection);

            return(offsetDir);
        }
Пример #12
0
 public void Start()
 {
     fetch_ = this;
 }
Пример #13
0
        public void ComputeTrajectory(Vessel vessel, float AoA, bool incremental)
        {
            DescentProfile profile = new DescentProfile(AoA);

            ComputeTrajectory(vessel, profile, incremental);
        }
Пример #14
0
        private IEnumerable<bool> computeTrajectoryIncrement(Vessel vessel, DescentProfile profile)
        {
            if (aerodynamicModel_ == null || !aerodynamicModel_.isValidFor(vessel, vessel.mainBody))
                aerodynamicModel_ = AerodynamicModelFactory.GetModel(vessel, vessel.mainBody);
            else
                aerodynamicModel_.IncrementalUpdate();

            var state = vessel.LandedOrSplashed ? null : new VesselState(vessel);
            for (int patchIdx = 0; patchIdx < Settings.fetch.MaxPatchCount; ++patchIdx)
            {
                if (state == null)
                    break;

                if (incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
                    yield return false;

                if (null != vessel_.patchedConicSolver)
                {
                    var maneuverNodes = vessel_.patchedConicSolver.maneuverNodes;
                    foreach (var node in maneuverNodes)
                    {
                        if (node.UT == state.time)
                        {
                            state.velocity += node.GetBurnVector(createOrbitFromState(state));
                            break;
                        }
                    }
                    foreach (var result in AddPatch(state, profile))
                        yield return false;
                }

                state = AddPatch_outState;
            }
        }
Пример #15
0
        private IEnumerable<bool> AddPatch(VesselState startingState, DescentProfile profile)
        {
            if (null == vessel_.patchedConicSolver)
            {
                UnityEngine.Debug.LogWarning("Trajectories: AddPatch() attempted when patchedConicsSolver is null; Skipping.");
                yield break;
            }

            CelestialBody body = startingState.referenceBody;

            var patch = new Patch();
            patch.startingState = startingState;
            patch.isAtmospheric = false;
            patch.spaceOrbit = startingState.stockPatch ?? createOrbitFromState(startingState);
            patch.endTime = patch.startingState.time + patch.spaceOrbit.period;

            // the flight plan does not always contain the first patches (before the first maneuver node), so we populate it with the current orbit and associated encounters etc.
            var flightPlan = new List<Orbit>();
            for (var orbit = vessel_.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
            {
                if (vessel_.patchedConicSolver.flightPlan.Contains(orbit))
                    break;
                flightPlan.Add(orbit);
            }

            foreach (var orbit in vessel_.patchedConicSolver.flightPlan)
            {
                flightPlan.Add(orbit);
            }

            Orbit nextStockPatch = null;
            if (startingState.stockPatch != null)
            {
                int planIdx = flightPlan.IndexOf(startingState.stockPatch);
                if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
                {
                    nextStockPatch = flightPlan[planIdx + 1];
                }
            }

            if (nextStockPatch != null)
            {
                patch.endTime = nextStockPatch.StartUT;
            }

            double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);

            double minAltitude = patch.spaceOrbit.PeA;
            if (patch.endTime < startingState.time + patch.spaceOrbit.timeToPe)
            {
                minAltitude = patch.spaceOrbit.getRelativePositionAtUT(patch.endTime).magnitude;
            }
            if (minAltitude < maxAtmosphereAltitude)
            {
                double entryTime;
                if (startingState.position.magnitude <= body.Radius + maxAtmosphereAltitude)
                {
                    // whole orbit is inside the atmosphere
                    entryTime = startingState.time;
                }
                else
                {
                    // binary search of entry time in atmosphere
                    // I guess an analytic solution could be found, but I'm too lazy to search it
                    double from = startingState.time;
                    double to = from + patch.spaceOrbit.timeToPe;

                    int loopCount = 0;
                    while (to - from > 0.1)
                    {
                        ++loopCount;
                        if (loopCount > 1000)
                        {
                            UnityEngine.Debug.Log("WARNING: infinite loop? (Trajectories.Trajectory.AddPatch, atmosphere limit search)");
                            ++errorCount_;
                            break;
                        }
                        double middle = (from + to) * 0.5;
                        if (patch.spaceOrbit.getRelativePositionAtUT(middle).magnitude < body.Radius + maxAtmosphereAltitude)
                        {
                            to = middle;
                        }
                        else
                        {
                            from = middle;
                        }
                    }

                    entryTime = to;
                }

                if (entryTime > startingState.time + 0.1)
                {
                    // add the space patch before atmospheric entry
                    patch.endTime = entryTime;

                    if (body.atmosphere)
                    {
                        patchesBackBuffer_.Add(patch);
                        AddPatch_outState = new VesselState
                        {
                            position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)),
                            referenceBody = body,
                            time = entryTime,
                            velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime))
                        };
                        yield break;
                    }
                    else
                    {
                        // the body has no atmosphere, so what we actually computed is the impact on the body surface
                        // now, go back in time until the impact point is above the ground to take ground height in account
                        // we assume the ground is horizontal around the impact position
                        double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)), entryTime)) + body.Radius;

                        double iterationSize = 1.0;
                        while (entryTime > startingState.time + iterationSize && patch.spaceOrbit.getRelativePositionAtUT(entryTime).magnitude < groundAltitude)
                            entryTime -= iterationSize;

                        patch.endTime = entryTime;
                        patch.rawImpactPosition = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
                        patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, entryTime);
                        patch.impactVelocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
                        patchesBackBuffer_.Add(patch);
                        AddPatch_outState = null;
                        yield break;
                    }
                }
                else
                {
                    if (patch.startingState.referenceBody != vessel_.mainBody)
                    {
                        // in current aerodynamic prediction code, we can't handle predictions for another body, so we stop here
                        AddPatch_outState = null;
                        yield break;
                    }

                    // simulate atmospheric flight (drag and lift), until landing (more likely to be a crash as we don't predict user piloting) or atmosphere exit (typically for an aerobraking maneuver)
                    // the simulation assumes a constant angle of attack

                    patch.isAtmospheric = true;
                    patch.startingState.stockPatch = null;

                    double dt = 0.1; // lower dt would be more accurate, but a tradeoff has to be found between performances and accuracy

                    int maxIterations = (int)(30.0 * 60.0 / dt); // some shallow entries can result in very long flight, for performances reasons, we limit the prediction duration

                    int chunkSize = 128;
                    double trajectoryInterval = 10.0; // time between two consecutive stored positions (more intermediate positions are computed for better accuracy), also used for ground collision checks
                    var buffer = new List<Point[]>();
                    buffer.Add(new Point[chunkSize]);
                    int nextPosIdx = 0;

                    Vector3d pos = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
                    Vector3d vel = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
                    Vector3d prevPos = pos - vel * dt;
                    //Util.PostSingleScreenMessage("initial vel", "initial vel = " + vel);
                    double currentTime = entryTime;
                    double lastPositionStoredUT = 0;
                    Vector3d lastPositionStored = new Vector3d();
                    bool hitGround = false;
                    int iteration = 0;
                    int incrementIterations = 0;
                    int minIterationsPerIncrement = maxIterations / Settings.fetch.MaxFramesPerPatch;
                    while (true)
                    {
                        ++iteration;
                        ++incrementIterations;

                        if (incrementIterations > minIterationsPerIncrement && incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
                        {
                            yield return false;
                            incrementIterations = 0;
                        }

                        double R = pos.magnitude;
                        double altitude = R - body.Radius;
                        double atmosphereCoeff = altitude / maxAtmosphereAltitude;
                        if (hitGround || atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 || iteration == maxIterations || currentTime > patch.endTime)
                        {
                            if (hitGround || atmosphereCoeff <= 0.0)
                            {
                                patch.rawImpactPosition = pos;
                                patch.impactPosition = calculateRotatedPosition(body, patch.rawImpactPosition.Value, currentTime);
                                patch.impactVelocity = vel;
                            }

                            patch.endTime = Math.Min(currentTime, patch.endTime);

                            int totalCount = (buffer.Count - 1) * chunkSize + nextPosIdx;
                            patch.atmosphericTrajectory = new Point[totalCount];
                            int outIdx = 0;
                            foreach (var chunk in buffer)
                            {
                                foreach (var p in chunk)
                                {
                                    if (outIdx == totalCount)
                                        break;
                                    patch.atmosphericTrajectory[outIdx++] = p;
                                }
                            }

                            if (iteration == maxIterations)
                            {
                                ScreenMessages.PostScreenMessage("WARNING: trajectory prediction stopped, too many iterations");
                                patchesBackBuffer_.Add(patch);
                                AddPatch_outState = null;
                                yield break;
                            }
                            else if (atmosphereCoeff <= 0.0 || hitGround)
                            {
                                patchesBackBuffer_.Add(patch);
                                AddPatch_outState = null;
                                yield break;
                            }
                            else
                            {
                                patchesBackBuffer_.Add(patch);
                                AddPatch_outState = new VesselState
                                {
                                    position = pos,
                                    velocity = vel,
                                    referenceBody = body,
                                    time = patch.endTime
                                };
                                yield break;
                            }
                        }

                        Vector3d gravityAccel = pos * (-body.gravParameter / (R * R * R));

                        //Util.PostSingleScreenMessage("prediction vel", "prediction vel = " + vel);
                        Vector3d airVelocity = vel - body.getRFrmVel(body.position + pos);
                        double angleOfAttack = profile.GetAngleOfAttack(body, pos, airVelocity);
                        Vector3d aerodynamicForce = aerodynamicModel_.GetForces(body, pos, airVelocity, angleOfAttack);
                        Vector3d acceleration = gravityAccel + aerodynamicForce / aerodynamicModel_.mass;

                        // acceleration in the vessel reference frame is acceleration - gravityAccel
                        maxAccelBackBuffer_ = Math.Max((float) (aerodynamicForce.magnitude / aerodynamicModel_.mass), maxAccelBackBuffer_);

                        //vel += acceleration * dt;
                        //pos += vel * dt;

                        // Verlet integration (more precise than using the velocity)
                        Vector3d ppos = prevPos;
                        prevPos = pos;
                        pos = pos + pos - ppos + acceleration * (dt * dt);
                        vel = (pos - prevPos) / dt;

                        currentTime += dt;

                        double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
                        if (currentTime >= lastPositionStoredUT + interval)
                        {
                            double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, pos, currentTime));
                            if (lastPositionStoredUT > 0)
                            {
                                // check terrain collision, to detect impact on mountains etc.
                                Vector3 rayOrigin = lastPositionStored;
                                Vector3 rayEnd = pos;
                                double absGroundAltitude = groundAltitude + body.Radius;
                                if (absGroundAltitude > rayEnd.magnitude)
                                {
                                    hitGround = true;
                                    float coeff = Math.Max(0.01f, (float)((absGroundAltitude - rayOrigin.magnitude) / (rayEnd.magnitude - rayOrigin.magnitude)));
                                    pos = rayEnd * coeff + rayOrigin * (1.0f - coeff);
                                    currentTime = currentTime * coeff + lastPositionStoredUT * (1.0f - coeff);
                                }
                            }

                            lastPositionStoredUT = currentTime;
                            if (nextPosIdx == chunkSize)
                            {
                                buffer.Add(new Point[chunkSize]);
                                nextPosIdx = 0;
                            }
                            Vector3d nextPos = pos;
                            if (Settings.fetch.BodyFixedMode)
                            {
                                nextPos = calculateRotatedPosition(body, nextPos, currentTime);
                            }
                            buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
                            buffer.Last()[nextPosIdx].orbitalVelocity = vel;
                            buffer.Last()[nextPosIdx].groundAltitude = (float)groundAltitude;
                            buffer.Last()[nextPosIdx].time = currentTime;
                            buffer.Last()[nextPosIdx++].pos = nextPos;
                            lastPositionStored = pos;
                        }
                    }
                }
            }
            else
            {
                // no atmospheric entry, just add the space orbit
                patchesBackBuffer_.Add(patch);
                if (nextStockPatch != null)
                {
                    AddPatch_outState = new VesselState
                    {
                        position = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(patch.endTime)),
                        velocity = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(patch.endTime)),
                        referenceBody = nextStockPatch == null ? body : nextStockPatch.referenceBody,
                        time = patch.endTime,
                        stockPatch = nextStockPatch
                    };
                    yield break;
                }
                else
                {
                    AddPatch_outState = null;
                    yield break;
                }
            }
        }
Пример #16
0
 public DescentProfile()
 {
     fetch_ = this;
 }
Пример #17
0
        public void ComputeTrajectory(Vessel vessel, float AoA)
        {
            DescentProfile profile = new DescentProfile(AoA);

            ComputeTrajectory(vessel, profile);
        }
Пример #18
0
        private IEnumerable <bool> AddPatch(VesselState startingState, DescentProfile profile)
        {
            if (null == vessel_.patchedConicSolver)
            {
                UnityEngine.Debug.LogWarning("Trajectories: AddPatch() attempted when patchedConicsSolver is null; Skipping.");
                yield break;
            }

            CelestialBody body = startingState.referenceBody;

            var patch = new Patch();

            patch.startingState = startingState;
            patch.isAtmospheric = false;
            patch.spaceOrbit    = startingState.stockPatch ?? createOrbitFromState(startingState);
            patch.endTime       = patch.startingState.time + patch.spaceOrbit.period;

            // the flight plan does not always contain the first patches (before the first maneuver node), so we populate it with the current orbit and associated encounters etc.
            var flightPlan = new List <Orbit>();

            for (var orbit = vessel_.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
            {
                if (vessel_.patchedConicSolver.flightPlan.Contains(orbit))
                {
                    break;
                }
                flightPlan.Add(orbit);
            }

            foreach (var orbit in vessel_.patchedConicSolver.flightPlan)
            {
                flightPlan.Add(orbit);
            }


            Orbit nextStockPatch = null;

            if (startingState.stockPatch != null)
            {
                int planIdx = flightPlan.IndexOf(startingState.stockPatch);
                if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
                {
                    nextStockPatch = flightPlan[planIdx + 1];
                }
            }

            if (nextStockPatch != null)
            {
                patch.endTime = nextStockPatch.StartUT;
            }

            double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);

            double minAltitude = patch.spaceOrbit.PeA;

            if (patch.endTime < startingState.time + patch.spaceOrbit.timeToPe)
            {
                minAltitude = patch.spaceOrbit.getRelativePositionAtUT(patch.endTime).magnitude;
            }
            if (minAltitude < maxAtmosphereAltitude)
            {
                double entryTime;
                if (startingState.position.magnitude <= body.Radius + maxAtmosphereAltitude)
                {
                    // whole orbit is inside the atmosphere
                    entryTime = startingState.time;
                }
                else
                {
                    // binary search of entry time in atmosphere
                    // I guess an analytic solution could be found, but I'm too lazy to search it
                    double from = startingState.time;
                    double to   = from + patch.spaceOrbit.timeToPe;

                    int loopCount = 0;
                    while (to - from > 0.1)
                    {
                        ++loopCount;
                        if (loopCount > 1000)
                        {
                            UnityEngine.Debug.Log("WARNING: infinite loop? (Trajectories.Trajectory.AddPatch, atmosphere limit search)");
                            ++errorCount_;
                            break;
                        }
                        double middle = (from + to) * 0.5;
                        if (patch.spaceOrbit.getRelativePositionAtUT(middle).magnitude < body.Radius + maxAtmosphereAltitude)
                        {
                            to = middle;
                        }
                        else
                        {
                            from = middle;
                        }
                    }

                    entryTime = to;
                }

                if (entryTime > startingState.time + 0.1)
                {
                    // add the space patch before atmospheric entry
                    patch.endTime = entryTime;

                    if (body.atmosphere)
                    {
                        patchesBackBuffer_.Add(patch);
                        AddPatch_outState = new VesselState
                        {
                            position      = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)),
                            referenceBody = body,
                            time          = entryTime,
                            velocity      = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime))
                        };
                        yield break;
                    }
                    else
                    {
                        // the body has no atmosphere, so what we actually computed is the impact on the body surface
                        // now, go back in time until the impact point is above the ground to take ground height in account
                        // we assume the ground is horizontal around the impact position
                        double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime)), entryTime)) + body.Radius;

                        double iterationSize = 1.0;
                        while (entryTime > startingState.time + iterationSize && patch.spaceOrbit.getRelativePositionAtUT(entryTime).magnitude < groundAltitude)
                        {
                            entryTime -= iterationSize;
                        }

                        patch.endTime           = entryTime;
                        patch.rawImpactPosition = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
                        patch.impactPosition    = calculateRotatedPosition(body, patch.rawImpactPosition.Value, entryTime);
                        patch.impactVelocity    = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
                        patchesBackBuffer_.Add(patch);
                        AddPatch_outState = null;
                        yield break;
                    }
                }
                else
                {
                    if (patch.startingState.referenceBody != vessel_.mainBody)
                    {
                        // in current aerodynamic prediction code, we can't handle predictions for another body, so we stop here
                        AddPatch_outState = null;
                        yield break;
                    }

                    // simulate atmospheric flight (drag and lift), until landing (more likely to be a crash as we don't predict user piloting) or atmosphere exit (typically for an aerobraking maneuver)
                    // the simulation assumes a constant angle of attack

                    patch.isAtmospheric            = true;
                    patch.startingState.stockPatch = null;

                    double dt = 0.1;                             // lower dt would be more accurate, but a tradeoff has to be found between performances and accuracy

                    int maxIterations = (int)(30.0 * 60.0 / dt); // some shallow entries can result in very long flight, for performances reasons, we limit the prediction duration

                    int    chunkSize          = 128;
                    double trajectoryInterval = 10.0; // time between two consecutive stored positions (more intermediate positions are computed for better accuracy), also used for ground collision checks
                    var    buffer             = new List <Point[]>();
                    buffer.Add(new Point[chunkSize]);
                    int nextPosIdx = 0;

                    Vector3d pos     = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(entryTime));
                    Vector3d vel     = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(entryTime));
                    Vector3d prevPos = pos - vel * dt;
                    //Util.PostSingleScreenMessage("initial vel", "initial vel = " + vel);
                    double   currentTime               = entryTime;
                    double   lastPositionStoredUT      = 0;
                    Vector3d lastPositionStored        = new Vector3d();
                    bool     hitGround                 = false;
                    int      iteration                 = 0;
                    int      incrementIterations       = 0;
                    int      minIterationsPerIncrement = maxIterations / Settings.fetch.MaxFramesPerPatch;
                    while (true)
                    {
                        ++iteration;
                        ++incrementIterations;

                        if (incrementIterations > minIterationsPerIncrement && incrementTime_.ElapsedMilliseconds > MaxIncrementTime)
                        {
                            yield return(false);

                            incrementIterations = 0;
                        }

                        double R               = pos.magnitude;
                        double altitude        = R - body.Radius;
                        double atmosphereCoeff = altitude / maxAtmosphereAltitude;
                        if (hitGround || atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 || iteration == maxIterations || currentTime > patch.endTime)
                        {
                            if (hitGround || atmosphereCoeff <= 0.0)
                            {
                                patch.rawImpactPosition = pos;
                                patch.impactPosition    = calculateRotatedPosition(body, patch.rawImpactPosition.Value, currentTime);
                                patch.impactVelocity    = vel;
                            }

                            patch.endTime = Math.Min(currentTime, patch.endTime);

                            int totalCount = (buffer.Count - 1) * chunkSize + nextPosIdx;
                            patch.atmosphericTrajectory = new Point[totalCount];
                            int outIdx = 0;
                            foreach (var chunk in buffer)
                            {
                                foreach (var p in chunk)
                                {
                                    if (outIdx == totalCount)
                                    {
                                        break;
                                    }
                                    patch.atmosphericTrajectory[outIdx++] = p;
                                }
                            }

                            if (iteration == maxIterations)
                            {
                                ScreenMessages.PostScreenMessage("WARNING: trajectory prediction stopped, too many iterations");
                                patchesBackBuffer_.Add(patch);
                                AddPatch_outState = null;
                                yield break;
                            }
                            else if (atmosphereCoeff <= 0.0 || hitGround)
                            {
                                patchesBackBuffer_.Add(patch);
                                AddPatch_outState = null;
                                yield break;
                            }
                            else
                            {
                                patchesBackBuffer_.Add(patch);
                                AddPatch_outState = new VesselState
                                {
                                    position      = pos,
                                    velocity      = vel,
                                    referenceBody = body,
                                    time          = patch.endTime
                                };
                                yield break;
                            }
                        }

                        Vector3d gravityAccel = pos * (-body.gravParameter / (R * R * R));

                        //Util.PostSingleScreenMessage("prediction vel", "prediction vel = " + vel);
                        Vector3d airVelocity      = vel - body.getRFrmVel(body.position + pos);
                        double   angleOfAttack    = profile.GetAngleOfAttack(body, pos, airVelocity);
                        Vector3d aerodynamicForce = aerodynamicModel_.GetForces(body, pos, airVelocity, angleOfAttack);
                        Vector3d acceleration     = gravityAccel + aerodynamicForce / aerodynamicModel_.mass;

                        // acceleration in the vessel reference frame is acceleration - gravityAccel
                        maxAccelBackBuffer_ = Math.Max((float)(aerodynamicForce.magnitude / aerodynamicModel_.mass), maxAccelBackBuffer_);


                        //vel += acceleration * dt;
                        //pos += vel * dt;

                        // Verlet integration (more precise than using the velocity)
                        Vector3d ppos = prevPos;
                        prevPos = pos;
                        pos     = pos + pos - ppos + acceleration * (dt * dt);
                        vel     = (pos - prevPos) / dt;

                        currentTime += dt;

                        double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
                        if (currentTime >= lastPositionStoredUT + interval)
                        {
                            double groundAltitude = GetGroundAltitude(body, calculateRotatedPosition(body, pos, currentTime));
                            if (lastPositionStoredUT > 0)
                            {
                                // check terrain collision, to detect impact on mountains etc.
                                Vector3 rayOrigin         = lastPositionStored;
                                Vector3 rayEnd            = pos;
                                double  absGroundAltitude = groundAltitude + body.Radius;
                                if (absGroundAltitude > rayEnd.magnitude)
                                {
                                    hitGround = true;
                                    float coeff = Math.Max(0.01f, (float)((absGroundAltitude - rayOrigin.magnitude) / (rayEnd.magnitude - rayOrigin.magnitude)));
                                    pos         = rayEnd * coeff + rayOrigin * (1.0f - coeff);
                                    currentTime = currentTime * coeff + lastPositionStoredUT * (1.0f - coeff);
                                }
                            }

                            lastPositionStoredUT = currentTime;
                            if (nextPosIdx == chunkSize)
                            {
                                buffer.Add(new Point[chunkSize]);
                                nextPosIdx = 0;
                            }
                            Vector3d nextPos = pos;
                            if (Settings.fetch.BodyFixedMode)
                            {
                                nextPos = calculateRotatedPosition(body, nextPos, currentTime);
                            }
                            buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
                            buffer.Last()[nextPosIdx].orbitalVelocity  = vel;
                            buffer.Last()[nextPosIdx].groundAltitude   = (float)groundAltitude;
                            buffer.Last()[nextPosIdx].time             = currentTime;
                            buffer.Last()[nextPosIdx++].pos            = nextPos;
                            lastPositionStored = pos;
                        }
                    }
                }
            }
            else
            {
                // no atmospheric entry, just add the space orbit
                patchesBackBuffer_.Add(patch);
                if (nextStockPatch != null)
                {
                    AddPatch_outState = new VesselState
                    {
                        position      = Util.SwapYZ(patch.spaceOrbit.getRelativePositionAtUT(patch.endTime)),
                        velocity      = Util.SwapYZ(patch.spaceOrbit.getOrbitalVelocityAtUT(patch.endTime)),
                        referenceBody = nextStockPatch == null ? body : nextStockPatch.referenceBody,
                        time          = patch.endTime,
                        stockPatch    = nextStockPatch
                    };
                    yield break;
                }
                else
                {
                    AddPatch_outState = null;
                    yield break;
                }
            }
        }
Пример #19
0
 public void ComputeTrajectory(Vessel vessel, float AoA, bool incremental)
 {
     DescentProfile profile = new DescentProfile(AoA);
     ComputeTrajectory(vessel, profile, incremental);
 }
Пример #20
0
        public void ComputeTrajectory(Vessel vessel, DescentProfile profile, bool incremental)
        {
            try
            {
                incrementTime_ = Stopwatch.StartNew();

                if (partialComputation_ == null || vessel != vessel_)
                {
                    patchesBackBuffer_.Clear();
                    maxAccelBackBuffer_ = 0;

                    vessel_ = vessel;

                    if (vessel == null)
                    {
                        patches_.Clear();
                        return;
                    }

                    if (partialComputation_ != null)
                        partialComputation_.Dispose();
                    partialComputation_ = computeTrajectoryIncrement(vessel, profile).GetEnumerator();
                }

                bool finished = !partialComputation_.MoveNext();

                if (finished)
                {
                    var tmp = patches_;
                    patches_ = patchesBackBuffer_;
                    patchesBackBuffer_ = tmp;

                    maxAccel_ = maxAccelBackBuffer_;

                    partialComputation_.Dispose();
                    partialComputation_ = null;
                }

                frameTime_ += (float)incrementTime_.ElapsedMilliseconds;
            }
            catch (Exception)
            {
                ++errorCount_;
                throw;
            }
        }
        private IEnumerable <bool> AddPatch(VesselState startingState)
        {
            if (null == AttachedVessel.patchedConicSolver)
            {
                AttachedVessel.AttachPatchedConicsSolver();
            }

            CelestialBody body = startingState.ReferenceBody;

            Patch patch = new Patch
            {
                StartingState = startingState,
                IsAtmospheric = false,
                SpaceOrbit    = startingState.StockPatch ?? CreateOrbitFromState(startingState)
            };

            patch.EndTime = patch.StartingState.Time + patch.SpaceOrbit.period;

            // the flight plan does not always contain the first patches (before the first maneuver node),
            // so we populate it with the current orbit and associated encounters etc.
            List <Orbit> flightPlan = new List <Orbit>();

            for (Orbit orbit = AttachedVessel.orbit; orbit != null && orbit.activePatch; orbit = orbit.nextPatch)
            {
                if (AttachedVessel.patchedConicSolver.flightPlan.Contains(orbit))
                {
                    break;
                }
                flightPlan.Add(orbit);
            }

            foreach (Orbit orbit in AttachedVessel.patchedConicSolver.flightPlan)
            {
                flightPlan.Add(orbit);
            }


            Orbit nextPatch = null;

            if (startingState.StockPatch == null)
            {
                nextPatch = patch.SpaceOrbit.nextPatch;
            }
            else
            {
                int planIdx = flightPlan.IndexOf(startingState.StockPatch);
                if (planIdx >= 0 && planIdx < flightPlan.Count - 1)
                {
                    nextPatch = flightPlan[planIdx + 1];
                }
            }

            if (nextPatch != null)
            {
                patch.EndTime = nextPatch.StartUT;
            }

            double maxAtmosphereAltitude = RealMaxAtmosphereAltitude(body);

            if (!body.atmosphere)
            {
                maxAtmosphereAltitude = body.pqsController.mapMaxHeight;
            }

            double minAltitude = patch.SpaceOrbit.PeA;

            if (patch.SpaceOrbit.timeToPe < 0 || patch.EndTime < startingState.Time + patch.SpaceOrbit.timeToPe)
            {
                minAltitude = Math.Min(
                    patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).magnitude,
                    patch.SpaceOrbit.getRelativePositionAtUT(patch.StartingState.Time + 1.0).magnitude
                    ) - body.Radius;
            }

            if (minAltitude < maxAtmosphereAltitude)
            {
                double entryTime;
                if (startingState.Position.magnitude <= body.Radius + maxAtmosphereAltitude)
                {
                    // whole orbit is inside the atmosphere
                    entryTime = startingState.Time;
                }
                else
                {
                    entryTime = FindOrbitBodyIntersection(
                        patch.SpaceOrbit,
                        startingState.Time, startingState.Time + patch.SpaceOrbit.timeToPe,
                        body.Radius + maxAtmosphereAltitude);
                }

                if (entryTime > startingState.Time + 0.1 || !body.atmosphere)
                {
                    if (body.atmosphere)
                    {
                        // add the space patch before atmospheric entry

                        patch.EndTime = entryTime;
                        patchesBackBuffer_.Add(patch);
                        AddPatch_outState = new VesselState
                        {
                            Position      = patch.SpaceOrbit.getRelativePositionAtUT(entryTime).SwapYZ(),
                            ReferenceBody = body,
                            Time          = entryTime,
                            Velocity      = patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime).SwapYZ()
                        };
                    }
                    else
                    {
                        // the body has no atmosphere, so what we actually computed is the entry
                        // inside the "ground sphere" (defined by the maximal ground altitude)
                        // now we iterate until the inner ground sphere (minimal altitude), and
                        // check if we hit the ground along the way
                        double groundRangeExit = FindOrbitBodyIntersection(
                            patch.SpaceOrbit,
                            startingState.Time, startingState.Time + patch.SpaceOrbit.timeToPe,
                            body.Radius - maxAtmosphereAltitude);

                        if (groundRangeExit <= entryTime)
                        {
                            groundRangeExit = startingState.Time + patch.SpaceOrbit.timeToPe;
                        }

                        double iterationSize = (groundRangeExit - entryTime) / 100.0;
                        double t;
                        bool   groundImpact = false;
                        for (t = entryTime; t < groundRangeExit; t += iterationSize)
                        {
                            Vector3d pos            = patch.SpaceOrbit.getRelativePositionAtUT(t);
                            double   groundAltitude = GetGroundAltitude(body, CalculateRotatedPosition(body, pos.SwapYZ(), t))
                                                      + body.Radius;
                            if (pos.magnitude < groundAltitude)
                            {
                                t           -= iterationSize;
                                groundImpact = true;
                                break;
                            }
                        }

                        if (groundImpact)
                        {
                            patch.EndTime           = t;
                            patch.RawImpactPosition = patch.SpaceOrbit.getRelativePositionAtUT(t).SwapYZ();
                            patch.ImpactPosition    = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, t);
                            patch.ImpactVelocity    = patch.SpaceOrbit.getOrbitalVelocityAtUT(t).SwapYZ();
                            patchesBackBuffer_.Add(patch);
                            AddPatch_outState = null;
                        }
                        else
                        {
                            // no impact, just add the space orbit
                            patchesBackBuffer_.Add(patch);
                            if (nextPatch != null)
                            {
                                AddPatch_outState = new VesselState
                                {
                                    Position      = patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).SwapYZ(),
                                    Velocity      = patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime).SwapYZ(),
                                    ReferenceBody = nextPatch.referenceBody,
                                    Time          = patch.EndTime,
                                    StockPatch    = nextPatch
                                };
                            }
                            else
                            {
                                AddPatch_outState = null;
                            }
                        }
                    }
                }
                else
                {
                    if (patch.StartingState.ReferenceBody != AttachedVessel.mainBody)
                    {
                        // currently, we can't handle predictions for another body, so we stop
                        AddPatch_outState = null;
                        yield break;
                    }

                    // simulate atmospheric flight (drag and lift), until impact or atmosphere exit
                    // (typically for an aerobraking maneuver) assuming a constant angle of attack
                    patch.IsAtmospheric            = true;
                    patch.StartingState.StockPatch = null;

                    // lower dt would be more accurate, but a trade-off has to be found between performances and accuracy
                    double dt = Settings.IntegrationStepSize;

                    // some shallow entries can result in very long flight. For performances reasons,
                    // we limit the prediction duration
                    int maxIterations = (int)(60.0 * 60.0 / dt);

                    int chunkSize = 128;

                    // time between two consecutive stored positions (more intermediate positions are computed for better accuracy),
                    // also used for ground collision checks
                    double trajectoryInterval = 10.0;

                    List <Point[]> buffer = new List <Point[]>
                    {
                        new Point[chunkSize]
                    };
                    int nextPosIdx = 0;

                    SimulationState state;
                    state.position = patch.SpaceOrbit.getRelativePositionAtUT(entryTime).SwapYZ();
                    state.velocity = patch.SpaceOrbit.getOrbitalVelocityAtUT(entryTime).SwapYZ();

                    // Initialize a patch with zero acceleration
                    Vector3d currentAccel = new Vector3d(0.0, 0.0, 0.0);


                    double   currentTime               = entryTime;
                    double   lastPositionStoredUT      = 0;
                    Vector3d lastPositionStored        = new Vector3d();
                    bool     hitGround                 = false;
                    int      iteration                 = 0;
                    int      incrementIterations       = 0;
                    int      minIterationsPerIncrement = maxIterations / Settings.MaxFramesPerPatch;

                    #region Acceleration Functor

                    // function that calculates the acceleration under current parameters
                    Vector3d AccelerationFunc(Vector3d position, Vector3d velocity)
                    {
                        Profiler.Start("accelerationFunc inside");

                        // gravity acceleration
                        double   R_      = position.magnitude;
                        Vector3d accel_g = position * (-body.gravParameter / (R_ * R_ * R_));

                        // aero force
                        Vector3d vel_air = velocity - body.getRFrmVel(body.position + position);

                        double aoa = DescentProfile.GetAngleOfAttack(body, position, vel_air) ?? 0d;

                        Profiler.Start("GetForces");
                        Vector3d force_aero = aerodynamicModel_.GetForces(body, position, vel_air, aoa);

                        Profiler.Stop("GetForces");

                        Vector3d accel = accel_g + force_aero / aerodynamicModel_.Mass;

                        accel += API.HandleAccel(AttachedVessel, body, position, velocity, aoa);

                        Profiler.Stop("accelerationFunc inside");
                        return(accel);
                    }

                    #endregion


                    #region Integration Loop

                    while (true)
                    {
                        ++iteration;
                        ++incrementIterations;

                        if (incrementIterations > minIterationsPerIncrement && Util.ElapsedMilliseconds(increment_time) > MAX_INCREMENT_TIME)
                        {
                            yield return(false);

                            incrementIterations = 0;
                        }


                        double R               = state.position.magnitude;
                        double altitude        = R - body.Radius;
                        double atmosphereCoeff = altitude / maxAtmosphereAltitude;
                        if (hitGround ||
                            atmosphereCoeff <= 0.0 || atmosphereCoeff >= 1.0 ||
                            iteration == maxIterations || currentTime > patch.EndTime)
                        {
                            //Util.PostSingleScreenMessage("atmo force", "Atmospheric accumulated force: " + accumulatedForces.ToString("0.00"));

                            if (hitGround || atmosphereCoeff <= 0.0)
                            {
                                patch.RawImpactPosition = state.position;
                                patch.ImpactPosition    = CalculateRotatedPosition(body, patch.RawImpactPosition.Value, currentTime);
                                patch.ImpactVelocity    = state.velocity;
                            }

                            patch.EndTime = Math.Min(currentTime, patch.EndTime);

                            int totalCount = (buffer.Count - 1) * chunkSize + nextPosIdx;
                            patch.AtmosphericTrajectory = new Point[totalCount];
                            int outIdx = 0;
                            foreach (Point[] chunk in buffer)
                            {
                                foreach (Point p in chunk)
                                {
                                    if (outIdx == totalCount)
                                    {
                                        break;
                                    }
                                    patch.AtmosphericTrajectory[outIdx++] = p;
                                }
                            }

                            if (iteration == maxIterations)
                            {
                                ScreenMessages.PostScreenMessage("WARNING: trajectory prediction stopped, too many iterations");
                                patchesBackBuffer_.Add(patch);
                                AddPatch_outState = null;
                                yield break;
                            }

                            if (atmosphereCoeff <= 0.0 || hitGround)
                            {
                                patchesBackBuffer_.Add(patch);
                                AddPatch_outState = null;
                                yield break;
                            }

                            patchesBackBuffer_.Add(patch);
                            AddPatch_outState = new VesselState
                            {
                                Position      = state.position,
                                Velocity      = state.velocity,
                                ReferenceBody = body,
                                Time          = patch.EndTime
                            };
                            yield break;
                        }

                        Vector3d        lastAccel = currentAccel;
                        SimulationState lastState = state;

                        Profiler.Start("IntegrationStep");

                        // Verlet integration (more precise than using the velocity)
                        // state = VerletStep(state, accelerationFunc, dt);
                        state = RK4Step(state, AccelerationFunc, dt, out currentAccel);

                        currentTime += dt;

                        // KSP presumably uses Euler integration for position updates. Since RK4 is actually more precise than that,
                        // we try to reintroduce an approximation of the error.

                        // The local truncation error for euler integration is:
                        // LTE = 1/2 * h^2 * y''(t)
                        // https://en.wikipedia.org/wiki/Euler_method#Local_truncation_error
                        //
                        // For us,
                        // h is the time step of the outer simulation (KSP), which is the physics time step
                        // y''(t) is the difference of the velocity/acceleration divided by the physics time step
                        state.position += 0.5 * TimeWarp.fixedDeltaTime * currentAccel * dt;
                        state.velocity += 0.5 * TimeWarp.fixedDeltaTime * (currentAccel - lastAccel);

                        Profiler.Stop("IntegrationStep");

                        // calculate gravity and aerodynamic force
                        Vector3d gravityAccel     = lastState.position * (-body.gravParameter / (R * R * R));
                        Vector3d aerodynamicForce = (currentAccel - gravityAccel) / aerodynamicModel_.Mass;

                        // acceleration in the vessel reference frame is acceleration - gravityAccel
                        maxAccelBackBuffer_ = Math.Max(
                            (float)(aerodynamicForce.magnitude / aerodynamicModel_.Mass),
                            maxAccelBackBuffer_);

                        #region Impact Calculation

                        Profiler.Start("AddPatch#impact");

                        double interval = altitude < 10000.0 ? trajectoryInterval * 0.1 : trajectoryInterval;
                        if (currentTime >= lastPositionStoredUT + interval)
                        {
                            double groundAltitude = GetGroundAltitude(body, CalculateRotatedPosition(body, state.position, currentTime));
                            if (lastPositionStoredUT > 0)
                            {
                                // check terrain collision, to detect impact on mountains etc.
                                Vector3 rayOrigin         = lastPositionStored;
                                Vector3 rayEnd            = state.position;
                                double  absGroundAltitude = groundAltitude + body.Radius;
                                if (absGroundAltitude > rayEnd.magnitude)
                                {
                                    hitGround = true;
                                    float coeff = Math.Max(0.01f, (float)((absGroundAltitude - rayOrigin.magnitude)
                                                                          / (rayEnd.magnitude - rayOrigin.magnitude)));
                                    state.position = rayEnd * coeff + rayOrigin * (1.0f - coeff);
                                    currentTime    = currentTime * coeff + lastPositionStoredUT * (1.0f - coeff);
                                }
                            }

                            lastPositionStoredUT = currentTime;
                            if (nextPosIdx == chunkSize)
                            {
                                buffer.Add(new Point[chunkSize]);
                                nextPosIdx = 0;
                            }

                            Vector3d nextPos = state.position;
                            if (Settings.BodyFixedMode)
                            {
                                nextPos = CalculateRotatedPosition(body, nextPos, currentTime);
                            }

                            buffer.Last()[nextPosIdx].aerodynamicForce = aerodynamicForce;
                            buffer.Last()[nextPosIdx].orbitalVelocity  = state.velocity;
                            buffer.Last()[nextPosIdx].groundAltitude   = (float)groundAltitude;
                            buffer.Last()[nextPosIdx].time             = currentTime;
                            buffer.Last()[nextPosIdx++].pos            = nextPos;
                            lastPositionStored = state.position;
                        }

                        Profiler.Stop("AddPatch#impact");

                        #endregion
                    }

                    #endregion
                }
            }
            else
            {
                // no atmospheric entry, just add the space orbit
                patchesBackBuffer_.Add(patch);
                if (nextPatch != null)
                {
                    AddPatch_outState = new VesselState
                    {
                        Position      = patch.SpaceOrbit.getRelativePositionAtUT(patch.EndTime).SwapYZ(),
                        Velocity      = patch.SpaceOrbit.getOrbitalVelocityAtUT(patch.EndTime).SwapYZ(),
                        ReferenceBody = nextPatch.referenceBody,
                        Time          = patch.EndTime,
                        StockPatch    = nextPatch
                    };
                }
                else
                {
                    AddPatch_outState = null;
                }
            }
        }
Пример #22
0
        private static void MainWindow(int id)
        {
            GUILayout.BeginHorizontal();

            Settings.DisplayTrajectories = GUILayout.Toggle(Settings.DisplayTrajectories, "Show trajectory", GUILayout.Width(125));

            Settings.DisplayTrajectoriesInFlight = GUILayout.Toggle(Settings.DisplayTrajectoriesInFlight, "In-Flight");

            // check that we have patched conics. If not, apologize to the user and return.
            if (Settings.DisplayTrajectories && !Util.IsPatchedConicsAvailable)
            {
                ScreenMessages.PostScreenMessage(
                    "Can't show trajectory because patched conics are not available." +
                    " Please update your tracking station facility.");
                Settings.DisplayTrajectories = false;
                return;
            }

            GUILayout.EndHorizontal();

            GUILayout.BeginHorizontal();

            Settings.BodyFixedMode = GUILayout.Toggle(Settings.BodyFixedMode, "Body-fixed mode");

            if (Settings.DisplayTrajectories)
            {
                Settings.DisplayCompleteTrajectory = GUILayout.Toggle(Settings.DisplayCompleteTrajectory, "complete", GUILayout.Width(70));
            }

            GUILayout.EndHorizontal();

            GUILayout.Label("Max G-force: " + guistring_gForce);

            GUILayout.Label(guistring_impactVelocity);
            GUILayout.Space(10);


            if (Settings.DisplayTargetGUI = ToggleGroup(Settings.DisplayTargetGUI, "Target"))
            {
                GUI.enabled = TargetProfile.WorldPosition.HasValue;

                GUILayout.Label(guistring_targetDistance);

                if (GUILayout.Button("Unset target"))
                {
                    TargetProfile.Clear();
                }
                GUI.enabled = true;

                GUILayout.BeginHorizontal();
                Trajectory.Patch patch = Trajectory.Patches.LastOrDefault();
                GUI.enabled = (patch != null && patch.ImpactPosition.HasValue);
                if (GUILayout.Button("Set current impact", GUILayout.Width(150)))
                {
                    if (patch.ImpactPosition.HasValue)
                    {
                        TargetProfile.SetFromWorldPos(patch.StartingState.ReferenceBody, patch.ImpactPosition.Value);
                    }
                }
                GUI.enabled = true;
                if (GUILayout.Button("Set KSC", GUILayout.Width(70)))
                {
                    CelestialBody homebody = FlightGlobals.GetHomeBody();

                    double latitude  = SpaceCenter.Instance.Latitude;
                    double longitude = SpaceCenter.Instance.Longitude;

                    if (homebody != null)
                    {
                        TargetProfile.SetFromLatLonAlt(homebody, latitude, longitude);
                    }
                }
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();

                Vessel targetVessel = FlightGlobals.fetch.VesselTarget?.GetVessel();
                GUI.enabled = (patch != null && targetVessel != null && targetVessel.Landed
                               // && targetVessel.lastBody == patch.startingState.referenceBody
                               );
                if (GUILayout.Button("Target vessel"))
                {
                    TargetProfile.SetFromWorldPos(targetVessel.lastBody, targetVessel.GetWorldPos3D() - targetVessel.lastBody.position);
                    ScreenMessages.PostScreenMessage("Targeting vessel " + targetVessel.GetName());
                }

                FinePrint.Waypoint navigationWaypoint = Trajectories.AttachedVessel?.navigationWaypoint;
                GUI.enabled = (navigationWaypoint != null);
                if (GUILayout.Button("Active waypoint"))
                {
                    TargetProfile.SetFromLatLonAlt(navigationWaypoint.celestialBody,
                                                   navigationWaypoint.latitude, navigationWaypoint.longitude, navigationWaypoint.altitude);
                    ScreenMessages.PostScreenMessage("Targeting waypoint " + navigationWaypoint.name);
                }
                GUILayout.EndHorizontal();

                GUI.enabled = true;

                GUILayout.BeginHorizontal();
                coords = GUILayout.TextField(TargetProfile.ManualText, GUILayout.Width(170));
                if (coords != TargetProfile.ManualText)
                {
                    TargetProfile.ManualText = coords;
                    TargetProfile.Save();
                }
                if (GUILayout.Button(new GUIContent("Set",
                                                    "Enter target latitude and longitude, separated by a comma, in decimal format (with a dot for decimal separator)"),
                                     GUILayout.Width(50)))
                {
                    string[]      latLng = coords.Split(new char[] { ',', ';' });
                    CelestialBody body   = FlightGlobals.currentMainBody;
                    if (latLng.Length == 2 && body != null)
                    {
                        double lat;
                        double lng;
                        if (double.TryParse(latLng[0].Trim(), out lat) && double.TryParse(latLng[1].Trim(), out lng))
                        {
                            TargetProfile.SetFromLatLonAlt(body, lat, lng);
                        }
                    }
                }
                GUILayout.EndHorizontal();
            }

            GUILayout.Space(10);

            GUILayout.BeginHorizontal();
            bool descentProfileGroup = Settings.DisplayDescentProfileGUI = ToggleGroup(Settings.DisplayDescentProfileGUI, "Descent profile", 120);

            DescentProfile.DoQuickControlsGUI();
            GUILayout.EndHorizontal();
            if (descentProfileGroup)
            {
                DescentProfile.DoGUI();
            }
            GUILayout.Space(10);

            if (Settings.DisplaySettingsGUI = ToggleGroup(Settings.DisplaySettingsGUI, "Settings"))
            {
                GUILayout.BeginHorizontal();
                GUILayout.Label("Max patches", GUILayout.Width(100));
                Settings.MaxPatchCount = Mathf.RoundToInt(GUILayout.HorizontalSlider((float)Settings.MaxPatchCount, 3, 10, GUILayout.Width(100)));
                GUILayout.Label(Settings.MaxPatchCount.ToString(), GUILayout.Width(15));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                GUILayout.Label("Max frames per patch", GUILayout.Width(100));
                Settings.MaxFramesPerPatch = Mathf.RoundToInt(GUILayout.HorizontalSlider((float)Settings.MaxFramesPerPatch, 1, 50, GUILayout.Width(100)));
                GUILayout.Label(Settings.MaxFramesPerPatch.ToString(), GUILayout.Width(15));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                Settings.UseCache = GUILayout.Toggle(Settings.UseCache, new GUIContent("Use Cache", "Toggle cache usage. Trajectory will be more precise when cache disabled, but computation time will be higher. It's not recommended to keep it unchecked, unless your CPU can handle the load."), GUILayout.Width(80));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                Settings.AutoUpdateAerodynamicModel = GUILayout.Toggle(Settings.AutoUpdateAerodynamicModel, new GUIContent("Auto update", "Auto-update of the aerodynamic model. For example if a part is decoupled, the model needs to be updated. This is independent from trajectory update."));
                if (GUILayout.Button("Update now"))
                {
                    Trajectory.InvalidateAerodynamicModel();
                }
                GUILayout.EndHorizontal();

                if (ToolbarManager.ToolbarAvailable)
                {
                    Settings.UseBlizzyToolbar = GUILayout.Toggle(Settings.UseBlizzyToolbar, new GUIContent("Use Blizzy's toolbar", "Will take effect after restart"));
                }

                if (Trajectories.IsVesselAttached)
                {
                    GUILayout.Label("Position:");
                    GUILayout.BeginHorizontal();
                    GUILayout.Label("lat=" + guistring_Latitude, GUILayout.Width(110));
                    GUILayout.Label("lng=" + guistring_Longitude, GUILayout.Width(110));
                    GUILayout.EndHorizontal();
                }

                GUILayout.Label("Aerodynamic model: " + Trajectory.AerodynamicModelName);
                GUILayout.BeginHorizontal();
                GUILayout.Label(string.Format("Perf: {0,5:F1}ms ({1,4:F1})%",
                                              Trajectory.ComputationTime * 1000.0f,
                                              Trajectory.ComputationTime / Trajectory.GameFrameTime * 100.0f
                                              ), GUILayout.Width(130));
                GUILayout.Label(Trajectory.ErrorCount + " error(s)", GUILayout.Width(80));
                GUILayout.EndHorizontal();

                GUILayout.BeginHorizontal();
                if (GUILayout.Toggle(Settings.NewGui, new GUIContent("New Gui", "Swap to the New Gui")))
                {
                    Settings.NewGui         = true;
                    Settings.MainGUIEnabled = true;
                    Settings.GUIEnabled     = false;
                    InputLockManager.RemoveControlLock("TrajectoriesFlightLock");
                    clickThroughLocked = false;
                }
                else
                {
                    Settings.NewGui         = false;
                    Settings.MainGUIEnabled = false;
                    Settings.GUIEnabled     = true;
                }
                GUILayout.EndHorizontal();
            }

            tooltip = GUI.tooltip;

            GUI.DragWindow();
        }