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_polar.h>
00025 #include <geometry/hom_transform.h>
00026 #include <cmath>
00027 #include <cstdio>
00028
00029 namespace fawkes {
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 HomPolar::HomPolar(float r, float phi)
00041 : HomVector(r, 0.0, 0.0)
00042 {
00043 m_r = r;
00044 m_phi_z = phi;
00045 m_phi_y = 0.0;
00046
00047 HomCoord::rotate_z(phi);
00048 }
00049
00050
00051
00052
00053
00054
00055 HomPolar::HomPolar(float r, float phi_z, float phi_y)
00056 : HomVector(r, 0.0, 0.0)
00057 {
00058 m_r = r;
00059 m_phi_z = phi_z;
00060 m_phi_y = phi_y;
00061
00062 HomCoord::rotate_z(m_phi_z);
00063 HomCoord::rotate_y(m_phi_y);
00064 }
00065
00066
00067
00068
00069 HomPolar::HomPolar(const HomCoord& h)
00070 : HomVector(h)
00071 {
00072 m_r = sqrt( x() * x() + y() * y() + z() * z() );
00073 m_phi_z = atan2f(y(), x());;
00074 m_phi_y = atan2f(z(), sqrt( x() * x() + y() * y() ) );
00075 }
00076
00077
00078 HomPolar::~HomPolar()
00079 {
00080 }
00081
00082
00083
00084
00085 float
00086 HomPolar::r() const
00087 {
00088 return m_r;
00089 }
00090
00091
00092
00093
00094 void
00095 HomPolar::r(float r)
00096 {
00097 if ( x() == 0.0 && y() == 0.0 && z() == 0.0 )
00098 {
00099 x() = 1.0;
00100 rotate_z(m_phi_z);
00101 rotate_y(m_phi_y);
00102 }
00103
00104 set_length(r);
00105 m_r = r;
00106 }
00107
00108
00109
00110
00111 float
00112 HomPolar::phi() const
00113 {
00114 return m_phi_z;
00115 }
00116
00117
00118
00119
00120 void
00121 HomPolar::phi(float phi)
00122 {
00123 float phi_y = m_phi_y;
00124
00125 x() = m_r;
00126 y() = 0.0;
00127 z() = 0.0;
00128
00129 HomTransform t;
00130 t.rotate_z(phi);
00131 t.rotate_y(m_phi_y);
00132
00133 *this = t * (*this);
00134
00135 m_phi_z = phi;
00136 m_phi_y = phi_y;
00137 }
00138
00139
00140
00141
00142 float
00143 HomPolar::phi_z() const
00144 {
00145 return m_phi_z;
00146 }
00147
00148
00149
00150
00151 void
00152 HomPolar::phi_z(float phi_z)
00153 {
00154 float phi_y = m_phi_y;
00155
00156 x() = m_r;
00157 y() = 0.0;
00158 z() = 0.0;
00159
00160 HomTransform t;
00161 t.rotate_z(phi_z);
00162 t.rotate_y(phi_y);
00163
00164 *this = t * (*this);
00165
00166 m_phi_z = phi_z;
00167 m_phi_y = phi_y;
00168 }
00169
00170
00171
00172
00173 float
00174 HomPolar::phi_y() const
00175 {
00176 return m_phi_y;
00177 }
00178
00179
00180
00181
00182 void
00183 HomPolar::phi_y(float phi_y)
00184 {
00185 float phi_z = m_phi_z;
00186 x() = m_r;
00187 y() = 0.0;
00188 z() = 0.0;
00189
00190 HomTransform t;
00191 t.rotate_z(phi_z);
00192 t.rotate_y(phi_y);
00193
00194 *this = t * (*this);
00195
00196 m_phi_z = phi_z;
00197 m_phi_y = phi_y;
00198 }
00199
00200
00201
00202
00203
00204 void
00205 HomPolar::phi(float phi_z, float phi_y)
00206 {
00207 x() = m_r;
00208 y() = 0.0;
00209 z() = 0.0;
00210
00211 HomTransform t;
00212 t.rotate_z(phi_z);
00213 t.rotate_y(phi_y);
00214
00215 *this = t * (*this);
00216
00217 m_phi_z = phi_z;
00218 m_phi_y = phi_y;
00219 }
00220
00221 HomPolar&
00222 HomPolar::rotate_x(float rad)
00223 {
00224 HomCoord::rotate_x(rad);
00225
00226 m_phi_z = atan2f(y(), x());
00227 m_phi_y = atan2f(z(), sqrt( x() * x() + y() * y() ) );
00228
00229 return *this;
00230 }
00231
00232 HomPolar&
00233 HomPolar::rotate_y(float rad)
00234 {
00235 HomCoord::rotate_y(rad);
00236
00237 m_phi_z = atan2f(y(), x());
00238 m_phi_y = atan2f(z(), sqrt( x() * x() + y() * y() ) );
00239
00240 return *this;
00241 }
00242
00243 HomPolar&
00244 HomPolar::rotate_z(float rad)
00245 {
00246 HomCoord::rotate_z(rad);
00247 m_phi_z += rad;
00248
00249 return *this;
00250 }
00251
00252
00253
00254
00255
00256
00257
00258
00259 HomPolar
00260 HomPolar::operator-(const HomPolar& p) const
00261 {
00262 HomPolar ret = HomPolar( HomCoord::operator-(p) );
00263
00264 ret.m_phi_z = atan2f(ret.y(), ret.x());
00265 ret.m_phi_y = atan2f(ret.z(), sqrt( ret.x() * ret.x() + ret.y() * ret.y() ) );
00266
00267 return ret;
00268 }
00269
00270
00271
00272
00273
00274 HomPolar&
00275 HomPolar::operator-=(const HomPolar& p)
00276 {
00277 *this = *this - p;
00278
00279 return *this;
00280 }
00281
00282
00283
00284
00285
00286
00287
00288
00289 HomPolar
00290 HomPolar::operator+(const HomPolar& p) const
00291 {
00292 HomPolar ret = HomPolar( HomCoord::operator+(p) );
00293
00294 ret.m_phi_z = atan2f(ret.y(), ret.x());
00295 ret.m_phi_y = atan2f(ret.z(), sqrt( ret.x() * ret.x() + ret.y() * ret.y() ) );
00296
00297 return ret;
00298 }
00299
00300
00301
00302
00303
00304 HomPolar&
00305 HomPolar::operator+=(const HomPolar& p)
00306 {
00307 *this = *this + p;
00308
00309 return *this;
00310 }
00311
00312
00313
00314
00315
00316 HomPolar&
00317 HomPolar::operator=(const HomPolar& p)
00318 {
00319 HomCoord::operator=(p);
00320
00321 m_r = p.m_r;
00322 m_phi_z = p.m_phi_z;
00323 m_phi_y = p.m_phi_y;
00324
00325 return *this;
00326 }
00327
00328
00329
00330
00331 HomVector
00332 HomPolar::get_vector() const
00333 {
00334 HomVector v;
00335 v.x() = x();
00336 v.y() = y();
00337 v.z() = z();
00338
00339 return v;
00340 }
00341
00342 }