//***************************************************************************** // METHOD: intersectRay() // // Service method for intersecting a ray with the elevation surface to // arrive at a ground point. The ray is expected to originate ABOVE the // surface and pointing down. // // NOTE: the gpt argument is expected to be initialized with the desired // datum, including ellipsoid, for the proper intersection point to be // computed. // // LIMITATION: This release supports only single valued solutions, i.e., it // is possible a ray passing through one side of a mountain and out the other // will return an intersection with the far side. Eventually, a more robust // algorithm will be employed. // //***************************************************************************** bool ossimElevSource::intersectRay(const ossimEcefRay& ray, ossimGpt& gpt, double defaultElevValue) { if (traceExec()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimElevSource::intersectRay: entering..." << std::endl; static const double CONVERGENCE_THRESHOLD = 0.001; // meters static const int MAX_NUM_ITERATIONS = 50; double h_ellips; // height above ellipsoid bool intersected; ossimEcefPoint prev_intersect_pt (ray.origin()); ossimEcefPoint new_intersect_pt; double distance; bool done = false; int iteration_count = 0; if(ray.hasNans()) { gpt.makeNan(); return false; } //*** // Set the initial guess for horizontal intersect position as the ray's // origin, and establish the datum and ellipsoid: //*** const ossimDatum* datum = gpt.datum(); const ossimEllipsoid* ellipsoid = datum->ellipsoid(); // double lat, lon, h; // ellipsoid->XYZToLatLonHeight(ray.origin().x(), // ray.origin().y(), // ray.origin().z(), // lat, lon, h); // ossimGpt nadirGpt(lat, lon, h); // std::cout << "nadir pt = " << nadirGpt << std::endl; gpt = ossimGpt(prev_intersect_pt, datum); // // Loop to iterate on ray intersection with variable elevation surface: // do { // // Intersect ray with ellipsoid inflated by h_ellips: // h_ellips = getHeightAboveEllipsoid(gpt); if ( ossim::isnan(h_ellips) ) h_ellips = defaultElevValue; intersected = ellipsoid->nearestIntersection(ray, h_ellips, new_intersect_pt); if (!intersected) { // // No intersection (looking over the horizon), so set ground point // to NaNs: // gpt.makeNan(); done = true; } else { // // Assign the ground point to the latest iteration's intersection // point: // gpt = ossimGpt(new_intersect_pt, datum); // // Determine if convergence achieved: // distance = (new_intersect_pt - prev_intersect_pt).magnitude(); if (distance < CONVERGENCE_THRESHOLD) done = true; else { prev_intersect_pt = new_intersect_pt; } } iteration_count++; } while ((!done) && (iteration_count < MAX_NUM_ITERATIONS)); if (iteration_count == MAX_NUM_ITERATIONS) { if(traceDebug()) { ossimNotify(ossimNotifyLevel_WARN) << "WARNING ossimElevSource::intersectRay: Max number of iterations reached solving for ground " << "point. Result is probably inaccurate." << std::endl; } } if (traceExec()) ossimNotify(ossimNotifyLevel_DEBUG) << "DEBUG ossimElevSource::intersectRay: returning..." << std::endl; return intersected; }