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

Generated on 28 Aug 2015 for Hugintrunk by  doxygen 1.4.7