Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
hom_polar.cpp
1 
2 /***************************************************************************
3  * hom_polar.cpp - A polar coordinate
4  *
5  * Created: Tue April 22 22:55:26 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_polar.h>
25 #include <geometry/hom_transform.h>
26 #include <cmath>
27 #include <cstdio>
28 
29 namespace fawkes {
30 
31 /** @class HomPolar <geometry/hom_polar.h>
32  * A homogeneous representation of a polar coordinate.
33  * @author Daniel Beck
34  */
35 
36 /** Constructor (two-dimensional).
37  * @param r the radius
38  * @param phi the rotation around the z-axis
39  */
40 HomPolar::HomPolar(float r, float phi)
41  : HomVector(r, 0.0, 0.0)
42 {
43  m_r = r;
44  m_phi_z = phi;
45  m_phi_y = 0.0;
46 
47  HomCoord::rotate_z(phi);
48 }
49 
50 /** Constructor (three-dimensional).
51  * @param r the radius
52  * @param phi_z the rotation around the z-axis
53  * @param phi_y the rotation around the new y-axis (after rotating around the z-axis)
54  */
55 HomPolar::HomPolar(float r, float phi_z, float phi_y)
56  : HomVector(r, 0.0, 0.0)
57 {
58  m_r = r;
59  m_phi_z = phi_z;
60  m_phi_y = phi_y;
61 
62  HomCoord::rotate_z(m_phi_z);
63  HomCoord::rotate_y(m_phi_y);
64 }
65 
66 /** Copy constructor.
67  * @param h a HomCoord
68  */
70  : HomVector(h)
71 {
72  m_r = sqrt( x() * x() + y() * y() + z() * z() );
73  m_phi_z = atan2f(y(), x());;
74  m_phi_y = atan2f(z(), sqrt( x() * x() + y() * y() ) );
75 }
76 
77 /** Desctructor. */
79 {
80 }
81 
82 /** Obtain the radius.
83  * @return the radius
84  */
85 float
86 HomPolar::r() const
87 {
88  return m_r;
89 }
90 
91 /** Set the radius.
92  * @param r the new radius
93  */
94 void
95 HomPolar::r(float r)
96 {
97  if ( x() == 0.0 && y() == 0.0 && z() == 0.0 )
98  {
99  x() = 1.0;
100  rotate_z(m_phi_z);
101  rotate_y(m_phi_y);
102  }
103 
104  set_length(r);
105  m_r = r;
106 }
107 
108 /** Get the rotation angle around the z-axis.
109  * @return the rotation angle around the z-axis
110  */
111 float
113 {
114  return m_phi_z;
115 }
116 
117 /** Set the rotation angle around the z-axis.
118  * @param phi the rotation angle around the z-axis
119  */
120 void
121 HomPolar::phi(float phi)
122 {
123  float phi_y = m_phi_y;
124 
125  x() = m_r;
126  y() = 0.0;
127  z() = 0.0;
128 
129  HomTransform t;
130  t.rotate_z(phi);
131  t.rotate_y(m_phi_y);
132 
133  *this = t * (*this);
134 
135  m_phi_z = phi;
136  m_phi_y = phi_y;
137 }
138 
139 /** Get the rotation angle around the z-axis.
140  * @return the rotation angle around the z-axis
141  */
142 float
144 {
145  return m_phi_z;
146 }
147 
148 /** Set the rotation angle around the z-axis.
149  * @param phi_z the rotation angle around the z-axis
150  */
151 void
152 HomPolar::phi_z(float phi_z)
153 {
154  float phi_y = m_phi_y;
155 
156  x() = m_r;
157  y() = 0.0;
158  z() = 0.0;
159 
160  HomTransform t;
161  t.rotate_z(phi_z);
162  t.rotate_y(phi_y);
163 
164  *this = t * (*this);
165 
166  m_phi_z = phi_z;
167  m_phi_y = phi_y;
168 }
169 
170 /** Obtain the rotation angle around the y-axis after rotating around the z-axis.
171  * @return the rotation angle around the y-axis
172  */
173 float
175 {
176  return m_phi_y;
177 }
178 
179 /** Set the rotation angle around the y-axis after rotating around the z-axis.
180  * @param phi_y the new rotation angle around the y-axis
181  */
182 void
183 HomPolar::phi_y(float phi_y)
184 {
185  float phi_z = m_phi_z;
186  x() = m_r;
187  y() = 0.0;
188  z() = 0.0;
189 
190  HomTransform t;
191  t.rotate_z(phi_z);
192  t.rotate_y(phi_y);
193 
194  *this = t * (*this);
195 
196  m_phi_z = phi_z;
197  m_phi_y = phi_y;
198 }
199 
200 /** Set both rotation angles.
201  * @param phi_z the rotation angle around the z-axis
202  * @param phi_y the rotation angle around the y-axis
203  */
204 void
205 HomPolar::phi(float phi_z, float phi_y)
206 {
207  x() = m_r;
208  y() = 0.0;
209  z() = 0.0;
210 
211  HomTransform t;
212  t.rotate_z(phi_z);
213  t.rotate_y(phi_y);
214 
215  *this = t * (*this);
216 
217  m_phi_z = phi_z;
218  m_phi_y = phi_y;
219 }
220 
221 HomPolar&
223 {
224  HomCoord::rotate_x(rad);
225 
226  m_phi_z = atan2f(y(), x());
227  m_phi_y = atan2f(z(), sqrt( x() * x() + y() * y() ) );
228 
229  return *this;
230 }
231 
232 HomPolar&
234 {
235  HomCoord::rotate_y(rad);
236 
237  m_phi_z = atan2f(y(), x());
238  m_phi_y = atan2f(z(), sqrt( x() * x() + y() * y() ) );
239 
240  return *this;
241 }
242 
243 HomPolar&
245 {
246  HomCoord::rotate_z(rad);
247  m_phi_z += rad;
248 
249  return *this;
250 }
251 
252 /** Substraction operator.
253  * The result of subtracting two polar positions from each other is another polar
254  * position that represent the cartesian vector which is the result of subtracting
255  * the corresponding cartesian vectors from each other.
256  * @param p another polar position
257  * @return the result of the substraction
258  */
259 HomPolar
261 {
262  HomPolar ret = HomPolar( HomCoord::operator-(p) );
263 
264  ret.m_phi_z = atan2f(ret.y(), ret.x());
265  ret.m_phi_y = atan2f(ret.z(), sqrt( ret.x() * ret.x() + ret.y() * ret.y() ) );
266 
267  return ret;
268 }
269 
270 /** Subtraction-assignment operator.
271  * @param p the other polar position
272  * @return reference of the result
273  */
274 HomPolar&
276 {
277  *this = *this - p;
278 
279  return *this;
280 }
281 
282 /** Addition operator.
283  * The result of adding two polar positions from each other is another polar
284  * position that represent the cartesian vector which is the result of adding
285  * the corresponding cartesian vectors to each other.
286  * @param p another polar position
287  * @return the result of the substraction
288  */
289 HomPolar
291 {
292  HomPolar ret = HomPolar( HomCoord::operator+(p) );
293 
294  ret.m_phi_z = atan2f(ret.y(), ret.x());
295  ret.m_phi_y = atan2f(ret.z(), sqrt( ret.x() * ret.x() + ret.y() * ret.y() ) );
296 
297  return ret;
298 }
299 
300 /** Addition-assignment operator.
301  * @param p the other polar position
302  * @return reference of the result
303  */
304 HomPolar&
306 {
307  *this = *this + p;
308 
309  return *this;
310 }
311 
312 /** Assignemnt operator.
313  * @param p the other polar position
314  * @return reference of the result
315  */
316 HomPolar&
318 {
320 
321  m_r = p.m_r;
322  m_phi_z = p.m_phi_z;
323  m_phi_y = p.m_phi_y;
324 
325  return *this;
326 }
327 
328 /** Convert the polar coordinate to a cartesian coordinate.
329  * @return the cartesian coordinate
330  */
331 HomVector
333 {
334  HomVector v;
335  v.x() = x();
336  v.y() = y();
337  v.z() = z();
338 
339  return v;
340 }
341 
342 } // end namespace fawkes
HomVector get_vector() const
Convert the polar coordinate to a cartesian coordinate.
Definition: hom_polar.cpp:332
float phi_z() const
Get the rotation angle around the z-axis.
Definition: hom_polar.cpp:143
virtual float y() const
RO-getter for y.
Definition: hom_coord.cpp:115
virtual HomCoord & rotate_x(float rad)
Convenience function to rotate the HomCoord around the x-axis.
Definition: hom_coord.cpp:206
virtual HomCoord & operator=(const HomCoord &h)
Assignment operator.
Definition: hom_coord.cpp:303
A homogeneous representation of a polar coordinate.
Definition: hom_polar.h:31
virtual ~HomPolar()
Desctructor.
Definition: hom_polar.cpp:78
void rotate_z(float rad)
Add rotation around the z-axis.
float r() const
Obtain the radius.
Definition: hom_polar.cpp:86
HomVector & set_length(float length)
Scales the vector such that it has the given length.
Definition: hom_vector.cpp:99
float phi_y() const
Obtain the rotation angle around the y-axis after rotating around the z-axis.
Definition: hom_polar.cpp:174
virtual HomPolar operator+(const HomPolar &h) const
Addition operator.
Definition: hom_polar.cpp:290
virtual HomPolar & rotate_x(float rad)
Convenience function to rotate the HomCoord around the x-axis.
Definition: hom_polar.cpp:222
virtual HomPolar & rotate_z(float rad)
Convenience function to rotate the HomCoord around the z-axis.
Definition: hom_polar.cpp:244
float phi() const
Get the rotation angle around the z-axis.
Definition: hom_polar.cpp:112
virtual HomCoord & rotate_y(float rad)
Convenience function to rotate the HomCoord around the y-axis.
Definition: hom_coord.cpp:220
This class describes a homogeneous transformation.
Definition: hom_transform.h:31
virtual HomPolar & operator=(const HomPolar &h)
Assignemnt operator.
Definition: hom_polar.cpp:317
virtual float z() const
RO-getter for z.
Definition: hom_coord.cpp:145
Base class for homogeneous primitives (vector and point).
Definition: hom_coord.h:34
virtual HomPolar & operator-=(const HomPolar &h)
Subtraction-assignment operator.
Definition: hom_polar.cpp:275
void rotate_y(float rad)
Add rotation around the y-axis.
A homogeneous vector.
Definition: hom_vector.h:31
virtual HomCoord & rotate_z(float rad)
Convenience function to rotate the HomCoord around the z-axis.
Definition: hom_coord.cpp:234
virtual HomPolar & rotate_y(float rad)
Convenience function to rotate the HomCoord around the y-axis.
Definition: hom_polar.cpp:233
virtual float x() const
RO-getter for x.
Definition: hom_coord.cpp:85
HomPolar(float r=0.0, float phi=0.0)
Constructor (two-dimensional).
Definition: hom_polar.cpp:40
virtual HomPolar & operator+=(const HomPolar &h)
Addition-assignment operator.
Definition: hom_polar.cpp:305
virtual HomPolar operator-(const HomPolar &h) const
Substraction operator.
Definition: hom_polar.cpp:260