Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * hom_polar.cpp - A polar coordinate 00004 * 00005 * Created: Tue April 22 22:55:26 2008 00006 * Copyright 2008 Daniel Beck 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00022 */ 00023 00024 #include <geometry/hom_polar.h> 00025 #include <geometry/hom_transform.h> 00026 #include <cmath> 00027 #include <cstdio> 00028 00029 namespace fawkes { 00030 00031 /** @class HomPolar <geometry/hom_polar.h> 00032 * A homogeneous representation of a polar coordinate. 00033 * @author Daniel Beck 00034 */ 00035 00036 /** Constructor (two-dimensional). 00037 * @param r the radius 00038 * @param phi the rotation around the z-axis 00039 */ 00040 HomPolar::HomPolar(float r, float phi) 00041 : HomVector(r, 0.0, 0.0) 00042 { 00043 m_r = r; 00044 m_phi_z = phi; 00045 m_phi_y = 0.0; 00046 00047 HomCoord::rotate_z(phi); 00048 } 00049 00050 /** Constructor (three-dimensional). 00051 * @param r the radius 00052 * @param phi_z the rotation around the z-axis 00053 * @param phi_y the rotation around the new y-axis (after rotating around the z-axis) 00054 */ 00055 HomPolar::HomPolar(float r, float phi_z, float phi_y) 00056 : HomVector(r, 0.0, 0.0) 00057 { 00058 m_r = r; 00059 m_phi_z = phi_z; 00060 m_phi_y = phi_y; 00061 00062 HomCoord::rotate_z(m_phi_z); 00063 HomCoord::rotate_y(m_phi_y); 00064 } 00065 00066 /** Copy constructor. 00067 * @param h a HomCoord 00068 */ 00069 HomPolar::HomPolar(const HomCoord& h) 00070 : HomVector(h) 00071 { 00072 m_r = sqrt( x() * x() + y() * y() + z() * z() ); 00073 m_phi_z = atan2f(y(), x());; 00074 m_phi_y = atan2f(z(), sqrt( x() * x() + y() * y() ) ); 00075 } 00076 00077 /** Desctructor. */ 00078 HomPolar::~HomPolar() 00079 { 00080 } 00081 00082 /** Obtain the radius. 00083 * @return the radius 00084 */ 00085 float 00086 HomPolar::r() const 00087 { 00088 return m_r; 00089 } 00090 00091 /** Set the radius. 00092 * @param r the new radius 00093 */ 00094 void 00095 HomPolar::r(float r) 00096 { 00097 if ( x() == 0.0 && y() == 0.0 && z() == 0.0 ) 00098 { 00099 x() = 1.0; 00100 rotate_z(m_phi_z); 00101 rotate_y(m_phi_y); 00102 } 00103 00104 set_length(r); 00105 m_r = r; 00106 } 00107 00108 /** Get the rotation angle around the z-axis. 00109 * @return the rotation angle around the z-axis 00110 */ 00111 float 00112 HomPolar::phi() const 00113 { 00114 return m_phi_z; 00115 } 00116 00117 /** Set the rotation angle around the z-axis. 00118 * @param phi the rotation angle around the z-axis 00119 */ 00120 void 00121 HomPolar::phi(float phi) 00122 { 00123 float phi_y = m_phi_y; 00124 00125 x() = m_r; 00126 y() = 0.0; 00127 z() = 0.0; 00128 00129 HomTransform t; 00130 t.rotate_z(phi); 00131 t.rotate_y(m_phi_y); 00132 00133 *this = t * (*this); 00134 00135 m_phi_z = phi; 00136 m_phi_y = phi_y; 00137 } 00138 00139 /** Get the rotation angle around the z-axis. 00140 * @return the rotation angle around the z-axis 00141 */ 00142 float 00143 HomPolar::phi_z() const 00144 { 00145 return m_phi_z; 00146 } 00147 00148 /** Set the rotation angle around the z-axis. 00149 * @param phi_z the rotation angle around the z-axis 00150 */ 00151 void 00152 HomPolar::phi_z(float phi_z) 00153 { 00154 float phi_y = m_phi_y; 00155 00156 x() = m_r; 00157 y() = 0.0; 00158 z() = 0.0; 00159 00160 HomTransform t; 00161 t.rotate_z(phi_z); 00162 t.rotate_y(phi_y); 00163 00164 *this = t * (*this); 00165 00166 m_phi_z = phi_z; 00167 m_phi_y = phi_y; 00168 } 00169 00170 /** Obtain the rotation angle around the y-axis after rotating around the z-axis. 00171 * @return the rotation angle around the y-axis 00172 */ 00173 float 00174 HomPolar::phi_y() const 00175 { 00176 return m_phi_y; 00177 } 00178 00179 /** Set the rotation angle around the y-axis after rotating around the z-axis. 00180 * @param phi_y the new rotation angle around the y-axis 00181 */ 00182 void 00183 HomPolar::phi_y(float phi_y) 00184 { 00185 float phi_z = m_phi_z; 00186 x() = m_r; 00187 y() = 0.0; 00188 z() = 0.0; 00189 00190 HomTransform t; 00191 t.rotate_z(phi_z); 00192 t.rotate_y(phi_y); 00193 00194 *this = t * (*this); 00195 00196 m_phi_z = phi_z; 00197 m_phi_y = phi_y; 00198 } 00199 00200 /** Set both rotation angles. 00201 * @param phi_z the rotation angle around the z-axis 00202 * @param phi_y the rotation angle around the y-axis 00203 */ 00204 void 00205 HomPolar::phi(float phi_z, float phi_y) 00206 { 00207 x() = m_r; 00208 y() = 0.0; 00209 z() = 0.0; 00210 00211 HomTransform t; 00212 t.rotate_z(phi_z); 00213 t.rotate_y(phi_y); 00214 00215 *this = t * (*this); 00216 00217 m_phi_z = phi_z; 00218 m_phi_y = phi_y; 00219 } 00220 00221 HomPolar& 00222 HomPolar::rotate_x(float rad) 00223 { 00224 HomCoord::rotate_x(rad); 00225 00226 m_phi_z = atan2f(y(), x()); 00227 m_phi_y = atan2f(z(), sqrt( x() * x() + y() * y() ) ); 00228 00229 return *this; 00230 } 00231 00232 HomPolar& 00233 HomPolar::rotate_y(float rad) 00234 { 00235 HomCoord::rotate_y(rad); 00236 00237 m_phi_z = atan2f(y(), x()); 00238 m_phi_y = atan2f(z(), sqrt( x() * x() + y() * y() ) ); 00239 00240 return *this; 00241 } 00242 00243 HomPolar& 00244 HomPolar::rotate_z(float rad) 00245 { 00246 HomCoord::rotate_z(rad); 00247 m_phi_z += rad; 00248 00249 return *this; 00250 } 00251 00252 /** Substraction operator. 00253 * The result of subtracting two polar positions from each other is another polar 00254 * position that represent the cartesian vector which is the result of subtracting 00255 * the corresponding cartesian vectors from each other. 00256 * @param p another polar position 00257 * @return the result of the substraction 00258 */ 00259 HomPolar 00260 HomPolar::operator-(const HomPolar& p) const 00261 { 00262 HomPolar ret = HomPolar( HomCoord::operator-(p) ); 00263 00264 ret.m_phi_z = atan2f(ret.y(), ret.x()); 00265 ret.m_phi_y = atan2f(ret.z(), sqrt( ret.x() * ret.x() + ret.y() * ret.y() ) ); 00266 00267 return ret; 00268 } 00269 00270 /** Subtraction-assignment operator. 00271 * @param p the other polar position 00272 * @return reference of the result 00273 */ 00274 HomPolar& 00275 HomPolar::operator-=(const HomPolar& p) 00276 { 00277 *this = *this - p; 00278 00279 return *this; 00280 } 00281 00282 /** Addition operator. 00283 * The result of adding two polar positions from each other is another polar 00284 * position that represent the cartesian vector which is the result of adding 00285 * the corresponding cartesian vectors to each other. 00286 * @param p another polar position 00287 * @return the result of the substraction 00288 */ 00289 HomPolar 00290 HomPolar::operator+(const HomPolar& p) const 00291 { 00292 HomPolar ret = HomPolar( HomCoord::operator+(p) ); 00293 00294 ret.m_phi_z = atan2f(ret.y(), ret.x()); 00295 ret.m_phi_y = atan2f(ret.z(), sqrt( ret.x() * ret.x() + ret.y() * ret.y() ) ); 00296 00297 return ret; 00298 } 00299 00300 /** Addition-assignment operator. 00301 * @param p the other polar position 00302 * @return reference of the result 00303 */ 00304 HomPolar& 00305 HomPolar::operator+=(const HomPolar& p) 00306 { 00307 *this = *this + p; 00308 00309 return *this; 00310 } 00311 00312 /** Assignemnt operator. 00313 * @param p the other polar position 00314 * @return reference of the result 00315 */ 00316 HomPolar& 00317 HomPolar::operator=(const HomPolar& p) 00318 { 00319 HomCoord::operator=(p); 00320 00321 m_r = p.m_r; 00322 m_phi_z = p.m_phi_z; 00323 m_phi_y = p.m_phi_y; 00324 00325 return *this; 00326 } 00327 00328 /** Convert the polar coordinate to a cartesian coordinate. 00329 * @return the cartesian coordinate 00330 */ 00331 HomVector 00332 HomPolar::get_vector() const 00333 { 00334 HomVector v; 00335 v.x() = x(); 00336 v.y() = y(); 00337 v.z() = z(); 00338 00339 return v; 00340 } 00341 00342 } // end namespace fawkes