1 ///*******************************************************************
2 //
3 // License:  See top level LICENSE.txt file.
4 //
5 // DESCRIPTION:
6 //   Contains implementation of class ossimEllipsoid. The implementation is
7 //   actually for an OBLATE SPHEROID (x.radius = y.radius) as Earth is
8 //   considered.
9 //
10 // SOFTWARE HISTORY:
11 //>
12 //   06Aug2001  Garrett Potts, Oscar Kramer
13 //              Initial coding.
14 //<
15 //*****************************************************************************
16 //  $Id: ossimEllipsoid.cpp 22858 2014-08-05 17:24:50Z dburken $
17 
18 #include <ossim/base/ossimEllipsoid.h>
19 #include <ossim/base/ossimDpt.h>
20 #include <ossim/base/ossimGpt.h>
21 #include <ossim/base/ossimEllipsoidFactory.h>
22 #include <ossim/base/ossimEcefRay.h>
23 #include <ossim/base/ossimEcefPoint.h>
24 #include <ossim/base/ossimEcefVector.h>
25 #include <ossim/base/ossimKeywordNames.h>
26 #include <ossim/base/ossimKeywordlist.h>
27 #include <ossim/base/ossimMatrix4x4.h>
28 
29 //***
30 // Define Trace flags for use within this file:
31 //***
32 #include <ossim/base/ossimTrace.h>
33 #include <cmath>
34 static ossimTrace traceExec  ("ossimEllipsoid:exec");
35 static ossimTrace traceDebug ("ossimEllipsoid:debug");
36 
37 
38 //*****************************************************************************
39 //  CONSTRUCTOR: ossimEllipsoid #1 (COPY)
40 //
41 //*****************************************************************************
ossimEllipsoid(const ossimEllipsoid & ellipsoid)42 ossimEllipsoid::ossimEllipsoid(const ossimEllipsoid &ellipsoid)
43    :
44       theName(ellipsoid.theName),
45       theCode(ellipsoid.theCode),
46       theEpsgCode(ellipsoid.theEpsgCode),
47       theA(ellipsoid.theA),
48       theB(ellipsoid.theB),
49       theFlattening(ellipsoid.theFlattening),
50       theA_squared(ellipsoid.theA_squared),
51       theB_squared(ellipsoid.theB_squared),
52       theEccentricitySquared(ellipsoid.theEccentricitySquared)
53 {
54    if ( theEpsgCode == 0 )
55    {
56       theEpsgCode = ossimEllipsoidFactory::instance()->findEpsgCode(theCode);
57    }
58 }
59 
60 //*****************************************************************************
61 //  CONSTRUCTOR: ossimEllipsoid #2
62 //
63 //*****************************************************************************
ossimEllipsoid(const ossimString & name,const ossimString & code,const double & a,const double & b,ossim_uint32 epsg_code)64 ossimEllipsoid::ossimEllipsoid(const ossimString &name,
65                                const ossimString &code,
66                                const double &a,
67                                const double &b,
68                                ossim_uint32 epsg_code)
69    :
70       theName(name),
71       theCode(code),
72       theEpsgCode(epsg_code),
73       theA(a),
74       theB(b),
75       theA_squared(a*a),
76       theB_squared(b*b)
77 {
78    if (theEpsgCode == 0)
79    {
80       theEpsgCode = ossimEllipsoidFactory::instance()->findEpsgCode(theCode);
81    }
82 
83    computeFlattening();
84    theEccentricitySquared = 2*theFlattening - theFlattening*theFlattening;
85 }
86 
ossimEllipsoid()87 ossimEllipsoid::ossimEllipsoid()
88 {
89    const ossimEllipsoid* ellipse = ossimEllipsoidFactory::instance()->wgs84();
90 
91    *this = *ellipse;
92 }
93 
94 //*****************************************************************************
95 //  CONSTRUCTOR: ossimEllipsoid #3
96 //
97 //*****************************************************************************
ossimEllipsoid(const double & a,const double & b)98 ossimEllipsoid::ossimEllipsoid(const double &a,
99                                const double &b)
100    :
101       theName(""), // initialize to empty
102       theCode(""),
103       theEpsgCode(0),
104       theA(a),
105       theB(b),
106       theA_squared(a*a),
107       theB_squared(b*b)
108 {
109    // First check if this is just WGS84:
110    const ossimEllipsoid* wgs84 = ossimEllipsoidFactory::instance()->wgs84();
111    if ((theA == wgs84->theA) && (theB == wgs84->theB))
112    {
113       *this = *wgs84;
114    }
115    else
116    {
117       computeFlattening();
118       theEccentricitySquared = 2*theFlattening - theFlattening*theFlattening;
119    }
120 }
121 
122 //*****************************************************************************
123 //  METHOD: ossimEllipsoid::nearestIntersection
124 //
125 //*****************************************************************************
nearestIntersection(const ossimEcefRay & ray,ossimEcefPoint & rtnPt) const126 bool ossimEllipsoid::nearestIntersection(const ossimEcefRay &ray,
127                                          ossimEcefPoint& rtnPt) const
128 {
129    return nearestIntersection(ray, 0.0, rtnPt);
130 }
131 
132 //*****************************************************************************
133 //  METHOD: ossimEllipsoid::nearestIntersection
134 //
135 //   geographic objects that are derive this class will assume that
136 //   the reference datum is wgs84 and that the ray origin is a
137 //   geocentric coordinate relative to the wgs84 datum.  Will return
138 //   true if the object was intersected and false otherwise.
139 //
140 //   The nearest intersection will use the ray sphere intersection
141 //   found in most ray tracers.  We will take a Ray defined by the
142 //   parametric equation:
143 //
144 //     x = x0 + dxt
145 //     y = y0 + dyt
146 //     z = z0 + dzt
147 //
148 //   and intersect this with the equation of a spheroid:
149 //
150 //     x^2/theXRadius^2 + y^2/theYRadius^2 + z^2/theZRadius^2 = 1
151 //
152 //   the intersection is achieved by substituting the parametric line
153 //   into the equation of the spheroid.  By doing this you should
154 //   get a quadratic in t and the equation should look like this:
155 //
156 //    a*t^2 + b*t + c = 0
157 //
158 //      let a = dx^2/theXRadius^2 + dy^2/theYRadius^2 + dz^2/theZRadius^2
159 //      let b = 2*(x0*dx/theXRadius^2 +y0*dy/theYRadius^2 + z0*dz/theZRadius^2
160 //      let c = x0^2/theXRadius^2 + y0^2/theYRadius^2 + z0^2/theZRadius^2 - 1
161 //
162 //    Now solve the quadratic (-b +- sqrt(b^2 - 4ac) ) / 2a
163 //
164 //    After solving for t, the parameter is applied to the ray to determine
165 //    the 3D point position in X,Y,Z, passed back in rtnPt. The boolean
166 //    "true" is returned if an intersection was found.
167 //
168 //    ESH 1/14/2016: I noticed that the radii used for the spheroid are dependent
169 //    on latitude and this was not taken into account, leading to small errors in
170 //    the results. The current point on the ray is now used to estimate latitude and
171 //    update the radii. Iterations stop when the intersection point has an elevation
172 //    value close to the ellipsoid 'offset'. This approach appears to converge very
173 //    fast (<=3 iterations). Before this fix, it was noticed that the rtnPt can be
174 //    off as the absolute value of 'offset' gets large. For example, at 10000 meters,
175 //    rtnPt can be off by almost 2 centimeters.
176 //
177 //*****************************************************************************
nearestIntersection(const ossimEcefRay & ray,const double & offset,ossimEcefPoint & rtnPt) const178 bool ossimEllipsoid::nearestIntersection( const ossimEcefRay& ray,
179                                           const double&       offset,
180                                           ossimEcefPoint&     rtnPt ) const
181 {
182    static const char MODULE[] = "ossimEllipsoid::nearestIntersection";
183    if (traceExec())
184    {
185       ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG: " << MODULE << ", entering...\n";
186    }
187 
188    bool success = false;
189 
190    const double eccSqComp = 1.0 - theEccentricitySquared;
191    const double CONVERGENCE_THRESHOLD = 0.0001;
192    const int    MAX_NUM_ITERATIONS    = 10;
193 
194    //***
195    // get the origin and direction of ray:
196    //***
197    ossimEcefPoint  start = ray.origin();
198    ossimEcefVector direction = ray.direction();
199 
200    double start_x = start.x();
201    double start_y = start.y();
202    double start_z = start.z();
203 
204    double direction_x = direction.x();
205    double direction_y = direction.y();
206    double direction_z = direction.z();
207 
208    ossimGpt rayGpt( start );
209 
210    int iterations = 0;
211    bool done = false;
212    do
213    {
214       double rayLat    = rayGpt.latd();
215       double raySinLat = ossim::sind( rayLat );
216       double rayScale  = 1.0 / sqrt( 1.0 - theEccentricitySquared * raySinLat * raySinLat );
217       double rayN      = theA * rayScale;
218 
219       double A_offset = rayN + offset;
220       double B_offset = rayN * eccSqComp + offset;
221 
222       double A_offset_inv = 1.0 / A_offset;
223       double B_offset_inv = 1.0 / B_offset;
224 
225       double scaled_x = start_x * A_offset_inv;
226       double scaled_y = start_y * A_offset_inv;
227       double scaled_z = start_z * B_offset_inv;
228 
229       double A_over_B = A_offset * B_offset_inv;
230 
231       //***
232       // Solve the coefficients of the quadratic formula
233       //***
234 
235       double a = (direction_x*direction_x + direction_y*direction_y) * A_offset_inv;
236       a += (direction_z*direction_z * B_offset_inv * A_over_B);
237 
238       double b = 2.0 * ( scaled_x*direction_x + scaled_y*direction_y +
239                          scaled_z*direction_z*A_over_B );
240 
241       double c = scaled_x*start_x + scaled_y*start_y;
242       c += (scaled_z*start_z * A_over_B);
243       c -= A_offset;
244 
245       //***
246       // solve the quadratic
247       //***
248       double root = b*b - 4.0*a*c;
249       if ( root < 0.0 )
250       {
251          return false;
252       }
253 
254       double squareRoot = sqrt(root);
255       double oneOver2a  = 1.0 / (2.0*a);
256 
257       double t1 = (-b + squareRoot) * oneOver2a;
258       double t2 = (-b - squareRoot) * oneOver2a;
259 
260       //***
261       // sort t1 and t2 and take the nearest intersection if they
262       // are in front of the ray.
263       //***
264 //      if ( t2 < t1 )
265 //      {
266 //         double temp = t1;
267 //         t1 = t2;
268 //         t2 = temp;
269 //      }
270 //
271 //      double tEstimate = ( t1 > 0.0 ) ? t1 : t2;
272 
273       // OLK: Alternate means to find closest intersection, not necessarily "in front of" origin:
274       double tEstimate = t1;
275       if (fabs(t2) < fabs(t1))
276          tEstimate = t2;
277 
278       // Get estimated intersection point.
279       ossimEcefPoint rayEcef = ray.extend( tEstimate );
280 
281       rayGpt = ossimGpt( rayEcef );
282       double offsetError = fabs( rayGpt.height() - offset );
283       if ( offsetError < CONVERGENCE_THRESHOLD )
284       {
285          done = true;
286          success = true;
287          rtnPt = rayEcef;
288       }
289 
290    }  while ( (!done) && (iterations++ < MAX_NUM_ITERATIONS) );
291 
292    if (iterations >= MAX_NUM_ITERATIONS)
293    {
294       if(traceDebug())
295       {
296          ossimNotify(ossimNotifyLevel_WARN)
297             << "WARNING ossimEllipsoid::nearestIntersection: Max number of iterations reached"
298             << " solving for intersection point. Result is probably inaccurate." << std::endl;
299       }
300    }
301 
302    return success;
303 }
304 
305 //*****************************************************************************
306 //  METHOD: ossimEllipsoid::evaluate(ossimColumnVector3d)
307 //
308 //  Returns neg number if inside, 0 if on, and pos number if outside of
309 //  ellipsoid.
310 //
311 //*****************************************************************************
evaluate(const ossimEcefPoint & location) const312 double ossimEllipsoid::evaluate(const ossimEcefPoint &location)const
313 {
314    //***
315    // get the axis
316    //***
317    return (((location.x() * location.x())/theA_squared) +
318            ((location.y() * location.y())/theA_squared) +
319            ((location.z() * location.z())/theB_squared) - 1.0);
320 }
321 
322 //*****************************************************************************
323 //  METHOD: ossimEllipsoid::gradient()  version A
324 //
325 //  Returns vector normal to the ellipsoid at point specified.
326 //
327 //*****************************************************************************
gradient(const ossimEcefPoint & location,ossimEcefVector & result) const328 void ossimEllipsoid::gradient(const ossimEcefPoint& location,
329                               ossimEcefVector&      result) const
330 {
331    result.x() = (2.0*location.x())/theA_squared;
332    result.y() = (2.0*location.y())/theA_squared;
333    result.z() = (2.0*location.z())/theB_squared;
334 }
335 
336 //*****************************************************************************
337 //  METHOD: ossimEllipsoid::gradient()  version B
338 //
339 //  Returns vector normal to the ellipsoid at point specified.
340 //
341 //*****************************************************************************
342 ossimEcefVector
gradient(const ossimEcefPoint & location) const343 ossimEllipsoid::gradient(const ossimEcefPoint &location)const
344 {
345    ossimEcefVector result;
346    gradient(location, result);
347    return result;
348 }
349 
350 
loadState(const ossimKeywordlist & kwl,const char * prefix)351 bool ossimEllipsoid::loadState(const ossimKeywordlist& kwl,
352                                const char* prefix)
353 {
354    const char* lookup = kwl.find(prefix, ossimKeywordNames::ELLIPSE_CODE_KW);
355    bool foundCode = false;
356    if(lookup)
357    {
358       const ossimEllipsoid* ellipse = ossimEllipsoidFactory::instance()->create(ossimString(lookup));
359 
360       if(ellipse)
361       {
362          foundCode = true;
363          *this = *ellipse;
364       }
365    }
366 
367    lookup = kwl.find(prefix, ossimKeywordNames::ELLIPSE_EPSG_CODE_KW);
368    if (lookup)
369    {
370       theEpsgCode = ossimString(lookup).toUInt32();
371    }
372 
373    if(!foundCode)
374    {
375       const char* majorAxis = kwl.find(prefix,
376                                        ossimKeywordNames::MAJOR_AXIS_KW);
377       const char* minorAxis = kwl.find(prefix,
378                                        ossimKeywordNames::MAJOR_AXIS_KW);
379 
380       theName = "";
381       theCode = "";
382       if(majorAxis && minorAxis)
383       {
384          theA = ossimString(majorAxis).toDouble();
385          theB = ossimString(minorAxis).toDouble();
386 
387          computeFlattening();
388          theA_squared = theA*theA;
389          theB_squared = theB*theB;
390       }
391       else
392       {
393          const ossimEllipsoid* ellipse = ossimEllipsoidFactory::instance()->wgs84();
394 
395          *this = *ellipse;
396       }
397    }
398 
399    return true;
400 }
401 
saveState(ossimKeywordlist & kwl,const char * prefix) const402 bool ossimEllipsoid::saveState(ossimKeywordlist& kwl,
403                                const char* prefix)const
404 {
405    if(theCode != "")
406    {
407       kwl.add(prefix,
408               ossimKeywordNames::ELLIPSE_CODE_KW,
409               theCode.c_str(),
410               true);
411 
412       kwl.add(prefix,
413               ossimKeywordNames::ELLIPSE_NAME_KW,
414               theName.c_str(),
415               true);
416    }
417    if (theEpsgCode)
418    {
419       kwl.add(prefix, ossimKeywordNames::ELLIPSE_EPSG_CODE_KW, theEpsgCode, true);
420    }
421 
422    kwl.add(prefix,
423            ossimKeywordNames::MAJOR_AXIS_KW,
424            theA,
425            true);
426 
427    kwl.add(prefix,
428            ossimKeywordNames::MINOR_AXIS_KW,
429            theB,
430            true);
431 
432    return true;
433 }
434 
435 
436 //*****************************************************************************
437 //  METHOD: ossimEllipsoid::prinRadiiOfCurv()
438 //
439 //  Computes the meridional radius and prime vertical at given point.
440 //
441 //*****************************************************************************
prinRadiiOfCurv(const ossimEcefPoint & location,double & merRadius,double & primeVert) const442 void ossimEllipsoid::prinRadiiOfCurv(const ossimEcefPoint& location,
443                                            double& merRadius,
444                                            double& primeVert) const
445 {
446    double lat, lon, hgt;
447    XYZToLatLonHeight(location.x(), location.y(), location.z(), lat, lon, hgt);
448 
449    double sinPhi = sin(lat*RAD_PER_DEG);
450    double phiFac = 1.0 - theEccentricitySquared*sinPhi*sinPhi;
451    primeVert = theA / sqrt(phiFac);
452    merRadius = theA*(1.0-theEccentricitySquared) / sqrt(phiFac*phiFac*phiFac);
453 }
454 
455 
456 //*****************************************************************************
457 //  METHOD: ossimEllipsoid::jacobianWrtEcef()
458 //
459 //  Forms Jacobian of partials of geodetic WRT ECF at given point.
460 //           -                           -
461 //           | pLat/pX  pLat/pY  pLat/pZ |
462 //    jMat = | pLon/pX  pLon/pY  pLon/pZ |
463 //           | pHgt/pX  pHgt/pY  pHgt/pZ |
464 //           -                           -
465 //
466 //*****************************************************************************
jacobianWrtEcef(const ossimEcefPoint & location,NEWMAT::Matrix & jMat) const467 void ossimEllipsoid::jacobianWrtEcef(const ossimEcefPoint& location,
468                                            NEWMAT::Matrix& jMat) const
469 {
470    double primeVert;
471    double merRadius;
472    double lat, lon, hgt;
473 
474    XYZToLatLonHeight(location.x(), location.y(), location.z(), lat, lon, hgt);
475    prinRadiiOfCurv(location, merRadius, primeVert);
476 
477    double sinPhi = sin(lat*RAD_PER_DEG);
478    double cosPhi = cos(lat*RAD_PER_DEG);
479    double sinLam = sin(lon*RAD_PER_DEG);
480    double cosLam = cos(lon*RAD_PER_DEG);
481    double N_plus_h = primeVert + hgt;
482    double M_plus_h = merRadius + hgt;
483 
484    jMat(1,1) = -sinPhi * cosLam / M_plus_h;
485    jMat(2,1) = -sinLam / (cosPhi * N_plus_h);
486    jMat(3,1) = cosPhi * cosLam;
487    jMat(1,2) = -sinPhi * sinLam / M_plus_h;
488    jMat(2,2) =  cosLam / (cosPhi * N_plus_h);
489    jMat(3,2) = cosPhi * sinLam;
490    jMat(1,3) = cosPhi / M_plus_h;
491    jMat(2,3) = 0.0;
492    jMat(3,3) = sinPhi;
493 }
494 
495 
496 //*****************************************************************************
497 //  METHOD: ossimEllipsoid::jacobianWrtGeo()
498 //
499 //  Forms Jacobian of partials of ECF WRT geodetic at given point.
500 //           -                           -
501 //           | pX/pLat  pX/pLon  pX/pHgt |
502 //    jMat = | pY/pLat  pY/pLon  pY/pHgt |
503 //           | pZ/pLat  pZ/pLon  pZ/pHgt |
504 //           -                           -
505 //
506 //*****************************************************************************
jacobianWrtGeo(const ossimEcefPoint & location,NEWMAT::Matrix & jMat) const507 void ossimEllipsoid::jacobianWrtGeo(const ossimEcefPoint& location,
508                                           NEWMAT::Matrix& jMat) const
509 {
510    double primeVert;
511    double merRadius;
512    double lat, lon, hgt;
513 
514    XYZToLatLonHeight(location.x(), location.y(), location.z(), lat, lon, hgt);
515    prinRadiiOfCurv(location, merRadius, primeVert);
516 
517    double sinPhi = sin(lat*RAD_PER_DEG);
518    double cosPhi = cos(lat*RAD_PER_DEG);
519    double sinLam = sin(lon*RAD_PER_DEG);
520    double cosLam = cos(lon*RAD_PER_DEG);
521    double N_plus_h = primeVert + hgt;
522    double M_plus_h = merRadius + hgt;
523 
524    jMat(1,1) = -M_plus_h * sinPhi * cosLam;
525    jMat(2,1) = -M_plus_h * sinPhi * sinLam;
526    jMat(3,1) =  M_plus_h * cosPhi;
527    jMat(1,2) = -N_plus_h * cosPhi * sinLam;
528    jMat(2,2) =  N_plus_h * cosPhi * cosLam;
529    jMat(3,2) = 0.0;
530    jMat(1,3) = cosPhi * cosLam;
531    jMat(2,3) = cosPhi * sinLam;
532    jMat(3,3) = sinPhi;
533 }
534 
535 
536 //*****************************************************************************
537 //  METHOD: ossimEllipsoid::geodeticRadius()
538 //
539 //  Computes the "geodetic" radius for a given latitude in degrees
540 //
541 //*****************************************************************************
geodeticRadius(const double & lat) const542 double ossimEllipsoid::geodeticRadius(const double& lat) const
543 {
544    double cos_lat = ossim::cosd(lat);
545    double sin_lat = ossim::sind(lat);
546    double cos2_lat = cos_lat*cos_lat;
547    double sin2_lat = sin_lat*sin_lat;
548    double a2_cos = theA_squared*cos_lat;
549    double b2_sin = theB_squared*sin_lat;
550 
551    return sqrt( ( (a2_cos*a2_cos) + (b2_sin*b2_sin) )/ (theA_squared*cos2_lat + theB_squared*sin2_lat));
552 }
553 
554 //*************************************************************************************************
555 //  Computes the "geodetic" radius of curvature of the ellipsoid in the east-west (x) and
556 //  north-south (y) directions for a given latitude in DEGREES.
557 //  Taken from http://en.wikipedia.org/wiki/Earth_radius
558 //*************************************************************************************************
geodeticRadii(const double & lat,ossimDpt & radii) const559 void ossimEllipsoid::geodeticRadii(const double& lat, ossimDpt& radii) const
560 {
561    double cos_lat = ossim::cosd(lat);
562    double sin_lat = ossim::sind(lat);
563    double cos2_lat = cos_lat*cos_lat;
564    double sin2_lat = sin_lat*sin_lat;
565    double H = theA_squared*cos2_lat + theB_squared*sin2_lat;
566    double H3 = H*H*H;
567 
568    radii.x = theA_squared/sqrt(H);
569    radii.y = theA_squared*theB_squared/sqrt(H3);
570 }
571 
latLonHeightToXYZ(double lat,double lon,double height,double & x,double & y,double & z) const572 void ossimEllipsoid::latLonHeightToXYZ(double lat, double lon, double height,
573                                        double &x, double &y, double &z)const
574 {
575     double sin_latitude = ossim::sind(lat);
576     double cos_latitude = ossim::cosd(lat);
577     double N = theA / sqrt( 1.0 - theEccentricitySquared*sin_latitude*sin_latitude);
578     x = (N+height)*cos_latitude*ossim::cosd(lon);
579     y = (N+height)*cos_latitude*ossim::sind(lon);
580     z = (N*(1-theEccentricitySquared)+height)*sin_latitude;
581 }
582 
XYZToLatLonHeight(double x,double y,double z,double & lat,double & lon,double & height) const583 void ossimEllipsoid::XYZToLatLonHeight(double x, double y, double z,
584                                        double& lat, double& lon, double& height)const
585 {
586 
587 #if 1
588   // Author: Norman J. Goldstein (ngoldstein@SystemSolutionsRD.com,
589 //                              normvcr@telus.net)
590 
591   const double tol = 1e-15;
592   const double d = sqrt(x*x + y*y);
593   const int MAX_ITER = 10;
594 
595   const double a2 = theA * theA;
596   const double b2 = theB * theB;
597   const double pa2 = d * d * a2;
598   const double qb2 = z * z * b2;
599   const double ae2 = a2 * eccentricitySquared();
600   const double ae4 = ae2 * ae2;
601 
602   const double c3 = -( ae4/2 + pa2 + qb2 );          // s^2
603   const double c4 = ae2*( pa2 - qb2 );               // s^1
604   const double c5 = ae4/4 * ( ae4/4 - pa2 - qb2 );   // s^0
605 
606   double s0 = 0.5 * (a2 + b2) * hypot( d/theA, z/theB );
607 
608   for( int iterIdx = 0; iterIdx < MAX_ITER; ++iterIdx )
609   {
610     const double pol = c5 + s0 * ( c4 + s0 * ( c3 + s0 * s0 ) );
611     const double der = c4 + s0 * ( 2 * c3  + 4 * s0 * s0 );
612 
613     const double ds = - pol / der;
614     s0 += ds;
615 
616     if( fabs( ds ) < tol * s0 )
617     {
618       const double t = s0 - 0.5 * (a2 + b2);
619       const double x_ell = d / ( 1.0 + t/a2 );
620       const double y_ell = z / ( 1.0 + t/b2 );
621 
622       height = ( d - x_ell ) * x_ell/a2 + ( z - y_ell ) * y_ell/b2;
623       height /= hypot( x_ell/a2 ,  y_ell/b2 );
624 
625       lat = atan2( y_ell/b2, x_ell/a2 ) * DEG_PER_RAD;
626       lon = atan2( y, x ) * DEG_PER_RAD;
627 
628       return;
629     }
630   }
631 
632   #else
633    double d = sqrt(x*x + y*y);
634 
635    double phi2 = z / ((1 - theEccentricitySquared) * d);
636    double p = 1.0;
637    double phi1 = 0.0;
638    double N1 = 0.0;
639    double height1 = 0.0;
640    int iterIdx = 0;
641    const int MAX_ITER = 10;
642    if (fabs(phi2) > 1e-16 )
643    {
644       while ( (p > 1e-17) && (iterIdx < MAX_ITER))
645       {
646          phi1 = phi2;
647          N1 = theA / sqrt(1.0 - (theEccentricitySquared * pow(sin(phi1), 2.0)));
648          height1 = (d / cos(phi1) - N1);
649          phi2 = atan((z / d) * (1.0 + (theEccentricitySquared * N1 * sin(phi1)) / z));
650          p = fabs(phi2 - phi1);
651          ++iterIdx;
652          /* printf("phi: %e   phi2: %e   p: %e  \n", phi1, phi2, p); */
653       }
654    }
655    else
656    {
657       phi1 = phi2;
658       N1 = theA / sqrt(1.0 - (theEccentricitySquared * pow(sin(phi1), 2.0)));
659       height1 = (d / cos(phi1)) - N1;
660    }
661 
662    /* *Latitude = phi2 * 180/PI; */
663    /* *Longitude = atan2(Y, X) * 180/PI; */
664    lat = phi2*DEG_PER_RAD;
665    lon = atan2(y, x)*DEG_PER_RAD;
666    height = height1;
667 #endif
668 }
669 
computeLocalToWorldTransformFromXYZ(double x,double y,double z,ossimMatrix4x4 & localToWorld) const670 void ossimEllipsoid::computeLocalToWorldTransformFromXYZ(double x, double y, double z,
671                                                          ossimMatrix4x4& localToWorld)const
672 {
673    localToWorld = ossimMatrix4x4::createIdentity();
674    NEWMAT::Matrix& m = localToWorld.getData();
675 
676    // put in the translation
677    m[0][3] = x;
678    m[1][3] = y;
679    m[2][3] = z;
680 
681 
682 
683     // normalize X,Y,Z
684     double inverse_length = 1.0/sqrt(x*x + y*y + z*z);
685 
686     x *= inverse_length;
687     y *= inverse_length;
688     z *= inverse_length;
689 
690     double length_XY = sqrt(x*x + y*y);
691     double inverse_length_XY = 1.0/length_XY;
692 
693     // Vx = |(-Y,X,0)|
694     m[0][0] = -y*inverse_length_XY;
695     m[1][0] = x*inverse_length_XY;
696     m[2][0] = 0.0;
697 
698     // Vy = /(-Z*X/(sqrt(X*X+Y*Y), -Z*Y/(sqrt(X*X+Y*Y),sqrt(X*X+Y*Y))|
699     double Vy_x = -z*x*inverse_length_XY;
700     double Vy_y = -z*y*inverse_length_XY;
701     double Vy_z = length_XY;
702     inverse_length = 1.0/sqrt(Vy_x*Vy_x + Vy_y*Vy_y + Vy_z*Vy_z);
703     m[0][1] = Vy_x*inverse_length;
704     m[1][1] = Vy_y*inverse_length;
705     m[2][1] = Vy_z*inverse_length;
706 
707     // Vz = (X,Y,Z)
708     m[0][2] = x;
709     m[1][2] = y;
710     m[2][2] = z;
711 
712 }
713 
getEpsgCode() const714 ossim_uint32 ossimEllipsoid::getEpsgCode() const
715 {
716    if (!theCode.empty() && (theEpsgCode == 0))
717       theEpsgCode = ossimEllipsoidFactory::instance()->findEpsgCode(theCode);
718    return theEpsgCode;
719 }
720 
operator =(const ossimEllipsoid & copy_me)721 const ossimEllipsoid& ossimEllipsoid::operator=(const ossimEllipsoid& copy_me)
722 {
723    if (this != &copy_me)
724    {
725       theName = copy_me.theName;
726       theCode = copy_me.theCode;
727       theEpsgCode = copy_me.theEpsgCode;
728       theA = copy_me.theA;
729       theB = copy_me.theB;
730       theFlattening = copy_me.theFlattening;
731       theA_squared = copy_me.theA_squared;
732       theB_squared = copy_me.theB_squared;
733       theEccentricitySquared = copy_me.theEccentricitySquared;
734    }
735    return *this;
736 }
737