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