Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound 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)
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 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 
00055   return *this;
00056 }
00057 
00058 template<const int dim>
00059 bool RotMatrix<dim>::isEqualTo(const RotMatrix<dim>& m, double epsilon) const
00060 {
00061   // Since the sum of the squares of the elements in any row or column add
00062   // up to 1, all the elements lie between -1 and 1, and each row has
00063   // at least one element whose magnitude is at least 1/sqrt(dim).
00064   // Therefore, we don't need to scale epsilon, as we did for
00065   // Vector<> and Point<>.
00066 
00067   assert(epsilon > 0);
00068 
00069   for(int i = 0; i < dim; ++i)
00070     for(int j = 0; j < dim; ++j)
00071       if(fabs(m_elem[i][j] - m.m_elem[i][j]) > epsilon)
00072         return false;
00073 
00074   // Don't need to test m_flip, it's determined by the values of m_elem.
00075 
00076   assert("Generated values, must be equal if all components are equal" &&
00077          m_flip == m.m_flip);
00078 
00079   return true;
00080 }
00081 
00082 template<const int dim>
00083 bool RotMatrix<dim>::operator< (const RotMatrix<dim>& m) const
00084 {
00085   if(operator==(m))
00086     return false;
00087 
00088   for(int i = 0; i < dim; ++i)
00089     for(int j = 0; j < dim; ++j)
00090       if(m_elem[i][j] != m.m_elem[i][j])
00091         return m_elem[i][j] < m.m_elem[i][j];
00092 
00093   assert(false);
00094 }
00095 
00096 template<const int dim> // m1 * m2
00097 RotMatrix<dim> Prod(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00098 {
00099   RotMatrix<dim> out;
00100 
00101   for(int i = 0; i < dim; ++i) {
00102     for(int j = 0; j < dim; ++j) {
00103       out.m_elem[i][j] = 0;
00104       for(int k = 0; k < dim; ++k) {
00105         out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[k][j];
00106       }
00107     }
00108   }
00109 
00110   out.m_flip = (m1.m_flip != m2.m_flip); // XOR
00111   out.m_valid = m1.m_valid && m2.m_valid;
00112 
00113   return out;
00114 }
00115 
00116 template<const int dim> // m1 * m2^-1
00117 RotMatrix<dim> ProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00118 {
00119   RotMatrix<dim> out;
00120 
00121   for(int i = 0; i < dim; ++i) {
00122     for(int j = 0; j < dim; ++j) {
00123       out.m_elem[i][j] = 0;
00124       for(int k = 0; k < dim; ++k) {
00125         out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[j][k];
00126       }
00127     }
00128   }
00129 
00130   out.m_flip = (m1.m_flip != m2.m_flip); // XOR
00131   out.m_valid = m1.m_valid && m2.m_valid;
00132 
00133   return out;
00134 }
00135 
00136 template<const int dim> // m1^-1 * m2
00137 RotMatrix<dim> InvProd(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00138 {
00139   RotMatrix<dim> out;
00140 
00141   for(int i = 0; i < dim; ++i) {
00142     for(int j = 0; j < dim; ++j) {
00143       out.m_elem[i][j] = 0;
00144       for(int k = 0; k < dim; ++k) {
00145         out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[k][j];
00146       }
00147     }
00148   }
00149 
00150   out.m_flip = (m1.m_flip != m2.m_flip); // XOR
00151   out.m_valid = m1.m_valid && m2.m_valid;
00152 
00153   return out;
00154 }
00155 
00156 template<const int dim> // m1^-1 * m2^-1
00157 RotMatrix<dim> InvProdInv(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
00158 {
00159   RotMatrix<dim> out;
00160 
00161   for(int i = 0; i < dim; ++i) {
00162     for(int j = 0; j < dim; ++j) {
00163       out.m_elem[i][j] = 0;
00164       for(int k = 0; k < dim; ++k) {
00165         out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[j][k];
00166       }
00167     }
00168   }
00169 
00170   out.m_flip = (m1.m_flip != m2.m_flip); // XOR
00171   out.m_valid = m1.m_valid && m2.m_valid;
00172 
00173   return out;
00174 }
00175 
00176 template<const int dim> // m * v
00177 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<const int dim> // m^-1 * v
00194 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<const 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<const 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<const 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<const 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<const int dim>
00235 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<const int dim>
00241 bool RotMatrix<dim>::setVals(const CoordType vals[dim][dim], double 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<const int dim>
00254 bool RotMatrix<dim>::setVals(const CoordType vals[dim*dim], double 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, double precision);
00267 
00268 template<const int dim>
00269 bool RotMatrix<dim>::_setVals(CoordType *vals, double 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 
00288   return true;
00289 }
00290 
00291 template<const int dim>
00292 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 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 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_valid = m_valid;
00327 
00328   return m;
00329 }
00330 
00331 template<const int dim>
00332 RotMatrix<dim>& RotMatrix<dim>::identity()
00333 {
00334   for(int i = 0; i < dim; ++i)
00335     for(int j = 0; j < dim; ++j)
00336       m_elem[i][j] = (CoordType) ((i == j) ? 1 : 0);
00337 
00338   m_flip = false;
00339   m_valid = true;
00340 
00341   return *this;
00342 }
00343 
00344 template<const int dim>
00345 CoordType RotMatrix<dim>::trace() const
00346 {
00347   CoordType out = dim ? m_elem[0][0] : 0;
00348 
00349   for(int i = 1; i < dim; ++i)
00350     out += m_elem[i][i];
00351 
00352   return out;
00353 }
00354 
00355 template<const int dim>
00356 RotMatrix<dim>& RotMatrix<dim>::rotation (const int i, const int j,
00357                                           CoordType theta)
00358 {
00359   assert(i >= 0 && i < dim && j >= 0 && j < dim && i != j);
00360 
00361   CoordType ctheta = cos(theta), stheta = sin(theta);
00362 
00363   for(int k = 0; k < dim; ++k) {
00364     for(int l = 0; l < dim; ++l) {
00365       if(k == l) {
00366         if(k == i || k == j)
00367           m_elem[k][l] = ctheta;
00368         else
00369           m_elem[k][l] = 1;
00370       }
00371       else {
00372         if(k == i && l == j)
00373           m_elem[k][l] = stheta;
00374         else if(k == j && l == i)
00375           m_elem[k][l] = -stheta;
00376         else
00377           m_elem[k][l] = 0;
00378       }
00379     }
00380   }
00381 
00382   m_flip = false;
00383   m_valid = true;
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 
00428   return *this;
00429 }
00430 
00431 template<const int dim>
00432 RotMatrix<dim>& RotMatrix<dim>::rotation(const Vector<dim>& from,
00433                                          const Vector<dim>& to)
00434 {
00435   // This is sort of similar to the above, with the rotation angle determined
00436   // by the angle between the vectors
00437 
00438   CoordType fromSqrMag = from.sqrMag();
00439   assert(("need nonzero length vector", fromSqrMag > 0));
00440   CoordType toSqrMag = to.sqrMag();
00441   assert(("need nonzero length vector", toSqrMag > 0));
00442   CoordType dot = Dot(from, to);
00443   CoordType sqrmagprod = fromSqrMag * toSqrMag;
00444   CoordType magprod = sqrt(sqrmagprod);
00445   CoordType ctheta_plus_1 = dot / magprod + 1;
00446 
00447   if(ctheta_plus_1 < WFMATH_EPSILON) {
00448     // 180 degree rotation, rotation plane indeterminate
00449     throw ColinearVectors<dim>(from, to);
00450   }
00451 
00452   for(int i = 0; i < dim; ++i) {
00453     for(int j = i; j < dim; ++j) {
00454       CoordType projfrom = from[i] * from[j] / fromSqrMag;
00455       CoordType projto = to[i] * to[j] / toSqrMag;
00456 
00457       CoordType ijprod = from[i] * to[j], jiprod = to[i] * from[j];
00458 
00459       CoordType termthree = (ijprod + jiprod) * dot / sqrmagprod;
00460 
00461       CoordType combined = (projfrom + projto - termthree) / ctheta_plus_1;
00462 
00463       if(i == j) {
00464         m_elem[i][i] = 1 - combined;
00465       }
00466       else {
00467         diffterm = (jiprod - ijprod) / magprod;
00468 
00469         m_elem[i][j] = -diffterm - combined;
00470         m_elem[j][i] = diffterm - combined;
00471       }
00472     }
00473   }
00474 
00475   m_valid = true;
00476   return *this;
00477 }
00478 
00479 #ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION
00480 template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis,
00481                                                  CoordType theta);
00482 template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis);
00483 template<> RotMatrix<3>& RotMatrix<3>::fromQuaternion(const Quaternion& q,
00484                                                       const bool not_flip);
00485 #else
00486 void _NCFS_RotMatrix3_rotation (RotMatrix<3>& m, const Vector<3>& axis, CoordType theta);
00487 void _NCFS_RotMatrix3_rotation (RotMatrix<3>& m, const Vector<3>& axis);
00488 void _NCFS_RotMatrix3_fromQuaternion(RotMatrix<3>& m, const Quaternion& q,
00489                                      const bool not_flip, CoordType m_elem[3][3],
00490                                      bool& m_flip);
00491 
00492 template<>
00493 inline RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis, CoordType theta)
00494 {
00495   _NCFS_RotMatrix3_rotation(*this, axis, theta);
00496   return *this;
00497 }
00498 
00499 template<>
00500 inline RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis)
00501 {
00502   _NCFS_RotMatrix3_rotation(*this, axis);
00503   return *this;
00504 }
00505 
00506 template<>
00507 inline RotMatrix<3>& RotMatrix<3>::fromQuaternion(const Quaternion& q,
00508                                                   const bool not_flip)
00509 {
00510   _NCFS_RotMatrix3_fromQuaternion(*this, q, not_flip, m_elem, m_flip);
00511   m_valid = true;
00512   return *this;
00513 }
00514 
00515 #endif
00516 
00517 template<const int dim>
00518 RotMatrix<dim>& RotMatrix<dim>::mirror(const int i)
00519 {
00520   assert(i >= 0 && i < dim);
00521 
00522   identity();
00523   m_elem[i][i] = -1;
00524   m_flip = true;
00525   m_valid = true;
00526 
00527   return *this;
00528 }
00529 
00530 template<const int dim>
00531 RotMatrix<dim>& RotMatrix<dim>::mirror  (const Vector<dim>& v)
00532 {
00533   // Get a flip by subtracting twice the projection operator in the
00534   // direction of the vector. A projection operator is idempotent (P*P == P),
00535   // and symmetric, so I - 2P is an orthogonal matrix.
00536   //
00537   // (I - 2P) * (I - 2P)^T == (I - 2P)^2 == I - 4P + 4P^2 == I
00538 
00539   CoordType sqr_mag = v.sqrMag();
00540 
00541   assert(("need nonzero length vector", sqr_mag > 0));
00542 
00543   // off diagonal
00544   for(int i = 0; i < dim - 1; ++i)
00545     for(int j = i + 1; j < dim; ++j)
00546       m_elem[i][j] = m_elem[j][i] = -2 * v[i] * v[j] / sqr_mag;
00547 
00548   // diagonal
00549   for(int i = 0; i < dim; ++i)
00550     m_elem[i][i] = 1 - 2 * v[i] * v[i] / sqr_mag;
00551 
00552   m_flip = true;
00553   m_valid = true;
00554 
00555   return *this;
00556 }
00557 
00558 template<const int dim>
00559 RotMatrix<dim>& RotMatrix<dim>::mirror()
00560 {
00561   for(int i = 0; i < dim; ++i)
00562     for(int j = 0; j < dim; ++j)
00563       m_elem[i][j] = (i == j) ? -1 : 0;
00564 
00565   m_flip = dim%2;
00566   m_valid = true;
00567 
00568   return *this;
00569 }
00570 
00571 } // namespace WFMath
00572 
00573 #endif // WFMATH_ROTMATRIX_FUNCS_H

Generated on Wed May 28 09:20:32 2003 for WFMath by doxygen1.3-rc3