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   if (!success) {
00611     return;
00612   }
00613 
00614   for(int i = 0; i < dim; ++i) {
00615     for(int j = 0; j < dim; ++j) {
00616       CoordType& elem = m_elem[i][j];
00617       elem += buf2[i*dim + j];
00618       elem /= 2;
00619     }
00620   }
00621 
00622   m_age = 1;
00623 }
00624 
00625 } // namespace WFMath
00626 
00627 #endif // WFMATH_ROTMATRIX_FUNCS_H

Generated on Tue Dec 11 06:29:45 2007 for WFMath by  doxygen 1.5.4