MathHelper: move deg2rad to utilities

Controls/Waypoints/Spline2.cs

@@ -108,7 +108,7 @@ int32_t pv_get_lon(Vector3 pos_vec)
         float longitude_scale(PointLatLngAlt loc)
         {
             float scale = 1.0f;
-            scale = (float) Math.Cos(loc.Lat*deg2rad);
+            scale = (float) Math.Cos(loc.Lat*MathHelper.deg2rad);
             return scale;
         }
 
@@ -383,7 +383,7 @@ public void advance_spline_target_along_track(float dt)
 
         private float RadiansToCentiDegrees(double p)
         {
-            return (float) (p*rad2deg);
+            return (float) (p*MathHelper.rad2deg);
         }
 
 

CurrentState.cs

@@ -540,7 +540,7 @@ public float radius
             get
             {
                 if (groundspeed <= 1) return 0;
-                return ((groundspeed*groundspeed)/(float) (9.8f*Math.Tan(roll*deg2rad)));
+                return ((groundspeed*groundspeed)/(float) (9.8f*Math.Tan(roll*MathHelper.deg2rad)));
             }
         }
 
@@ -747,7 +747,7 @@ public float GeoFenceDist
                         if (angle < 0)
                             angle += 360;
 
-                        var alongline = Math.Cos(angle*deg2rad)*distToLocation;
+                        var alongline = Math.Cos(angle*MathHelper.deg2rad)*distToLocation;
 
                         // check to see if our point is still within the line length
                         if (alongline > lineDist)
@@ -756,9 +756,9 @@ public float GeoFenceDist
                             continue;
                         }
 
-                        var dXt2 = Math.Sin(angle*deg2rad)*distToLocation;
+                        var dXt2 = Math.Sin(angle*MathHelper.deg2rad)*distToLocation;
 
-                        var dXt = Math.Asin(Math.Sin(distToLocation/R)*Math.Sin(angle*deg2rad))*R;
+                        var dXt = Math.Asin(Math.Sin(distToLocation/R)*Math.Sin(angle*MathHelper.deg2rad))*R;
 
                         disttotal = (float) Math.Min(disttotal, Math.Abs(dXt2));
 
@@ -834,7 +834,7 @@ public float ELToMAV
 
                 float altdiff = (float) (_altasl - TrackerLocation.Alt);
 
-                float angle = (float) (Math.Atan(altdiff/dist)*rad2deg);
+                float angle = (float) (Math.Atan(altdiff/dist)*MathHelper.rad2deg);
 
                 return angle;
             }
@@ -1110,9 +1110,6 @@ public void ResetInternals()
             }
         }
 
-        const double rad2deg = (180/Math.PI);
-        const double deg2rad = (1.0/rad2deg);
-
         private DateTime lastupdate = DateTime.Now;
 
         private DateTime lastsecondcounter = DateTime.Now;
