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