Main Page | Namespace List | Class List | File List | Namespace Members | Class Members

rotmatrix_funcs.h

00001 // rotmatrix_funcs.h (RotMatrix<> template functions) 00002 // 00003 // The WorldForge Project 00004 // Copyright (C) 2001 The WorldForge Project 00005 // 00006 // This program is free software; you can redistribute it and/or modify 00007 // it under the terms of the GNU General Public License as published by 00008 // the Free Software Foundation; either version 2 of the License, or 00009 // (at your option) any later version. 00010 // 00011 // This program is distributed in the hope that it will be useful, 00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 // GNU General Public License for more details. 00015 // 00016 // You should have received a copy of the GNU General Public License 00017 // along with this program; if not, write to the Free Software 00018 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 00019 // 00020 // For information about WorldForge and its authors, please contact 00021 // the Worldforge Web Site at http://www.worldforge.org. 00022 00023 // Author: Ron Steinke 00024 // Created: 2001-12-7 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 // Since the sum of the squares of the elements in any row or column add 00063 // up to 1, all the elements lie between -1 and 1, and each row has 00064 // at least one element whose magnitude is at least 1/sqrt(dim). 00065 // Therefore, we don't need to scale epsilon, as we did for 00066 // Vector<> and Point<>. 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 // Don't need to test m_flip, it's determined by the values of m_elem. 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> // m1 * m2 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); // XOR 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> // m1 * m2^-1 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); // XOR 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> // m1^-1 * m2 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); // XOR 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> // m1^-1 * m2^-1 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); // XOR 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> // m * v 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> // m^-1 * v 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> // v * m 00206 inline Vector<dim> Prod(const Vector<dim>& v, const RotMatrix<dim>& m) 00207 { 00208 return InvProd(m, v); // Since transpose() and inverse() are the same 00209 } 00210 00211 template<const int dim> // v * m^-1 00212 inline Vector<dim> ProdInv(const Vector<dim>& v, const RotMatrix<dim>& m) 00213 { 00214 return Prod(m, v); // Since transpose() and inverse() are the same 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); // Since transpose() and inverse() are the same 00233 } 00234 00235 template<const int dim> 00236 inline bool RotMatrix<dim>::setVals(const CoordType vals[dim][dim], double precision) 00237 { 00238 // Scratch space for the backend 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 // Scratch space for the backend 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 // Cheaper to allocate space on the stack here than with 00267 // new in _MatrixSetValsImpl() 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 // Do the assignment 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; // 1 and 0 are exact, no roundoff error 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 // Get an in-plane vector which is perpendicular to v1 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 // The original vectors were parallel 00405 throw ColinearVectors<dim>(v1, v2); 00406 } 00407 00408 // If we were rotating a vector vin, the answer would be 00409 // vin + Dot(v1, vin) * (v1 (cos(theta) - 1)/ v1_sqr_mag 00410 // + vperp * sin(theta) / sqrt(v1_sqr_mag * vperp_sqr_mag)) 00411 // + Dot(vperp, vin) * (a similar term). From this, we find 00412 // the matrix components. 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(); // Initialize to identity matrix 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 // This is sort of similar to the above, with the rotation angle determined 00437 // by the angle between the vectors 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 // 180 degree rotation, rotation plane indeterminate 00450 if(dim == 2) { // special case, only one rotation plane possible 00451 m_elem[0][0] = m_elem[1][1] = ctheta_plus_1 - 1; 00452 CoordType sin_theta = sqrt(2 * ctheta_plus_1); // to leading order 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 // m_valid and m_age already set correctly 00543 00544 return *this; 00545 } 00546 00547 template<const int dim> 00548 inline RotMatrix<dim>& RotMatrix<dim>::mirror (const Vector<dim>& v) 00549 { 00550 // Get a flip by subtracting twice the projection operator in the 00551 // direction of the vector. A projection operator is idempotent (P*P == P), 00552 // and symmetric, so I - 2P is an orthogonal matrix. 00553 // 00554 // (I - 2P) * (I - 2P)^T == (I - 2P)^2 == I - 4P + 4P^2 == I 00555 00556 CoordType sqr_mag = v.sqrMag(); 00557 00558 assert(("need nonzero length vector", sqr_mag > 0)); 00559 00560 // off diagonal 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 // diagonal 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; // -1 and 0 are exact, no roundoff error 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 // average the matrix with it's inverse transpose, 00597 // that will clean up the error to linear order 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); // matrix can't be degenerate 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 } // namespace WFMath 00623 00624 #endif // WFMATH_ROTMATRIX_FUNCS_H

Generated on Tue Jul 27 21:41:57 2004 for WFMath by doxygen 1.3.7