WFMath  1.0.2
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/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