@@ -1749,9 +1746,9 @@ public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool up
                     {
                         var att = mavLinkMessage.ToStructure<MAVLink.mavlink_attitude_t>();
 
-                        roll = (float)(att.roll*rad2deg);
-                        pitch = (float)(att.pitch*rad2deg);
-                        yaw = (float)(att.yaw*rad2deg);
+                        roll = (float)(att.roll*MathHelper.rad2deg);
+                        pitch = (float)(att.pitch*MathHelper.rad2deg);
+                        yaw = (float)(att.yaw*MathHelper.rad2deg);
 
                         //Console.WriteLine(MAV.sysid + " " +roll + " " + pitch + " " + yaw);
 
@@ -2217,10 +2214,10 @@ public void dowindcalc()
             if (airspeed < 1 || groundspeed < 1)
                 return;
 
-            double Wn_error = airspeed*Math.Cos((yaw)*deg2rad)*Math.Cos(pitch*deg2rad) -
-                              groundspeed*Math.Cos((groundcourse)*deg2rad) - Wn_fgo;
-            double We_error = airspeed*Math.Sin((yaw)*deg2rad)*Math.Cos(pitch*deg2rad) -
-                              groundspeed*Math.Sin((groundcourse)*deg2rad) - We_fgo;
+            double Wn_error = airspeed*Math.Cos((yaw)*MathHelper.deg2rad)*Math.Cos(pitch*MathHelper.deg2rad) -
+                              groundspeed*Math.Cos((groundcourse)*MathHelper.deg2rad) - Wn_fgo;
+            double We_error = airspeed*Math.Sin((yaw)*MathHelper.deg2rad)*Math.Cos(pitch*MathHelper.deg2rad) -
+                              groundspeed*Math.Sin((groundcourse)*MathHelper.deg2rad) - We_fgo;
 
             Wn_fgo = Wn_fgo + Kw*Wn_error;
             We_fgo = We_fgo + Kw*We_error;

ExtLibs/Utilities/Math.cs

@@ -0,0 +1,23 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+namespace MissionPlanner.Utilities
+{
+    public static class MathHelper
+    {
+        public const double rad2deg = (180 / Math.PI);
+        public const double deg2rad = (1.0 / rad2deg);
+
+        public static double Degrees(double rad)
+        {
+            return rad * rad2deg;
+        }
+
+        public static double Radians(double deg)
+        {
+            return deg * deg2rad;
+        }
+    }
+}

ExtLibs/Utilities/MissionPlanner.Utilities.csproj

@@ -94,6 +94,7 @@
     <Compile Include="L10NU.cs" />
     <Compile Include="LangUtility.cs" />
     <Compile Include="locationwp.cs" />
+    <Compile Include="Math.cs" />
     <Compile Include="MeasureString.cs" />
     <Compile Include="MissionFile.cs" />
     <Compile Include="PointLatLngAlt.cs" />

ExtLibs/Utilities/PointLatLngAlt.cs

@@ -22,9 +22,6 @@ public class PointLatLngAlt: IComparable
         public string Tag2 = "";
         public Color color = Color.White;
 
-        const double rad2deg = (180 / Math.PI);
-        const double deg2rad = (1.0 / rad2deg);
-
         static CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();
         static GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84;
 
@@ -209,18 +206,18 @@ public PointLatLngAlt newpos(double bearing, double distance)
             // from math import sin, asin, cos, atan2, radians, degrees
             double radius_of_earth = 6378100.0;//# in meters
 
-            double lat1 = deg2rad * (this.Lat);
-            double lon1 = deg2rad * (this.Lng);
-            double brng = deg2rad * (bearing);
+            double lat1 = MathHelper.deg2rad * (this.Lat);
+            double lon1 = MathHelper.deg2rad * (this.Lng);
+            double brng = MathHelper.deg2rad * (bearing);
             double dr = distance / radius_of_earth;
 
             double lat2 = Math.Asin(Math.Sin(lat1) * Math.Cos(dr) +
                         Math.Cos(lat1) * Math.Sin(dr) * Math.Cos(brng));
             double lon2 = lon1 + Math.Atan2(Math.Sin(brng) * Math.Sin(dr) * Math.Cos(lat1),
                                 Math.Cos(dr) - Math.Sin(lat1) * Math.Sin(lat2));
 
-            double latout = rad2deg * (lat2);
-            double lngout = rad2deg * (lon2);
+            double latout = MathHelper.rad2deg * (lat2);
+            double lngout = MathHelper.rad2deg * (lon2);
 
             return new PointLatLngAlt(latout, lngout, this.Alt, this.Tag);
         }
@@ -233,21 +230,21 @@ public PointLatLngAlt newpos(double bearing, double distance)
         /// <returns></returns>
         public PointLatLngAlt gps_offset(double east, double north)
         {
-            double bearing = Math.Atan2(east, north) * rad2deg;
+            double bearing = Math.Atan2(east, north) * MathHelper.rad2deg;
             double distance = Math.Sqrt(Math.Pow(east, 2) + Math.Pow(north, 2));
             return newpos(bearing, distance);
         }
 
         public double GetBearing(PointLatLngAlt p2)
         {
-            var latitude1 = deg2rad * (this.Lat);
-            var latitude2 = deg2rad * (p2.Lat);
-            var longitudeDifference = deg2rad * (p2.Lng - this.Lng);
+            var latitude1 = MathHelper.deg2rad * (this.Lat);
+            var latitude2 = MathHelper.deg2rad * (p2.Lat);
+            var longitudeDifference = MathHelper.deg2rad * (p2.Lng - this.Lng);
 
             var y = Math.Sin(longitudeDifference) * Math.Cos(latitude2);
             var x = Math.Cos(latitude1) * Math.Sin(latitude2) - Math.Sin(latitude1) * Math.Cos(latitude2) * Math.Cos(longitudeDifference);
 
-            return (rad2deg * (Math.Atan2(y, x)) + 360) % 360;
+            return (MathHelper.rad2deg * (Math.Atan2(y, x)) + 360) % 360;
         }
 
         /// <summary>
