24 #include <geometry/hom_pose_2d.h>
25 #include <geometry/hom_point.h>
26 #include <geometry/hom_vector.h>
44 m_orientation =
new HomVector(orientation);
46 m_orientation->
unit();
47 m_yaw = atan2f( m_orientation->
y(), m_orientation->
x() );
73 m_position =
new HomPoint( *p.m_position );
74 m_orientation =
new HomVector( *p.m_orientation );
95 (*m_position) = (*p.m_position);
96 (*m_orientation) = (*p.m_orientation);
109 return m_position->
x();
127 return m_position->
y();
157 m_yaw = yaw - 2 * M_PI * floorf( yaw / ( 2 * M_PI ) );
162 delete m_orientation;
191 return *m_orientation;
204 m_yaw = atan2f( m_orientation->
y(), m_orientation->
x() );
209 m_yaw = m_yaw - 2 * M_PI * floorf( m_yaw / ( 2 * M_PI ) );