euclidean_distance.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*- */
00002 /*
00003  *  Description: Euclidean distance functions
00004  *
00005  *  Copyright (C) 2009 Austin Robot Technology                    
00006  *
00007  *  License: Modified BSD Software License Agreement
00008  *
00009  *  $Id: euclidean_distance.h 1876 2011-11-26 23:48:08Z jack.oquin $
00010  */
00011 
00012 #ifndef _EUCLIDEAN_DISTANCE_H_
00013 #define _EUCLIDEAN_DISTANCE_H_
00014 
00020 #include <art/epsilon.h>
00021 #include <art/infinity.h>
00022 #include <art_map/types.h>
00023 
00024 namespace Euclidean
00025 {
00026 
00030   void inline DistanceFromLine(float cx, float cy,
00031                                float ax, float ay ,
00032                                float bx, float by,
00033                                float &distanceSegment,
00034                                float &distanceLine)
00035     {
00036       // find the distance from the point (cx,cy) to the line
00037       // determined by the points (ax,ay) and (bx,by)
00038       //
00039       // distanceSegment = distance from the point to the line segment
00040       // distanceLine = distance from the point to the line
00041       //                    (assuming infinite extent in both directions)
00042       //
00043       // copied from http://www.codeguru.com/forum/printthread.php?t=194400
00044 
00045       float r_numerator = (cx-ax)*(bx-ax) + (cy-ay)*(by-ay);
00046       float r_denomenator = (bx-ax)*(bx-ax) + (by-ay)*(by-ay);
00047       float r = r_numerator / r_denomenator;
00048 
00049       float s =  ((ay-cy)*(bx-ax)-(ax-cx)*(by-ay) ) / r_denomenator;
00050 
00051       distanceLine = fabs(s)*sqrtf(r_denomenator);
00052 
00053       if ( (r >= 0) && (r <= 1) )
00054         {
00055           distanceSegment = distanceLine;
00056         }
00057       else
00058         {
00059 
00060           float dist1 = (cx-ax)*(cx-ax) + (cy-ay)*(cy-ay);
00061           float dist2 = (cx-bx)*(cx-bx) + (cy-by)*(cy-by);
00062           if (dist1 < dist2)
00063             {
00064               distanceSegment = sqrtf(dist1);
00065             }
00066           else
00067             {
00068               distanceSegment = sqrtf(dist2);
00069             }
00070         }
00071     }
00072 
00073   void inline DistanceFromLine(const MapXY& c,
00074                                const MapXY& a,
00075                                const MapXY& b,
00076                                float &distanceSegment,
00077                                float &distanceLine)
00078   {
00079     return DistanceFromLine(c.x,c.y,a.x,a.y,b.x,b.y,distanceSegment, 
00080                             distanceLine);
00081   }
00082 
00084   float inline DistanceTo(MapPose p1, MapPose p2)
00085     {
00086       float x_dist = p1.map.x - p2.map.x;
00087       float y_dist = p1.map.y - p2.map.y;
00088       return sqrtf(x_dist*x_dist + y_dist*y_dist);
00089     }
00090 
00092   float inline DistanceTo(float p1x, float p1y, float p2x, float p2y)
00093     {
00094       float x_dist = p1x - p2x;
00095       float y_dist = p1y - p2y;
00096       return sqrtf(x_dist*x_dist + y_dist*y_dist);
00097     }
00098 
00100   float inline DistanceTo(MapXY p1, MapXY p2)
00101     {
00102       MapXY dist = p1 - p2;
00103       return sqrtf(dist.x*dist.x + dist.y*dist.y);
00104     }
00105 
00109   float inline DistanceToTime(float distance, float speed)
00110     {
00111       float abs_speed = fabs(speed);
00112       if (abs_speed < Epsilon::speed)
00113         return (Infinite::time);
00114       else
00115         return (distance / abs_speed);
00116     }
00117 
00119   float inline DistanceToWaypt(const MapPose &pose,
00120                                const WayPointNode &waypt)
00121     {
00122       return DistanceTo(pose.map, waypt.map);
00123     }
00124 
00126   float inline DistanceToWaypt(MapXY point, const WayPointNode &waypt)
00127     {
00128       return DistanceTo(point, waypt.map);
00129     }
00130 
00133   float inline DistanceToWaypt(Polar polar,
00134                                const MapPose &origin,
00135                                const WayPointNode &waypt)
00136     {
00137       return DistanceTo(Coordinates::Polar_to_MapXY(polar, origin),
00138                         waypt.map);
00139     }
00140 
00142   bool inline point_in_line_segment(MapXY point, MapXY lp1, MapXY lp2)
00143     {
00144       return (fabs(DistanceTo(lp1, point) + DistanceTo(point, lp2)
00145                    - DistanceTo(lp1, lp2))
00146               < Epsilon::distance);
00147     }
00148 
00149 }
00150 
00151 #endif // _EUCLIDEAN_DISTANCE_H_ //
00152 


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34