Wind.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MissionPlanner.Utilities;

namespace MissionPlanner.HIL
{
    public class Wind : Utils
    {
        Wind self;
        public float speed;
        public float direction;
        public float turbulance;
        public double cross_section;
        public double turbulance_time_constant;
        public DateTime tlast;
        public double turbulance_mul;

        //'''a wind generation object//'''
        public Wind(string windstring, double cross_section = 0.1)
        {
            self = this;

            string[] a = windstring.Split(',');
            if (len(a) != 3)
            {
                return;
                //raise RuntimeError("Expected wind in speed,direction,turbulance form, not %s" % windstring);
            }
            self.speed = float.Parse(a[0]); //# m/s
            self.direction = float.Parse(a[1]); //# direction the wind is going in
            self.turbulance = float.Parse(a[2]); //# turbulance factor (standard deviation)

            //# the cross-section of the aircraft to wind. This is multiplied by the
            //# difference in the wind and the velocity of the aircraft to give the acceleration
            self.cross_section = cross_section;

            //# the time constant for the turbulance - the average period of the
            //# changes over time
            self.turbulance_time_constant = 5.0;

            //# wind time record
            self.tlast = DateTime.Now;

            //# initial turbulance multiplier
            self.turbulance_mul = 1.0;
        }

        public void current(double deltat, out double speed, out double direction)
        {
            //'''return current wind speed and direction as a tuple
            //speed is in m/s, direction in degrees
            //'''
            if (deltat == 0)
            {
                DateTime tnow = DateTime.Now;
                deltat = (tnow - self.tlast).TotalSeconds;
                self.tlast = tnow;
            }

            //# update turbulance random walk
            double w_delta = sqrt(deltat)*(1.0 - new GaussianRandom().NextGaussian(1.0, self.turbulance + 0.01));
            w_delta -= (self.turbulance_mul - 1.0)*(deltat/self.turbulance_time_constant);
            self.turbulance_mul += w_delta;
            speed = self.speed*(float) fabs(self.turbulance_mul);

            direction = self.direction;
        }

        //# Calculate drag.
        public Vector3 drag(Vector3 velocity, double deltat = 0) //, testing=None) 
        {
            //'''return current wind force in Earth frame.  The velocity parameter is
            //   a Vector3 of the current velocity of the aircraft in earth frame, m/s//'''

            //# (m/s, degrees) : wind vector as a magnitude and angle.
            double speed, direction;
            self.current(deltat, out speed, out direction);
            //# speed = self.speed
            //# direction = self.direction

            //# Get the wind vector.
            Vector3 w = toVec(speed, Utils.radians(direction));

            double obj_speed = velocity.length();

            //# Compute the angle between the object vector and wind vector by taking
            //# the dot product and dividing by the magnitudes.
            double alpha = 0;
            double d = w.length()*obj_speed;
            if (d == 0)
            {
                alpha = 0;
            }
            else
            {
                //int checkme; // the python code this was copied from has an error
                //alpha = Utils.acos(((w * velocity).length() / d));
            }

            //# Get the relative wind speed and angle from the object.  Note that the
            //# relative wind speed includes the velocity of the object; i.e., there
            //# is a headwind equivalent to the object's speed even if there is no
            //# absolute wind.
            double rel_speed, beta;
            apparent_wind(speed, obj_speed, alpha, out rel_speed, out beta);

            //# Return the vector of the relative wind, relative to the coordinate
            //# system.
            Vector3 relWindVec = toVec(rel_speed, beta + Utils.atan2(velocity.y, velocity.x));

            //# Combine them to get the acceleration vector.
            return new Vector3(acc(relWindVec.x, drag_force(self, relWindVec.x))
                , acc(relWindVec.y, drag_force(self, relWindVec.y))
                , 0);
        }

//# http://en.wikipedia.org/wiki/Apparent_wind
//#
//# Returns apparent wind speed and angle of apparent wind.  Alpha is the angle
//# between the object and the true wind.  alpha of 0 rads is a headwind; pi a
//# tailwind.  Speeds should always be positive.
        public void apparent_wind(double wind_sp, double obj_speed, double alpha, out double rel_speed, out double beta)
        {
            double delta = wind_sp*Utils.cos(alpha);
            double x = Math.Pow(wind_sp, 2) + Math.Pow(obj_speed, 2) + 2*obj_speed*delta;
            rel_speed = Utils.sqrt(x);
            if (rel_speed == 0)
            {
                beta = Math.PI;
            }
            else
            {
                beta = acos((delta + obj_speed)/rel_speed);
            }
        }

//# See http://en.wikipedia.org/wiki/Drag_equation
//#
//# Drag equation is F(a) = cl * p/2 * v^2 * a, where cl : drag coefficient
//# (let's assume it's low, .e.g., 0.2), p : density of air (assume about 1
//# kg/m^3, the density just over 1500m elevation), v : relative speed of wind
//# (to the body), a : area acted on (this is captured by the cross_section
//# paramter).
//# 
//# So then we have 
//# F(a) = 0.2 * 1/2 * v^2 * cross_section = 0.1 * v^2 * cross_section
        public double drag_force(Wind wind, double sp)
        {
            return (Math.Pow(sp, 2.0))*0.1*wind.cross_section;
        }

//# Function to make the force vector.  relWindVec is the direction the apparent
//# wind comes *from*.  We want to compute the accleration vector in the direction
//# the wind blows to.
        public double acc(double val, double mag)
        {
            if (val == 0)
            {
                return mag;
            }
            else
            {
                return (val/Utils.fabs(val))*(0 - mag);
            }
        }

//# Converts a magnitude and angle (radians) to a vector in the xy plane.
        public Vector3 toVec(double magnitude, double angle)
        {
            Vector3 v = new Vector3(magnitude, 0, 0);
            Matrix3 m = new Matrix3();
            m.from_euler(0, 0, angle);
            return m.transposed()*v;
        }
    }

    public sealed class GaussianRandom
    {
        private bool _hasDeviate;
        private double _storedDeviate;
        private readonly Random _random;

        public GaussianRandom(Random random = null)
        {
            _random = random ?? new Random();
        }

        /// <summary>
        /// Obtains normally (Gaussian) distributed random numbers, using the Box-Muller
        /// transformation.  This transformation takes two uniformly distributed deviates
        /// within the unit circle, and transforms them into two independently
        /// distributed normal deviates.
        /// </summary>
        /// <param name="mu">The mean of the distribution.  Default is zero.</param>
        /// <param name="sigma">The standard deviation of the distribution.  Default is one.</param>
        /// <returns></returns>
        public double NextGaussian(double mu = 0, double sigma = 1)
        {
            if (sigma <= 0)
                throw new ArgumentOutOfRangeException("sigma", "Must be greater than zero.");

            if (_hasDeviate)
            {
                _hasDeviate = false;
                return _storedDeviate*sigma + mu;
            }

            double v1, v2, rSquared;
            do
            {
                // two random values between -1.0 and 1.0
                v1 = 2*_random.NextDouble() - 1;
                v2 = 2*_random.NextDouble() - 1;
                rSquared = v1*v1 + v2*v2;
                // ensure within the unit circle
            } while (rSquared >= 1 || rSquared == 0);

            // calculate polar tranformation for each deviate
            var polar = Math.Sqrt(-2*Math.Log(rSquared)/rSquared);
            // store first deviate
            _storedDeviate = v2*polar;
            _hasDeviate = true;
            // return second deviate
            return v1*polar*sigma + mu;
        }
    }
}