public static Geo3dPoint XYZToLatLon( in Gcgf3dPoint gcgf3dPoint ) { Contract.Requires( gcgf3dPoint != null && gcgf3dPoint.isValid, "The input GCGF point is not valid! ({0})", gcgf3dPoint ); Geo3dPoint geoPoint = null; // From "Transformation from Cartesian to Geodetic Coordinates // Accelerated by Halley’s Method" --Fukushima 2006 double x = gcgf3dPoint.point.x; double y = gcgf3dPoint.point.y; double z = gcgf3dPoint.point.z; Ellipsoid ellipsoid = gcgf3dPoint.datum.ellipsoid; double a = ellipsoid.semiMajorAxis; double f = 1.0 / ellipsoid.inverseFlattening; double e2 = 2.0*f - (f*f); double ec = System.Math.Sqrt( 1.0 - e2 ); double p = System.Math.Sqrt( x*x + y*y ); double b = a*ec; // output Geo3dPoint parameters double phi = double.NaN; double lambda = double.NaN; double h = double.NaN; //------------------------------------------------------ // lambda //------------------------------------------------------ lambda = 2.0 * System.Math.Atan2( y, (x + System.Math.Sqrt(x*x + y*y)) ); Contract.Ensures( !MathUtils.DoubleIsNanOrInfinity(lambda), "Cannot determine longitude for GCGF point! ({0})", gcgf3dPoint ); // we can short-circuit this method when the coordinate is at // the origin or lies on only one axis (X, Y, or Z) if( MathUtils.AlmostZero(x) && MathUtils.AlmostZero(y) && MathUtils.AlmostZero(z) ) { phi = 0.0; lambda = 0.0; h = -a; } else if( MathUtils.AlmostZero(x) && MathUtils.AlmostZero(y) ) { phi = System.Math.Sign(z) * CoreDefs.HALF_PI; // technically, this situation is undefined for lambda but Z-only // coords are possible (eg, north and south poles) so we should // allow it; arbitrarily set it to zero lambda = 0.0; h = System.Math.Abs(z) - b; } else if( MathUtils.AlmostZero(x) && MathUtils.AlmostZero(z) ) { phi = 0.0; // lambda calculated above h = System.Math.Abs(y) - a; } else if( MathUtils.AlmostZero(y) && MathUtils.AlmostZero(z) ) { phi = 0.0; // lambda calculated above h = System.Math.Abs(x) - a; } else // we need to do the full computation { //------------------------------------------------------ // phi //------------------------------------------------------ int MAX_ITERATIONS = 2; double TARGET_ACCURACY_S = 1E-7; double TARGET_ACCURACY_C = 1E-7; // double P = (1.0/a)*p; double Z = ((1.0/a)*ec) * System.Math.Abs(z); double E = e2; // // iteration variables double A = double.NaN; double A3 = double.NaN; double B = double.NaN; double D = double.NaN; double F = double.NaN; double S1 = double.NaN; double C1 = double.NaN; double dS = double.NaN; double dC = double.NaN; // // iteration starting estimates double S = Z; double C = ec * P; // for( int i=0; i < MAX_ITERATIONS; i++ ) { A = System.Math.Sqrt( S*S + C*C ); // as per equation (18) in the paper, use a simplified correction // factor for Halley's method for the first iteration if( i == 0 ) { B = 1.5*(E*E) * P*(S*S)*(C*C) * (A - ec); } else // use the complete correction factor { B = 1.5*E*S*(C*C) * ((P*S - Z*C)*A - E*S*C); } A3 = System.Math.Pow(A,3.0); D = Z*A3 + E*System.Math.Pow(S,3.0); F = P*A3 - E*System.Math.Pow(C,3.0); S1 = D*F - B*S; C1 = (F*F) - B*C; // From http://docs.ros.org/fuerte/api/enu/html/coord__system_8c_source.html // // The original algorithm as presented in the paper by Fukushima has a // problem with numerical stability. S and C can grow very large or small // and over or underflow a double. In the paper this is acknowledged and // the proposed resolution is to non-dimensionalise the equations for S and // C. However, this does not completely solve the problem. The author caps // the solution to only a couple of iterations and in this period over or // underflow is unlikely but as we require a bit more precision and hence // more iterations so this is still a concern for us. // // As the only thing that is important is the ratio T = S/C, my solution is // to divide both S and C by either S or C. The scaling is chosen such that // one of S or C is scaled to unity whilst the other is scaled to a value // less than one. By dividing by the larger of S or C we ensure that we do // not divide by zero as only one of S or C should ever be zero. // // This incurs an extra division each iteration which the author was // explicitly trying to avoid and it may be that this solution is just // reverting back to the method of iterating on T directly, perhaps this // bears more thought? // if( S1 > C1 ) { C1 = C1 / S1; S1 = 1.0; } else { S1 = S1 / C1; C1 = 1.0; } dS = System.Math.Abs( S1 - S ); dC = System.Math.Abs( C1 - C ); S = S1; C = C1; if( dS < TARGET_ACCURACY_S && dC < TARGET_ACCURACY_C ) { break; } if( i == (MAX_ITERATIONS - 1) ) { BT_Logger.Warn( "XYZToLatLon() did not converge to a target " + "accuracy of [TAS = {0}, TAC = {1}] " + "after the max of {2} iterations " + "(dS = {3}, dC = {4})", TARGET_ACCURACY_S, TARGET_ACCURACY_C, MAX_ITERATIONS, dS, dC ); } } // for each iteration A = System.Math.Sqrt( S*S + C*C ); double signz = (double) System.Math.Sign(z); double Cc = ec*C; phi = signz * System.Math.Atan2( S, Cc ); Contract.Ensures( !MathUtils.DoubleIsNanOrInfinity(phi), "Cannot determine latitude for GCGF point! ({0})", gcgf3dPoint ); //------------------------------------------------------ // height //------------------------------------------------------ h = (p*Cc + System.Math.Abs(z)*S - b*A) / // --------------------------------------- System.Math.Sqrt( Cc*Cc + S*S ); Contract.Ensures( !MathUtils.DoubleIsNanOrInfinity(h), "Cannot determine height for GCGF point! ({0})", gcgf3dPoint ); } geoPoint = new Geo3dPoint( phi, lambda, h, gcgf3dPoint.datum, gcgf3dPoint.datum, gcgf3dPoint.name ); return geoPoint; }