WFMath 0.3.11
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<const 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<const 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<const int dim>
00065 inline bool RotMatrix<dim>::isEqualTo(const RotMatrix<dim>& m, double 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(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<const 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<const 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<const 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<const 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<const 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<const 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<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 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<const int dim>
00241 inline 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 inline 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 inline 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   m_age = 1;
00288 
00289   return true;
00290 }
00291 
00292 template<const 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<const 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<const 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<const 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] = (CoordType) ((i == j) ? 1 : 0);
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<const 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<const 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 = cos(theta), stheta = 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<const 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 * WFMATH_EPSILON * WFMATH_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 = (CoordType) sqrt(v1_sqr_mag * vperp_sqr_mag);
00420   CoordType ctheta = (CoordType) cos(theta), stheta = (CoordType) sin(theta);
00421 
00422   identity(); // Initialize to identity matrix
00423 
00424   for(int i = 0; i < dim; ++i)
00425     for(int j = 0; j < dim; ++j)
00426       m_elem[i][j] += ((ctheta - 1) * (v1[i] * v1[j] / v1_sqr_mag
00427                       + vperp[i] * vperp[j] / vperp_sqr_mag)
00428                       - stheta * (vperp[i] * v1[j] - v1[i] * vperp[j]) / mag_prod);
00429 
00430   m_flip = false;
00431   m_valid = true;
00432   m_age = 1;
00433 
00434   return *this;
00435 }
00436 
00437 template<const int dim>
00438 RotMatrix<dim>& RotMatrix<dim>::rotation(const Vector<dim>& from,
00439                                          const Vector<dim>& to)
00440 {
00441   // This is sort of similar to the above, with the rotation angle determined
00442   // by the angle between the vectors
00443 
00444   CoordType fromSqrMag = from.sqrMag();
00445   assert("need nonzero length vector" && fromSqrMag > 0);
00446   CoordType toSqrMag = to.sqrMag();
00447   assert("need nonzero length vector" && toSqrMag > 0);
00448   CoordType dot = Dot(from, to);
00449   CoordType sqrmagprod = fromSqrMag * toSqrMag;
00450   CoordType magprod = sqrt(sqrmagprod);
00451   CoordType ctheta_plus_1 = dot / magprod + 1;
00452 
00453   if(ctheta_plus_1 < WFMATH_EPSILON) {
00454     // 180 degree rotation, rotation plane indeterminate
00455     if(dim == 2) { // special case, only one rotation plane possible
00456       m_elem[0][0] = m_elem[1][1] = ctheta_plus_1 - 1;
00457       CoordType sin_theta = sqrt(2 * ctheta_plus_1); // to leading order
00458       bool direction = ((to[0] * from[1] - to[1] * from[0]) >= 0);
00459       m_elem[0][1] = direction ?  sin_theta : -sin_theta;
00460       m_elem[1][0] = -m_elem[0][1];
00461       m_flip = false;
00462       m_valid = true;
00463       m_age = 1;
00464       return *this;
00465     }
00466     throw ColinearVectors<dim>(from, to);
00467   }
00468 
00469   for(int i = 0; i < dim; ++i) {
00470     for(int j = i; j < dim; ++j) {
00471       CoordType projfrom = from[i] * from[j] / fromSqrMag;
00472       CoordType projto = to[i] * to[j] / toSqrMag;
00473 
00474       CoordType ijprod = from[i] * to[j], jiprod = to[i] * from[j];
00475 
00476       CoordType termthree = (ijprod + jiprod) * dot / sqrmagprod;
00477 
00478       CoordType combined = (projfrom + projto - termthree) / ctheta_plus_1;
00479 
00480       if(i == j) {
00481         m_elem[i][i] = 1 - combined;
00482       }
00483       else {
00484         CoordType diffterm = (jiprod - ijprod) / magprod;
00485 
00486         m_elem[i][j] = -diffterm - combined;
00487         m_elem[j][i] = diffterm - combined;
00488       }
00489     }
00490   }
00491 
00492   m_flip = false;
00493   m_valid = true;
00494   m_age = 1;
00495 
00496   return *this;
00497 }
00498 
00499 #ifndef WFMATH_NO_CLASS_FUNCTION_SPECIALIZATION
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 #else
00506 void _NCFS_RotMatrix3_rotation (RotMatrix<3>& m, const Vector<3>& axis, CoordType theta);
00507 void _NCFS_RotMatrix3_rotation (RotMatrix<3>& m, const Vector<3>& axis);
00508 void _NCFS_RotMatrix3_fromQuaternion(RotMatrix<3>& m, const Quaternion& q,
00509                                      const bool not_flip, CoordType m_elem[3][3],
00510                                      bool& m_flip);
00511 
00512 template<>
00513 inline RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis, CoordType theta)
00514 {
00515   _NCFS_RotMatrix3_rotation(*this, axis, theta);
00516   return *this;
00517 }
00518 
00519 template<>
00520 inline RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis)
00521 {
00522   _NCFS_RotMatrix3_rotation(*this, axis);
00523   return *this;
00524 }
00525 
00526 template<>
00527 inline RotMatrix<3>& RotMatrix<3>::fromQuaternion(const Quaternion& q,
00528                                                   const bool not_flip)
00529 {
00530   _NCFS_RotMatrix3_fromQuaternion(*this, q, not_flip, m_elem, m_flip);
00531   m_valid = true;
00532   return *this;
00533 }
00534 
00535 #endif
00536 
00537 template<> RotMatrix<3>& RotMatrix<3>::rotate(const Quaternion&);
00538 
00539 template<const int dim>
00540 inline RotMatrix<dim>& RotMatrix<dim>::mirror(const int i)
00541 {
00542   assert(i >= 0 && i < dim);
00543 
00544   identity();
00545   m_elem[i][i] = -1;
00546   m_flip = true;
00547   // m_valid and m_age already set correctly
00548 
00549   return *this;
00550 }
00551 
00552 template<const int dim>
00553 inline RotMatrix<dim>& RotMatrix<dim>::mirror   (const Vector<dim>& v)
00554 {
00555   // Get a flip by subtracting twice the projection operator in the
00556   // direction of the vector. A projection operator is idempotent (P*P == P),
00557   // and symmetric, so I - 2P is an orthogonal matrix.
00558   //
00559   // (I - 2P) * (I - 2P)^T == (I - 2P)^2 == I - 4P + 4P^2 == I
00560 
00561   CoordType sqr_mag = v.sqrMag();
00562 
00563   assert("need nonzero length vector" && sqr_mag > 0);
00564 
00565   // off diagonal
00566   for(int i = 0; i < dim - 1; ++i)
00567     for(int j = i + 1; j < dim; ++j)
00568       m_elem[i][j] = m_elem[j][i] = -2 * v[i] * v[j] / sqr_mag;
00569 
00570   // diagonal
00571   for(int i = 0; i < dim; ++i)
00572     m_elem[i][i] = 1 - 2 * v[i] * v[i] / sqr_mag;
00573 
00574   m_flip = true;
00575   m_valid = true;
00576   m_age = 1;
00577 
00578   return *this;
00579 }
00580 
00581 template<const int dim>
00582 inline RotMatrix<dim>& RotMatrix<dim>::mirror()
00583 {
00584   for(int i = 0; i < dim; ++i)
00585     for(int j = 0; j < dim; ++j)
00586       m_elem[i][j] = (i == j) ? -1 : 0;
00587 
00588   m_flip = dim%2;
00589   m_valid = true;
00590   m_age = 0; // -1 and 0 are exact, no roundoff error
00591 
00592 
00593   return *this;
00594 }
00595 
00596 bool _MatrixInverseImpl(const int size, CoordType* in, CoordType* out);
00597 
00598 template<const int dim>
00599 inline void RotMatrix<dim>::normalize()
00600 {
00601   // average the matrix with it's inverse transpose,
00602   // that will clean up the error to linear order
00603 
00604   CoordType buf1[dim*dim], buf2[dim*dim]; 
00605 
00606   for(int i = 0; i < dim; ++i) {
00607     for(int j = 0; j < dim; ++j) {
00608       buf1[j*dim + i] = m_elem[i][j];
00609       buf2[j*dim + i] = (CoordType)((i == j) ? 1 : 0);
00610     }
00611   }
00612 
00613   bool success = _MatrixInverseImpl(dim, buf1, buf2);
00614   assert(success); // matrix can't be degenerate
00615   if (!success) {
00616     return;
00617   }
00618 
00619   for(int i = 0; i < dim; ++i) {
00620     for(int j = 0; j < dim; ++j) {
00621       CoordType& elem = m_elem[i][j];
00622       elem += buf2[i*dim + j];
00623       elem /= 2;
00624     }
00625   }
00626 
00627   m_age = 1;
00628 }
00629 
00630 } // namespace WFMath
00631 
00632 #endif // WFMATH_ROTMATRIX_FUNCS_H