averageVector3d() public static méthode

public static averageVector3d ( UnityEngine.Vector3d vectorArray, UnityEngine.Vector3d newVector ) : UnityEngine.Vector3d
vectorArray UnityEngine.Vector3d
newVector UnityEngine.Vector3d
Résultat UnityEngine.Vector3d
Exemple #1
0
        public void drive(FlightCtrlState s)
        {
            if (attitideActive)
            {
                updateAvailableTorque();

                attitudeError = Math.Abs(Vector3d.Angle(attitudeGetReferenceRotation(Reference) * Target * Vector3d.forward, vessel.ReferenceTransform.up));
                // Used in the drive_limit calculation
                double precision = Math.Max(0.5, Math.Min(10.0, (torquePYAvailable + torqueThrustPYAvailable * s.mainThrottle) * 20.0 / MoI.magnitude));

                // Direction we want to be facing
                Quaternion target = attitudeGetReferenceRotation(Reference) * Target;
                Quaternion delta  = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.ReferenceTransform.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(
                    torquePYAvailable + torqueThrustPYAvailable * s.mainThrottle,
                    torqueRAvailable,
                    torquePYAvailable + torqueThrustPYAvailable * s.mainThrottle
                    );

                Vector3d inertia = Vector3d.Scale(
                    RTUtils.Sign(angularMomentum) * 1.8f,
                    Vector3d.Scale(
                        Vector3d.Scale(angularMomentum, angularMomentum),
                        RTUtils.Invert(Vector3d.Scale(torque, MoI))
                        )
                    );

                // Determine the current level of error, this is then used to determine what act to apply
                Vector3d err = deltaEuler * Math.PI / 180.0F;
                err += RTUtils.Reorder(inertia, 132);
                err.Scale(Vector3d.Scale(MoI, RTUtils.Invert(torque)));
                prev_err = err;

                // Make sure we are taking into account the correct timeframe we are working with
                integral += err * TimeWarp.fixedDeltaTime;

                // The inital value of act
                // Act is ultimately what determines what the pitch, yaw and roll will be set to
                // We make alterations to act between here and passing it into the pod controls
                Vector3d act = Mathf.Clamp((float)attitudeError, 1.0F, 3.0F) * Kp * err;
                //+ Ki * integral + Kd * deriv; //Ki and Kd are always 0 so they have been commented out

                // The maximum value the controls may be
                float drive_limit = Mathf.Clamp01((float)(err.magnitude * 450 / precision));

                // Reduce z by reduceZfactor, z seems to act far too high in relation to x and y
                act.z = act.z / 18.0F;

                // Reduce all 3 axis to a maximum of drive_limit
                act.x = Mathf.Clamp((float)act.x, drive_limit * -1, drive_limit);
                act.y = Mathf.Clamp((float)act.y, drive_limit * -1, drive_limit);
                act.z = Mathf.Clamp((float)act.z, drive_limit * -1, drive_limit);

                // Met is the time in seconds from take off
                double met = Planetarium.GetUniversalTime() - vessel.launchTime;

                // Reduce effects of controls after launch and returns them gradually
                // This helps to reduce large wobbles experienced in the first few seconds
                if (met < 3.0F)
                {
                    act = act * ((met / 3.0F) * (met / 3.0F));
                }

                // Averages out the act with the last few results, determined by the size of the a_avg_act array
                act = RTUtils.averageVector3d(a_avg_act, act);

                // Looks for rapid toggling of each axis and if found, then rest that axis for a while
                // This helps prevents wobbles from getting larger
                rapidRestX = RTUtils.restForPeriod(rapidToggleX, act.x, rapidRestX);
                rapidRestY = RTUtils.restForPeriod(rapidToggleY, act.y, rapidRestY);
                rapidRestZ = RTUtils.restForPeriod(rapidToggleZ, act.z, rapidRestZ);

                // Reduce axis by rapidToggleRestFactor if rapid toggle rest has been triggered
                if (rapidRestX > 0)
                {
                    act.x = act.x / rapidToggleRestFactor;
                }
                if (rapidRestY > 0)
                {
                    act.y = act.y / rapidToggleRestFactor;
                }
                if (rapidRestZ > 0)
                {
                    act.z = act.z / rapidToggleRestFactor;
                }

                // Sets the SetFlightCtrlState for pitch, yaw and roll
                if (!double.IsNaN(act.z))
                {
                    s.roll = Mathf.Clamp((float)(act.z), -1, 1);
                }
                if (!double.IsNaN(act.x))
                {
                    s.pitch = Mathf.Clamp((float)(act.x), -1, 1);
                }
                if (!double.IsNaN(act.y))
                {
                    s.yaw = Mathf.Clamp((float)(act.y), -1, 1);
                }
            }
            if (throttleActive)
            {
                s.mainThrottle = throttle;
                if (throttle == 0)
                {
                    throttleActive = false;
                }
            }

            if (roverActive)
            {
                if (roverState.Steer)
                {
                    if (Quaternion.Angle(roverState.roverRotation, core.vessel.ReferenceTransform.rotation) < roverState.Target)
                    {
                        s.wheelThrottle = roverState.reverse ? -throttlePID.Control(roverState.Speed - (float)vessel.horizontalSrfSpeed) : throttlePID.Control(roverState.Speed - (float)vessel.horizontalSrfSpeed);
                        s.wheelSteer    = roverState.Steering;
                    }
                    else
                    {
                        s.wheelThrottle = 0;
                        s.wheelSteer    = 0;
                        roverActive     = false;
                        core.vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
                    }
                }
                else
                {
                    if ((float)Vector3d.Distance(core.vessel.mainBody.position + altitude * core.vessel.mainBody.GetSurfaceNVector(roverState.latitude, roverState.longitude), core.vessel.transform.position) < roverState.Target)
                    {
                        s.wheelThrottle = roverState.reverse ? -throttlePID.Control(roverState.Speed - (float)vessel.horizontalSrfSpeed) : throttlePID.Control(roverState.Speed - (float)vessel.horizontalSrfSpeed);
                        s.wheelSteer    = roverState.Steering;
                    }
                    else
                    {
                        s.wheelThrottle = 0;
                        s.wheelSteer    = 0;
                        roverActive     = false;
                        core.vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
                    }
                }
            }
        }