00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00030
00031
00032
00033
00034
00035 namespace fawkes {
00036
00037
00038
00039
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
00053
00054
00055
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
00069
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
00082 HomPose2d::~HomPose2d()
00083 {
00084 delete m_position;
00085 delete m_orientation;
00086 }
00087
00088
00089
00090
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
00104
00105
00106 float
00107 HomPose2d::x() const
00108 {
00109 return m_position->x();
00110 }
00111
00112
00113
00114
00115 void
00116 HomPose2d::x(float x)
00117 {
00118 m_position->x(x);
00119 }
00120
00121
00122
00123
00124 float
00125 HomPose2d::y() const
00126 {
00127 return m_position->y();
00128 }
00129
00130
00131
00132
00133 void
00134 HomPose2d::y(float y)
00135 {
00136 m_position->y(y);
00137 }
00138
00139
00140
00141
00142 float
00143 HomPose2d::yaw() const
00144 {
00145 return m_yaw;
00146 }
00147
00148
00149
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
00168
00169
00170 const HomPoint&
00171 HomPose2d::position() const
00172 {
00173 return *m_position;
00174 }
00175
00176
00177
00178
00179 void
00180 HomPose2d::set_position(const HomPoint& p)
00181 {
00182 *m_position = p;
00183 }
00184
00185
00186
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 }