//end Virindi contribution :D /// <returns>float in degrees</returns> public static float CalcLocalizerDeviation(double bearingToLoc, Runway currentRwy) { var RwyHdgCorrection = 180 - Utils.makeAngle0to360(currentRwy.hdg); return((float)(Utils.makeAngle0to360(currentRwy.hdg + RwyHdgCorrection) - Utils.makeAngle0to360(bearingToLoc + RwyHdgCorrection))); //return (float)(Utils.makeAngle0to360(currentRwy.hdg) - Utils.makeAngle0to360(bearingToLoc)) * -1; }
/// <returns>double in degrees</returns> public static double CalcLocalizerDeviation(Vessel thisVessel, Runway currentRwy) { try { /* * { * //Old algorithm. * double rwyhdg = (double)currentRwy.hdg; * double RwyHdgCorrection = 180d - Utils.makeAngle0to360(rwyhdg); * Debug.Log("Old answer: " + (Utils.makeAngle0to360(rwyhdg + RwyHdgCorrection) - Utils.makeAngle0to360(Utils.CalcBearingToBeacon(thisVessel, currentRwy) + RwyHdgCorrection)).ToString()); * } */ //We are going to measure the angle in the plane of the runway (normal to the surface). //So, we will pretend everything is at this altitude. double bodyradius = FlightGlobals.Bodies.Find(body => body.name == currentRwy.body).Radius + (double)currentRwy.altMSL; Vector3d rwy = SphericalToCartesianCoordinates(currentRwy.locLatitude, currentRwy.locLongitude, bodyradius); Vector3d ship = SphericalToCartesianCoordinates(thisVessel.latitude, thisVessel.longitude, bodyradius); Vector3d northpole = new Vector3d(0, bodyradius, 0); Vector3d eastfromrwy = Vector3d.Cross(northpole, rwy).normalized; Vector3d northfromrwy = Vector3d.Cross(eastfromrwy, rwy).normalized; double rwyheadingradians = -Utils.CalcRadiansFromDeg(currentRwy.hdg); Vector3d rwynormalized = rwy.normalized; //Rodrigues' rotation formula //This vector represents the runway direction of travel in the plane of the runway normal to the surface (the surface plane). Vector3d offsetloc = northfromrwy * Math.Cos(rwyheadingradians) + Vector3d.Cross(rwynormalized, northfromrwy) * Math.Sin(rwyheadingradians) + rwynormalized * Vector3d.Dot(rwynormalized, northfromrwy) * (1d - Math.Cos(rwyheadingradians)); //Normal for the plane encompassing the runway, in the direction of travel, and the center of the body (the centerline plane). Vector3d planenormal = Vector3d.Cross(offsetloc, rwy).normalized; //Distance left to right from the centerline plane. double cldist = Vector3d.Dot(planenormal, ship); //Distance forward and back to the plane encompassing the runway point, the runway normal, and the origin (how far in front or behind you are from the marker) double fwbackdist = Vector3d.Dot(offsetloc.normalized, ship); //We are computing the angle that is projected into the surface plane. double ret = -CalcDegFromRadians(Math.Atan2(cldist, fwbackdist)); //Debug.Log ("new answer: " + ret.ToString ()); return(ret); } catch (Exception ex) { Debug.LogError(ex.ToString()); return(0d); } }
public static System.Collections.Generic.List<Runway> GetRunwayListFromConfig(string sSettingURL) { System.Collections.Generic.List<Runway> runwayList = new System.Collections.Generic.List<Runway>(); runwayList.Clear(); ConfigNode runways = ConfigNode.Load(KSPUtil.ApplicationRootPath + sSettingURL); foreach (ConfigNode node in runways.GetNodes("Runway")) { if (GlobalVariables.Settings.enableDebugging) Debug.Log("NavUtil: Found Runway Node"); try { Runway rwy = new Runway(); rwy.ident = node.GetValue("ident"); if (GlobalVariables.Settings.enableDebugging) Debug.Log("NavUtil: Loading " + rwy.ident); rwy.shortID = node.GetValue("shortID"); if (rwy.shortID.Length > 4) rwy.shortID.Remove(4); rwy.hdg = float.Parse(node.GetValue("hdg")); rwy.body = node.GetValue("body"); rwy.altMSL = float.Parse(node.GetValue("altMSL")); rwy.gsLatitude = float.Parse(node.GetValue("gsLatitude")); rwy.gsLongitude = float.Parse(node.GetValue("gsLongitude")); rwy.locLatitude = float.Parse(node.GetValue("locLatitude")); rwy.locLongitude = float.Parse(node.GetValue("locLongitude")); rwy.outerMarkerDist = float.Parse(node.GetValue("outerMarkerDist")); rwy.middleMarkerDist = float.Parse(node.GetValue("middleMarkerDist")); rwy.innerMarkerDist = float.Parse(node.GetValue("innerMarkerDist")); runwayList.Add(rwy); if (GlobalVariables.Settings.enableDebugging) Debug.Log("NavUtil: Found " + rwy.ident); } catch (Exception) { if (GlobalVariables.Settings.enableDebugging) Debug.Log("NavUtil: Error loading runway"); throw; } } return runwayList; }
private static byte inBeaconArea(float locDeviation, Vessel v, Runway r) { if (3f > locDeviation && locDeviation > -3f) { var gcd = NavUtilLib.Utils.CalcGreatCircleDistance(v.latitude, v.longitude, r.gsLatitude, r.gsLongitude, r.body); if (r.outerMarkerDist + 200 > gcd && r.outerMarkerDist - 200 < gcd) { return(1); } if (r.middleMarkerDist + 100 > gcd && r.middleMarkerDist - 100 < gcd) { return(2); } if (r.innerMarkerDist + 50 > gcd && r.innerMarkerDist - 50 < gcd) { return(3); } } return(0); }
//the following section is courtesy Virindi /// <returns>double in degrees</returns> public static double CalcProjectedRunwayHeading(Vessel thisVessel, Runway currentRwy) { try { double bodyradius = FlightGlobals.Bodies.Find(body => body.name == currentRwy.body).Radius + (double)currentRwy.altMSL; Vector3d rwy = SphericalToCartesianCoordinates(currentRwy.locLatitude, currentRwy.locLongitude, bodyradius); Vector3d ship = SphericalToCartesianCoordinates(thisVessel.latitude, thisVessel.longitude, bodyradius); Vector3d northpole = new Vector3d(0, bodyradius, 0); Vector3d eastfromrwy = Vector3d.Cross(northpole, rwy).normalized; Vector3d northfromrwy = Vector3d.Cross(eastfromrwy, rwy).normalized; Vector3d eastfromship = Vector3d.Cross(northpole, ship).normalized; Vector3d northfromship = Vector3d.Cross(eastfromship, ship).normalized; double rwyheadingradians = -Utils.CalcRadiansFromDeg(currentRwy.hdg); Vector3d rwynormalized = rwy.normalized; //Rodrigues' rotation formula //This vector represents the runway direction of travel in the plane of the runway normal to the surface (the surface plane). Vector3d offsetloc = northfromrwy * Math.Cos(rwyheadingradians) + Vector3d.Cross(rwynormalized, northfromrwy) * Math.Sin(rwyheadingradians) + rwynormalized * Vector3d.Dot(rwynormalized, northfromrwy) * (1d - Math.Cos(rwyheadingradians)); //Project runway heading vector on to our bearing axes double northportion = Vector3d.Dot(northfromship, offsetloc); double eastportion = Vector3d.Dot(eastfromship, offsetloc); double ret = makeAngle0to360(CalcDegFromRadians(Math.Atan2(northportion, eastportion)) - 90d); //Debug.Log ("Old rwy heading: " + currentRwy.hdg.ToString()); //Debug.Log ("New rwy heading: " + ret.ToString ()); return(ret); } catch (Exception ex) { Debug.LogError(ex.ToString()); return(0d); } }
/// <returns>double in degrees</returns> public static double CalcElevationAngle(Vessel thisVessel, Runway currentRwy) { return Utils.CalcElevationFrom(thisVessel.latitude, thisVessel.longitude, thisVessel.altitude, currentRwy.gsLatitude, currentRwy.gsLongitude, currentRwy.altMSL, currentRwy.body) - 90; //we subtract 90 degrees since 0 is towards the center of the body and 180 is directly outwards from the body; we want to know the angle relative to the tangent }
///<returns>double in meters</returns> public static double CalcDistanceToBeacon(Vessel thisVessel, Runway currentRwy) { return Utils.CalcDistance(thisVessel.latitude, thisVessel.longitude, thisVessel.altitude, currentRwy.gsLatitude, currentRwy.gsLongitude, currentRwy.altMSL, currentRwy.body); }
/// <returns>double in degrees</returns> public static double CalcBearingToBeacon(Vessel thisVessel, Runway currentRwy) { var lonDelta = currentRwy.locLongitude - thisVessel.longitude; return Utils.CalcBearingTo(Utils.CalcRadiansFromDeg(lonDelta), Utils.CalcRadiansFromDeg(thisVessel.latitude), Utils.CalcRadiansFromDeg(currentRwy.locLatitude)); }
//the following section is courtesy Virindi /// <returns>double in degrees</returns> public static double CalcProjectedRunwayHeading(Vessel thisVessel, Runway currentRwy) { try { double bodyradius = FlightGlobals.Bodies.Find(body => body.name == currentRwy.body).Radius + (double)currentRwy.altMSL; Vector3d rwy = SphericalToCartesianCoordinates(currentRwy.locLatitude, currentRwy.locLongitude, bodyradius); Vector3d ship = SphericalToCartesianCoordinates(thisVessel.latitude, thisVessel.longitude, bodyradius); Vector3d northpole = new Vector3d(0, bodyradius, 0); Vector3d eastfromrwy = Vector3d.Cross(northpole, rwy).normalized; Vector3d northfromrwy = Vector3d.Cross(eastfromrwy, rwy).normalized; Vector3d eastfromship = Vector3d.Cross(northpole, ship).normalized; Vector3d northfromship = Vector3d.Cross(eastfromship, ship).normalized; double rwyheadingradians = -Utils.CalcRadiansFromDeg(currentRwy.hdg); Vector3d rwynormalized = rwy.normalized; //Rodrigues' rotation formula //This vector represents the runway direction of travel in the plane of the runway normal to the surface (the surface plane). Vector3d offsetloc = northfromrwy * Math.Cos(rwyheadingradians) + Vector3d.Cross(rwynormalized, northfromrwy) * Math.Sin(rwyheadingradians) + rwynormalized * Vector3d.Dot(rwynormalized, northfromrwy) * (1d - Math.Cos(rwyheadingradians)); //Project runway heading vector on to our bearing axes double northportion = Vector3d.Dot(northfromship, offsetloc); double eastportion = Vector3d.Dot(eastfromship, offsetloc); double ret = makeAngle0to360(CalcDegFromRadians(Math.Atan2(northportion, eastportion)) - 90d); //Debug.Log ("Old rwy heading: " + currentRwy.hdg.ToString()); //Debug.Log ("New rwy heading: " + ret.ToString ()); return ret; } catch (Exception ex) { Debug.LogError(ex.ToString()); return 0d; } }
//end Virindi contribution :D /// <returns>float in degrees</returns> public static float CalcLocalizerDeviation(double bearingToLoc, Runway currentRwy) { var RwyHdgCorrection = 180 - Utils.makeAngle0to360(currentRwy.hdg); return (float) (Utils.makeAngle0to360(currentRwy.hdg + RwyHdgCorrection) - Utils.makeAngle0to360(bearingToLoc + RwyHdgCorrection)); //return (float)(Utils.makeAngle0to360(currentRwy.hdg) - Utils.makeAngle0to360(bearingToLoc)) * -1; }
/// <returns>double in degrees</returns> public static double CalcLocalizerDeviation(Vessel thisVessel, Runway currentRwy) { try { /* { //Old algorithm. double rwyhdg = (double)currentRwy.hdg; double RwyHdgCorrection = 180d - Utils.makeAngle0to360(rwyhdg); Debug.Log("Old answer: " + (Utils.makeAngle0to360(rwyhdg + RwyHdgCorrection) - Utils.makeAngle0to360(Utils.CalcBearingToBeacon(thisVessel, currentRwy) + RwyHdgCorrection)).ToString()); } */ //We are going to measure the angle in the plane of the runway (normal to the surface). //So, we will pretend everything is at this altitude. double bodyradius = FlightGlobals.Bodies.Find(body => body.name == currentRwy.body).Radius + (double)currentRwy.altMSL; Vector3d rwy = SphericalToCartesianCoordinates(currentRwy.locLatitude, currentRwy.locLongitude, bodyradius); Vector3d ship = SphericalToCartesianCoordinates(thisVessel.latitude, thisVessel.longitude, bodyradius); Vector3d northpole = new Vector3d(0, bodyradius, 0); Vector3d eastfromrwy = Vector3d.Cross(northpole, rwy).normalized; Vector3d northfromrwy = Vector3d.Cross(eastfromrwy, rwy).normalized; double rwyheadingradians = -Utils.CalcRadiansFromDeg(currentRwy.hdg); Vector3d rwynormalized = rwy.normalized; //Rodrigues' rotation formula //This vector represents the runway direction of travel in the plane of the runway normal to the surface (the surface plane). Vector3d offsetloc = northfromrwy * Math.Cos(rwyheadingradians) + Vector3d.Cross(rwynormalized, northfromrwy) * Math.Sin(rwyheadingradians) + rwynormalized * Vector3d.Dot(rwynormalized, northfromrwy) * (1d - Math.Cos(rwyheadingradians)); //Normal for the plane encompassing the runway, in the direction of travel, and the center of the body (the centerline plane). Vector3d planenormal = Vector3d.Cross(offsetloc, rwy).normalized; //Distance left to right from the centerline plane. double cldist = Vector3d.Dot(planenormal, ship); //Distance forward and back to the plane encompassing the runway point, the runway normal, and the origin (how far in front or behind you are from the marker) double fwbackdist = Vector3d.Dot(offsetloc.normalized, ship); //We are computing the angle that is projected into the surface plane. double ret = -CalcDegFromRadians(Math.Atan2(cldist, fwbackdist)); //Debug.Log ("new answer: " + ret.ToString ()); return ret; } catch (Exception ex) { Debug.LogError(ex.ToString()); return 0d; } }
public static System.Collections.Generic.List <Runway> GetRunwayListFromConfig(string sSettingURL) { System.Collections.Generic.List <Runway> runwayList = new System.Collections.Generic.List <Runway>(); runwayList.Clear(); ConfigNode runways = ConfigNode.Load(KSPUtil.ApplicationRootPath + sSettingURL); foreach (ConfigNode node in runways.GetNodes("Runway")) { if (GlobalVariables.Settings.enableDebugging) { Debug.Log("NavUtil: Found Runway Node"); } try { Runway rwy = new Runway(); rwy.ident = node.GetValue("ident"); if (GlobalVariables.Settings.enableDebugging) { Debug.Log("NavUtil: Loading " + rwy.ident); } rwy.shortID = node.GetValue("shortID"); if (rwy.shortID.Length > 4) { rwy.shortID.Remove(4); } rwy.hdg = float.Parse(node.GetValue("hdg")); rwy.body = node.GetValue("body"); rwy.altMSL = float.Parse(node.GetValue("altMSL")); rwy.gsLatitude = float.Parse(node.GetValue("gsLatitude")); rwy.gsLongitude = float.Parse(node.GetValue("gsLongitude")); rwy.locLatitude = float.Parse(node.GetValue("locLatitude")); rwy.locLongitude = float.Parse(node.GetValue("locLongitude")); rwy.outerMarkerDist = float.Parse(node.GetValue("outerMarkerDist")); rwy.middleMarkerDist = float.Parse(node.GetValue("middleMarkerDist")); rwy.innerMarkerDist = float.Parse(node.GetValue("innerMarkerDist")); runwayList.Add(rwy); if (GlobalVariables.Settings.enableDebugging) { Debug.Log("NavUtil: Found " + rwy.ident); } } catch (Exception) { if (GlobalVariables.Settings.enableDebugging) { Debug.Log("NavUtil: Error loading runway"); } throw; } } return(runwayList); }
private static byte inBeaconArea(float locDeviation, Vessel v, Runway r) { if (3f > locDeviation && locDeviation > -3f) { var gcd = NavUtilLib.Utils.CalcGreatCircleDistance(v.latitude, v.longitude, r.gsLatitude, r.gsLongitude, r.body); if (r.outerMarkerDist + 200 > gcd && r.outerMarkerDist - 200 < gcd) { return 1; } if (r.middleMarkerDist + 100 > gcd && r.middleMarkerDist - 100 < gcd) { return 2; } if (r.innerMarkerDist + 50 > gcd && r.innerMarkerDist - 50 < gcd) { return 3; } } return 0; }
/// <returns>double in degrees</returns> public static double CalcElevationAngle(Vessel thisVessel, Runway currentRwy) { return(Utils.CalcElevationFrom(thisVessel.latitude, thisVessel.longitude, thisVessel.altitude, currentRwy.gsLatitude, currentRwy.gsLongitude, currentRwy.altMSL, currentRwy.body) - 90); //we subtract 90 degrees since 0 is towards the center of the body and 180 is directly outwards from the body; we want to know the angle relative to the tangent }
///<returns>double in meters</returns> public static double CalcDistanceToBeacon(Vessel thisVessel, Runway currentRwy) { return(Utils.CalcDistance(thisVessel.latitude, thisVessel.longitude, thisVessel.altitude, currentRwy.gsLatitude, currentRwy.gsLongitude, currentRwy.altMSL, currentRwy.body)); }
/// <returns>double in degrees</returns> public static double CalcBearingToBeacon(Vessel thisVessel, Runway currentRwy) { var lonDelta = currentRwy.locLongitude - thisVessel.longitude; return(Utils.CalcBearingTo(Utils.CalcRadiansFromDeg(lonDelta), Utils.CalcRadiansFromDeg(thisVessel.latitude), Utils.CalcRadiansFromDeg(currentRwy.locLatitude))); }