geometry2D.h
00001 /*
00002 Copyright (c) 2004, Tim Bailey
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007 
00008     * Redistributions of source code must retain the above copyright notice,
00009       this list of conditions and the following disclaimer.
00010     * Redistributions in binary form must reproduce the above copyright notice,
00011       this list of conditions and the following disclaimer in the documentation
00012       and/or other materials provided with the distribution.
00013     * Neither the name of the Player Project nor the names of its contributors
00014       may be used to endorse or promote products derived from this software
00015       without specific prior written permission.
00016 
00017 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
00021 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00024 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 */
00028 
00029 
00030 /* Simple 2D geometric operations with points, poses, and lines.
00031  *
00032  * These algorithms were worked out by me from first principles several years
00033  * ago. They work ok, but are not particularly good implementations. I recommend
00034  * the geometric source-code and resources by David Eberly on the "Magic Software"
00035  * website: www.magic-software.com.
00036  *
00037  * Tim Bailey 2004.
00038  */
00039 #ifndef GEOMETRY_2D_H_
00040 #define GEOMETRY_2D_H_
00041 
00042 #include <vector>
00043 
00044 namespace Geom2D {
00045 
00046 //
00047 // Basic Structures
00048 //
00049 
00050 struct Point 
00051 {
00052         double x;
00053         double y;
00054         short int laser_index;
00055 };
00056 
00057 struct Pose 
00058 {
00059         Point p;
00060         double phi;
00061 };
00062 
00063 struct Line {
00064         Point first;
00065         Point second;
00066 };
00067 
00068 //
00069 // Utility functions
00070 //
00071 
00072 const double PI = 3.14159265358979;
00073 
00074 inline 
00075 double sqr(double x) { return x*x; }
00076 
00077 inline 
00078 double abs(double x) { return (x<0.) ? -x : x; }
00079 
00080 inline
00081 double round(double x) { 
00082         return (x<0.) ? -static_cast<int>(0.5-x) : static_cast<int>(0.5+x); 
00083 }
00084 /*
00085 template<class T>
00086 inline
00087 void swap(T& a, T& b) 
00088 {
00089         T tmp(a);
00090         a = b;
00091         b = tmp;
00092 }
00093 */
00094 inline
00095 double pi_to_pi(double angle) { // normalise an angle to within +/- PI
00096         while (angle < -PI)
00097                 angle += 2.*PI;
00098         while (angle > PI)
00099                 angle -= 2.*PI;
00100         return angle;
00101 }
00102 
00103 //
00104 // Point and Pose algorithms
00105 //
00106 
00107 inline 
00108 double dist_sqr(const Point& p, const Point& q) { // squared distance between two Points
00109         return (sqr(p.x-q.x) + sqr(p.y-q.y));
00110 }
00111 
00112 double dist(const Point& p, const Point& q);
00113 Pose compute_relative_pose(const std::vector<Point>& a, const std::vector<Point>& b);
00114 
00115 //
00116 // Line algorithms
00117 //
00118 
00119 bool intersection_line_line (Point& p, const Line& l, const Line& m);
00120 double distance_line_point (const Line& lne, const Point& p);
00121 void intersection_line_point(Point& p, const Line& l, const Point& q);
00122 
00123 //
00124 // Basic transformations on 2-D Points (x,y) and Poses (x,y,phi).
00125 //
00126 
00127 class Transform2D {
00128 public:
00129         Transform2D(const Pose& ref);
00130 
00131         void transform_to_relative(Point &p);
00132         void transform_to_relative(Pose &p);
00133         void transform_to_global(Point &p);
00134         void transform_to_global(Pose &p);
00135 
00136 private:
00137         const Pose base;
00138         double c;
00139         double s;
00140 };
00141 
00142 inline
00143 void Transform2D::transform_to_relative(Point &p) {
00144         p.x -= base.p.x; 
00145         p.y -= base.p.y;
00146         double t(p.x);
00147         p.x = p.x*c + p.y*s;
00148         p.y = p.y*c -   t*s;
00149 }
00150 
00151 inline
00152 void Transform2D::transform_to_global(Point &p) {
00153         double t(p.x); 
00154         p.x = base.p.x + c*p.x - s*p.y;
00155         p.y = base.p.y + s*t   + c*p.y;
00156 }
00157 
00158 inline
00159 void Transform2D::transform_to_relative(Pose &p) {
00160         transform_to_relative(p.p);
00161         p.phi= pi_to_pi(p.phi-base.phi);
00162 }
00163 
00164 inline
00165 void Transform2D::transform_to_global(Pose &p) {
00166         transform_to_global(p.p);
00167         p.phi= pi_to_pi(p.phi+base.phi);
00168 }
00169 
00170 } // namespace Geom2D
00171 
00172 #endif

Last updated 12 September 2005 21:38:45