GreatCircles.cpp

Go to the documentation of this file.
00001 // -*- c-basic-offset: 4 -*-
00025 #ifdef __WXMAC__
00026 #include "panoinc_WX.h"
00027 #include "panoinc.h"
00028 #endif
00029 
00030 #include "GreatCircles.h"
00031 
00032 // for some reason #include <GL/gl.h> isn't portable enough.
00033 #include <wx/platform.h>
00034 #ifdef __WXMAC__
00035 #include <OpenGL/gl.h>
00036 #else
00037 #ifdef __WXMSW__
00038 #include <vigra/windows.h>
00039 #endif
00040 #include <GL/gl.h>
00041 #endif
00042 
00043 // we use the trigonometric functions.
00044 #include <cmath>
00045 #include <limits>
00046 
00047 // The number of segments to use in subdivision of the line.
00048 // Higher numbers increase the accuracy of the line, but it is slower.
00049 // Must be at least two. More is much better.
00050 const unsigned int segments = 48;
00051 
00052 void GreatCircles::setVisualizationState(VisualizationState * visualizationStateIn)
00053 {
00054     m_visualizationState = visualizationStateIn;
00055 }
00056 
00057 void GreatCircles::drawLineFromSpherical(double startLat, double startLong,
00058                                          double endLat, double endLong, double width)
00059 {
00060     DEBUG_ASSERT(m_visualizationState); 
00061     GreatCircleArc(startLat, startLong, endLat, endLong, *m_visualizationState).draw(true, width);
00062 }
00063 
00064 GreatCircleArc::GreatCircleArc()
00065 {
00066 }
00067 
00068 GreatCircleArc::GreatCircleArc(double startLat, double startLong,
00069                        double endLat, double endLong,
00070                        VisualizationState & visualizationState)
00071 
00072 {
00073     m_visualizationState = &visualizationState;
00074     // get the output projection
00075     const HuginBase::PanoramaOptions & options = *(visualizationState.GetOptions());
00076     // make an image to transform spherical coordinates into the output projection
00077     HuginBase::SrcPanoImage equirectangularImage;
00078     equirectangularImage.setProjection(HuginBase::SrcPanoImage::EQUIRECTANGULAR);
00079     equirectangularImage.setHFOV(360.0);
00080     equirectangularImage.setSize(vigra::Size2D(360.0, 180.0));
00081     
00082     // make a transformation from spherical coordinates to the output projection
00083     HuginBase::PTools::Transform transform;
00084     transform.createInvTransform(equirectangularImage, options);
00085     
00086         m_xscale = visualizationState.GetScale();
00087 
00093     if (startLat == 360.0 - endLat && startLong == 180.0 - endLong)
00094     {
00095         // we should probably check to see if we already go through (180, 90).
00096         if (startLat == 180.0 && startLong == 90.0)
00097         {
00098             // foiled again: pick one going through (180, 0) instead.
00099             *this = GreatCircleArc(startLat, startLong, 180.0, 0.0, visualizationState);
00100             GreatCircleArc other(180.0, 0.0, endLat, endLong, visualizationState);
00101             m_lines.insert(m_lines.end(), other.m_lines.begin(), other.m_lines.end());
00102             return;
00103         }
00104         *this = GreatCircleArc(startLat, startLong, 180.0, 90.0, visualizationState);
00105         GreatCircleArc other(180.0, 90.0, endLat, endLong, visualizationState);
00106         m_lines.insert(m_lines.end(), other.m_lines.begin(), other.m_lines.end());
00107         return;
00108     }
00109     
00110     // convert start and end positions so that they don't go across the +/-180
00111     // degree seam
00112     if (startLat < 90.0 && endLat > 270.0)
00113     {
00114         endLat -= 360.0;
00115     }
00116     // convert to radians
00117     startLat *= (M_PI / 180.0);
00118     startLong *= (M_PI / 180.0);
00119     endLat *= (M_PI / 180.0);
00120     endLong *= (M_PI / 180.0);
00121     
00122     // find sines and cosines, they are used multiple times.
00123     double sineStartLat = std::sin(startLat);
00124     double sineStartLong = std::sin(startLong);
00125     double sineEndLat = std::sin(endLat);
00126     double sineEndLong = std::sin(endLong);
00127     double cosineStartLat = std::cos(startLat);
00128     double cosineStartLong = std::cos(startLong);
00129     double cosineEndLat = std::cos(endLat);
00130     double cosineEndLong = std::cos(endLong);
00131     
00132     /* to get points on the great circle, we linearly interpolate between the
00133      * two 3D coordinates for the given spherical coordinates, then normalise
00134      * the vector to get back on the sphere. This works everywhere except exact
00135      * opposite points, where we'll get the original points repeated several
00136      * times (and if we are even more unlucky we will hit the origin where the
00137      * normal isn't defined.)
00138      */
00139     // convert locations to 3d coordinates.
00140     double p1[3] = {cosineStartLat * sineStartLong, sineStartLat * sineStartLong, cosineStartLong};
00141     double p2[3] = {cosineEndLat * sineEndLong, sineEndLat * sineEndLong, cosineEndLong};
00142     
00144     bool hasSeam = true;
00145     // draw a line strip and transform the coordinates as we go.
00146     double b1 = 0.0;
00147     double b2 = 1.0;
00148     const double bDifference = 1.0 / double(segments);
00149     // for discontinuity detection.
00150     int lastSegment = 1;
00151     // The last processed vertex's position.
00152     hugin_utils::FDiff2D last_vertex;
00153     /* true if we shouldn't use last_vertex to make a line segment
00154      * i.e. We just crossed a discontinuity. */
00155     bool skip = true;
00156     for (unsigned int segment_index = 0; segment_index < segments;
00157          segment_index++, b1 += bDifference, b2 -= bDifference)
00158     {
00159         // linearly interpolate positions
00160         double v[3] = {p1[0] * b1 + p2[0] * b2, p1[1] * b1 + p2[1] * b2, p1[2] * b1 + p2[2] * b2};
00161         // normalise
00162         double length = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
00163         v[0] /= length;
00164         v[1] /= length;
00165         v[2] /= length;
00166         /*double longitude = atan2(numerator,
00167                                  cosineStartLong * (c1 * std::sin(latitude) -
00168                                                     c2 * std::cos(latitude)));*/
00169         double longitude = std::acos(v[2]);
00170         // acos returns values between 0 and M_PI. The other
00171         // latitudes are on the back of the sphere, so check y coordinate (v1)
00172         double latitude = std::acos(v[0] / std::sin(longitude));
00173         if (v[1] < 0.0)
00174         {
00175             // on the back.
00176             latitude = -latitude + 2 * M_PI;
00177         }
00178         
00179         double vx, vy;
00180         bool infront =  transform.transformImgCoord(vx,
00181                                                     vy,
00182                                                     latitude  * 180.0 / M_PI,
00183                                                     longitude  * 180.0 / M_PI);
00184         // don't draw across +/- 180 degree seems.
00185         if (hasSeam)
00186         {
00187             // we divide the width of the panorama into 3 segments. If we jump
00188             // across the middle section, we split the line into two.
00189             int newSegment = vx / (options.getWidth() / 3);
00190             if ((newSegment < 1 && lastSegment > 1) ||
00191                 (newSegment > 1 && lastSegment < 1))
00192             {
00193                 skip = true;
00194             }
00195             lastSegment = newSegment;
00196         }
00197         if (infront)
00198         {
00199             if (!skip)
00200             {
00201                 LineSegment line;
00202                 line.vertices[0] = last_vertex;
00203                 line.vertices[1] = hugin_utils::FDiff2D(vx, vy);
00204                 m_lines.push_back(line);
00205             }
00206             // The next line segment should be a valid one.
00207             last_vertex = hugin_utils::FDiff2D(vx, vy);
00208             skip = false;
00209         } else {
00210             skip = true;
00211         }
00212     }
00213 }
00214                        
00215 void GreatCircleArc::draw(bool withCross, double width) const
00216 {
00217     // Just draw all the previously worked out line segments
00222         LineSegment * pre;
00223         LineSegment * pro;
00224         for (unsigned int i = 0 ; i < m_lines.size() ; i++) {
00225             if (i != 0) {
00226                 pre = (LineSegment*) &(m_lines[i-1]);
00227             } else {
00228                 pre = NULL;
00229             }
00230             if (i != m_lines.size() - 1) {
00231                 pro = (LineSegment*)&(m_lines[i+1]);
00232             } else {
00233                 pro = NULL;
00234             }
00235             m_lines[i].doGL(width, m_visualizationState,pre,pro);
00236         }
00237     glEnd();
00238     // The arc might contain no line segments in some circumstances,
00239     // so check for this before drawing crosses at the ends.
00240     if(withCross && !m_lines.empty())
00241     {
00242         double scale = 4 / getxscale();
00243         // The scale to draw them: this is 5 pixels outside in every direction.
00244         {
00245             std::vector<GreatCircleArc::LineSegment>::const_iterator it;
00246             it = m_lines.begin();
00247             it->doGLcross(0,scale, m_visualizationState);
00248 
00249             it = m_lines.end();
00250             it--;       //.end points beyond last point.
00251             it->doGLcross(1,scale, m_visualizationState);
00252         }
00253     };
00254 }
00255 
00256 void GreatCircleArc::LineSegment::doGL(double width, VisualizationState * state, GreatCircleArc::LineSegment * preceding, GreatCircleArc::LineSegment * proceeding) const
00257 {
00258     double m = (vertices[1].y - vertices[0].y) / (vertices[1].x - vertices[0].x);
00259     double b = vertices[1].y - m * vertices[1].x;
00260     hugin_utils::FDiff2D rect[4];
00261     double d = width / state->GetScale() / 2.0;
00262 //DEBUG_DEBUG("drawing lines " << d);
00263     double yd = d * sqrt(m*m + 1);
00264     double xd =  d * sin(atan(m));
00265 
00266     if (vertices[1].x < vertices[0].x) {
00267         yd *= -1;
00268     } else {
00269         xd *= -1;
00270     }
00271 
00272     //first find out if it is a special case
00273 
00274     bool vertical = false;
00275     if (abs(vertices[1].x - vertices[0].x) < 0.00001) {
00276         xd = d;
00277         if (vertices[1].y > vertices[0].y) {
00278             xd *= -1; 
00279         }
00280         yd = 0;
00281         vertical = true;
00282     }
00283 
00284     bool horizontal = false;
00285     if(abs(vertices[1].y - vertices[0].y) < 0.00001) {
00286         xd = 0;
00287         yd = d;
00288         if (vertices[1].x > vertices[0].x) {
00289             yd *= -1; 
00290         }
00291         m = 0;
00292         b = vertices[0].y;
00293         horizontal = true;
00294     }
00295 
00296     //TODO: line segment ends of special cases correspondend to preceding and proceeding segments
00297     if (vertical || horizontal) {
00298         rect[0].x = vertices[0].x - xd;
00299         rect[0].y = vertices[0].y + yd;
00300         rect[1].x = vertices[0].x + xd;
00301         rect[1].y = vertices[0].y - yd;
00302         rect[2].x = vertices[1].x + xd;
00303         rect[2].y = vertices[1].y - yd;
00304         rect[3].x = vertices[1].x - xd;
00305         rect[3].y = vertices[1].y + yd;
00306     } else {
00307 
00308         //The mathematics behind this is to get the equation for the line of the segment in the form of y = m * x + b
00309         //then shift the line + - in the y direction to get two lines, and finally do this for the preceding and proceeding line segments and find the intersection
00310         //or just make the ends perpendicular
00311         bool def = false;
00312         if (preceding != NULL) {
00313 
00314             double m_pre = (preceding->vertices[1].y - preceding->vertices[0].y) / (preceding->vertices[1].x - preceding->vertices[0].x);
00315             double b_pre = preceding->vertices[1].y - m_pre * preceding->vertices[1].x;
00316 
00317             if (m_pre == m) {
00318                 def = true;
00319             } else {
00320 
00321                 double yd_pre = d * sqrt(m_pre*m_pre + 1);
00322                 if (preceding->vertices[1].x < preceding->vertices[0].x) {
00323                     yd_pre *= -1;
00324                 }
00325 
00326 
00327                 rect[1].x = ((b_pre + yd_pre) - (b + yd)) / (m - m_pre);
00328                 rect[1].y = m_pre * rect[1].x + b_pre + yd_pre;
00329 
00330                 rect[0].x = ((b_pre - yd_pre) - (b - yd)) / (m - m_pre);
00331                 rect[0].y = m_pre * rect[0].x + b_pre - yd_pre;
00332             }
00333             
00334         } else {
00335             def = true;
00336         }
00337 
00338         if (def) {
00339 
00340             //in this case return just a proper rectangle
00341             rect[1].x = vertices[0].x     + xd;
00342             rect[1].y = m * rect[1].x + b + yd;
00343 
00344             rect[0].x = vertices[0].x     - xd;
00345             rect[0].y = m * rect[0].x + b - yd;
00346 
00347         }
00348 
00349 //        DEBUG_DEBUG("");
00350 //        DEBUG_DEBUG("drawing line " << vertices[0].x << " " << vertices[0].y << " ; " << vertices[1].x << " " << vertices[1].y);
00351 //        DEBUG_DEBUG("drawing line r " << rect[0].x << " " << rect[0].y << " ; " << rect[1].x << " " << rect[1].y);
00352 //        DEBUG_DEBUG("drawing line a " << m << " " << b << " yd: " << yd << " xd: " << xd << " ; " << vertices[1].x - vertices[0].x);
00353 
00354         def = false;
00355         if (proceeding != NULL) {
00356 
00357 
00358             double m_pro = (proceeding->vertices[1].y - proceeding->vertices[0].y) / (proceeding->vertices[1].x - proceeding->vertices[0].x);
00359             double b_pro = proceeding->vertices[1].y - m_pro * proceeding->vertices[1].x;
00360 
00361             if (m_pro == m) {
00362                 def = true;
00363             } else {
00364 
00365                 double yd_pro = d * sqrt(m_pro*m_pro + 1);
00366                 if (proceeding->vertices[1].x < proceeding->vertices[0].x) {
00367                     yd_pro *= -1;
00368                 }
00369 
00370 
00371                 rect[3].x = ((b_pro - yd_pro) - (b - yd)) / (m - m_pro);
00372                 rect[3].y = m_pro * rect[3].x + b_pro - yd_pro;
00373 
00374                 rect[2].x = ((b_pro + yd_pro) - (b + yd)) / (m - m_pro);
00375                 rect[2].y = m_pro * rect[2].x + b_pro + yd_pro;
00376 
00377             }
00378 
00379         } else {
00380             def = true;
00381         }
00382 
00383         if (def) {
00384 
00385             rect[3].x = vertices[1].x     - xd;
00386             rect[3].y = m * rect[3].x + b - yd;
00387 
00388             rect[2].x = vertices[1].x     + xd;
00389             rect[2].y = m * rect[2].x + b + yd;
00390         }
00391 
00392 //    DEBUG_DEBUG("");
00393 //    DEBUG_DEBUG("drawing line " << vertices[0].x << " " << vertices[0].y << " ; " << vertices[1].x << " " << vertices[1].y);
00394 //    DEBUG_DEBUG("drawing line r " << rect[0].x << " " << rect[0].y << " ; " << rect[1].x << " " << rect[1].y);
00395 //    DEBUG_DEBUG("drawing line r " << rect[2].x << " " << rect[2].y << " ; " << rect[3].x << " " << rect[3].y);
00396 
00397     }
00398     
00399 
00400 
00401     MeshManager::MeshInfo::Coord3D res1 = state->GetMeshManager()->GetCoord3D((hugin_utils::FDiff2D&)rect[0]);
00402     MeshManager::MeshInfo::Coord3D res2 = state->GetMeshManager()->GetCoord3D((hugin_utils::FDiff2D&)rect[1]);
00403     MeshManager::MeshInfo::Coord3D res3 = state->GetMeshManager()->GetCoord3D((hugin_utils::FDiff2D&)rect[2]);
00404     MeshManager::MeshInfo::Coord3D res4 = state->GetMeshManager()->GetCoord3D((hugin_utils::FDiff2D&)rect[3]);
00405     glBegin(GL_QUADS);
00406     glVertex3d(res1.x,res1.y,res1.z);
00407     glVertex3d(res2.x,res2.y,res2.z);
00408     glVertex3d(res3.x,res3.y,res3.z);
00409     glVertex3d(res4.x,res4.y,res4.z);
00410     glEnd();
00411 
00412 //    glBegin(GL_QUADS);
00413 //    glVertex3d(rect[0].x,rect[0].y,0);
00414 //    glVertex3d(rect[1].x,rect[1].y,0);
00415 //    glVertex3d(rect[2].x,rect[2].y,0);
00416 //    glVertex3d(rect[3].x,rect[3].y,0);
00417 //    glEnd();
00418 
00419 //    MeshManager::MeshInfo::Coord3D res1 = state->GetMeshManager()->GetCoord3D((hugin_utils::FDiff2D&)vertices[0]);
00420 //    MeshManager::MeshInfo::Coord3D res2 = state->GetMeshManager()->GetCoord3D((hugin_utils::FDiff2D&)vertices[1]);
00421 //    glVertex3d(res1.x,res1.y,res1.z);
00422 //    glVertex3d(res2.x,res2.y,res2.z);
00423 }
00424 
00425 double GreatCircleArc::getxscale(void) const
00426 {
00427         return m_xscale;
00428 }
00429 
00430 
00431 void GreatCircleArc::LineSegment::doGLcross(int point, double xscale, VisualizationState * state) const
00432 {
00433     //TODO: cross with polygons instead of lines
00434     double vx, vy;
00435 
00436         vx = vertices[point].x;
00437         vy = vertices[point].y;
00438 
00439     hugin_utils::FDiff2D p1(vx - xscale, vy - xscale);
00440     hugin_utils::FDiff2D p2(vx + xscale, vy + xscale);
00441 
00442     hugin_utils::FDiff2D p3(vx - xscale, vy + xscale);
00443     hugin_utils::FDiff2D p4(vx + xscale, vy - xscale);
00444 
00445     MeshManager::MeshInfo::Coord3D res1 = state->GetMeshManager()->GetCoord3D(p1);
00446     MeshManager::MeshInfo::Coord3D res2 = state->GetMeshManager()->GetCoord3D(p2);
00447 
00448     MeshManager::MeshInfo::Coord3D res3 = state->GetMeshManager()->GetCoord3D(p3);
00449     MeshManager::MeshInfo::Coord3D res4 = state->GetMeshManager()->GetCoord3D(p4);
00450 
00451     glBegin(GL_LINES);
00452 
00453         // main diagonal
00454         glVertex3f(res1.x,res1.y,res1.z);
00455         glVertex3f(res2.x,res2.y,res2.z);
00456         // second diagonal
00457         glVertex3f(res3.x,res3.y,res3.z);
00458         glVertex3f(res4.x,res4.y,res4.z);
00459 
00460     glEnd();
00461 }
00462 
00463 
00464 float GreatCircleArc::squareDistance(hugin_utils::FDiff2D point) const
00465 {
00466     // find the minimal distance.
00467     float distance = std::numeric_limits<float>::max();
00468     for (std::vector<GreatCircleArc::LineSegment>::const_iterator it = m_lines.begin();
00469          it != m_lines.end();
00470          it++)
00471     {
00472         float this_distance = it->squareDistance(point);
00473         if (this_distance < distance)
00474         {
00475             distance = this_distance;
00476         }
00477     }
00478     return distance;
00479 }
00480 
00481 float GreatCircleArc::LineSegment::squareDistance(hugin_utils::FDiff2D point) const
00482 {
00483     // minimal distance between a point and a line segment
00484 
00485     // Does the line segment start and end in the same place?
00486     if (vertices[0]  == vertices[1])    
00487 
00488     {
00489         // yes, so return the distance to the point where the 'line' segment is.
00490         return point.squareDistance(vertices[0]);
00491     }
00492     // the vector along the line segment.
00493     hugin_utils::FDiff2D line_vector = vertices[1] - vertices[0];
00494     // the parameter indicating the point's nearest position along the line.
00495     float t = line_vector.x == 0 ?
00496                     (point.y - vertices[0].y) / line_vector.y
00497                   : (point.x - vertices[0].x) / line_vector.x;
00498     // check if the point lies along the line segment.
00499     if (t <= 0.0)
00500     {
00501         // no, nearest vertices[0].
00502         return vertices[0].squareDistance(point);
00503     }
00504     if (t >= 1.0)
00505     {
00506         // no, nearest vertices[1].
00507         return vertices[1].squareDistance(point);
00508     }
00509     
00510     // Find the intersection with the line segment and the tangent line.
00511     hugin_utils::FDiff2D intersection(vertices[0] + line_vector * t);
00512     // Finally, find the distance between the intersection and the point.
00513     return intersection.squareDistance(point);
00514 }
00515 
00516 

Generated on 31 Oct 2014 for Hugintrunk by  doxygen 1.4.7