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 #ifndef WFMATH_ROTMATRIX_FUNCS_H
00027 #define WFMATH_ROTMATRIX_FUNCS_H
00028
00029 #include <wfmath/vector.h>
00030 #include <wfmath/rotmatrix.h>
00031 #include <wfmath/error.h>
00032 #include <wfmath/const.h>
00033
00034 namespace WFMath {
00035
00036 template<const int dim>
00037 inline RotMatrix<dim>::RotMatrix(const RotMatrix<dim>& m)
00038 : m_flip(m.m_flip), m_valid(m.m_valid), m_age(1)
00039 {
00040 for(int i = 0; i < dim; ++i)
00041 for(int j = 0; j < dim; ++j)
00042 m_elem[i][j] = m.m_elem[i][j];
00043 }
00044
00045 template<const int dim>
00046 inline RotMatrix<dim>& RotMatrix<dim>::operator=(const RotMatrix<dim>& m)
00047 {
00048 for(int i = 0; i < dim; ++i)
00049 for(int j = 0; j < dim; ++j)
00050 m_elem[i][j] = m.m_elem[i][j];
00051
00052 m_flip = m.m_flip;
00053 m_valid = m.m_valid;
00054 m_age = m.m_age;
00055
00056 return *this;
00057 }
00058
00059 template<const int dim>
00060 inline bool RotMatrix<dim>::isEqualTo(const RotMatrix<dim>& m, double epsilon) const
00061 {
00062
00063
00064
00065
00066
00067
00068 assert(epsilon > 0);
00069
00070 for(int i = 0; i < dim; ++i)
00071 for(int j = 0; j < dim; ++j)
00072 if(fabs(m_elem[i][j] - m.m_elem[i][j]) > epsilon)
00073 return false;
00074
00075
00076
00077 assert("Generated values, must be equal if all components are equal" &&
00078 m_flip == m.m_flip);
00079
00080 return true;
00081 }
00082
00083 template<const int dim>
00084 inline RotMatrix<dim> Prod(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00085 {
00086 RotMatrix<dim> out;
00087
00088 for(int i = 0; i < dim; ++i) {
00089 for(int j = 0; j < dim; ++j) {
00090 out.m_elem[i][j] = 0;
00091 for(int k = 0; k < dim; ++k) {
00092 out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[k][j];
00093 }
00094 }
00095 }
00096
00097 out.m_flip = (m1.m_flip != m2.m_flip);
00098 out.m_valid = m1.m_valid && m2.m_valid;
00099 out.m_age = m1.m_age + m2.m_age;
00100 out.checkNormalization();
00101
00102 return out;
00103 }
00104
00105 template<const int dim>
00106 inline RotMatrix<dim> ProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00107 {
00108 RotMatrix<dim> out;
00109
00110 for(int i = 0; i < dim; ++i) {
00111 for(int j = 0; j < dim; ++j) {
00112 out.m_elem[i][j] = 0;
00113 for(int k = 0; k < dim; ++k) {
00114 out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[j][k];
00115 }
00116 }
00117 }
00118
00119 out.m_flip = (m1.m_flip != m2.m_flip);
00120 out.m_valid = m1.m_valid && m2.m_valid;
00121 out.m_age = m1.m_age + m2.m_age;
00122 out.checkNormalization();
00123
00124 return out;
00125 }
00126
00127 template<const int dim>
00128 inline RotMatrix<dim> InvProd(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00129 {
00130 RotMatrix<dim> out;
00131
00132 for(int i = 0; i < dim; ++i) {
00133 for(int j = 0; j < dim; ++j) {
00134 out.m_elem[i][j] = 0;
00135 for(int k = 0; k < dim; ++k) {
00136 out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[k][j];
00137 }
00138 }
00139 }
00140
00141 out.m_flip = (m1.m_flip != m2.m_flip);
00142 out.m_valid = m1.m_valid && m2.m_valid;
00143 out.m_age = m1.m_age + m2.m_age;
00144 out.checkNormalization();
00145
00146 return out;
00147 }
00148
00149 template<const int dim>
00150 inline RotMatrix<dim> InvProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00151 {
00152 RotMatrix<dim> out;
00153
00154 for(int i = 0; i < dim; ++i) {
00155 for(int j = 0; j < dim; ++j) {
00156 out.m_elem[i][j] = 0;
00157 for(int k = 0; k < dim; ++k) {
00158 out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[j][k];
00159 }
00160 }
00161 }
00162
00163 out.m_flip = (m1.m_flip != m2.m_flip);
00164 out.m_valid = m1.m_valid && m2.m_valid;
00165 out.m_age = m1.m_age + m2.m_age;
00166 out.checkNormalization();
00167
00168 return out;
00169 }
00170
00171 template<const int dim>
00172 inline Vector<dim> Prod(const RotMatrix<dim>& m, const Vector<dim>& v)
00173 {
00174 Vector<dim> out;
00175
00176 for(int i = 0; i < dim; ++i) {
00177 out.m_elem[i] = 0;
00178 for(int j = 0; j < dim; ++j) {
00179 out.m_elem[i] += m.m_elem[i][j] * v.m_elem[j];
00180 }
00181 }
00182
00183 out.m_valid = m.m_valid && v.m_valid;
00184
00185 return out;
00186 }
00187
00188 template<const int dim>
00189 inline Vector<dim> InvProd(const RotMatrix<dim>& m, const Vector<dim>& v)
00190 {
00191 Vector<dim> out;
00192
00193 for(int i = 0; i < dim; ++i) {
00194 out.m_elem[i] = 0;
00195 for(int j = 0; j < dim; ++j) {
00196 out.m_elem[i] += m.m_elem[j][i] * v.m_elem[j];
00197 }
00198 }
00199
00200 out.m_valid = m.m_valid && v.m_valid;
00201
00202 return out;
00203 }
00204
00205 template<const int dim>
00206 inline Vector<dim> Prod(const Vector<dim>& v, const RotMatrix<dim>& m)
00207 {
00208 return InvProd(m, v);
00209 }
00210
00211 template<const int dim>
00212 inline Vector<dim> ProdInv(const Vector<dim>& v, const RotMatrix<dim>& m)
00213 {
00214 return Prod(m, v);
00215 }
00216
00217 template<const int dim>
00218 inline RotMatrix<dim> operator*(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00219 {
00220 return Prod(m1, m2);
00221 }
00222
00223 template<const int dim>
00224 inline Vector<dim> operator*(const RotMatrix<dim>& m, const Vector<dim>& v)
00225 {
00226 return Prod(m, v);
00227 }
00228
00229 template<const int dim>
00230 inline Vector<dim> operator*(const Vector<dim>& v, const RotMatrix<dim>& m)
00231 {
00232 return InvProd(m, v);
00233 }
00234
00235 template<const int dim>
00236 inline bool RotMatrix<dim>::setVals(const CoordType vals[dim][dim], double precision)
00237 {
00238
00239 CoordType scratch_vals[dim*dim];
00240
00241 for(int i = 0; i < dim; ++i)
00242 for(int j = 0; j < dim; ++j)
00243 scratch_vals[i*dim+j] = vals[i][j];
00244
00245 return _setVals(scratch_vals, precision);
00246 }
00247
00248 template<const int dim>
00249 inline bool RotMatrix<dim>::setVals(const CoordType vals[dim*dim], double precision)
00250 {
00251
00252 CoordType scratch_vals[dim*dim];
00253
00254 for(int i = 0; i < dim*dim; ++i)
00255 scratch_vals[i] = vals[i];
00256
00257 return _setVals(scratch_vals, precision);
00258 }
00259
00260 bool _MatrixSetValsImpl(const int size, CoordType* vals, bool& flip,
00261 CoordType* buf1, CoordType* buf2, double precision);
00262
00263 template<const int dim>
00264 inline bool RotMatrix<dim>::_setVals(CoordType *vals, double precision)
00265 {
00266
00267
00268 CoordType buf1[dim*dim], buf2[dim*dim];
00269 bool flip;
00270
00271 if(!_MatrixSetValsImpl(dim, vals, flip, buf1, buf2, precision))
00272 return false;
00273
00274
00275
00276 for(int i = 0; i < dim; ++i)
00277 for(int j = 0; j < dim; ++j)
00278 m_elem[i][j] = vals[i*dim+j];
00279
00280 m_flip = flip;
00281 m_valid = true;
00282 m_age = 1;
00283
00284 return true;
00285 }
00286
00287 template<const int dim>
00288 inline Vector<dim> RotMatrix<dim>::row(const int i) const
00289 {
00290 Vector<dim> out;
00291
00292 for(int j = 0; j < dim; ++j)
00293 out[j] = m_elem[i][j];
00294
00295 out.setValid(m_valid);
00296
00297 return out;
00298 }
00299
00300 template<const int dim>
00301 inline Vector<dim> RotMatrix<dim>::column(const int i) const
00302 {
00303 Vector<dim> out;
00304
00305 for(int j = 0; j < dim; ++j)
00306 out[j] = m_elem[j][i];
00307
00308 out.setValid(m_valid);
00309
00310 return out;
00311 }
00312
00313 template<const int dim>
00314 inline RotMatrix<dim> RotMatrix<dim>::inverse() const
00315 {
00316 RotMatrix<dim> m;
00317
00318 for(int i = 0; i < dim; ++i)
00319 for(int j = 0; j < dim; ++j)
00320 m.m_elem[j][i] = m_elem[i][j];
00321
00322 m.m_flip = m_flip;
00323 m.m_valid = m_valid;
00324 m.m_age = m_age + 1;
00325
00326 return m;
00327 }
00328
00329 template<const int dim>
00330 inline RotMatrix<dim>& RotMatrix<dim>::identity()
00331 {
00332 for(int i = 0; i < dim; ++i)
00333 for(int j = 0; j < dim; ++j)
00334 m_elem[i][j] = (CoordType) ((i == j) ? 1 : 0);
00335
00336 m_flip = false;
00337 m_valid = true;
00338 m_age = 0;
00339
00340 return *this;
00341 }
00342
00343 template<const int dim>
00344 inline CoordType RotMatrix<dim>::trace() const
00345 {
00346 CoordType out = 0;
00347
00348 for(int i = 0; i < dim; ++i)
00349 out += m_elem[i][i];
00350
00351 return out;
00352 }
00353
00354 template<const int dim>
00355 RotMatrix<dim>& RotMatrix<dim>::rotation (const int i, const int j,
00356 CoordType theta)
00357 {
00358 assert(i >= 0 && i < dim && j >= 0 && j < dim && i != j);
00359
00360 CoordType ctheta = cos(theta), stheta = sin(theta);
00361
00362 for(int k = 0; k < dim; ++k) {
00363 for(int l = 0; l < dim; ++l) {
00364 if(k == l) {
00365 if(k == i || k == j)
00366 m_elem[k][l] = ctheta;
00367 else
00368 m_elem[k][l] = 1;
00369 }
00370 else {
00371 if(k == i && l == j)
00372 m_elem[k][l] = stheta;
00373 else if(k == j && l == i)
00374 m_elem[k][l] = -stheta;
00375 else
00376 m_elem[k][l] = 0;
00377 }
00378 }
00379 }
00380
00381 m_flip = false;
00382 m_valid = true;
00383 m_age = 1;
00384
00385 return *this;
00386 }
00387
00388 template<const int dim>
00389 RotMatrix<dim>& RotMatrix<dim>::rotation (const Vector<dim>& v1,
00390 const Vector<dim>& v2,
00391 CoordType theta)
00392 {
00393 CoordType v1_sqr_mag = v1.sqrMag();
00394
00395 assert("need nonzero length vector" && v1_sqr_mag > 0);
00396
00397
00398
00399 Vector<dim> vperp = v2 - v1 * Dot(v1, v2) / v1_sqr_mag;
00400 CoordType vperp_sqr_mag = vperp.sqrMag();
00401
00402 if((vperp_sqr_mag / v1_sqr_mag) < (dim * WFMATH_EPSILON * WFMATH_EPSILON)) {
00403 assert("need nonzero length vector" && v2.sqrMag() > 0);
00404
00405 throw ColinearVectors<dim>(v1, v2);
00406 }
00407
00408
00409
00410
00411
00412
00413
00414 CoordType mag_prod = (CoordType) sqrt(v1_sqr_mag * vperp_sqr_mag);
00415 CoordType ctheta = (CoordType) cos(theta), stheta = (CoordType) sin(theta);
00416
00417 identity();
00418
00419 for(int i = 0; i < dim; ++i)
00420 for(int j = 0; j < dim; ++j)
00421 m_elem[i][j] += ((ctheta - 1) * (v1[i] * v1[j] / v1_sqr_mag
00422 + vperp[i] * vperp[j] / vperp_sqr_mag)
00423 - stheta * (vperp[i] * v1[j] - v1[i] * vperp[j]) / mag_prod);
00424
00425 m_flip = false;
00426 m_valid = true;
00427 m_age = 1;
00428
00429 return *this;
00430 }
00431
00432 template<const int dim>
00433 RotMatrix<dim>& RotMatrix<dim>::rotation(const Vector<dim>& from,
00434 const Vector<dim>& to)
00435 {
00436
00437
00438
00439 CoordType fromSqrMag = from.sqrMag();
00440 assert("need nonzero length vector" && fromSqrMag > 0);
00441 CoordType toSqrMag = to.sqrMag();
00442 assert("need nonzero length vector" && toSqrMag > 0);
00443 CoordType dot = Dot(from, to);
00444 CoordType sqrmagprod = fromSqrMag * toSqrMag;
00445 CoordType magprod = sqrt(sqrmagprod);
00446 CoordType ctheta_plus_1 = dot / magprod + 1;
00447
00448 if(ctheta_plus_1 < WFMATH_EPSILON) {
00449
00450 if(dim == 2) {
00451 m_elem[0][0] = m_elem[1][1] = ctheta_plus_1 - 1;
00452 CoordType sin_theta = sqrt(2 * ctheta_plus_1);
00453 bool direction = ((to[0] * from[1] - to[1] * from[0]) >= 0);
00454 m_elem[0][1] = direction ? sin_theta : -sin_theta;
00455 m_elem[1][0] = -m_elem[0][1];
00456 m_flip = false;
00457 m_valid = true;
00458 m_age = 1;
00459 return *this;
00460 }
00461 throw ColinearVectors<dim>(from, to);
00462 }
00463
00464 for(int i = 0; i < dim; ++i) {
00465 for(int j = i; j < dim; ++j) {
00466 CoordType projfrom = from[i] * from[j] / fromSqrMag;
00467 CoordType projto = to[i] * to[j] / toSqrMag;
00468
00469 CoordType ijprod = from[i] * to[j], jiprod = to[i] * from[j];
00470
00471 CoordType termthree = (ijprod + jiprod) * dot / sqrmagprod;
00472
00473 CoordType combined = (projfrom + projto - termthree) / ctheta_plus_1;
00474
00475 if(i == j) {
00476 m_elem[i][i] = 1 - combined;
00477 }
00478 else {
00479 CoordType diffterm = (jiprod - ijprod) / magprod;
00480
00481 m_elem[i][j] = -diffterm - combined;
00482 m_elem[j][i] = diffterm - combined;
00483 }
00484 }
00485 }
00486
00487 m_flip = false;
00488 m_valid = true;
00489 m_age = 1;
00490
00491 return *this;
00492 }
00493
00494 #ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION
00495 template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis,
00496 CoordType theta);
00497 template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis);
00498 template<> RotMatrix<3>& RotMatrix<3>::fromQuaternion(const Quaternion& q,
00499 const bool not_flip);
00500 #else
00501 void _NCFS_RotMatrix3_rotation (RotMatrix<3>& m, const Vector<3>& axis, CoordType theta);
00502 void _NCFS_RotMatrix3_rotation (RotMatrix<3>& m, const Vector<3>& axis);
00503 void _NCFS_RotMatrix3_fromQuaternion(RotMatrix<3>& m, const Quaternion& q,
00504 const bool not_flip, CoordType m_elem[3][3],
00505 bool& m_flip);
00506
00507 template<>
00508 inline RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis, CoordType theta)
00509 {
00510 _NCFS_RotMatrix3_rotation(*this, axis, theta);
00511 return *this;
00512 }
00513
00514 template<>
00515 inline RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis)
00516 {
00517 _NCFS_RotMatrix3_rotation(*this, axis);
00518 return *this;
00519 }
00520
00521 template<>
00522 inline RotMatrix<3>& RotMatrix<3>::fromQuaternion(const Quaternion& q,
00523 const bool not_flip)
00524 {
00525 _NCFS_RotMatrix3_fromQuaternion(*this, q, not_flip, m_elem, m_flip);
00526 m_valid = true;
00527 return *this;
00528 }
00529
00530 #endif
00531
00532 template<> RotMatrix<3>& RotMatrix<3>::rotate(const Quaternion&);
00533
00534 template<const int dim>
00535 inline RotMatrix<dim>& RotMatrix<dim>::mirror(const int i)
00536 {
00537 assert(i >= 0 && i < dim);
00538
00539 identity();
00540 m_elem[i][i] = -1;
00541 m_flip = true;
00542
00543
00544 return *this;
00545 }
00546
00547 template<const int dim>
00548 inline RotMatrix<dim>& RotMatrix<dim>::mirror (const Vector<dim>& v)
00549 {
00550
00551
00552
00553
00554
00555
00556 CoordType sqr_mag = v.sqrMag();
00557
00558 assert(("need nonzero length vector", sqr_mag > 0));
00559
00560
00561 for(int i = 0; i < dim - 1; ++i)
00562 for(int j = i + 1; j < dim; ++j)
00563 m_elem[i][j] = m_elem[j][i] = -2 * v[i] * v[j] / sqr_mag;
00564
00565
00566 for(int i = 0; i < dim; ++i)
00567 m_elem[i][i] = 1 - 2 * v[i] * v[i] / sqr_mag;
00568
00569 m_flip = true;
00570 m_valid = true;
00571 m_age = 1;
00572
00573 return *this;
00574 }
00575
00576 template<const int dim>
00577 inline RotMatrix<dim>& RotMatrix<dim>::mirror()
00578 {
00579 for(int i = 0; i < dim; ++i)
00580 for(int j = 0; j < dim; ++j)
00581 m_elem[i][j] = (i == j) ? -1 : 0;
00582
00583 m_flip = dim%2;
00584 m_valid = true;
00585 m_age = 0;
00586
00587
00588 return *this;
00589 }
00590
00591 bool _MatrixInverseImpl(const int size, CoordType* in, CoordType* out);
00592
00593 template<const int dim>
00594 inline void RotMatrix<dim>::normalize()
00595 {
00596
00597
00598
00599 CoordType buf1[dim*dim], buf2[dim*dim];
00600
00601 for(int i = 0; i < dim; ++i) {
00602 for(int j = 0; j < dim; ++j) {
00603 buf1[j*dim + i] = m_elem[i][j];
00604 buf2[j*dim + i] = (CoordType)((i == j) ? 1 : 0);
00605 }
00606 }
00607
00608 bool success = _MatrixInverseImpl(dim, buf1, buf2);
00609 assert(success);
00610
00611 for(int i = 0; i < dim; ++i) {
00612 for(int j = 0; j < dim; ++j) {
00613 CoordType& elem = m_elem[i][j];
00614 elem += buf2[i*dim + j];
00615 elem /= 2;
00616 }
00617 }
00618
00619 m_age = 1;
00620 }
00621
00622 }
00623
00624 #endif // WFMATH_ROTMATRIX_FUNCS_H