C# Orbit.UpdateFromStateVectors方法代码示例

 public static Orbit computeOrbit(Vessel vessel, Vector3d deltaV, double UT)
 {
     Orbit ret = new Orbit();
     ret.UpdateFromStateVectors(vessel.findWorldCenterOfMass()-vessel.mainBody.position, vessel.orbit.GetVel() + deltaV, vessel.mainBody, UT);
     return ret;
 }

 private Orbit createOrbitFromState(VesselState state)
 {
     var orbit = new Orbit();
     orbit.UpdateFromStateVectors(Util.SwapYZ(state.position), Util.SwapYZ(state.velocity), state.referenceBody, state.time);
     return orbit;
 }

 public static Orbit CreateRandomOrbitFlyBy(Orbit targetOrbit, double timeToPeriapsis, double periapsis, double deltaVatPeriapsis)
 {
     double universalTime = Planetarium.GetUniversalTime();
     Vector3d relativePositionAtUT = targetOrbit.getRelativePositionAtUT(universalTime + timeToPeriapsis);
     Vector3d orbitalVelocityAtUT = targetOrbit.getOrbitalVelocityAtUT(universalTime + timeToPeriapsis);
     Orbit orbit = new Orbit();
     Vector3d vector3d = relativePositionAtUT + (Vector3d)UnityEngine.Random.onUnitSphere * periapsis;
     Vector3d vector3d2 = orbitalVelocityAtUT + (orbitalVelocityAtUT.normalized + UnityEngine.Random.onUnitSphere) * deltaVatPeriapsis;
     orbit.UpdateFromStateVectors(vector3d, vector3d2, targetOrbit.referenceBody, universalTime + timeToPeriapsis);
     return orbit;
 }

 /// <summary>
 /// Set vessel orbit, transform, coordinates.
 /// </summary>
 /// <param name="sv">Stored vessel</param>
 void position_vessel(StoredVessel sv)
 {
     ProtoVessel pv = sv.vessel;
     //state
     pv.splashed = vessel.Landed;
     pv.landed   = vessel.Splashed;
     //rotation
     //it is essential to use BackupVessel() instead of vessel.protoVessel,
     //because in general the latter does not store the current flight state of the vessel
     ProtoVessel hpv = vessel.BackupVessel();
     Quaternion proto_rot  = hpv.rotation;
     Quaternion hangar_rot = vessel.vesselTransform.rotation;
     //rotate launchTransform.rotation to protovessel's reference frame
     pv.rotation = proto_rot*hangar_rot.Inverse()*launchTransform.rotation;
     //calculate launch offset from vessel bounds
     Vector3 bounds_offset = launchTransform.TransformDirection(sv.CoM - sv.CoG);
     //set vessel's orbit
     Orbit horb = vessel.orbit;
     Orbit vorb = new Orbit();
     Vector3 d_pos = launchTransform.position-vessel.findWorldCenterOfMass()+bounds_offset;
     Vector3d vpos = horb.pos+new Vector3d(d_pos.x, d_pos.z, d_pos.y);
     vorb.UpdateFromStateVectors(vpos, horb.vel, horb.referenceBody, Planetarium.GetUniversalTime());
     pv.orbitSnapShot = new OrbitSnapshot(vorb);
     //position on a surface
     if(vessel.LandedOrSplashed)
     {
         //calculate launch offset from vessel bounds
         bounds_offset = launchTransform.TransformDirection(-sv.CoG);
         //set vessel's position
         vpos = Vector3d.zero+launchTransform.position+bounds_offset;
         pv.longitude  = vessel.mainBody.GetLongitude(vpos);
         pv.latitude   = vessel.mainBody.GetLatitude(vpos);
         pv.altitude   = vessel.mainBody.GetAltitude(vpos);
     }
 }

 private void UpdateBody(CelestialBody body, double universal_time)
 {
     plugin_.UpdateCelestialHierarchy(
     body.flightGlobalsIndex,
     body.orbit.referenceBody.flightGlobalsIndex);
     QP from_parent = plugin_.CelestialFromParent(body.flightGlobalsIndex);
     // TODO(egg): Some of this might be be superfluous and redundant.
     Orbit original = body.orbit;
     Orbit copy = new Orbit(original.inclination, original.eccentricity,
                original.semiMajorAxis, original.LAN,
                original.argumentOfPeriapsis,
                original.meanAnomalyAtEpoch, original.epoch,
                original.referenceBody);
     copy.UpdateFromStateVectors((Vector3d)from_parent.q,
                     (Vector3d)from_parent.p,
                     copy.referenceBody,
                     universal_time);
     body.orbit.inclination = copy.inclination;
     body.orbit.eccentricity = copy.eccentricity;
     body.orbit.semiMajorAxis = copy.semiMajorAxis;
     body.orbit.LAN = copy.LAN;
     body.orbit.argumentOfPeriapsis = copy.argumentOfPeriapsis;
     body.orbit.meanAnomalyAtEpoch = copy.meanAnomalyAtEpoch;
     body.orbit.epoch = copy.epoch;
     body.orbit.referenceBody = copy.referenceBody;
     body.orbit.Init();
     body.orbit.UpdateFromUT(universal_time);
     body.CBUpdate();
     body.orbit.UpdateFromStateVectors((Vector3d)from_parent.q,
                           (Vector3d)from_parent.p,
                           copy.referenceBody,
                           universal_time);
 }

        public override void OnFixedUpdate() {
            if (FlightGlobals.fetch != null) {
                solar_force_d = 0;
                if (!IsEnabled) { return; }
                double sunlightFactor = 1.0;
                Vector3 sunVector = FlightGlobals.fetch.bodies[0].position - part.orgPos;

                if (!PluginHelper.lineOfSightToSun(vessel)) {
                    sunlightFactor = 0.0f;
                }

                //Debug.Log("Detecting sunlight: " + sunlightFactor.ToString());
                Vector3d solarForce = CalculateSolarForce() * sunlightFactor;
                //print(surfaceArea);

                Vector3d solar_accel = solarForce / vessel.GetTotalMass() / 1000.0 * TimeWarp.fixedDeltaTime;
                if (!this.vessel.packed) {
                    vessel.ChangeWorldVelocity(solar_accel);
                } else {
                    if (sunlightFactor > 0) {
                        double temp1 = solar_accel.y;
                        solar_accel.y = solar_accel.z;
                        solar_accel.z = temp1;
                        Vector3d position = vessel.orbit.getRelativePositionAtUT(Planetarium.GetUniversalTime());
                        Orbit orbit2 = new Orbit(vessel.orbit.inclination, vessel.orbit.eccentricity, vessel.orbit.semiMajorAxis, vessel.orbit.LAN, vessel.orbit.argumentOfPeriapsis, vessel.orbit.meanAnomalyAtEpoch, vessel.orbit.epoch, vessel.orbit.referenceBody);
                        orbit2.UpdateFromStateVectors(position, vessel.orbit.vel + solar_accel, vessel.orbit.referenceBody, Planetarium.GetUniversalTime());
                        //print(orbit2.timeToAp);
                        if (!double.IsNaN(orbit2.inclination) && !double.IsNaN(orbit2.eccentricity) && !double.IsNaN(orbit2.semiMajorAxis) && orbit2.timeToAp > TimeWarp.fixedDeltaTime) {
                            vessel.orbit.inclination = orbit2.inclination;
                            vessel.orbit.eccentricity = orbit2.eccentricity;
                            vessel.orbit.semiMajorAxis = orbit2.semiMajorAxis;
                            vessel.orbit.LAN = orbit2.LAN;
                            vessel.orbit.argumentOfPeriapsis = orbit2.argumentOfPeriapsis;
                            vessel.orbit.meanAnomalyAtEpoch = orbit2.meanAnomalyAtEpoch;
                            vessel.orbit.epoch = orbit2.epoch;
                            vessel.orbit.referenceBody = orbit2.referenceBody;
                            vessel.orbit.Init();

                            //vessel.orbit.UpdateFromOrbitAtUT(orbit2, Planetarium.GetUniversalTime(), orbit2.referenceBody);
                            vessel.orbit.UpdateFromUT(Planetarium.GetUniversalTime());
                        }

                    }
                }
                solar_force_d = solarForce.magnitude;
                solar_acc_d = solar_accel.magnitude / TimeWarp.fixedDeltaTime;
                //print(solarForce.x.ToString() + ", " + solarForce.y.ToString() + ", " + solarForce.z.ToString());
            }
            count++;
        }

 /// <summary>
 /// Set vessel orbit, transform, coordinates.
 /// </summary>
 /// <param name="sv">Stored vessel</param>
 void position_vessel(StoredVessel sv)
 {
     var pv = sv.proto_vessel;
     //state
     pv.splashed = vessel.Splashed;
     pv.landed   = vessel.Landed;
     pv.landedAt = vessel.landedAt;
     //rotation
     //rotate spawn_transform.rotation to protovessel's reference frame
     var spawn_transform = get_spawn_transform(sv);
     pv.rotation = vessel.mainBody.bodyTransform.rotation.Inverse() * spawn_transform.rotation;
     //set vessel's orbit
     var UT    = Planetarium.GetUniversalTime();
     var horb  = vessel.orbitDriver.orbit;
     var vorb  = new Orbit();
     var d_pos = spawn_transform.position-vessel.CurrentCoM+get_vessel_offset(spawn_transform, sv);
     var vpos  = horb.pos - horb.GetRotFrameVel(horb.referenceBody)*TimeWarp.fixedDeltaTime + new Vector3d(d_pos.x, d_pos.z, d_pos.y);
     var vvel  = horb.vel;
     if(LaunchWithPunch && LaunchVelocity != Vector3.zero)
     {
         //honor the momentum conservation law
         //:calculate launched vessel velocity
         var hM = vessel.GetTotalMass();
         var tM = hM + sv.mass;
         var d_vel = part.transform.TransformDirection(LaunchVelocity);
         vvel += (Vector3d.zero + d_vel*hM/tM).xzy;
         //:calculate hangar's vessel velocity
         deltaV = d_vel*(-sv.mass)/tM;
         change_velocity = true;
     }
     vorb.UpdateFromStateVectors(vpos, vvel, horb.referenceBody, UT);
     pv.orbitSnapShot = new OrbitSnapshot(vorb);
     //position on a surface
     if(vessel.LandedOrSplashed)
     {
         vpos = spawn_transform.position+get_vessel_offset(spawn_transform, sv);
         pv.longitude = vessel.mainBody.GetLongitude(vpos);
         pv.latitude  = vessel.mainBody.GetLatitude(vpos);
         pv.altitude  = vessel.mainBody.GetAltitude(vpos);
     }
 }

		static void DistanceToTarget(double[] x, double[] fi, object obj)
		{
			OptimizerData data = (OptimizerData)obj;

			double t = data.original_UT + x[3];
			Vector3d DV = new Vector3d(x[0], x[1], x[2]);

			Orbit orbit = new Orbit();
			orbit.UpdateFromStateVectors(data.initial_orbit.getRelativePositionAtUT(t), data.initial_orbit.getOrbitalVelocityAtUT(t) + DV.xzy, data.initial_orbit.referenceBody, t);
			orbit.StartUT = t;

			var pars = new PatchedConics.SolverParameters();
			Vector3d pos;

			while(true)
			{
				Orbit next_orbit = new Orbit();
				PatchedConics.CalculatePatch(orbit, next_orbit, orbit.StartUT, pars, null);

				if (orbit.EndUT > data.UT_arrival)
				{
					pos = orbit.getTruePositionAtUT(data.UT_arrival);
					Vector3d err = pos - data.pos_arrival;

					fi[0] = err.x;
					fi[1] = err.y;
					fi[2] = err.z;
					return;
				}

				// As of 0.25 CalculatePatch fails if the orbit does not change SoI
				if (next_orbit.referenceBody == null)
				{
					next_orbit.UpdateFromOrbitAtUT(orbit, orbit.StartUT + orbit.period, orbit.referenceBody);
				}
				orbit = next_orbit;
			}
		}

		static ManeuverParameters ComputeEjectionManeuver(Vector3d exit_velocity, Orbit initial_orbit, double UT_0)
		{
			double GM = initial_orbit.referenceBody.gravParameter;
			double C3 = exit_velocity.sqrMagnitude;
			double sma = -GM / C3;
			double Rpe = initial_orbit.semiMajorAxis;
			double ecc = 1 - Rpe / sma;
			double theta = Math.Acos(-1 / ecc);

			Vector3d intersect_1;
			Vector3d intersect_2;

			// intersect_1 is the optimal position on the orbit assuming the orbit is circular and the sphere of influence is infinite
			IntersectConePlane(exit_velocity, theta, initial_orbit.h, out intersect_1, out intersect_2);

			Vector3d eccvec = initial_orbit.GetEccVector();

			double true_anomaly = AngleAboutAxis(eccvec, intersect_1, initial_orbit.h);
			// GetMeanAnomalyAtTrueAnomaly expects degrees and returns radians
			double mean_anomaly = initial_orbit.GetMeanAnomalyAtTrueAnomaly(true_anomaly * 180 / Math.PI);

			double delta_mean_anomaly = MuUtils.ClampRadiansPi(mean_anomaly - initial_orbit.MeanAnomalyAtUT(UT_0));

			double UT = UT_0 + delta_mean_anomaly / initial_orbit.MeanMotion();

			Vector3d V_0 = initial_orbit.getOrbitalVelocityAtUT(UT);
			Vector3d pos = initial_orbit.getRelativePositionAtUT(UT);

			double final_vel = Math.Sqrt(C3 + 2 * GM / pos.magnitude);

			Vector3d x = pos.normalized;
			Vector3d z = Vector3d.Cross(x, exit_velocity).normalized;
			Vector3d y = Vector3d.Cross(z, x);

			theta = Math.PI / 2;
			double dtheta = 0.001;
			Orbit sample = new Orbit();

			double theta_err = Double.MaxValue;

			for (int iteration = 0 ; iteration < 50 ; ++iteration)
			{
				Vector3d V_1 = final_vel * (Math.Cos(theta) * x + Math.Sin(theta) * y);
				sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
				theta_err = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
				if (double.IsNaN(theta_err))
					return null;
				if (Math.Abs(theta_err) <= Math.PI / 1800)
					return new ManeuverParameters((V_1 - V_0).xzy, UT);

				V_1 = final_vel * (Math.Cos(theta + dtheta) * x + Math.Sin(theta + dtheta) * y);
				sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
				double theta_err_2 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);

				V_1 = final_vel * (Math.Cos(theta - dtheta) * x + Math.Sin(theta - dtheta) * y);
				sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
				double theta_err_3 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);

				double derr = MuUtils.ClampRadiansPi(theta_err_2 - theta_err_3) / (2 * dtheta);

				theta = MuUtils.ClampRadiansTwoPi(theta - theta_err / derr);

				// if theta > pi, replace with 2pi - theta, the function ejection_angle=f(theta) is symmetrc wrt theta=pi
				if (theta > Math.PI)
					theta = 2 * Math.PI - theta;
			}

			return null;
		}

