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