hom_coord.cpp
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_coord.h>
00025 #include <geometry/hom_transform.h>
00026 #include <geometry/vector.h>
00027
00028 #include <cstdio>
00029 #include <iomanip>
00030
00031 namespace fawkes {
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 HomCoord::HomCoord(float x, float y, float z, float w)
00049 {
00050 m_vector = new Vector(4);
00051
00052 m_vector->set(0, x);
00053 m_vector->set(1, y);
00054 m_vector->set(2, z);
00055 m_vector->set(3, w);
00056 }
00057
00058
00059
00060
00061 HomCoord::HomCoord(const HomCoord& c)
00062 {
00063 const Vector v = *(c.m_vector);
00064 m_vector = new Vector(v);
00065 }
00066
00067
00068
00069
00070 HomCoord::HomCoord(const Vector& v)
00071 {
00072 m_vector = new Vector(v);
00073 }
00074
00075
00076 HomCoord::~HomCoord()
00077 {
00078 delete m_vector;
00079 }
00080
00081
00082
00083
00084 float
00085 HomCoord::x() const
00086 {
00087 return m_vector->get(0);
00088 }
00089
00090
00091
00092
00093 float&
00094 HomCoord::x()
00095 {
00096 float& val = m_vector->get(0);
00097 return val;
00098 }
00099
00100
00101
00102
00103
00104 HomCoord&
00105 HomCoord::x(float x)
00106 {
00107 m_vector->set(0, x);
00108 return *this;
00109 }
00110
00111
00112
00113
00114 float
00115 HomCoord::y() const
00116 {
00117 return m_vector->get(1);
00118 }
00119
00120
00121
00122
00123 float&
00124 HomCoord::y()
00125 {
00126 float& val = m_vector->get(1);
00127 return val;
00128 }
00129
00130
00131
00132
00133
00134 HomCoord&
00135 HomCoord::y(float y)
00136 {
00137 m_vector->set(1, y);
00138 return *this;
00139 }
00140
00141
00142
00143
00144 float
00145 HomCoord::z() const
00146 {
00147 return m_vector->get(2);
00148 }
00149
00150
00151
00152
00153 float&
00154 HomCoord::z()
00155 {
00156 float& val = m_vector->get(2);
00157 return val;
00158 }
00159
00160
00161
00162
00163
00164 HomCoord&
00165 HomCoord::z(float z)
00166 {
00167 m_vector->set(2, z);
00168 return *this;
00169 }
00170
00171
00172
00173
00174 float
00175 HomCoord::w() const
00176 {
00177 return m_vector->get(3);
00178 }
00179
00180
00181
00182
00183 float&
00184 HomCoord::w()
00185 {
00186 float& val = m_vector->get(3);
00187 return val;
00188 }
00189
00190
00191
00192
00193
00194 HomCoord&
00195 HomCoord::w(float w)
00196 {
00197 m_vector->set(3, w);
00198 return *this;
00199 }
00200
00201
00202
00203
00204
00205 HomCoord&
00206 HomCoord::rotate_x(float rad)
00207 {
00208 HomTransform t;
00209 t.rotate_x(rad);
00210 transform(t);
00211
00212 return *this;
00213 }
00214
00215
00216
00217
00218
00219 HomCoord&
00220 HomCoord::rotate_y(float rad)
00221 {
00222 HomTransform t;
00223 t.rotate_y(rad);
00224 transform(t);
00225
00226 return *this;
00227 }
00228
00229
00230
00231
00232
00233 HomCoord&
00234 HomCoord::rotate_z(float rad)
00235 {
00236 HomTransform t;
00237 t.rotate_z(rad);
00238 transform(t);
00239
00240 return *this;
00241 }
00242
00243
00244
00245
00246
00247 HomCoord
00248 HomCoord::operator-(const HomCoord& h) const
00249 {
00250 Vector v = (*m_vector) - (*h.m_vector);
00251 HomCoord result(v);
00252 float w = result.w();
00253 result.w() = (w > 1.0) ? 1.0 : w;
00254 return result;
00255 }
00256
00257
00258
00259
00260
00261 HomCoord&
00262 HomCoord::operator-=(const HomCoord& h)
00263 {
00264 (*m_vector) -= (*h.m_vector);
00265 float w = this->w();
00266 this->w() = (w > 1.0) ? 1.0 : w;
00267 return *this;
00268 }
00269
00270
00271
00272
00273
00274 HomCoord
00275 HomCoord::operator+(const HomCoord& h) const
00276 {
00277 Vector v = (*m_vector) + (*h.m_vector);
00278 HomCoord result(v);
00279 float w = result.w();
00280 result.w() = (w > 1.0) ? 1.0 : w;
00281 return result;
00282 }
00283
00284
00285
00286
00287
00288 HomCoord&
00289 HomCoord::operator+=(const HomCoord& h)
00290 {
00291 (*m_vector) += (*h.m_vector);
00292 float w = this->w();
00293 this->w() = (w > 1.0) ? 1.0 : w;
00294 return *this;
00295 }
00296
00297
00298
00299
00300
00301
00302 HomCoord&
00303 HomCoord::operator=(const HomCoord& h)
00304 {
00305 (*m_vector) = (*h.m_vector);
00306
00307 return *this;
00308 }
00309
00310
00311
00312
00313
00314 float
00315 HomCoord::operator*(const HomCoord& h) const
00316 {
00317 return x() * h.x() + y() * h.y() + z() * h.z();
00318 }
00319
00320
00321
00322
00323
00324
00325 HomCoord
00326 HomCoord::operator*(const float s) const
00327 {
00328 HomCoord result;
00329 result.x() = x() * s;
00330 result.y() = y() * s;
00331 result.z() = z() * s;
00332 result.w() = w();
00333
00334 return result;
00335 }
00336
00337
00338
00339
00340
00341
00342 HomCoord&
00343 HomCoord::operator*=(const float s)
00344 {
00345 x() *= s;
00346 y() *= s;
00347 z() *= s;
00348
00349 return *this;
00350 }
00351
00352
00353
00354
00355
00356 bool
00357 HomCoord::operator==(const HomCoord& h) const
00358 {
00359 return (*m_vector == *h.m_vector) ? true : false;
00360 }
00361
00362
00363
00364
00365
00366 bool
00367 HomCoord::operator!=(const HomCoord& h) const
00368 {
00369 return (*m_vector == *h.m_vector) ? false : true;
00370 }
00371
00372
00373
00374
00375
00376 std::ostream&
00377 HomCoord::print(std::ostream& stream) const
00378 {
00379 return stream << "[" << x() << ", " << y() << ", " << z() << ", " << w() << "]T";
00380 }
00381
00382
00383
00384
00385
00386 HomCoord&
00387 HomCoord::transform(const HomTransform& t)
00388 {
00389 Matrix m = t.get_matrix();
00390 (*m_vector) = m * (*m_vector);
00391
00392 return *this;
00393 }
00394
00395 }
00396