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