/// <summary>
/// Converts from (x, y, z) ECEF coordinates to a wgs84 Location.
/// </summary>
/// <param name="ecef"></param>
/// <returns></returns>
public static Location EcefToLocation(DVector3 ecef)
{
var a = ARLocation.Config.EarthEquatorialRadiusInKM * 1000.0;
var e2 = ARLocation.Config.EarthFirstEccentricitySquared;
var b = a * Math.Sqrt(1 - e2);
var x = ecef.x;
var y = ecef.y;
var z = ecef.z;
var r = Math.Sqrt(x * x + y * y);
var E2 = (a * a - b * b) / (b * b);
var F = 54 * b * b * z * z;
var G = r * r + (1 - e2) * z * z - e2 * (a * a - b * b);
var c = (e2 * e2 * F * r * r) / (G * G * G);
var s = Math.Pow(1 + c + Math.Sqrt(c * c + 2 * c), 1.0 / 3.0);
var P = F / (3 * Math.Pow(s + (1.0 / s) + 1, 2.0) * G * G);
var Q = Math.Sqrt(1 + 2 * e2 * e2 * P);
var r0 = -(P * e2 * r) / (1 + Q) + Math.Sqrt(((a * a * 0.5) * (1.0 + (1.0 / Q))) - ((P * (1 - e2) * z * z) / (Q * (1.0 + Q))) - (P * r * r * 0.5));
var U = Math.Sqrt(Math.Pow(r - e2 * r0, 2) + z * z);
var V = Math.Sqrt(Math.Pow(r - e2 * r0, 2) + (1 - e2) * z * z);
var z0 = (b * b * z) / (a * V);
var h = U * (1 - ((b * b) / (a * V)));
var phi = Math.Atan((z + E2 * z0) / r);
var lambda = Math.Atan2(y, x);
var rad2deg = 180.0 / Math.PI;
return(new Location()
{
Latitude = rad2deg * phi,
Longitude = rad2deg * lambda,
Altitude = h,
AltitudeMode = AltitudeMode.GroundRelative
});
}
/// <summary>
/// Equals the specified v and e.
/// </summary>
/// <returns>The equals.</returns>
/// <param name="v">V.</param>
/// <param name="e">E.</param>
public bool Equals(DVector3 v, double e = 0.00005)
{
return((M.Abs(x - v.x) <= e) && (M.Abs(y - v.y) <= e) && (M.Abs(z - v.z) <= e));
}
/// <summary>
/// Lerp the specified a, b and t.
/// </summary>
/// <returns>The lerp.</returns>
/// <param name="a">The alpha component.</param>
/// <param name="b">The blue component.</param>
/// <param name="t">T.</param>
public static DVector3 Lerp(DVector3 a, DVector3 b, double t)
{
double s = M.Max(0, M.Min(t, 1));
return(a * (1 - s) + b * s);
}
/// <summary>
/// Distance the specified a and b.
/// </summary>
/// <returns>The distance.</returns>
/// <param name="a">The alpha component.</param>
/// <param name="b">The blue component.</param>
public static double Distance(DVector3 a, DVector3 b)
{
return(M.Sqrt(a.x * b.x + a.y * b.y + a.z * b.z));
}
/// <summary>
/// Dot the specified a and b.
/// </summary>
/// <returns>The dot.</returns>
/// <param name="a">The alpha component.</param>
/// <param name="b">The blue component.</param>
public static double Dot(DVector3 a, DVector3 b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z);
}
