00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #ifndef FCL_MATRIX_3F_H
00038 #define FCL_MATRIX_3F_H
00039
00040 #include "fcl/math/vec_3f.h"
00041
00042 namespace fcl
00043 {
00044
00046 template<typename T>
00047 class Matrix3fX
00048 {
00049 public:
00050 typedef typename T::meta_type U;
00051 typedef typename T::vector_type S;
00052 T data;
00053
00054 Matrix3fX() {}
00055 Matrix3fX(U xx, U xy, U xz,
00056 U yx, U yy, U yz,
00057 U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz)
00058 {}
00059
00060 Matrix3fX(const Vec3fX<S>& v1, const Vec3fX<S>& v2, const Vec3fX<S>& v3)
00061 : data(v1.data, v2.data, v3.data) {}
00062
00063 Matrix3fX(const Matrix3fX<T>& other) : data(other.data) {}
00064
00065 Matrix3fX(const T& data_) : data(data_) {}
00066
00067 inline Vec3fX<S> getColumn(size_t i) const
00068 {
00069 return Vec3fX<S>(data.getColumn(i));
00070 }
00071
00072 inline Vec3fX<S> getRow(size_t i) const
00073 {
00074 return Vec3fX<S>(data.getRow(i));
00075 }
00076
00077 inline U operator () (size_t i, size_t j) const
00078 {
00079 return data(i, j);
00080 }
00081
00082 inline U& operator () (size_t i, size_t j)
00083 {
00084 return data(i, j);
00085 }
00086
00087 inline Vec3fX<S> operator * (const Vec3fX<S>& v) const
00088 {
00089 return Vec3fX<S>(data * v.data);
00090 }
00091
00092 inline Matrix3fX<T> operator * (const Matrix3fX<T>& m) const
00093 {
00094 return Matrix3fX<T>(data * m.data);
00095 }
00096
00097 inline Matrix3fX<T> operator + (const Matrix3fX<T>& other) const
00098 {
00099 return Matrix3fX<T>(data + other.data);
00100 }
00101
00102 inline Matrix3fX<T> operator - (const Matrix3fX<T>& other) const
00103 {
00104 return Matrix3fX<T>(data - other.data);
00105 }
00106
00107 inline Matrix3fX<T> operator + (U c) const
00108 {
00109 return Matrix3fX<T>(data + c);
00110 }
00111
00112 inline Matrix3fX<T> operator - (U c) const
00113 {
00114 return Matrix3fX<T>(data - c);
00115 }
00116
00117 inline Matrix3fX<T> operator * (U c) const
00118 {
00119 return Matrix3fX<T>(data * c);
00120 }
00121
00122 inline Matrix3fX<T> operator / (U c) const
00123 {
00124 return Matrix3fX<T>(data / c);
00125 }
00126
00127 inline Matrix3fX<T>& operator *= (const Matrix3fX<T>& other)
00128 {
00129 data *= other.data;
00130 return *this;
00131 }
00132
00133 inline Matrix3fX<T>& operator += (const Matrix3fX<T>& other)
00134 {
00135 data += other.data;
00136 return *this;
00137 }
00138
00139 inline Matrix3fX<T>& operator -= (const Matrix3fX<T>& other)
00140 {
00141 data -= other.data;
00142 return *this;
00143 }
00144
00145 inline Matrix3fX<T>& operator += (U c)
00146 {
00147 data += c;
00148 return *this;
00149 }
00150
00151 inline Matrix3fX<T>& operator -= (U c)
00152 {
00153 data -= c;
00154 return *this;
00155 }
00156
00157 inline Matrix3fX<T>& operator *= (U c)
00158 {
00159 data *= c;
00160 return *this;
00161 }
00162
00163 inline Matrix3fX<T>& operator /= (U c)
00164 {
00165 data /= c;
00166 return *this;
00167 }
00168
00169 inline void setIdentity()
00170 {
00171 data.setIdentity();
00172 }
00173
00174 inline void setZero()
00175 {
00176 data.setZero();
00177 }
00178
00179 inline U determinant() const
00180 {
00181 return data.determinant();
00182 }
00183
00184 Matrix3fX<T>& transpose()
00185 {
00186 data.transpose();
00187 return *this;
00188 }
00189
00190 Matrix3fX<T>& inverse()
00191 {
00192 data.inverse();
00193 return *this;
00194 }
00195
00196 Matrix3fX<T>& abs()
00197 {
00198 data.abs();
00199 return *this;
00200 }
00201
00202 static const Matrix3fX<T>& getIdentity()
00203 {
00204 static const Matrix3fX<T> I((U)1, (U)0, (U)0,
00205 (U)0, (U)1, (U)0,
00206 (U)0, (U)0, (U)1);
00207 return I;
00208 }
00209
00210 Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
00211 {
00212 return Matrix3fX<T>(data.transposeTimes(other.data));
00213 }
00214
00215 Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
00216 {
00217 return Matrix3fX<T>(data.timesTranspose(other.data));
00218 }
00219
00220 Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
00221 {
00222 return Vec3fX<S>(data.transposeTimes(v.data));
00223 }
00224
00225 Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
00226 {
00227 Matrix3fX<T> res(*this);
00228 res *= m;
00229 return res.timesTranspose(*this);
00230 }
00231
00232 inline U transposeDotX(const Vec3fX<S>& v) const
00233 {
00234 return data.transposeDot(0, v.data);
00235 }
00236
00237 inline U transposeDotY(const Vec3fX<S>& v) const
00238 {
00239 return data.transposeDot(1, v.data);
00240 }
00241
00242 inline U transposeDotZ(const Vec3fX<S>& v) const
00243 {
00244 return data.transposeDot(2, v.data);
00245 }
00246
00247 inline U transposeDot(size_t i, const Vec3fX<S>& v) const
00248 {
00249 return data.transposeDot(i, v.data);
00250 }
00251
00252 inline U dotX(const Vec3fX<S>& v) const
00253 {
00254 return data.dot(0, v.data);
00255 }
00256
00257 inline U dotY(const Vec3fX<S>& v) const
00258 {
00259 return data.dot(1, v.data);
00260 }
00261
00262 inline U dotZ(const Vec3fX<S>& v) const
00263 {
00264 return data.dot(2, v.data);
00265 }
00266
00267 inline U dot(size_t i, const Vec3fX<S>& v) const
00268 {
00269 return data.dot(i, v.data);
00270 }
00271
00272 inline void setValue(U xx, U xy, U xz,
00273 U yx, U yy, U yz,
00274 U zx, U zy, U zz)
00275 {
00276 data.setValue(xx, xy, xz,
00277 yx, yy, yz,
00278 zx, zy, zz);
00279 }
00280
00281 inline void setValue(U x)
00282 {
00283 data.setValue(x);
00284 }
00285 };
00286
00287 template<typename T>
00288 void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_type>& t1,
00289 const Matrix3fX<T>& R2, const Vec3fX<typename T::vector_type>& t2,
00290 Matrix3fX<T>& R, Vec3fX<typename T::vector_type>& t)
00291 {
00292 R = R1.transposeTimes(R2);
00293 t = R1.transposeTimes(t2 - t1);
00294 }
00295
00297 template<typename T>
00298 void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
00299 {
00300 Matrix3fX<T> R(m);
00301 int n = 3;
00302 int j, iq, ip, i;
00303 typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c;
00304 int nrot;
00305 typename T::meta_type b[3];
00306 typename T::meta_type z[3];
00307 typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
00308 typename T::meta_type d[3];
00309
00310 for(ip = 0; ip < n; ++ip)
00311 {
00312 b[ip] = d[ip] = R(ip, ip);
00313 z[ip] = 0;
00314 }
00315
00316 nrot = 0;
00317
00318 for(i = 0; i < 50; ++i)
00319 {
00320 sm = 0;
00321 for(ip = 0; ip < n; ++ip)
00322 for(iq = ip + 1; iq < n; ++iq)
00323 sm += std::abs(R(ip, iq));
00324 if(sm == 0.0)
00325 {
00326 vout[0].setValue(v[0][0], v[0][1], v[0][2]);
00327 vout[1].setValue(v[1][0], v[1][1], v[1][2]);
00328 vout[2].setValue(v[2][0], v[2][1], v[2][2]);
00329 dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
00330 return;
00331 }
00332
00333 if(i < 3) tresh = 0.2 * sm / (n * n);
00334 else tresh = 0.0;
00335
00336 for(ip = 0; ip < n; ++ip)
00337 {
00338 for(iq= ip + 1; iq < n; ++iq)
00339 {
00340 g = 100.0 * std::abs(R(ip, iq));
00341 if(i > 3 &&
00342 std::abs(d[ip]) + g == std::abs(d[ip]) &&
00343 std::abs(d[iq]) + g == std::abs(d[iq]))
00344 R(ip, iq) = 0.0;
00345 else if(std::abs(R(ip, iq)) > tresh)
00346 {
00347 h = d[iq] - d[ip];
00348 if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h;
00349 else
00350 {
00351 theta = 0.5 * h / (R(ip, iq));
00352 t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta));
00353 if(theta < 0.0) t = -t;
00354 }
00355 c = 1.0 / std::sqrt(1 + t * t);
00356 s = t * c;
00357 tau = s / (1.0 + c);
00358 h = t * R(ip, iq);
00359 z[ip] -= h;
00360 z[iq] += h;
00361 d[ip] -= h;
00362 d[iq] += h;
00363 R(ip, iq) = 0.0;
00364 for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
00365 for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
00366 for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); }
00367 for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
00368 nrot++;
00369 }
00370 }
00371 }
00372 for(ip = 0; ip < n; ++ip)
00373 {
00374 b[ip] += z[ip];
00375 d[ip] = b[ip];
00376 z[ip] = 0.0;
00377 }
00378 }
00379
00380 std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
00381
00382 return;
00383 }
00384
00385 template<typename T>
00386 Matrix3fX<T> abs(const Matrix3fX<T>& R)
00387 {
00388 return Matrix3fX<T>(abs(R.data));
00389 }
00390
00391 template<typename T>
00392 Matrix3fX<T> transpose(const Matrix3fX<T>& R)
00393 {
00394 return Matrix3fX<T>(transpose(R.data));
00395 }
00396
00397 template<typename T>
00398 Matrix3fX<T> inverse(const Matrix3fX<T>& R)
00399 {
00400 return Matrix3fX<T>(inverse(R.data));
00401 }
00402
00403 template<typename T>
00404 typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
00405 {
00406 return v.dot(R * v);
00407 }
00408
00409
00410 typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
00411
00412
00413 static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m)
00414 {
00415 o << "[(" << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << ")("
00416 << m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << ")("
00417 << m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << ")]";
00418 return o;
00419 }
00420
00421
00422
00424 class Variance3f
00425 {
00426 public:
00428 Matrix3f Sigma;
00429
00431 FCL_REAL sigma[3];
00432
00434 Vec3f axis[3];
00435
00436 Variance3f() {}
00437
00438 Variance3f(const Matrix3f& S) : Sigma(S)
00439 {
00440 init();
00441 }
00442
00444 void init()
00445 {
00446 eigen(Sigma, sigma, axis);
00447 }
00448
00450 Variance3f& sqrt()
00451 {
00452 for(std::size_t i = 0; i < 3; ++i)
00453 {
00454 if(sigma[i] < 0) sigma[i] = 0;
00455 sigma[i] = std::sqrt(sigma[i]);
00456 }
00457
00458
00459 Sigma.setZero();
00460 for(std::size_t i = 0; i < 3; ++i)
00461 {
00462 for(std::size_t j = 0; j < 3; ++j)
00463 {
00464 Sigma(i, j) += sigma[0] * axis[0][i] * axis[0][j];
00465 Sigma(i, j) += sigma[1] * axis[1][i] * axis[1][j];
00466 Sigma(i, j) += sigma[2] * axis[2][i] * axis[2][j];
00467 }
00468 }
00469
00470 return *this;
00471 }
00472 };
00473
00474 }
00475
00476
00477
00478 #endif