point_utils.cpp
Go to the documentation of this file.
00001 
00037 #include <bwi_mapper/point_utils.h>
00038 
00039 namespace bwi_mapper {
00040 
00041   /* http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment */
00042   float minimumDistanceToLineSegment(Point2f v, Point2f w, Point2f p) {
00043     // Return minimum distance between line segment vw and point p
00044     const float l2 = getMagnitude(w-v);  
00045     if (l2 == 0.0) return getMagnitude(p-v);   // v == w case
00046     // Consider the line extending the segment, parameterized as v + t (w - v).
00047     // We find projection of point p onto the line. 
00048     // It falls where t = [(p-v) . (w-v)] / |w-v|^2
00049     const float t = (p - v).dot(w - v) / (l2 * l2);
00050     if (t < 0.0) return getMagnitude(p - v);  // Beyond the 'v' end 
00051     else if (t > 1.0) return getMagnitude(p - w);  // Beyond the 'w' 
00052     const Point2f projection = v + t * (w - v); 
00053     return getMagnitude(p - projection);
00054   }
00055 
00056   float getMagnitude(Point2f p) {
00057     return cv::norm(p);
00058   }
00059   
00060 } /* bwi_mapper */


bwi_mapper
Author(s): Piyush Khandelwal
autogenerated on Thu Jun 6 2019 17:57:21