@@ -272,10 +269,10 @@ public double GetDistance2(PointLatLngAlt p2)
         {
             //http://www.movable-type.co.uk/scripts/latlong.html
             var R = 6371.0; // 6371 km
-            var dLat = (p2.Lat - Lat) * deg2rad;
-            var dLon = (p2.Lng - Lng) * deg2rad;
-            var lat1 = Lat * deg2rad;
-            var lat2 = p2.Lat * deg2rad;
+            var dLat = (p2.Lat - Lat) * MathHelper.deg2rad;
+            var dLon = (p2.Lng - Lng) * MathHelper.deg2rad;
+            var lat1 = Lat * MathHelper.deg2rad;
+            var lat2 = p2.Lat * MathHelper.deg2rad;
 
             var a = Math.Sin(dLat / 2.0) * Math.Sin(dLat / 2.0) +
                     Math.Sin(dLon / 2.0) * Math.Sin(dLon / 2.0) * Math.Cos(lat1) * Math.Cos(lat2);

GCSViews/ConfigurationView/ConfigHWAirspeed.cs

@@ -7,8 +7,6 @@ namespace MissionPlanner.GCSViews.ConfigurationView
 {
     public partial class ConfigHWAirspeed : UserControl, IActivate
     {
-        private const double rad2deg = (180/Math.PI);
-        private const double deg2rad = (1.0/rad2deg);
         private bool startup;
 
         public ConfigHWAirspeed()

GCSViews/ConfigurationView/ConfigHWBT.cs

@@ -11,8 +11,6 @@ namespace MissionPlanner.GCSViews.ConfigurationView
 {
     public partial class ConfigHWBT : UserControl, IActivate
     {
-        private const double rad2deg = (180/Math.PI);
-        private const double deg2rad = (1.0/rad2deg);
         private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
         private readonly Dictionary<int, int> baudmap = new Dictionary<int, int>

GCSViews/ConfigurationView/ConfigHWCompass.cs

@@ -11,8 +11,6 @@ namespace MissionPlanner.GCSViews.ConfigurationView
 {
     public partial class ConfigHWCompass : UserControl, IActivate
     {
-        private const double rad2deg = (180/Math.PI);
-        private const double deg2rad = (1.0/rad2deg);
         private const int THRESHOLD_OFS_RED = 600;
         private const int THRESHOLD_OFS_YELLOW = 400;
         private bool startup;
@@ -45,7 +43,7 @@ public void Activate()
             CHK_compass_learn.setup(1, 0, "COMPASS_LEARN", MainV2.comPort.MAV.param);
             if (MainV2.comPort.MAV.param["COMPASS_DEC"] != null)
             {
-                var dec = MainV2.comPort.MAV.param["COMPASS_DEC"].Value*rad2deg;
+                var dec = MainV2.comPort.MAV.param["COMPASS_DEC"].Value*MathHelper.rad2deg;
 
                 var min = (dec - (int) dec)*60;
 
@@ -255,7 +253,7 @@ private void TXT_declination_Validated(object sender, EventArgs e)
                         return;
                     }
 
-                    MainV2.comPort.setParam("COMPASS_DEC", dec*deg2rad);
+                    MainV2.comPort.setParam("COMPASS_DEC", dec*MathHelper.deg2rad);
                 }
             }
             catch

GCSViews/ConfigurationView/ConfigHWOSD.cs

@@ -6,9 +6,6 @@ namespace MissionPlanner.GCSViews.ConfigurationView
 {
     public partial class ConfigHWOSD : UserControl, IActivate
     {
-        private const double rad2deg = (180/Math.PI);
-        private const double deg2rad = (1.0/rad2deg);
-
         public ConfigHWOSD()
         {
             InitializeComponent();

GCSViews/ConfigurationView/ConfigHWOptFlow.cs

@@ -6,8 +6,6 @@ namespace MissionPlanner.GCSViews.ConfigurationView
 {
     public partial class ConfigHWOptFlow : UserControl, IActivate
     {
-        private const double rad2deg = (float) (180/Math.PI);
-        private const double deg2rad = (float) (1.0/rad2deg);
         private bool startup;
 
         public ConfigHWOptFlow()

GCSViews/ConfigurationView/ConfigHWPX4Flow.cs

@@ -8,8 +8,6 @@ namespace MissionPlanner.GCSViews.ConfigurationView
 {
     public partial class ConfigHWPX4Flow : UserControl, IActivate, IDeactivate
     {
-        private const double rad2deg = (180/Math.PI);
-        private const double deg2rad = (1.0/rad2deg);
         bool focusmode = false;
 
         OpticalFlow flow = null;

GCSViews/ConfigurationView/ConfigHWRangeFinder.cs

@@ -8,8 +8,6 @@ namespace MissionPlanner.GCSViews.ConfigurationView
     public partial class ConfigHWRangeFinder : UserControl, IActivate, IDeactivate
     {
         bool startup = true;
-        private const double rad2deg = (180/Math.PI);
-        private const double deg2rad = (1.0/rad2deg);
 
         public ConfigHWRangeFinder()
         {

GCSViews/ConfigurationView/ConfigHWUAVCAN.cs

@@ -7,9 +7,6 @@ namespace MissionPlanner.GCSViews.ConfigurationView
 {
     public partial class ConfigHWUAVCAN : UserControl, IActivate
     {
-        private const double rad2deg = (180/Math.PI);
-        private const double deg2rad = (1.0/rad2deg);
-
         public ConfigHWUAVCAN()
         {
             InitializeComponent();

GCSViews/FlightData.cs

@@ -90,10 +90,6 @@ public partial class FlightData : MyUserControl, IActivate, IDeactivate
 
         List<PointLatLng> trackPoints = new List<PointLatLng>();
 
-        const double rad2deg = (180/Math.PI);
-
-        const double deg2rad = (1.0/rad2deg);
-
         public static HUD myhud;
         public static myGMAP mymap;
 

GCSViews/FlightPlanner.cs

@@ -38,8 +38,6 @@
 using Placemark = SharpKml.Dom.Placemark;
 using Point = System.Drawing.Point;
 using System.Text.RegularExpressions;
-using netDxf;
-using netDxf.Entities;
 
 namespace MissionPlanner.GCSViews
 {
@@ -4083,9 +4081,6 @@ RectLatLng getPolyMinMax(GMapPolygon poly)
             return new RectLatLng(maxy, minx, Math.Abs(maxx - minx), Math.Abs(miny - maxy));
         }
 
-        const double rad2deg = (180/Math.PI);
-        const double deg2rad = (1.0/rad2deg);
-
         private void BUT_grid_Click(object sender, EventArgs e)
         {
         }
@@ -5032,13 +5027,13 @@ private void createWpCircleToolStripMenuItem_Click(object sender, EventArgs e)
                 float d = Radius;
                 float R = 6371000;
 
-                var lat2 = Math.Asin(Math.Sin(MouseDownEnd.Lat*deg2rad)*Math.Cos(d/R) +
-                                     Math.Cos(MouseDownEnd.Lat*deg2rad)*Math.Sin(d/R)*Math.Cos(a*deg2rad));
-                var lon2 = MouseDownEnd.Lng*deg2rad +
-                           Math.Atan2(Math.Sin(a*deg2rad)*Math.Sin(d/R)*Math.Cos(MouseDownEnd.Lat*deg2rad),
-                               Math.Cos(d/R) - Math.Sin(MouseDownEnd.Lat*deg2rad)*Math.Sin(lat2));
+                var lat2 = Math.Asin(Math.Sin(MouseDownEnd.Lat*MathHelper.deg2rad)*Math.Cos(d/R) +
+                                     Math.Cos(MouseDownEnd.Lat*MathHelper.deg2rad)*Math.Sin(d/R)*Math.Cos(a*MathHelper.deg2rad));
+                var lon2 = MouseDownEnd.Lng*MathHelper.deg2rad +
+                           Math.Atan2(Math.Sin(a*MathHelper.deg2rad)*Math.Sin(d/R)*Math.Cos(MouseDownEnd.Lat*MathHelper.deg2rad),
+                               Math.Cos(d/R) - Math.Sin(MouseDownEnd.Lat*MathHelper.deg2rad)*Math.Sin(lat2));
 
-                PointLatLng pll = new PointLatLng(lat2*rad2deg, lon2*rad2deg);
+                PointLatLng pll = new PointLatLng(lat2*MathHelper.rad2deg, lon2*MathHelper.rad2deg);
 
                 setfromMap(pll.Lat, pll.Lng, (int) float.Parse(TXT_DefaultAlt.Text));
             }
@@ -6711,13 +6706,13 @@ private void createSplineCircleToolStripMenuItem_Click(object sender, EventArgs
                     float d = Radius;
                     float R = 6371000;
 
-                    var lat2 = Math.Asin(Math.Sin(MouseDownEnd.Lat*deg2rad)*Math.Cos(d/R) +
-                                         Math.Cos(MouseDownEnd.Lat*deg2rad)*Math.Sin(d/R)*Math.Cos(a*deg2rad));
-                    var lon2 = MouseDownEnd.Lng*deg2rad +
-                               Math.Atan2(Math.Sin(a*deg2rad)*Math.Sin(d/R)*Math.Cos(MouseDownEnd.Lat*deg2rad),
-                                   Math.Cos(d/R) - Math.Sin(MouseDownEnd.Lat*deg2rad)*Math.Sin(lat2));
+                    var lat2 = Math.Asin(Math.Sin(MouseDownEnd.Lat*MathHelper.deg2rad)*Math.Cos(d/R) +
+                                         Math.Cos(MouseDownEnd.Lat*MathHelper.deg2rad)*Math.Sin(d/R)*Math.Cos(a*MathHelper.deg2rad));
+                    var lon2 = MouseDownEnd.Lng*MathHelper.deg2rad +
+                               Math.Atan2(Math.Sin(a*MathHelper.deg2rad)*Math.Sin(d/R)*Math.Cos(MouseDownEnd.Lat*MathHelper.deg2rad),
+                                   Math.Cos(d/R) - Math.Sin(MouseDownEnd.Lat*MathHelper.deg2rad)*Math.Sin(lat2));
 
-                    PointLatLng pll = new PointLatLng(lat2*rad2deg, lon2*rad2deg);
+                    PointLatLng pll = new PointLatLng(lat2*MathHelper.rad2deg, lon2*MathHelper.rad2deg);
 
                     setfromMap(pll.Lat, pll.Lng, stepalt);
 

GeoRef/georefimage.cs

@@ -40,10 +40,6 @@ private enum PROCESSING_MODE
 
         private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
-        // CONSTS
-        private const double rad2deg = (180/Math.PI);
-        private const double deg2rad = (1.0/rad2deg);
-
         // Key = path of file, Value = object with picture information
         private Dictionary<string, PictureInformation> picturesInfo;
 
@@ -1305,7 +1301,7 @@ private int GenPhotoStationRecord(XmlTextWriter swloctrim, string imgname, doubl
             JXL_StationIDs.Add(photoStationID);
 
             // conver tto rads
-            yaw = -yaw*deg2rad;
+            yaw = -yaw*MathHelper.deg2rad;
 
             swloctrim.WriteStartElement("PhotoStationRecord");
             swloctrim.WriteAttributeString("ID", (photoStationID).ToString("0000000"));
@@ -1424,12 +1420,12 @@ private RectangleF getboundingbox(double centery, double centerx, double alt, do
 
         public static double radians(double val)
         {
-            return val*deg2rad;
+            return val*MathHelper.deg2rad;
         }
 
         public static double degrees(double val)
         {
-            return val*rad2deg;
+            return val*MathHelper.rad2deg;
         }
 
         private void newpos(ref double lat, ref double lon, double bearing, double distance)