public static XYZ ConvertLLAToXYZ(LLA position, LLA positionOrigin)
{
var R = PhysicalConstants.EarthRadiusEquatorial;
var f = PhysicalConstants.EarthFlatteningFactor;
var u0 = positionOrigin.Latitude;
var l0 = positionOrigin.Longitude;
var numerator = 1 - (2 * f - f * f);
var denominator = 1 - (2 * f - f * f) * Pow(Sin(u0), 2);
var Rn = R / Sqrt(denominator);
var Rm = Rn * numerator / denominator;
var u = position.Latitude;
var l = position.Longitude;
var du = u - u0;
var dl = l - l0;
var positionNorth = du / Atan2(1, Rm);
var positionEast = dl / Atan2(1, (Rn * Cos(u0)));
var positionDown = -(position.Altitude - positionOrigin.Altitude);
var xyz = new XYZ()
{
X = positionNorth,
Y = positionEast,
Z = positionDown
};
return(xyz);
}
public override object ConvertFrom(ITypeDescriptorContext context, System.Globalization.CultureInfo culture, object value)
{
if (value is string)
{
return(LLA.TryParse((string)value));
}
return(base.ConvertFrom(context, culture, value));
}
/// <summary>
/// bl to xy
/// </summary>
/// <param name="lla"></param>
/// <param name="zone"></param>
/// <returns></returns>
private Cartesian bl2xy(LLA lla, int zone)
{
Cartesian carte;
double dl, dx, mx0;
double et2, w, N, tn2;
double x2, x4, x6, x8, y1, y3, y5, y7;
double B0, L0;
double sinb, cosb, tanb;
double M, m;
B0 = B_origin[zone] * Math.PI / 180.0;
L0 = L_origin[zone] * Math.PI / 180.0;
dl = lla.longitude - L0;
cosb = Math.Cos(lla.latitude);
sinb = Math.Sin(lla.latitude);
tanb = Math.Tan(lla.latitude);
mx0 = mx(B0);
dx = mx(lla.latitude) - mx0;
et2 = e12 * Math.Pow(cosb, 2.0);
w = 1.0 - e2 * Math.Pow(sinb, 2.0);
N = a / Math.Pow(w, 0.5);
M = N * (1.0 - e2) / w;
tn2 = Math.Pow(tanb, 2.0);
x2 = N * sinb * cosb * Math.Pow(dl, 2.0) / 2.0;
x4 = N * sinb * Math.Pow(cosb, 3.0) *
(5.0 - tn2 + 9.0 * et2 + 4.0 * Math.Pow(et2, 2.0)) * Math.Pow(dl, 4.0) / 24.0;
x6 = N * sinb * Math.Pow(cosb, 5.0) *
(61.0 - 58.0 * tn2 + Math.Pow(tn2, 2.0) + 270.0 * et2 - 330.0 * tn2 * et2) *
Math.Pow(dl, 6.0) / 720.0;
x8 = N * sinb * Math.Pow(cosb, 7.0) *
(1385.0 - 3111.0 * tn2 + 543.0 * Math.Pow(tn2, 2.0) - Math.Pow(tn2, 3.0)) *
Math.Pow(dl, 8.0) / 40320.0;
carte.x = m0 * (x8 + x6 + x4 + x2 + dx);
y1 = N * cosb * dl;
y3 = N * Math.Pow(cosb, 3.0) * (1.0 - tn2 + et2) * Math.Pow(dl, 3.0) / 6.0;
y5 = N * Math.Pow(cosb, 5.0) *
(5.0 - 18.0 * tn2 + Math.Pow(tn2, 2.0) + 14.0 * et2 - 58.0 * tn2 * et2) *
Math.Pow(dl, 5.0) / 120.0;
y7 = N * Math.Pow(cosb, 7.0) *
(61.0 - 479.0 * tn2 + 179.0 * Math.Pow(tn2, 2.0) - Math.Pow(tn2, 3.0)) *
Math.Pow(dl, 7.0) / 5040.0;
carte.y = m0 * (y7 + y5 + y3 + y1);
carte.c = (sinb * dl + sinb * Math.Pow(cosb, 2.0) * (1.0 + 3.0 * et2) * Math.Pow(dl, 3.0) / 3.0);
m = 1 + Math.Pow(cosb, 2.0) * (1.0 + Math.Sqrt(e12) * Math.Pow(cosb, 2.0)) *
Math.Pow(dl, 2.0) / 2.0 + Math.Pow(cosb, 4.0) * (5.0 - Math.Pow(tanb, 2.0)) * Math.Pow(dl, 4.0) / 24;
return(carte);
}
/// <summary>
/// WGS-84 to Cartesian coordinates
/// </summary>
/// <param name="lat84"></param>
/// <param name="lon84"></param>
/// <param name="alt84"></param>
public void w84toxyh(double lat84, double lon84, double alt84)
{
int datum = 0;
// STEP 1 : WGS-84 to XYZ
double lat = this.deg2rad(lat84);
double lon = this.deg2rad(lon84);
double alt = this.deg2rad(alt84);
datum = 1;
XYZ84 xyz84 = this.dms_to_XYZ(datum, lat, lon, alt);
// STEP 2 : WGS-84 to TOKYO
XYZ_Tokyo tokyo = this.WGS84_to_Tokyo(this.param, xyz84);
// STEP 3 : XYZ to LAT_LON_ALT
datum = 2;
LLA lla = this.XYZ_to_dms(datum, tokyo);
result = this.bl2xy(lla, this.zone);
}
public static LLA ConvertXYZToLLA(XYZ position, LLA positionOrigin)
{
var R = PhysicalConstants.EarthRadiusEquatorial;
var f = PhysicalConstants.EarthFlatteningFactor;
var u0 = positionOrigin.Latitude;
var l0 = positionOrigin.Longitude;
var dN = position.X;
var dE = position.Y;
var numerator = 1 - (2 * f - f * f);
var denominator = 1 - (2 * f - f * f) * Pow(Sin(u0), 2);
var Rn = R / Sqrt(denominator);
var Rm = Rn * numerator / denominator;
var du = Atan2(1, Rm) * dN;
var dl = Atan2(1, (Rn * Cos(u0))) * dE;
var altitude = -position.Z;
var u = u0 + du;
var l = l0 + dl;
var d = altitude - positionOrigin.Altitude;
var uDeg = u.ToDegrees();
var lDeg = l.ToDegrees();
var lla = new LLA()
{
LatitudeDeg = uDeg,
LongitudeDeg = lDeg,
Altitude = d
};
return(lla);
}
public static ScenarioSettings Example_1()
{
var lla = new LLA()
{
LatitudeDeg = 54.0,
LongitudeDeg = 12.0,
Altitude = 0.0
};
var flightpathSettings1 = new FlightpathSettings()
{
FlightpathId = 1,
FlightpathName = "Ownship_XXX",
PlatformType = "aircraft",
PlatformIcon = "c-130_hercules",
InitialPositionNorth = 0.0,
InitialPositionEast = 0.0,
InitialPositionDown = -5000.0,
InitialVelocityNorth = 0.0,
InitialVelocityEast = 0.0,
InitialVelocityDown = 0.0
};
var flightpathSettings2 = new FlightpathSettings()
{
FlightpathId = 2,
FlightpathName = "Target_001",
PlatformType = "landsite",
PlatformIcon = "sa-15_launcher_des",
InitialPositionNorth = 30000.0,
InitialPositionEast = 20000.0,
InitialPositionDown = 0.0,
InitialVelocityNorth = 0.0,
InitialVelocityEast = 0.0,
InitialVelocityDown = 0.0
};
var flightpathSettings3 = new FlightpathSettings()
{
FlightpathId = 3,
FlightpathName = "Target_002",
PlatformType = "landsite",
PlatformIcon = "sa-15_launcher_des",
InitialPositionNorth = -30000.0,
InitialPositionEast = 30000.0,
InitialPositionDown = 0.0,
InitialVelocityNorth = 0.0,
InitialVelocityEast = 0.0,
InitialVelocityDown = 0.0
};
var flightpathSettings = new List <FlightpathSettings>()
{
flightpathSettings1,
flightpathSettings2,
flightpathSettings3
};
var s = new ScenarioSettings()
{
LLAOrigin = lla,
FlightpathSettings = flightpathSettings
};
return(s);
}
/// <summary>
/// bl to xy
/// </summary>
/// <param name="lla"></param>
/// <param name="zone"></param>
/// <returns></returns>
private Cartesian bl2xy(LLA lla, int zone)
{
Cartesian carte;
double dl, dx, mx0;
double et2, w, N, tn2;
double x2, x4, x6, x8, y1, y3, y5, y7;
double B0, L0;
double sinb, cosb, tanb;
double M, m;
B0 = B_origin[zone] * Math.PI / 180.0;
L0 = L_origin[zone] * Math.PI / 180.0;
dl = lla.longitude - L0;
cosb = Math.Cos(lla.latitude);
sinb = Math.Sin(lla.latitude);
tanb = Math.Tan(lla.latitude);
mx0 = mx(B0);
dx = mx(lla.latitude) - mx0;
et2 = e12 * Math.Pow(cosb, 2.0);
w = 1.0 - e2 * Math.Pow(sinb, 2.0);
N = a / Math.Pow(w, 0.5);
M = N * (1.0 - e2) / w;
tn2 = Math.Pow(tanb, 2.0);
x2 = N * sinb * cosb * Math.Pow(dl, 2.0) / 2.0;
x4 = N * sinb * Math.Pow(cosb, 3.0) *
(5.0 - tn2 + 9.0 * et2 + 4.0 * Math.Pow(et2, 2.0)) * Math.Pow(dl, 4.0) / 24.0;
x6 = N * sinb * Math.Pow(cosb, 5.0) *
(61.0 - 58.0 * tn2 + Math.Pow(tn2, 2.0) + 270.0 * et2 - 330.0 * tn2 * et2) *
Math.Pow(dl, 6.0) / 720.0;
x8 = N * sinb * Math.Pow(cosb, 7.0) *
(1385.0 - 3111.0 * tn2 + 543.0 * Math.Pow(tn2, 2.0) - Math.Pow(tn2, 3.0)) *
Math.Pow(dl, 8.0) / 40320.0;
carte.x = m0 * (x8 + x6 + x4 + x2 + dx);
y1 = N * cosb * dl;
y3 = N * Math.Pow(cosb, 3.0) * (1.0 - tn2 + et2) * Math.Pow(dl, 3.0) / 6.0;
y5 = N * Math.Pow(cosb, 5.0) *
(5.0 - 18.0 * tn2 + Math.Pow(tn2, 2.0) + 14.0 * et2 - 58.0 * tn2 * et2) *
Math.Pow(dl, 5.0) / 120.0;
y7 = N * Math.Pow(cosb, 7.0) *
(61.0 - 479.0 * tn2 + 179.0 * Math.Pow(tn2, 2.0) - Math.Pow(tn2, 3.0)) *
Math.Pow(dl, 7.0) / 5040.0;
carte.y = m0 * (y7 + y5 + y3 + y1);
carte.c = (sinb * dl + sinb * Math.Pow(cosb, 2.0) * (1.0 + 3.0 * et2) * Math.Pow(dl, 3.0) / 3.0);
m = 1 + Math.Pow(cosb, 2.0) * (1.0 + Math.Sqrt(e12) * Math.Pow(cosb, 2.0)) *
Math.Pow(dl, 2.0) / 2.0 + Math.Pow(cosb, 4.0) * (5.0 - Math.Pow(tanb, 2.0)) * Math.Pow(dl, 4.0) / 24;
return carte;
}