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