hom_pose_2d.cpp

00001 
00002 /***************************************************************************
00003  *  hom_pose_2d.cpp - 2-dimensional Homogenous Pose
00004  *
00005  *  Created: Fri Oct 10 11:13:32 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_pose_2d.h>
00025 #include <geometry/hom_point.h>
00026 #include <geometry/hom_vector.h>
00027 #include <cmath>
00028 
00029 /** @class fawkes::HomPose2d <geometry/hom_pose_2d.h>
00030  * A 2-dimensional pose, i.e. a combination of position and
00031  * orientation.
00032  * @author Daniel Beck
00033  */
00034 
00035 namespace fawkes {
00036 
00037 /** Constructor.
00038  * @param position the posititon
00039  * @param orientation the orientation
00040  */
00041 HomPose2d::HomPose2d(const HomPoint& position, const HomVector& orientation)
00042 {
00043   m_position  = new HomPoint(position);
00044   m_orientation = new HomVector(orientation);
00045 
00046   m_orientation->unit();
00047   m_yaw = atan2f( m_orientation->y(), m_orientation->x() );
00048 
00049   register_primitives();
00050 }
00051 
00052 /** Constructor.
00053  * @param x the x-coordinate of the position
00054  * @param y the y-coordinate of the position
00055  * @param yaw the angle of the orientation wrt. the current frame
00056  */
00057 HomPose2d::HomPose2d(float x, float y, float yaw)
00058 {
00059   m_position  = new HomPoint(x, y);
00060   m_orientation = new HomVector(1.0, 0.0);
00061 
00062   m_orientation->rotate_z(yaw);
00063   m_yaw = yaw;
00064 
00065   register_primitives();
00066 }
00067 
00068 /** Copy constructor.
00069  * @param p the other pose
00070  */
00071 HomPose2d::HomPose2d(const HomPose2d& p)
00072 {
00073   m_position  = new HomPoint( *p.m_position );
00074   m_orientation = new HomVector( *p.m_orientation );
00075 
00076   m_yaw = p.m_yaw;
00077 
00078   register_primitives();
00079 }
00080 
00081 /** Destructor. */
00082 HomPose2d::~HomPose2d()
00083 {
00084   delete m_position;
00085   delete m_orientation;
00086 }
00087 
00088 /** Assignment operator.
00089  * @param p the rhs pose
00090  * @return reference to the assigned pose
00091  */
00092 const HomPose2d&
00093 HomPose2d::operator=(const HomPose2d& p)
00094 {
00095   (*m_position)  = (*p.m_position);
00096   (*m_orientation) = (*p.m_orientation);
00097 
00098   m_yaw = p.m_yaw;
00099 
00100   return *this;
00101 }
00102 
00103 /** Get the x-coordinate of the position.
00104  * @return the x-coordinate of the position.
00105  */
00106 float
00107 HomPose2d::x() const
00108 {
00109   return m_position->x();
00110 }
00111 
00112 /** Set the x-coordinate of the position.
00113  * @param x the new x-coordinate of the position.
00114  */
00115 void
00116 HomPose2d::x(float x)
00117 {
00118   m_position->x(x);
00119 }
00120 
00121 /** Get the y-coordinate of the position.
00122  * @return the y-coordinate of the position.
00123  */
00124 float
00125 HomPose2d::y() const
00126 {
00127   return m_position->y();
00128 }
00129 
00130 /** Set the y-coordinate of the position.
00131  * @param y the new x-coordinate of the position.
00132  */
00133 void
00134 HomPose2d::y(float y)
00135 {
00136   m_position->y(y);
00137 }
00138 
00139 /** Get the angle of the current orientation [0...2pi].
00140  * @return the angle of the current orientation
00141  */
00142 float
00143 HomPose2d::yaw() const
00144 {
00145   return m_yaw;
00146 }
00147 
00148 /** Set the angle of the orientation.
00149  * @param yaw the new angle of the orientation
00150  */
00151 void
00152 HomPose2d::yaw(float yaw)
00153 {
00154   if ( yaw < 0 ||
00155        yaw > 2 * M_PI )
00156   {
00157     m_yaw = yaw - 2 * M_PI * floorf( yaw / ( 2 * M_PI ) );
00158   }
00159   else
00160   { m_yaw = yaw; }
00161 
00162   delete m_orientation;
00163   m_orientation = new HomVector(1.0, 0.0);
00164   m_orientation->rotate_z(m_yaw);
00165 }
00166 
00167 /** Get the position.
00168  * @return the position
00169  */
00170 const HomPoint&
00171 HomPose2d::position() const
00172 {
00173   return *m_position;
00174 }
00175 
00176 /** Set the positional part of the pose.
00177  * @param p the new position
00178  */
00179 void
00180 HomPose2d::set_position(const HomPoint& p)
00181 {
00182   *m_position = p;
00183 }
00184 
00185 /** Get the orientation vector.
00186  * @return the orientation vector
00187  */
00188 const HomVector&
00189 HomPose2d::orientation() const
00190 {
00191   return *m_orientation;
00192 }
00193 
00194 void
00195 HomPose2d::register_primitives()
00196 {
00197   add_primitive( m_position );
00198   add_primitive( m_orientation );
00199 }
00200 
00201 void
00202 HomPose2d::post_transform()
00203 {
00204   m_yaw = atan2f( m_orientation->y(), m_orientation->x() );
00205 
00206   if ( m_yaw < 0 ||
00207        m_yaw > 2 * M_PI )
00208   {
00209     m_yaw = m_yaw - 2 * M_PI * floorf( m_yaw / ( 2 * M_PI ) );
00210   }
00211 }
00212 
00213 } // end namespace fawkes

Generated on Tue Feb 22 13:32:27 2011 for Fawkes API by  doxygen 1.4.7