hom_polar.cpp

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