//.........这里部分代码省略.........
				hasTarget = (camTarget != null) ? true : false;
				
				
				Vector3 rightAxis = -Vector3.Cross(vessel.srf_velocity, vessel.upAxis).normalized;
				//Vector3 upAxis = flightCamera.transform.up;
				

				if(autoFlybyPosition)
				{
					setPresetOffset = false;
					Vector3 velocity = vessel.srf_velocity;
					if(referenceMode == ReferenceModes.Orbit) velocity = vessel.obt_velocity;
					
					Vector3 clampedVelocity = Mathf.Clamp((float) vessel.srfSpeed, 0, maxRelV) * velocity.normalized;
					float clampedSpeed = clampedVelocity.magnitude;
					float sideDistance = Mathf.Clamp(20 + (clampedSpeed/10), 20, 150);
					float distanceAhead = Mathf.Clamp(4 * clampedSpeed, 30, 3500);
					
					flightCamera.transform.rotation = Quaternion.LookRotation(vessel.transform.position - flightCamera.transform.position, cameraUp);
					
					
					if(referenceMode == ReferenceModes.Surface && vessel.srfSpeed > 0)
					{
						flightCamera.transform.position = vessel.transform.position + (distanceAhead * vessel.srf_velocity.normalized);
					}
					else if(referenceMode == ReferenceModes.Orbit && vessel.obt_speed > 0)
					{
						flightCamera.transform.position = vessel.transform.position + (distanceAhead * vessel.obt_velocity.normalized);
					}
					else
					{
						flightCamera.transform.position = vessel.transform.position + (distanceAhead * vessel.vesselTransform.up);
					}
					
					
					if(flightCamera.mode == FlightCamera.Modes.FREE || FlightCamera.GetAutoModeForVessel(vessel) == FlightCamera.Modes.FREE)
					{
						flightCamera.transform.position += (sideDistance * rightAxis) + (15 * cameraUp);
					}
					else if(flightCamera.mode == FlightCamera.Modes.ORBITAL || FlightCamera.GetAutoModeForVessel(vessel) == FlightCamera.Modes.ORBITAL)
					{
						flightCamera.transform.position += (sideDistance * FlightGlobals.getUpAxis()) + (15 * Vector3.up);
					}


				}
				else if(manualOffset)
				{
					setPresetOffset = false;
					float sideDistance = manualOffsetRight;
					float distanceAhead = manualOffsetForward;
					
					
					flightCamera.transform.rotation = Quaternion.LookRotation(vessel.transform.position - flightCamera.transform.position, cameraUp);
					
					if(referenceMode == ReferenceModes.Surface && vessel.srfSpeed > 4)
					{
						flightCamera.transform.position = vessel.transform.position + (distanceAhead * vessel.srf_velocity.normalized);
					}
					else if(referenceMode == ReferenceModes.Orbit && vessel.obt_speed > 4)
					{
						flightCamera.transform.position = vessel.transform.position + (distanceAhead * vessel.obt_velocity.normalized);
					}
					else
					{
						flightCamera.transform.position = vessel.transform.position + (distanceAhead * vessel.vesselTransform.up);
					}
					
					if(flightCamera.mode == FlightCamera.Modes.FREE || FlightCamera.GetAutoModeForVessel(vessel) == FlightCamera.Modes.FREE)
					{
						flightCamera.transform.position += (sideDistance * rightAxis) + (manualOffsetUp * cameraUp);
					}
					else if(flightCamera.mode == FlightCamera.Modes.ORBITAL || FlightCamera.GetAutoModeForVessel(vessel) == FlightCamera.Modes.ORBITAL)
					{
						flightCamera.transform.position += (sideDistance * FlightGlobals.getUpAxis()) + (manualOffsetUp * Vector3.up);
					}
				}
				else if(setPresetOffset)
				{
					flightCamera.transform.position = presetOffset;
					//setPresetOffset = false;
				}
				
				initialVelocity = vessel.srf_velocity;
				initialOrbit = new Orbit();
				initialOrbit.UpdateFromStateVectors(vessel.orbit.pos, vessel.orbit.vel, FlightGlobals.currentMainBody, Planetarium.GetUniversalTime());
				initialUT = Planetarium.GetUniversalTime();
				
				cameraToolActive = true;

				SetDoppler(true);
				AddAtmoAudioControllers(true);
			}
			else
			{
				Debug.Log ("CameraTools: Stationary Camera failed. Active Vessel is null.");	
			}
			resetPositionFix = flightCamera.transform.position;
			Debug.Log ("flightCamera position post init: "+flightCamera.transform.position);
		}

        private Orbit createOrbitFromState(VesselState state)
        {
            var orbit = new Orbit();
            orbit.UpdateFromStateVectors(Util.SwapYZ(state.position), Util.SwapYZ(state.velocity), state.referenceBody, state.time);
			var pars = new PatchedConics.SolverParameters();
			pars.FollowManeuvers = false;
			PatchedConics.CalculatePatch(orbit, new Orbit(), state.time, pars, null);
            return orbit;
        }

		public static ManeuverParameters ComputeEjectionManeuver(Vector3d exit_velocity, Orbit initial_orbit, double UT_0)
		{
			double GM = initial_orbit.referenceBody.gravParameter;
			double C3 = exit_velocity.sqrMagnitude;
            double Mh = initial_orbit.referenceBody.sphereOfInfluence;
            double Rpe = initial_orbit.semiMajorAxis;

            double isma = 2 / Mh - C3 / GM; //inverted Semi-major Axis, will work for parabolic orbit
            double ecc = 1.0 - Rpe * isma;

            //double vstart = Math.Sqrt(GM * (2 / Rpe - isma)); //{ total start boost}
            //double slat = Rpe * Rpe * vstart * vstart / GM;

            //the problem here should be R for circular orbit instead of Rpe
            double slat = 2 * Rpe - isma * Rpe * Rpe; //but we don't know start point (yet) in elliptic orbit

            double theta = Math.Acos((slat / Mh - 1) / ecc);


            /*
            //old way infinity hyperbolic:
            //problems: it's not necessary hyperbolic (in case of low speed exit_velocity), 
            //and exit_velocity appears not infinite far from celestial body, but only sphereOfInfluence far
            //i.e. Mh in previous statements(theta, isma) is not infinity!

			double sma = -GM / C3;
			
			double ecc = 1 - Rpe / sma;
			double theta = Math.Acos(-1 / ecc);

            */
            Vector3d intersect_1;
			Vector3d intersect_2;

			// intersect_1 is the optimal position on the orbit assuming the orbit is circular and the sphere of influence is infinite
			IntersectConePlane(exit_velocity, theta, initial_orbit.h, out intersect_1, out intersect_2);

			Vector3d eccvec = initial_orbit.GetEccVector();

			double true_anomaly = AngleAboutAxis(eccvec, intersect_1, initial_orbit.h);
			// GetMeanAnomalyAtTrueAnomaly expects degrees and returns radians
			double mean_anomaly = initial_orbit.GetMeanAnomalyAtTrueAnomaly(true_anomaly * 180 / Math.PI);

			double delta_mean_anomaly = MuUtils.ClampRadiansPi(mean_anomaly - initial_orbit.MeanAnomalyAtUT(UT_0));

			double UT = UT_0 + delta_mean_anomaly / initial_orbit.MeanMotion();

			Vector3d V_0 = initial_orbit.getOrbitalVelocityAtUT(UT);
			Vector3d pos = initial_orbit.getRelativePositionAtUT(UT);

			double final_vel = Math.Sqrt(C3 + 2 * GM / pos.magnitude);

			Vector3d x = pos.normalized;
			Vector3d z = Vector3d.Cross(x, exit_velocity).normalized;
			Vector3d y = Vector3d.Cross(z, x);

			theta = Math.PI / 2;
			double dtheta = 0.001;
			Orbit sample = new Orbit();

			double theta_err = Double.MaxValue;

			for (int iteration = 0 ; iteration < 50 ; ++iteration)
			{
				Vector3d V_1 = final_vel * (Math.Cos(theta) * x + Math.Sin(theta) * y);
				sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
				theta_err = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);
				if (double.IsNaN(theta_err))
					return null;
				if (Math.Abs(theta_err) <= Math.PI / 1800)
					return new ManeuverParameters((V_1 - V_0).xzy, UT);

				V_1 = final_vel * (Math.Cos(theta + dtheta) * x + Math.Sin(theta + dtheta) * y);
				sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
				double theta_err_2 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);

				V_1 = final_vel * (Math.Cos(theta - dtheta) * x + Math.Sin(theta - dtheta) * y);
				sample.UpdateFromStateVectors(pos, V_1, initial_orbit.referenceBody, UT);
				double theta_err_3 = AngleAboutAxis(exit_velocity, sample.getOrbitalVelocityAtUT(OrbitExtensions.NextTimeOfRadius(sample, UT, sample.referenceBody.sphereOfInfluence)), z);

				double derr = MuUtils.ClampRadiansPi(theta_err_2 - theta_err_3) / (2 * dtheta);

				theta = MuUtils.ClampRadiansTwoPi(theta - theta_err / derr);

				// if theta > pi, replace with 2pi - theta, the function ejection_angle=f(theta) is symmetrc wrt theta=pi
				if (theta > Math.PI)
					theta = 2 * Math.PI - theta;
			}

			return null;
		}