Fawkes API  Fawkes Development Version
hom_pose_2d.cpp
1 
2 /***************************************************************************
3  * hom_pose_2d.cpp - 2-dimensional Homogenous Pose
4  *
5  * Created: Fri Oct 10 11:13:32 2008
6  * Copyright 2008 Daniel Beck
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include <geometry/hom_pose_2d.h>
25 #include <geometry/hom_point.h>
26 #include <geometry/hom_vector.h>
27 #include <cmath>
28 
29 /** @class fawkes::HomPose2d <geometry/hom_pose_2d.h>
30  * A 2-dimensional pose, i.e. a combination of position and
31  * orientation.
32  * @author Daniel Beck
33  */
34 
35 namespace fawkes {
36 
37 /** Constructor.
38  * @param position the posititon
39  * @param orientation the orientation
40  */
41 HomPose2d::HomPose2d(const HomPoint& position, const HomVector& orientation)
42 {
43  m_position = new HomPoint(position);
44  m_orientation = new HomVector(orientation);
45 
46  m_orientation->unit();
47  m_yaw = atan2f( m_orientation->y(), m_orientation->x() );
48 
50 }
51 
52 /** Constructor.
53  * @param x the x-coordinate of the position
54  * @param y the y-coordinate of the position
55  * @param yaw the angle of the orientation wrt. the current frame
56  */
57 HomPose2d::HomPose2d(float x, float y, float yaw)
58 {
59  m_position = new HomPoint(x, y);
60  m_orientation = new HomVector(1.0, 0.0);
61 
62  m_orientation->rotate_z(yaw);
63  m_yaw = yaw;
64 
66 }
67 
68 /** Copy constructor.
69  * @param p the other pose
70  */
72 {
73  m_position = new HomPoint( *p.m_position );
74  m_orientation = new HomVector( *p.m_orientation );
75 
76  m_yaw = p.m_yaw;
77 
79 }
80 
81 /** Destructor. */
83 {
84  delete m_position;
85  delete m_orientation;
86 }
87 
88 /** Assignment operator.
89  * @param p the rhs pose
90  * @return reference to the assigned pose
91  */
92 const HomPose2d&
94 {
95  (*m_position) = (*p.m_position);
96  (*m_orientation) = (*p.m_orientation);
97 
98  m_yaw = p.m_yaw;
99 
100  return *this;
101 }
102 
103 /** Get the x-coordinate of the position.
104  * @return the x-coordinate of the position.
105  */
106 float
108 {
109  return m_position->x();
110 }
111 
112 /** Set the x-coordinate of the position.
113  * @param x the new x-coordinate of the position.
114  */
115 void
116 HomPose2d::x(float x)
117 {
118  m_position->x(x);
119 }
120 
121 /** Get the y-coordinate of the position.
122  * @return the y-coordinate of the position.
123  */
124 float
126 {
127  return m_position->y();
128 }
129 
130 /** Set the y-coordinate of the position.
131  * @param y the new x-coordinate of the position.
132  */
133 void
134 HomPose2d::y(float y)
135 {
136  m_position->y(y);
137 }
138 
139 /** Get the angle of the current orientation [0...2pi].
140  * @return the angle of the current orientation
141  */
142 float
144 {
145  return m_yaw;
146 }
147 
148 /** Set the angle of the orientation.
149  * @param yaw the new angle of the orientation
150  */
151 void
152 HomPose2d::yaw(float yaw)
153 {
154  if ( yaw < 0 ||
155  yaw > 2 * M_PI )
156  {
157  m_yaw = yaw - 2 * M_PI * floorf( yaw / ( 2 * M_PI ) );
158  }
159  else
160  { m_yaw = yaw; }
161 
162  delete m_orientation;
163  m_orientation = new HomVector(1.0, 0.0);
164  m_orientation->rotate_z(m_yaw);
165 }
166 
167 /** Get the position.
168  * @return the position
169  */
170 const HomPoint&
172 {
173  return *m_position;
174 }
175 
176 /** Set the positional part of the pose.
177  * @param p the new position
178  */
179 void
181 {
182  *m_position = p;
183 }
184 
185 /** Get the orientation vector.
186  * @return the orientation vector
187  */
188 const HomVector&
190 {
191  return *m_orientation;
192 }
193 
194 void
196 {
197  add_primitive( m_position );
198  add_primitive( m_orientation );
199 }
200 
201 void
203 {
204  m_yaw = atan2f( m_orientation->y(), m_orientation->x() );
205 
206  if ( m_yaw < 0 ||
207  m_yaw > 2 * M_PI )
208  {
209  m_yaw = m_yaw - 2 * M_PI * floorf( m_yaw / ( 2 * M_PI ) );
210  }
211 }
212 
213 } // end namespace fawkes
~HomPose2d()
Destructor.
Definition: hom_pose_2d.cpp:82
void register_primitives()
Here, a derived class should register its primitives (HomPoints and HomVectors) by calling add_primit...
virtual float y() const
RO-getter for y.
Definition: hom_coord.cpp:115
Fawkes library namespace.
A 2-dimensional pose, i.e.
Definition: hom_pose_2d.h:33
HomVector & unit()
Brings the vector to unit-length.
Definition: hom_vector.cpp:87
HomPose2d(const HomPoint &pos, const HomVector &orientation)
Constructor.
Definition: hom_pose_2d.cpp:41
A homogeneous point.
Definition: hom_point.h:33
void post_transform()
This method is called after the primitives are transformed.
void set_position(const HomPoint &p)
Set the positional part of the pose.
float yaw() const
Get the angle of the current orientation [0...2pi].
A homogeneous vector.
Definition: hom_vector.h:31
const HomPose2d & operator=(const HomPose2d &p)
Assignment operator.
Definition: hom_pose_2d.cpp:93
virtual HomCoord & rotate_z(float rad)
Convenience function to rotate the HomCoord around the z-axis.
Definition: hom_coord.cpp:234
float x() const
Get the x-coordinate of the position.
const HomPoint & position() const
Get the position.
virtual float x() const
RO-getter for x.
Definition: hom_coord.cpp:85
float y() const
Get the y-coordinate of the position.
void add_primitive(HomCoord *c)
Add a primitive to the list of primitives that is transformed.
const HomVector & orientation() const
Get the orientation vector.