• Main Page
  • Namespaces
  • Classes
  • Files
  • File List

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 #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   // Since the sum of the squares of the elements in any row or column add
00067   // up to 1, all the elements lie between -1 and 1, and each row has
00068   // at least one element whose magnitude is at least 1/sqrt(dim).
00069   // Therefore, we don't need to scale epsilon, as we did for
00070   // Vector<> and Point<>.
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   // Don't need to test m_flip, it's determined by the values of m_elem.
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> // m1 * m2
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); // XOR
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> // m1 * m2^-1
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); // XOR
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> // m1^-1 * m2
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); // XOR
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> // m1^-1 * m2^-1
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); // XOR
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> // m * v
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> // m^-1 * v
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> // v * m
00210 inline Vector<dim> Prod(const Vector<dim>& v, const RotMatrix<dim>& m)
00211 {
00212   return InvProd(m, v); // Since transpose() and inverse() are the same
00213 }
00214 
00215 template<const int dim> // v * m^-1
00216 inline Vector<dim> ProdInv(const Vector<dim>& v, const RotMatrix<dim>& m)
00217 {
00218   return Prod(m, v); // Since transpose() and inverse() are the same
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); // Since transpose() and inverse() are the same
00237 }
00238 
00239 template<const int dim>
00240 inline bool RotMatrix<dim>::setVals(const CoordType vals[dim][dim], double precision)
00241 {
00242   // Scratch space for the backend
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   // Scratch space for the backend
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   // Cheaper to allocate space on the stack here than with
00271   // new in _MatrixSetValsImpl()
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   // Do the assignment
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; // 1 and 0 are exact, no roundoff error
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   // Get an in-plane vector which is perpendicular to v1
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     // The original vectors were parallel
00409     throw ColinearVectors<dim>(v1, v2);
00410   }
00411 
00412   // If we were rotating a vector vin, the answer would be
00413   // vin + Dot(v1, vin) * (v1 (cos(theta) - 1)/ v1_sqr_mag
00414   // + vperp * sin(theta) / sqrt(v1_sqr_mag * vperp_sqr_mag))
00415   // + Dot(vperp, vin) * (a similar term). From this, we find
00416   // the matrix components.
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(); // Initialize to identity matrix
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   // This is sort of similar to the above, with the rotation angle determined
00441   // by the angle between the vectors
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     // 180 degree rotation, rotation plane indeterminate
00454     if(dim == 2) { // special case, only one rotation plane possible
00455       m_elem[0][0] = m_elem[1][1] = ctheta_plus_1 - 1;
00456       CoordType sin_theta = sqrt(2 * ctheta_plus_1); // to leading order
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   // m_valid and m_age already set correctly
00547 
00548   return *this;
00549 }
00550 
00551 template<const int dim>
00552 inline RotMatrix<dim>& RotMatrix<dim>::mirror   (const Vector<dim>& v)
00553 {
00554   // Get a flip by subtracting twice the projection operator in the
00555   // direction of the vector. A projection operator is idempotent (P*P == P),
00556   // and symmetric, so I - 2P is an orthogonal matrix.
00557   //
00558   // (I - 2P) * (I - 2P)^T == (I - 2P)^2 == I - 4P + 4P^2 == I
00559 
00560   CoordType sqr_mag = v.sqrMag();
00561 
00562   assert("need nonzero length vector" && sqr_mag > 0);
00563 
00564   // off diagonal
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   // diagonal
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; // -1 and 0 are exact, no roundoff error
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   // average the matrix with it's inverse transpose,
00601   // that will clean up the error to linear order
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); // matrix can't be degenerate
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 } // namespace WFMath
00630 
00631 #endif // WFMATH_ROTMATRIX_FUNCS_H

Generated for WFMath by  doxygen 1.7.1