예제 #1
0
        //Estimate the delta-V of the correction burn that would be required to put us on
        //course for the target
        public Vector3d ComputeCourseCorrection(bool allowPrograde)
        {
            //actualLandingPosition is the predicted actual landing position
            Vector3d actualLandingPosition = RotatedLandingSite - mainBody.position;

            //orbitLandingPosition is the point where our current orbit intersects the planet
            double   endRadius            = mainBody.Radius + DecelerationEndAltitude() - 100;
            Vector3d orbitLandingPosition = orbit.SwappedRelativePositionAtUT(orbit.NextTimeOfRadius(vesselState.time, endRadius));

            //convertOrbitToActual is a rotation that rotates orbitLandingPosition on actualLandingPosition
            Quaternion convertOrbitToActual = Quaternion.FromToRotation(orbitLandingPosition, actualLandingPosition);

            //Consider the effect small changes in the velocity in each of these three directions
            Vector3d[] perturbationDirections = { vesselState.velocityVesselSurfaceUnit, vesselState.radialPlusSurface, vesselState.normalPlusSurface };

            //Compute the effect burns in these directions would
            //have on the landing position, where we approximate the landing position as the place
            //the perturbed orbit would intersect the planet.
            Vector3d[] deltas = new Vector3d[3];
            for (int i = 0; i < 3; i++)
            {
                double perturbationDeltaV = 1;                                                                                      //warning: hard experience shows that setting this too low leads to bewildering bugs due to finite precision of Orbit functions
                Orbit  perturbedOrbit     = orbit.PerturbedOrbit(vesselState.time, perturbationDeltaV * perturbationDirections[i]); //compute the perturbed orbit
                double perturbedLandingTime;
                if (perturbedOrbit.PeR < endRadius)
                {
                    perturbedLandingTime = perturbedOrbit.NextTimeOfRadius(vesselState.time, endRadius);
                }
                else
                {
                    perturbedLandingTime = perturbedOrbit.NextPeriapsisTime(vesselState.time);
                }
                Vector3d perturbedLandingPosition = perturbedOrbit.SwappedRelativePositionAtUT(perturbedLandingTime); //find where it hits the planet
                Vector3d landingDelta             = perturbedLandingPosition - orbitLandingPosition;                  //find the difference between that and the original orbit's intersection point
                landingDelta = convertOrbitToActual * landingDelta;                                                   //rotate that difference vector so that we can now think of it as starting at the actual landing position
                landingDelta = Vector3d.Exclude(actualLandingPosition, landingDelta);                                 //project the difference vector onto the plane tangent to the actual landing position
                deltas[i]    = landingDelta / perturbationDeltaV;                                                     //normalize by the delta-V considered, so that deltas now has units of meters per (meter/second) [i.e., seconds]
            }

            //Now deltas stores the predicted offsets in landing position produced by each of the three perturbations.
            //We now figure out the offset we actually want

            //First we compute the target landing position. We have to convert the latitude and longitude of the target
            //into a position. We can't just get the current position of those coordinates, because the planet will
            //rotate during the descent, so we have to account for that.
            Vector3d   desiredLandingPosition         = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0);
            float      bodyRotationAngleDuringDescent = (float)(360 * (prediction.endUT - vesselState.time) / mainBody.rotationPeriod);
            Quaternion bodyRotationDuringFall         = Quaternion.AngleAxis(bodyRotationAngleDuringDescent, mainBody.angularVelocity.normalized);

            desiredLandingPosition = bodyRotationDuringFall * desiredLandingPosition;

            Vector3d desiredDelta = desiredLandingPosition - actualLandingPosition;

            desiredDelta = Vector3d.Exclude(actualLandingPosition, desiredDelta);

            //Now desiredDelta gives the desired change in our actual landing position (projected onto a plane
            //tangent to the actual landing position).

            Vector3d downrangeDirection;
            Vector3d downrangeDelta;

            if (allowPrograde)
            {
                //Construct the linear combination of the prograde and radial+ perturbations
                //that produces the largest effect on the landing position. The Math.Sign is to
                //detect and handle the case where radial+ burns actually bring the landing sign closer
                //(e.g. when we are traveling close to straight up)
                downrangeDirection = (deltas[0].magnitude * perturbationDirections[0]
                                      + Math.Sign(Vector3d.Dot(deltas[0], deltas[1])) * deltas[1].magnitude * perturbationDirections[1]).normalized;

                downrangeDelta = Vector3d.Dot(downrangeDirection, perturbationDirections[0]) * deltas[0]
                                 + Vector3d.Dot(downrangeDirection, perturbationDirections[1]) * deltas[1];
            }
            else
            {
                //If we aren't allowed to burn prograde, downrange component of the landing
                //position has to be controlled by radial+/- burns:
                downrangeDirection = perturbationDirections[1];
                downrangeDelta     = deltas[1];
            }

            //Now solve a 2x2 system of linear equations to determine the linear combination
            //of perturbationDirection01 and normal+ that will give the desired offset in the
            //predicted landing position.
            Matrix2x2 A = new Matrix2x2(
                downrangeDelta.sqrMagnitude, Vector3d.Dot(downrangeDelta, deltas[2]),
                Vector3d.Dot(downrangeDelta, deltas[2]), deltas[2].sqrMagnitude
                );

            Vector2d b = new Vector2d(Vector3d.Dot(desiredDelta, downrangeDelta), Vector3d.Dot(desiredDelta, deltas[2]));

            Vector2d coeffs = A.inverse() * b;

            Vector3d courseCorrection = coeffs.x * downrangeDirection + coeffs.y * perturbationDirections[2];

            return(courseCorrection);
        }
        // Estimate the delta-V of the correction burn that would be required to put us on
        // course for the target
        public Vector3d ComputeCourseCorrection(bool allowPrograde)
        {
            // actualLandingPosition is the predicted actual landing position
            Vector3d actualLandingPosition = RotatedLandingSite - mainBody.position;

            // orbitLandingPosition is the point where our current orbit intersects the planet
            double endRadius = mainBody.Radius + DecelerationEndAltitude() - 100;

            // Seems we are already landed ?
            if (endRadius > orbit.ApR || vessel.LandedOrSplashed)
                StopLanding();

            Vector3d orbitLandingPosition;
            if (orbit.PeR < endRadius)
                orbitLandingPosition = orbit.SwappedRelativePositionAtUT(orbit.NextTimeOfRadius(vesselState.time, endRadius));
            else
                orbitLandingPosition = orbit.SwappedRelativePositionAtUT(orbit.NextPeriapsisTime(vesselState.time));

            // convertOrbitToActual is a rotation that rotates orbitLandingPosition on actualLandingPosition
            Quaternion convertOrbitToActual = Quaternion.FromToRotation(orbitLandingPosition, actualLandingPosition);

            // Consider the effect small changes in the velocity in each of these three directions
            Vector3d[] perturbationDirections = { vesselState.surfaceVelocity.normalized, vesselState.radialPlusSurface, vesselState.normalPlusSurface };

            // Compute the effect burns in these directions would
            // have on the landing position, where we approximate the landing position as the place
            // the perturbed orbit would intersect the planet.
            Vector3d[] deltas = new Vector3d[3];
            for (int i = 0; i < 3; i++)
            {
                const double perturbationDeltaV = 1; //warning: hard experience shows that setting this too low leads to bewildering bugs due to finite precision of Orbit functions
                Orbit perturbedOrbit = orbit.PerturbedOrbit(vesselState.time, perturbationDeltaV * perturbationDirections[i]); //compute the perturbed orbit
                double perturbedLandingTime;
                if (perturbedOrbit.PeR < endRadius) perturbedLandingTime = perturbedOrbit.NextTimeOfRadius(vesselState.time, endRadius);
                else perturbedLandingTime = perturbedOrbit.NextPeriapsisTime(vesselState.time);
                Vector3d perturbedLandingPosition = perturbedOrbit.SwappedRelativePositionAtUT(perturbedLandingTime); //find where it hits the planet
                Vector3d landingDelta = perturbedLandingPosition - orbitLandingPosition; //find the difference between that and the original orbit's intersection point
                landingDelta = convertOrbitToActual * landingDelta; //rotate that difference vector so that we can now think of it as starting at the actual landing position
                landingDelta = Vector3d.Exclude(actualLandingPosition, landingDelta); //project the difference vector onto the plane tangent to the actual landing position
                deltas[i] = landingDelta / perturbationDeltaV; //normalize by the delta-V considered, so that deltas now has units of meters per (meter/second) [i.e., seconds]
            }

            // Now deltas stores the predicted offsets in landing position produced by each of the three perturbations. 
            // We now figure out the offset we actually want

            // First we compute the target landing position. We have to convert the latitude and longitude of the target
            // into a position. We can't just get the current position of those coordinates, because the planet will
            // rotate during the descent, so we have to account for that.
            Vector3d desiredLandingPosition = mainBody.GetRelSurfacePosition(core.target.targetLatitude, core.target.targetLongitude, 0);
            float bodyRotationAngleDuringDescent = (float)(360 * (prediction.endUT - vesselState.time) / mainBody.rotationPeriod);
            Quaternion bodyRotationDuringFall = Quaternion.AngleAxis(bodyRotationAngleDuringDescent, mainBody.angularVelocity.normalized);
            desiredLandingPosition = bodyRotationDuringFall * desiredLandingPosition;

            Vector3d desiredDelta = desiredLandingPosition - actualLandingPosition;
            desiredDelta = Vector3d.Exclude(actualLandingPosition, desiredDelta);

            // Now desiredDelta gives the desired change in our actual landing position (projected onto a plane
            // tangent to the actual landing position).

            Vector3d downrangeDirection;
            Vector3d downrangeDelta;
            if (allowPrograde)
            {
                // Construct the linear combination of the prograde and radial+ perturbations 
                // that produces the largest effect on the landing position. The Math.Sign is to
                // detect and handle the case where radial+ burns actually bring the landing sign closer
                // (e.g. when we are traveling close to straight up)
                downrangeDirection = (deltas[0].magnitude * perturbationDirections[0]
                    + Math.Sign(Vector3d.Dot(deltas[0], deltas[1])) * deltas[1].magnitude * perturbationDirections[1]).normalized;

                downrangeDelta = Vector3d.Dot(downrangeDirection, perturbationDirections[0]) * deltas[0]
                    + Vector3d.Dot(downrangeDirection, perturbationDirections[1]) * deltas[1];
            }
            else
            {
                // If we aren't allowed to burn prograde, downrange component of the landing
                // position has to be controlled by radial+/- burns:
                downrangeDirection = perturbationDirections[1];
                downrangeDelta = deltas[1];
            }

            // Now solve a 2x2 system of linear equations to determine the linear combination
            // of perturbationDirection01 and normal+ that will give the desired offset in the
            // predicted landing position.
            Matrix2x2 A = new Matrix2x2(
                downrangeDelta.sqrMagnitude, Vector3d.Dot(downrangeDelta, deltas[2]),
                Vector3d.Dot(downrangeDelta, deltas[2]), deltas[2].sqrMagnitude
            );

            Vector2d b = new Vector2d(Vector3d.Dot(desiredDelta, downrangeDelta), Vector3d.Dot(desiredDelta, deltas[2]));

            Vector2d coeffs = A.inverse() * b;

            Vector3d courseCorrection = coeffs.x * downrangeDirection + coeffs.y * perturbationDirections[2];

            return courseCorrection;
        }
        ///////////////////////////////////////
        // REENTRY SIMULATION /////////////////
        ///////////////////////////////////////
        //figure out what we wish our velocity were right now if we want to land at the user's target
        //we do this by first doing a pair of hypothetical simulations, which we have extra velocity in
        //two different orthogonal directions. By observing how this affects the landing site we compute the change in our velocity
        //necessary to produce the desired change in our predicted landing location
        void computeVelocityCorrection()
        {
            double offsetMagnitude = -5.0;

            //simulate a trajectory where we have extra velocity in the A direction
            LandingPrediction resultA = simulateReentryRK4(simulationTimeStep * 10.0, offsetMagnitude * velAUnit);
            //if simulation doesn't hit the ground, we should burn retrograde until it does. set a goal that will cause us to deorbit
            if (resultA.outcome != LandingPrediction.Outcome.LANDED)
            {
                if (landStep == LandStep.COURSE_CORRECTIONS && resultA.outcome == LandingPrediction.Outcome.NO_REENTRY)
                {
                    gaveUpOnCourseCorrections = true;
                    turnOffSteering();
                    core.controlRelease(this);
                    print("A: gaveup, end condition = " + resultA.outcome);
                }
                velocityCorrection = -10 * vesselState.velocityVesselSurfaceUnit;
                return;
            }

            //simulate a trajectory where we have extra velocity in the B direction
            LandingPrediction resultB = simulateReentryRK4(simulationTimeStep * 10.0, offsetMagnitude * velBUnit);
            if (resultB.outcome != LandingPrediction.Outcome.LANDED)
            {
                if (landStep == LandStep.COURSE_CORRECTIONS && resultB.outcome == LandingPrediction.Outcome.NO_REENTRY)
                {
                    gaveUpOnCourseCorrections = true;
                    turnOffSteering();
                    core.controlRelease(this);
                    print("B: gaveup, end condition = " + resultB.outcome);
                }
                velocityCorrection = -10 * vesselState.velocityVesselSurfaceUnit;
                return;
            }

            //simulate a trajectory with our current velocity
            LandingPrediction resultCurrent = simulateReentryRK4(simulationTimeStep * 10.0, Vector3d.zero);
            if (resultCurrent.outcome != LandingPrediction.Outcome.LANDED)
            {
                if (landStep == LandStep.COURSE_CORRECTIONS && resultCurrent.outcome == LandingPrediction.Outcome.NO_REENTRY)
                {
                    gaveUpOnCourseCorrections = true;
                    turnOffSteering();
                    core.controlRelease(this);
                    print("C: gaveup, end condition = " + resultCurrent.outcome);
                }
                velocityCorrection = -10 * vesselState.velocityVesselSurfaceUnit;
                return;
            }

            Vector2d sepA = latLonSeparation(resultCurrent.landingLatitude, resultCurrent.landingLongitude,
                                             resultA.landingLatitude, resultA.landingLongitude) / offsetMagnitude;

            Vector2d sepB = latLonSeparation(resultCurrent.landingLatitude, resultCurrent.landingLongitude,
                                 resultB.landingLatitude, resultB.landingLongitude) / offsetMagnitude;

            //construct a matrix that, when multiplied by a velocity offset vector2, gives the resulting offset in the landing position
            Matrix2x2 partialsMatrix = new Matrix2x2(sepA.x, sepB.x, sepA.y, sepB.y);

            //invert this to get a matrix that tells how to change our velocity to produce a
            //give change in our landing site:
            Matrix2x2 inversePartialsMatrix = partialsMatrix.inverse();

            Vector2d sepDesired = latLonSeparation(prediction.landingLatitude, prediction.landingLongitude, targetLatitude, targetLongitude);

            Vector2d correctionMagnitudes = inversePartialsMatrix.times(sepDesired);

            velocityCorrection = correctionMagnitudes.x * velAUnit + correctionMagnitudes.y * velBUnit;
            velocityCorrectionSet = true;
        }