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 != ©_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