public static Runway createFrom(ConfigNode node)
{
string rwy_ident = node.GetValue("ident");
string customValue = node.GetValue("custom");
Runway runway = new Runway
{
ident = rwy_ident,
shortID = node.GetValue("shortID"),
custom = customValue != null && bool.Parse(customValue),
hdg = float.Parse(node.GetValue("hdg")),
body = node.GetValue("body"),
altMSL = float.Parse(node.GetValue("altMSL")),
gsLatitude = float.Parse(node.GetValue("gsLatitude")),
gsLongitude = float.Parse(node.GetValue("gsLongitude")),
locLatitude = float.Parse(node.GetValue("locLatitude")),
locLongitude = float.Parse(node.GetValue("locLongitude")),
outerMarkerDist = float.Parse(node.GetValue("outerMarkerDist")),
middleMarkerDist = float.Parse(node.GetValue("middleMarkerDist")),
innerMarkerDist = float.Parse(node.GetValue("innerMarkerDist"))
};
if (runway.shortID.Length > 4)
{
runway.shortID.Remove(4);
}
return(runway);
}
//end Virindi contribution :D
/// <returns>float in degrees</returns>
public static float CalcLocalizerDeviation(double bearingToLoc, Runway currentRwy)
{
double 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);
* Log.dbg("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));
Log.dbg("new answer: {0}", ret);
return(ret);
}
catch (Exception ex)
{
Log.ex(null, ex);
return(0d);
}
}
private static Runway CreateRunwayFromNode(ConfigNode node)
{
Log.detail("NavUtil: Found Runway Node");
try
{
Runway rwy = Runway.createFrom(node);
Log.detail("NavUtil: Found " + rwy.ident);
return(rwy);
}
catch (Exception)
{
Log.detail("NavUtil: Error loading runway");
throw;
}
}
public bool isINSTarget = false; //true indicates that the runway is not the actual runway and is used as a target point for INS
public static Runway fallback()
{
Runway runway = new Runway
{
ident = "---",
shortID = "---",
hdg = 0,
body = "",
altMSL = 0,
gsLatitude = 0,
gsLongitude = 0,
locLatitude = 0,
locLongitude = 0,
outerMarkerDist = 0,
middleMarkerDist = 0,
innerMarkerDist = 0
};
return(runway);
}
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);
Log.dbg("Old rwy heading: {0}", currentRwy.hdg);
Log.dbg("New rwy heading: {0}", ret);
return(ret);
}
catch (Exception ex)
{
Log.ex(null, ex);
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)
{
double lonDelta = currentRwy.locLongitude - thisVessel.longitude;
return(Utils.CalcBearingTo(Utils.CalcRadiansFromDeg(lonDelta), Utils.CalcRadiansFromDeg(thisVessel.latitude), Utils.CalcRadiansFromDeg(currentRwy.locLatitude)));
}
