//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)));
}
