C# Vector3d.Scale方法代码示例

        public void tuneTf(Vector3d torque)
        {
            Vector3d ratio = new Vector3d(
                torque.x != 0 ? vesselState.MoI.x / torque.x : 0,
                torque.z != 0 ? vesselState.MoI.z / torque.z : 0,   //y <=> z
                torque.y != 0 ? vesselState.MoI.y / torque.y : 0    //z <=> y
                );

            TfV = 0.05 * ratio;

            Vector3d delayFactor = Vector3d.one + 2 * vesselState.torqueReactionSpeed;

            TfV.Scale(delayFactor);


            TfV = TfV.Clamp(2.0 * TimeWarp.fixedDeltaTime, 1.0);
            TfV = TfV.Clamp(TfMin, TfMax);

            //Tf = Mathf.Clamp((float)ratio.magnitude / 20f, 2 * TimeWarp.fixedDeltaTime, 1f);
            //Tf = Mathf.Clamp((float)Tf, (float)TfMin, (float)TfMax);
        }

        public void Drive(FlightCtrlState s)
        {
            if (useSAS)
            {
                _requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0);
                if (!vessel.ActionGroups[KSPActionGroup.SAS])
                {
                    vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
                    vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
                    lastSAS = _requestedAttitude;
                }
                else if (Quaternion.Angle(lastSAS, _requestedAttitude) > 10)
                {
                    vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
                    lastSAS = _requestedAttitude;
                }
                else
                {
                    vessel.Autopilot.SAS.LockHeading(_requestedAttitude, true);
                }

            }
            else
            {
                // Direction we want to be facing
                _requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
                Transform vesselTransform = vessel.ReferenceTransform;
                Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselTransform.rotation) * _requestedAttitude);

                Vector3d deltaEuler = delta.DeltaEuler();

                // ( MoI / available torque ) factor:
                Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132);

                // Find out the real shorter way to turn were we wan to.
                // Thanks to HoneyFox
                Vector3d tgtLocalUp = vesselTransform.transform.rotation.Inverse() * _requestedAttitude * Vector3d.forward;
                Vector3d curLocalUp = Vector3d.up;

                double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
                Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
                rotDirection = rotDirection.normalized * turnAngle / 180.0;

                // And the lowest roll
                // Thanks to Crzyrndm
                Vector3 normVec = Vector3.Cross(_requestedAttitude * Vector3.forward, vesselTransform.up);
                Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * _requestedAttitude;
                float rollError = Vector3.Angle(vesselTransform.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselTransform.forward));

                error = new Vector3d(
                    -rotDirection.y * Math.PI,
                    rotDirection.x * Math.PI,
                    rollError * Mathf.Deg2Rad
                    );

                error.Scale(_axisControl);

                Vector3d err = error + inertia.Reorder(132) / 2d;
                err = new Vector3d(
                    Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
                    Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
                    Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));

                err.Scale(NormFactor);

                // angular velocity:
                Vector3d omega;
                omega.x = vessel.angularVelocity.x;
                omega.y = vessel.angularVelocity.z; // y <=> z
                omega.z = vessel.angularVelocity.y; // z <=> y
                omega.Scale(NormFactor);

                if (Tf_autoTune)
                    tuneTf(torque);
                setPIDParameters();

                // angular velocity limit:
                var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit),
                                           Math.Sqrt(NormFactor.y * Math.PI * kWlimit),
                                           Math.Sqrt(NormFactor.z * Math.PI * kWlimit));

                pidAction = pid.Compute(err, omega, Wlimit);

                // deadband
                pidAction.x = Math.Abs(pidAction.x) >= deadband ? pidAction.x : 0.0f;
                pidAction.y = Math.Abs(pidAction.y) >= deadband ? pidAction.y : 0.0f;
                pidAction.z = Math.Abs(pidAction.z) >= deadband ? pidAction.z : 0.0f;

                // low pass filter,  wf = 1/Tf:
                Vector3d act = lastAct;
                act.x += (pidAction.x - lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0));
                act.y += (pidAction.y - lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0));
                act.z += (pidAction.z - lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0));
                lastAct = act;

                SetFlightCtrlState(act, deltaEuler, s, 1);

                act = new Vector3d(s.pitch, s.yaw, s.roll);

                // Feed the control torque to the differential throttle
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                        break;
                }

                double t_err = (trans_spd_act - spd) / vesselState.maxThrustAccel;
                t_integral += t_err * TimeWarp.fixedDeltaTime;
                double t_deriv = (t_err - t_prev_err) / TimeWarp.fixedDeltaTime;
                double t_act = (t_Kp * t_err) + (t_Ki * t_integral) + (t_Kd * t_deriv);
                t_prev_err = t_err;

                double int_error = Math.Abs(Vector3d.Angle(attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Vector3d.forward, vesselState.forward));
                //print("int_error = " + int_error);
                if ((tmode != TMode.KEEP_VERTICAL) || (int_error < 2) || ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) < 1000) && (int_error < 90)))
                {
                    if (tmode == TMode.DIRECT)
                    {
                        trans_prev_thrust = s.mainThrottle = trans_spd_act / 100.0F;
                    }
                    else
                    {
                        trans_prev_thrust = s.mainThrottle = Mathf.Clamp(trans_prev_thrust + (float)t_act, 0, 1.0F);
                    }
                }
                else
                {
                    if ((int_error >= 2) && (vesselState.torqueThrustPYAvailable > vesselState.torquePYAvailable * 10))
                    {
                        trans_prev_thrust = s.mainThrottle = 0.1F;
                    }
                    else
                    {
                        trans_prev_thrust = s.mainThrottle = 0;
                    }
                }
            }

            if (attitudeActive)
            {
                int userCommanding = (Mathfx.Approx(s.pitch, 0, 0.1F) ? 0 : 1) + (Mathfx.Approx(s.yaw, 0, 0.1F) ? 0 : 2) + (Mathfx.Approx(s.roll, 0, 0.1F) ? 0 : 4);

                s.killRot = false;

                Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
                Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(part.vessel.transform.rotation) * target);
                /*
                if (Mathf.Abs(Quaternion.Angle(delta, Quaternion.identity)) > 30) {
                    delta = Quaternion.Slerp(Quaternion.identity, delta, 0.1F);
                }
                */
                Vector3d deltaEuler = delta.eulerAngles;
                Vector3d err = new Vector3d((deltaEuler.x > 180) ? (deltaEuler.x - 360.0F) : deltaEuler.x, -((deltaEuler.y > 180) ? (deltaEuler.y - 360.0F) : deltaEuler.y), (deltaEuler.z > 180) ? (deltaEuler.z - 360.0F) : deltaEuler.z) * Math.PI / 180.0F;
                Vector3d torque = new Vector3d(vesselState.torquePYAvailable + vesselState.torqueThrustPYAvailable * s.mainThrottle, vesselState.torqueRAvailable, vesselState.torquePYAvailable + vesselState.torqueThrustPYAvailable * s.mainThrottle);
                Vector3d inertia = Vector3d.Scale(MuUtils.Sign(vesselState.angularMomentum) * 1.1, Vector3d.Scale(Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum), MuUtils.Invert(Vector3d.Scale(torque, vesselState.MoI))));
                err += MuUtils.Reorder(inertia, 132);
                err.Scale(Vector3d.Scale(vesselState.MoI, MuUtils.Invert(torque)));

                integral += err * TimeWarp.fixedDeltaTime;
                Vector3d deriv = (err - prev_err) / TimeWarp.fixedDeltaTime;
                act = Kp * err + Ki * integral + Kd * deriv;
                prev_err = err;

                if (userCommanding != 0)
                {
                    if (attitudeKILLROT)
                    {
                        attitudeTo(Quaternion.LookRotation(part.vessel.transform.up, -part.vessel.transform.forward), AttitudeReference.INERTIAL, null);
                    }
                }
                if ((userCommanding & 4) > 0)
                {
                    prev_err = new Vector3d(prev_err.x, prev_err.y, 0);
                    integral = new Vector3d(integral.x, integral.y, 0);
                    if (!attitudeRollMatters)
                    {
                        attitudeTo(Quaternion.LookRotation(attitudeTarget * Vector3d.forward, attitudeWorldToReference(-part.vessel.transform.forward, attitudeReference)), attitudeReference, null);
                        _attitudeRollMatters = false;
                    }
                }
                else
                {
                    if (!double.IsNaN(act.z)) s.roll = Mathf.Clamp(s.roll + act.z, -1.0F, 1.0F);
                }
                if ((userCommanding & 3) > 0)
                {
                    prev_err = new Vector3d(0, 0, prev_err.z);
                    integral = new Vector3d(0, 0, integral.z);
                }
                else
                {
                    if (!double.IsNaN(act.x)) s.pitch = Mathf.Clamp(s.pitch + act.x, -1.0F, 1.0F);
                    if (!double.IsNaN(act.y)) s.yaw = Mathf.Clamp(s.yaw + act.y, -1.0F, 1.0F);
                }
                stress = Mathf.Min(Mathf.Abs(act.x), 1.0F) + Mathf.Min(Mathf.Abs(act.y), 1.0F) + Mathf.Min(Mathf.Abs(act.z), 1.0F);
            }

            if (double.IsNaN(s.mainThrottle))
            {
                print("MechJeb: caught throttle = NaN");
                s.mainThrottle = 0;
            }
        }

        /// <summary>
        /// Automatically guides the ship to face a desired orientation
        /// </summary>
        /// <param name="target">The desired orientation</param>
        /// <param name="c">The FlightCtrlState for the current vessel.</param>
        /// <param name="fc">The flight computer carrying out the slew</param>
        /// <param name="ignoreRoll">[optional] to ignore the roll</param>
        public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll)
        {
            // Add support for roll-less targets later -- Starstrider42
            bool fixedRoll = !ignoreRoll;
            Vessel vessel = fc.Vessel;
            Vector3d momentOfInertia = GetTrueMoI(vessel);
            Transform vesselReference = vessel.GetTransform();

            //---------------------------------------
            // Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42

            Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target);

            Vector3d torque = GetTorque(vessel, c.mainThrottle);
            Vector3d spinMargin = GetStoppingAngle(vessel, torque);

            // Allow for zero torque around some but not all axes
            Vector3d normFactor;
            normFactor.x = (torque.x != 0 ? momentOfInertia.x / torque.x : 0.0);
            normFactor.y = (torque.y != 0 ? momentOfInertia.y / torque.y : 0.0);
            normFactor.z = (torque.z != 0 ? momentOfInertia.z / torque.z : 0.0);
            normFactor = SwapYZ(normFactor);

            // Find out the real shorter way to turn were we want to.
            // Thanks to HoneyFox

            Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward;
            Vector3d curLocalUp = Vector3d.up;

            double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
            var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
            rotDirection = rotDirection.normalized * turnAngle / 180.0f;

            var err = new Vector3d(
                -rotDirection.y * Math.PI,
                rotDirection.x * Math.PI,
                fixedRoll ?
                    ((delta.eulerAngles.z > 180) ?
                        (delta.eulerAngles.z - 360.0F) :
                        delta.eulerAngles.z) * Math.PI / 180.0F
                    : 0F
            );

            err += SwapYZ(spinMargin);
            err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
                Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
                Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
            err.Scale(normFactor);

            // angular velocity:
            Vector3d omega = SwapYZ(vessel.angularVelocity);
            omega.Scale(normFactor);

            Vector3d pidAction = fc.pid.Compute(err, omega);

            // low pass filter, wf = 1/Tf:
            Vector3d act = fc.lastAct + (pidAction - fc.lastAct) * (1 / ((fc.Tf / TimeWarp.fixedDeltaTime) + 1));
            fc.lastAct = act;

            // end MechJeb import
            //---------------------------------------

            float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f);
            float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision));

            act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit);
            act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit);
            act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit);

            c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit);
            c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit);
            c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit);
        }

        public override void Drive(FlightCtrlState s)
        {
            if (useSAS)
            {
                _requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0);
                if (!part.vessel.ActionGroups[KSPActionGroup.SAS])
                {
                    part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
                    part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
                    lastSAS = _requestedAttitude;
                }
                else if (Quaternion.Angle(lastSAS, _requestedAttitude) > 10)
                {
                    part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude);
                    lastSAS = _requestedAttitude;
                }
                else
                {
                    part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude, true);
                }

                core.thrust.differentialThrottleDemandedTorque = Vector3d.zero;
            }
            else
            {
                // Direction we want to be facing
                _requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
                Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * _requestedAttitude);

                Vector3d deltaEuler = new Vector3d(
                                                        (delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
                                                        -((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
                                                        (delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
                                                    );

                Vector3d torque = vesselState.torqueAvailable + vesselState.torqueFromEngine * vessel.ctrlState.mainThrottle;
                if (core.thrust.differentialThrottleSuccess)
                    torque += vesselState.torqueFromDiffThrottle * vessel.ctrlState.mainThrottle / 2;

                Vector3d inertia = Vector3d.Scale(
                                                        vesselState.angularMomentum.Sign(),
                                                        Vector3d.Scale(
                                                            Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum),
                                                            Vector3d.Scale(torque, vesselState.MoI).Invert()
                                                        )
                                                    );

                // ( MoI / avaiable torque ) factor:
                Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132);

                // Find out the real shorter way to turn were we wan to.
                // Thanks to HoneyFox

                Vector3d tgtLocalUp = vessel.ReferenceTransform.transform.rotation.Inverse() * _requestedAttitude * Vector3d.forward;
                Vector3d curLocalUp = Vector3d.up;

                double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
                Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
                rotDirection = rotDirection.normalized * turnAngle / 180.0f;

                Vector3d err = new Vector3d(
                                                -rotDirection.y * Math.PI,
                                                rotDirection.x * Math.PI,
                                                attitudeRollMatters ? ((delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z) * Math.PI / 180.0F : 0F
                                            );

                err += inertia.Reorder(132) / 2;
                err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
                                   Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
                                   Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
                err.Scale(NormFactor);

                // angular velocity:
                Vector3d omega;
                omega.x = vessel.angularVelocity.x;
                omega.y = vessel.angularVelocity.z; // y <=> z
                omega.z = vessel.angularVelocity.y; // z <=> y
                omega.Scale(NormFactor);

                pidAction = pid.Compute(err, omega);

                // low pass filter,  wf = 1/Tf:
                Vector3d act = lastAct + (pidAction - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1));
                lastAct = act;

                SetFlightCtrlState(act, deltaEuler, s, 1);

                act = new Vector3d(s.pitch, s.yaw, s.roll);

                // Feed the control torque to the differential throttle
                if (core.thrust.differentialThrottleSuccess)
                    core.thrust.differentialThrottleDemandedTorque = -Vector3d.Scale(act.xzy, vesselState.torqueFromDiffThrottle * vessel.ctrlState.mainThrottle);
            }
        }

        public void UpdateStateVectors()
        {
            Direction targetdir = GetDirectionFromValue();
            if (targetdir == null)
            {
                shared.Logger.LogWarning(string.Format("SteeringManager target direction is null, Value = {0}", Value));
                return;
            }

            targetRot = targetdir.Rotation;
            centerOfMass = shared.Vessel.findWorldCenterOfMass();

            vesselTransform = shared.Vessel.ReferenceTransform;
            // Found that the default rotation has top pointing forward, forward pointing down, and right pointing starboard.
            // This fixes that rotation.
            vesselRotation = vesselTransform.rotation * Quaternion.Euler(-90, 0, 0);

            vesselForward = vesselRotation * Vector3d.forward;
            vesselTop = vesselRotation * Vector3d.up;
            vesselStarboard = vesselRotation * Vector3d.right;
            vesselUp = (centerOfMass - shared.Vessel.mainBody.position).normalized;

            targetForward = targetRot * Vector3d.forward;
            targetTop = targetRot * Vector3d.up;
            targetStarboard = targetRot * Vector3d.right;

            Vector3d oldOmega = omega;
            // omega is angular rotation.  need to correct the signs to agree with the facing axis
            omega = Quaternion.Inverse(vesselRotation) * shared.Vessel.rigidbody.angularVelocity;
            omega.x *= -1; //positive values pull the nose to the starboard.
            //omega.y *= -1; // positive values pull the nose up.
            omega.z *= -1; // positive values pull the starboard side up.

            // TODO: Currently adjustments to MOI are only enabled in debug compiles.  Using this feature seems to be buggy, but it has potential
            // to be more resiliant against random spikes in angular velocity.
            if (sessionTime > lastSessionTime)
            {
                double dt = sessionTime - lastSessionTime;
                angularAcceleration = (omega - oldOmega) / dt;
                angularAcceleration = new Vector3d(angularAcceleration.x, angularAcceleration.z, angularAcceleration.y);
                if (enableMoiAdjust)
                {
                    measuredMomentOfInertia = new Vector3d(
                        controlTorque.x * accPitch / angularAcceleration.x,
                        controlTorque.y * accRoll / angularAcceleration.y,
                        controlTorque.z * accYaw / angularAcceleration.z);

                    if (Math.Abs(accPitch) > EPSILON)
                    {
                        adjustMomentOfInertia.x = pitchMOICalc.Update(Math.Abs(measuredMomentOfInertia.x) / momentOfInertia.x);
                    }
                    if (Math.Abs(accYaw) > EPSILON)
                    {
                        adjustMomentOfInertia.z = yawMOICalc.Update(Math.Abs(measuredMomentOfInertia.z) / momentOfInertia.z);
                    }
                    if (Math.Abs(accRoll) > EPSILON)
                    {
                        adjustMomentOfInertia.y = rollMOICalc.Update(Math.Abs(measuredMomentOfInertia.y) / momentOfInertia.y);
                    }
                }
            }

            momentOfInertia = shared.Vessel.findLocalMOI(centerOfMass);
            momentOfInertia.Scale(adjustMomentOfInertia);
            adjustTorque = Vector3d.zero;
            measuredTorque = Vector3d.Scale(momentOfInertia, angularAcceleration);

            if (sessionTime > lastSessionTime && EnableTorqueAdjust)
            {
                if (Math.Abs(accPitch) > EPSILON)
                {
                    adjustTorque.x = Math.Min(Math.Abs(pitchTorqueCalc.Update(measuredTorque.x / accPitch)) - rawTorque.x, 0);
                    //adjustTorque.x = Math.Abs(pitchTorqueCalc.Update(measuredTorque.x / accPitch) / rawTorque.x);
                }
                else adjustTorque.x = Math.Abs(pitchTorqueCalc.Update(pitchTorqueCalc.Mean));
                if (Math.Abs(accYaw) > EPSILON)
                {
                    adjustTorque.z = Math.Min(Math.Abs(yawTorqueCalc.Update(measuredTorque.z / accYaw)) - rawTorque.z, 0);
                    //adjustTorque.z = Math.Abs(yawTorqueCalc.Update(measuredTorque.z / accYaw) / rawTorque.z);
                }
                else adjustTorque.z = Math.Abs(yawTorqueCalc.Update(yawTorqueCalc.Mean));
                if (Math.Abs(accRoll) > EPSILON)
                {
                    adjustTorque.y = Math.Min(Math.Abs(rollTorqueCalc.Update(measuredTorque.y / accRoll)) - rawTorque.y, 0);
                    //adjustTorque.y = Math.Abs(rollTorqueCalc.Update(measuredTorque.y / accRoll) / rawTorque.y);
                }
                else adjustTorque.y = Math.Abs(rollTorqueCalc.Update(rollTorqueCalc.Mean));
            }
        }

        public override void Drive(FlightCtrlState s)
        {
            if (useSAS)
            {
                Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0);
                if (!part.vessel.ActionGroups[KSPActionGroup.SAS])
                {
                    part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
                    part.vessel.VesselSAS.LockHeading(target);
                    lastSAS = target;
                }
                else if (Quaternion.Angle(lastSAS, target) > 10)
                {
                    part.vessel.VesselSAS.LockHeading(target);
                    lastSAS = target;
                }
                else
                {
                    part.vessel.VesselSAS.LockHeading(target, true);
                }
            }
            else
            {
                // Direction we want to be facing
                Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget;
                Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * target);

                Vector3d deltaEuler = new Vector3d(
                                                        (delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
                                                        -((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
                                                        (delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
                                                    );

                Vector3d torque = new Vector3d(
                                                        vesselState.torqueAvailable.x + vesselState.torqueThrustPYAvailable * s.mainThrottle,
                                                        vesselState.torqueAvailable.y,
                                                        vesselState.torqueAvailable.z + vesselState.torqueThrustPYAvailable * s.mainThrottle
                                                );

                Vector3d inertia = Vector3d.Scale(
                                                        vesselState.angularMomentum.Sign(),
                                                        Vector3d.Scale(
                                                            Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum),
                                                            Vector3d.Scale(torque, vesselState.MoI).Invert()
                                                        )
                                                    );

                // ( MoI / avaiable torque ) factor:
                Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132);

                // angular error:
                Vector3d err = deltaEuler * Math.PI / 180.0F;
                err += inertia.Reorder(132) / 2;
                err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
                                   Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
                                   Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));
                err.Scale(NormFactor);

                // angular velocity:
                Vector3d omega;
                omega.x = vessel.angularVelocity.x;
                omega.y = vessel.angularVelocity.z; // y <=> z
                omega.z = vessel.angularVelocity.y; // z <=> y
                omega.Scale(NormFactor);

                pidAction = pid.Compute(err, omega);

                // low pass filter,  wf = 1/Tf:
                Vector3d act = lastAct + (pidAction - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1));
                lastAct = act;

                SetFlightCtrlState(act, deltaEuler, s, 1);
                act = new Vector3d(s.pitch, s.yaw, s.roll);
            }
        }

        public Vector3d Compute(Vector3d error, Vector3d omega)
        {
            derivativeAct = omega;
            derivativeAct.Scale(Kd);

            // integral actíon + Anti Windup
            intAccum.x = (Math.Abs(derivativeAct.x) < 0.6 * max) ? intAccum.x + (error.x * Ki.x * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.x;
            intAccum.y = (Math.Abs(derivativeAct.y) < 0.6 * max) ? intAccum.y + (error.y * Ki.y * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.y;
            intAccum.z = (Math.Abs(derivativeAct.z) < 0.6 * max) ? intAccum.z + (error.z * Ki.z * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.z;

            propAct = error;
            propAct.Scale(Kp);

            Vector3d action = propAct + derivativeAct + intAccum;

            // action clamp
            action = new Vector3d(Math.Max(min, Math.Min(max, action.x)),
                                  Math.Max(min, Math.Min(max, action.y)),
                                  Math.Max(min, Math.Min(max, action.z)));
            return action;
        }

        /// <summary>
        /// Automatically guides the ship to face a desired orientation
        /// </summary>
        /// <param name="target">The desired orientation</param>
        /// <param name="c">The FlightCtrlState for the current vessel.</param>
        /// <param name="fc">The flight computer carrying out the slew</param>
        /// <param name="ignoreRoll">[optional] to ignore the roll</param>
        public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll)
        {
            // Add support for roll-less targets later -- Starstrider42
            bool fixedRoll = !ignoreRoll;
            Vessel vessel = fc.Vessel;
            Vector3d momentOfInertia = vessel.MOI;
            Transform vesselReference = vessel.GetTransform();
            Vector3d torque = GetVesselTorque(vessel);
            // -----------------------------------------------
            // Copied from MechJeb master on 18.04.2016 with some modifications to adapt to RemoteTech

            Vector3d _axisControl = new Vector3d();
            _axisControl.x = true ? 1 : 0;
            _axisControl.y = true ? 1 : 0;
            _axisControl.z = fixedRoll ? 1 : 0;

            Vector3d inertia = Vector3d.Scale(
                new Vector3d(vessel.angularMomentum.x, vessel.angularMomentum.y, vessel.angularMomentum.z).Sign(),
                Vector3d.Scale(
                    Vector3d.Scale(vessel.angularMomentum, vessel.angularMomentum),
                    Vector3d.Scale(torque, momentOfInertia).Invert()
                    )
                );

            Vector3d TfV = new Vector3d(0.3, 0.3, 0.3);

            double kpFactor = 3;
            double kiFactor = 6;
            double kdFactor = 0.5;
            double kWlimit = 0.15;
            double deadband = 0.0001;

            Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target);

            Vector3d deltaEuler = delta.DeltaEuler();

            // ( MoI / available torque ) factor:
            Vector3d NormFactor = Vector3d.Scale(momentOfInertia, torque.Invert()).Reorder(132);

            // Find out the real shorter way to turn were we want to.
            // Thanks to HoneyFox
            Vector3d tgtLocalUp = vesselReference.rotation.Inverse() * target * Vector3d.forward;
            Vector3d curLocalUp = Vector3d.up;

            double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp));
            Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z);
            rotDirection = rotDirection.normalized * turnAngle / 180.0;

            // And the lowest roll
            // Thanks to Crzyrndm
            Vector3 normVec = Vector3.Cross(target * Vector3.forward, vesselReference.up);
            Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * target;
            float rollError = Vector3.Angle(vesselReference.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselReference.forward));

            var error = new Vector3d(
                -rotDirection.y * Math.PI,
                rotDirection.x * Math.PI,
                rollError * Mathf.Deg2Rad
                );

            error.Scale(_axisControl);

            Vector3d err = error + inertia.Reorder(132) / 2d;
            err = new Vector3d(
                Math.Max(-Math.PI, Math.Min(Math.PI, err.x)),
                Math.Max(-Math.PI, Math.Min(Math.PI, err.y)),
                Math.Max(-Math.PI, Math.Min(Math.PI, err.z)));

            err.Scale(NormFactor);

            // angular velocity:
            Vector3d omega;
            omega.x = vessel.angularVelocity.x;
            omega.y = vessel.angularVelocity.z; // y <=> z
            omega.z = vessel.angularVelocity.y; // z <=> y
            omega.Scale(NormFactor);

            //if (Tf_autoTune)
            //    tuneTf(torque);

            Vector3d invTf = TfV.Invert();
            fc.pid.Kd = kdFactor * invTf;

            fc.pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * fc.pid.Kd;
            fc.pid.Kp.Scale(invTf);

            fc.pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * fc.pid.Kp;
            fc.pid.Ki.Scale(invTf);

            fc.pid.intAccum = fc.pid.intAccum.Clamp(-5, 5);

            // angular velocity limit:
            var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit),
//.........这里部分代码省略.........