WFMath  1.0.2
rotmatrix_funcs.h
1 // rotmatrix_funcs.h (RotMatrix<> template functions)
2 //
3 // The WorldForge Project
4 // Copyright (C) 2001 The WorldForge Project
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 //
20 // For information about WorldForge and its authors, please contact
21 // the Worldforge Web Site at http://www.worldforge.org.
22 
23 // Author: Ron Steinke
24 // Created: 2001-12-7
25 
26 #ifndef WFMATH_ROTMATRIX_FUNCS_H
27 #define WFMATH_ROTMATRIX_FUNCS_H
28 
29 #include <wfmath/rotmatrix.h>
30 
31 #include <wfmath/vector.h>
32 #include <wfmath/error.h>
33 #include <wfmath/const.h>
34 
35 #include <cmath>
36 
37 #include <cassert>
38 
39 namespace WFMath {
40 
41 template<int dim>
42 inline RotMatrix<dim>::RotMatrix(const RotMatrix<dim>& m)
43  : m_flip(m.m_flip), m_valid(m.m_valid), m_age(1)
44 {
45  for(int i = 0; i < dim; ++i)
46  for(int j = 0; j < dim; ++j)
47  m_elem[i][j] = m.m_elem[i][j];
48 }
49 
50 template<int dim>
51 inline RotMatrix<dim>& RotMatrix<dim>::operator=(const RotMatrix<dim>& m)
52 {
53  for(int i = 0; i < dim; ++i)
54  for(int j = 0; j < dim; ++j)
55  m_elem[i][j] = m.m_elem[i][j];
56 
57  m_flip = m.m_flip;
58  m_valid = m.m_valid;
59  m_age = m.m_age;
60 
61  return *this;
62 }
63 
64 template<int dim>
65 inline bool RotMatrix<dim>::isEqualTo(const RotMatrix<dim>& m, CoordType epsilon) const
66 {
67  // Since the sum of the squares of the elements in any row or column add
68  // up to 1, all the elements lie between -1 and 1, and each row has
69  // at least one element whose magnitude is at least 1/sqrt(dim).
70  // Therefore, we don't need to scale epsilon, as we did for
71  // Vector<> and Point<>.
72 
73  assert(epsilon > 0);
74 
75  for(int i = 0; i < dim; ++i)
76  for(int j = 0; j < dim; ++j)
77  if(std::fabs(m_elem[i][j] - m.m_elem[i][j]) > epsilon)
78  return false;
79 
80  // Don't need to test m_flip, it's determined by the values of m_elem.
81 
82  assert("Generated values, must be equal if all components are equal" &&
83  m_flip == m.m_flip);
84 
85  return true;
86 }
87 
88 template<int dim> // m1 * m2
89 inline RotMatrix<dim> Prod(const RotMatrix<dim>& m1, const RotMatrix<dim>& m2)
90 {
91  RotMatrix<dim> out;
92 
93  for(int i = 0; i < dim; ++i) {
94  for(int j = 0; j < dim; ++j) {
95  out.m_elem[i][j] = 0;
96  for(int k = 0; k < dim; ++k) {
97  out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[k][j];
98  }
99  }
100  }
101 
102  out.m_flip = (m1.m_flip != m2.m_flip); // XOR
103  out.m_valid = m1.m_valid && m2.m_valid;
104  out.m_age = m1.m_age + m2.m_age;
105  out.checkNormalization();
106 
107  return out;
108 }
109 
110 template<int dim> // m1 * m2^-1
112 {
113  RotMatrix<dim> out;
114 
115  for(int i = 0; i < dim; ++i) {
116  for(int j = 0; j < dim; ++j) {
117  out.m_elem[i][j] = 0;
118  for(int k = 0; k < dim; ++k) {
119  out.m_elem[i][j] += m1.m_elem[i][k] * m2.m_elem[j][k];
120  }
121  }
122  }
123 
124  out.m_flip = (m1.m_flip != m2.m_flip); // XOR
125  out.m_valid = m1.m_valid && m2.m_valid;
126  out.m_age = m1.m_age + m2.m_age;
127  out.checkNormalization();
128 
129  return out;
130 }
131 
132 template<int dim> // m1^-1 * m2
134 {
135  RotMatrix<dim> out;
136 
137  for(int i = 0; i < dim; ++i) {
138  for(int j = 0; j < dim; ++j) {
139  out.m_elem[i][j] = 0;
140  for(int k = 0; k < dim; ++k) {
141  out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[k][j];
142  }
143  }
144  }
145 
146  out.m_flip = (m1.m_flip != m2.m_flip); // XOR
147  out.m_valid = m1.m_valid && m2.m_valid;
148  out.m_age = m1.m_age + m2.m_age;
149  out.checkNormalization();
150 
151  return out;
152 }
153 
154 template<int dim> // m1^-1 * m2^-1
156 {
157  RotMatrix<dim> out;
158 
159  for(int i = 0; i < dim; ++i) {
160  for(int j = 0; j < dim; ++j) {
161  out.m_elem[i][j] = 0;
162  for(int k = 0; k < dim; ++k) {
163  out.m_elem[i][j] += m1.m_elem[k][i] * m2.m_elem[j][k];
164  }
165  }
166  }
167 
168  out.m_flip = (m1.m_flip != m2.m_flip); // XOR
169  out.m_valid = m1.m_valid && m2.m_valid;
170  out.m_age = m1.m_age + m2.m_age;
171  out.checkNormalization();
172 
173  return out;
174 }
175 
176 template<int dim> // m * v
177 inline Vector<dim> Prod(const RotMatrix<dim>& m, const Vector<dim>& v)
178 {
179  Vector<dim> out;
180 
181  for(int i = 0; i < dim; ++i) {
182  out.m_elem[i] = 0;
183  for(int j = 0; j < dim; ++j) {
184  out.m_elem[i] += m.m_elem[i][j] * v.m_elem[j];
185  }
186  }
187 
188  out.m_valid = m.m_valid && v.m_valid;
189 
190  return out;
191 }
192 
193 template<int dim> // m^-1 * v
194 inline Vector<dim> InvProd(const RotMatrix<dim>& m, const Vector<dim>& v)
195 {
196  Vector<dim> out;
197 
198  for(int i = 0; i < dim; ++i) {
199  out.m_elem[i] = 0;
200  for(int j = 0; j < dim; ++j) {
201  out.m_elem[i] += m.m_elem[j][i] * v.m_elem[j];
202  }
203  }
204 
205  out.m_valid = m.m_valid && v.m_valid;
206 
207  return out;
208 }
209 
210 template<int dim> // v * m
211 inline Vector<dim> Prod(const Vector<dim>& v, const RotMatrix<dim>& m)
212 {
213  return InvProd(m, v); // Since transpose() and inverse() are the same
214 }
215 
216 template<int dim> // v * m^-1
217 inline Vector<dim> ProdInv(const Vector<dim>& v, const RotMatrix<dim>& m)
218 {
219  return Prod(m, v); // Since transpose() and inverse() are the same
220 }
221 
222 template<int dim>
224 {
225  return Prod(m1, m2);
226 }
227 
228 template<int dim>
229 inline Vector<dim> operator*(const RotMatrix<dim>& m, const Vector<dim>& v)
230 {
231  return Prod(m, v);
232 }
233 
234 template<int dim>
235 inline Vector<dim> operator*(const Vector<dim>& v, const RotMatrix<dim>& m)
236 {
237  return InvProd(m, v); // Since transpose() and inverse() are the same
238 }
239 
240 template<int dim>
241 inline bool RotMatrix<dim>::setVals(const CoordType vals[dim][dim], CoordType precision)
242 {
243  // Scratch space for the backend
244  CoordType scratch_vals[dim*dim];
245 
246  for(int i = 0; i < dim; ++i)
247  for(int j = 0; j < dim; ++j)
248  scratch_vals[i*dim+j] = vals[i][j];
249 
250  return _setVals(scratch_vals, precision);
251 }
252 
253 template<int dim>
254 inline bool RotMatrix<dim>::setVals(const CoordType vals[dim*dim], CoordType precision)
255 {
256  // Scratch space for the backend
257  CoordType scratch_vals[dim*dim];
258 
259  for(int i = 0; i < dim*dim; ++i)
260  scratch_vals[i] = vals[i];
261 
262  return _setVals(scratch_vals, precision);
263 }
264 
265 bool _MatrixSetValsImpl(const int size, CoordType* vals, bool& flip,
266  CoordType* buf1, CoordType* buf2, CoordType precision);
267 
268 template<int dim>
269 inline bool RotMatrix<dim>::_setVals(CoordType *vals, CoordType precision)
270 {
271  // Cheaper to allocate space on the stack here than with
272  // new in _MatrixSetValsImpl()
273  CoordType buf1[dim*dim], buf2[dim*dim];
274  bool flip;
275 
276  if(!_MatrixSetValsImpl(dim, vals, flip, buf1, buf2, precision))
277  return false;
278 
279  // Do the assignment
280 
281  for(int i = 0; i < dim; ++i)
282  for(int j = 0; j < dim; ++j)
283  m_elem[i][j] = vals[i*dim+j];
284 
285  m_flip = flip;
286  m_valid = true;
287  m_age = 1;
288 
289  return true;
290 }
291 
292 template<int dim>
293 inline Vector<dim> RotMatrix<dim>::row(const int i) const
294 {
295  Vector<dim> out;
296 
297  for(int j = 0; j < dim; ++j)
298  out[j] = m_elem[i][j];
299 
300  out.setValid(m_valid);
301 
302  return out;
303 }
304 
305 template<int dim>
306 inline Vector<dim> RotMatrix<dim>::column(const int i) const
307 {
308  Vector<dim> out;
309 
310  for(int j = 0; j < dim; ++j)
311  out[j] = m_elem[j][i];
312 
313  out.setValid(m_valid);
314 
315  return out;
316 }
317 
318 template<int dim>
320 {
321  RotMatrix<dim> m;
322 
323  for(int i = 0; i < dim; ++i)
324  for(int j = 0; j < dim; ++j)
325  m.m_elem[j][i] = m_elem[i][j];
326 
327  m.m_flip = m_flip;
328  m.m_valid = m_valid;
329  m.m_age = m_age + 1;
330 
331  return m;
332 }
333 
334 template<int dim>
336 {
337  for(int i = 0; i < dim; ++i)
338  for(int j = 0; j < dim; ++j)
339  m_elem[i][j] = ((i == j) ? 1.0f : 0.0f);
340 
341  m_flip = false;
342  m_valid = true;
343  m_age = 0; // 1 and 0 are exact, no roundoff error
344 
345  return *this;
346 }
347 
348 template<int dim>
350 {
351  CoordType out = 0;
352 
353  for(int i = 0; i < dim; ++i)
354  out += m_elem[i][i];
355 
356  return out;
357 }
358 
359 template<int dim>
360 RotMatrix<dim>& RotMatrix<dim>::rotation (const int i, const int j,
361  CoordType theta)
362 {
363  assert(i >= 0 && i < dim && j >= 0 && j < dim && i != j);
364 
365  CoordType ctheta = std::cos(theta), stheta = std::sin(theta);
366 
367  for(int k = 0; k < dim; ++k) {
368  for(int l = 0; l < dim; ++l) {
369  if(k == l) {
370  if(k == i || k == j)
371  m_elem[k][l] = ctheta;
372  else
373  m_elem[k][l] = 1;
374  }
375  else {
376  if(k == i && l == j)
377  m_elem[k][l] = stheta;
378  else if(k == j && l == i)
379  m_elem[k][l] = -stheta;
380  else
381  m_elem[k][l] = 0;
382  }
383  }
384  }
385 
386  m_flip = false;
387  m_valid = true;
388  m_age = 1;
389 
390  return *this;
391 }
392 
393 template<int dim>
395  const Vector<dim>& v2,
396  CoordType theta)
397 {
398  CoordType v1_sqr_mag = v1.sqrMag();
399 
400  assert("need nonzero length vector" && v1_sqr_mag > 0);
401 
402  // Get an in-plane vector which is perpendicular to v1
403 
404  Vector<dim> vperp = v2 - v1 * Dot(v1, v2) / v1_sqr_mag;
405  CoordType vperp_sqr_mag = vperp.sqrMag();
406 
407  if((vperp_sqr_mag / v1_sqr_mag) < (dim * numeric_constants<CoordType>::epsilon() * numeric_constants<CoordType>::epsilon())) {
408  assert("need nonzero length vector" && v2.sqrMag() > 0);
409  // The original vectors were parallel
410  throw ColinearVectors<dim>(v1, v2);
411  }
412 
413  // If we were rotating a vector vin, the answer would be
414  // vin + Dot(v1, vin) * (v1 (cos(theta) - 1)/ v1_sqr_mag
415  // + vperp * sin(theta) / sqrt(v1_sqr_mag * vperp_sqr_mag))
416  // + Dot(vperp, vin) * (a similar term). From this, we find
417  // the matrix components.
418 
419  CoordType mag_prod = std::sqrt(v1_sqr_mag * vperp_sqr_mag);
420  CoordType ctheta = std::cos(theta),
421  stheta = std::sin(theta);
422 
423  identity(); // Initialize to identity matrix
424 
425  for(int i = 0; i < dim; ++i)
426  for(int j = 0; j < dim; ++j)
427  m_elem[i][j] += ((ctheta - 1) * (v1[i] * v1[j] / v1_sqr_mag
428  + vperp[i] * vperp[j] / vperp_sqr_mag)
429  - stheta * (vperp[i] * v1[j] - v1[i] * vperp[j]) / mag_prod);
430 
431  m_flip = false;
432  m_valid = true;
433  m_age = 1;
434 
435  return *this;
436 }
437 
438 template<int dim>
440  const Vector<dim>& to)
441 {
442  // This is sort of similar to the above, with the rotation angle determined
443  // by the angle between the vectors
444 
445  CoordType fromSqrMag = from.sqrMag();
446  assert("need nonzero length vector" && fromSqrMag > 0);
447  CoordType toSqrMag = to.sqrMag();
448  assert("need nonzero length vector" && toSqrMag > 0);
449  CoordType dot = Dot(from, to);
450  CoordType sqrmagprod = fromSqrMag * toSqrMag;
451  CoordType magprod = std::sqrt(sqrmagprod);
452  CoordType ctheta_plus_1 = dot / magprod + 1;
453 
454  if(ctheta_plus_1 < numeric_constants<CoordType>::epsilon()) {
455  // 180 degree rotation, rotation plane indeterminate
456  if(dim == 2) { // special case, only one rotation plane possible
457  m_elem[0][0] = m_elem[1][1] = ctheta_plus_1 - 1;
458  CoordType sin_theta = std::sqrt(2 * ctheta_plus_1); // to leading order
459  bool direction = ((to[0] * from[1] - to[1] * from[0]) >= 0);
460  m_elem[0][1] = direction ? sin_theta : -sin_theta;
461  m_elem[1][0] = -m_elem[0][1];
462  m_flip = false;
463  m_valid = true;
464  m_age = 1;
465  return *this;
466  }
467  throw ColinearVectors<dim>(from, to);
468  }
469 
470  for(int i = 0; i < dim; ++i) {
471  for(int j = i; j < dim; ++j) {
472  CoordType projfrom = from[i] * from[j] / fromSqrMag;
473  CoordType projto = to[i] * to[j] / toSqrMag;
474 
475  CoordType ijprod = from[i] * to[j], jiprod = to[i] * from[j];
476 
477  CoordType termthree = (ijprod + jiprod) * dot / sqrmagprod;
478 
479  CoordType combined = (projfrom + projto - termthree) / ctheta_plus_1;
480 
481  if(i == j) {
482  m_elem[i][i] = 1 - combined;
483  }
484  else {
485  CoordType diffterm = (jiprod - ijprod) / magprod;
486 
487  m_elem[i][j] = -diffterm - combined;
488  m_elem[j][i] = diffterm - combined;
489  }
490  }
491  }
492 
493  m_flip = false;
494  m_valid = true;
495  m_age = 1;
496 
497  return *this;
498 }
499 
500 template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis,
501  CoordType theta);
502 template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis);
504  const bool not_flip);
505 
506 template<> RotMatrix<3>& RotMatrix<3>::rotate(const Quaternion&);
507 
508 template<int dim>
510 {
511  // Get a flip by subtracting twice the projection operator in the
512  // direction of the vector. A projection operator is idempotent (P*P == P),
513  // and symmetric, so I - 2P is an orthogonal matrix.
514  //
515  // (I - 2P) * (I - 2P)^T == (I - 2P)^2 == I - 4P + 4P^2 == I
516 
517  CoordType sqr_mag = v.sqrMag();
518 
519  assert("need nonzero length vector" && sqr_mag > 0);
520 
521  // off diagonal
522  for(int i = 0; i < dim - 1; ++i)
523  for(int j = i + 1; j < dim; ++j)
524  m_elem[i][j] = m_elem[j][i] = -2 * v[i] * v[j] / sqr_mag;
525 
526  // diagonal
527  for(int i = 0; i < dim; ++i)
528  m_elem[i][i] = 1 - 2 * v[i] * v[i] / sqr_mag;
529 
530  m_flip = true;
531  m_valid = true;
532  m_age = 1;
533 
534  return *this;
535 }
536 
537 template<int dim>
539 {
540  for(int i = 0; i < dim; ++i)
541  for(int j = 0; j < dim; ++j)
542  m_elem[i][j] = (i == j) ? -1 : 0;
543 
544  m_flip = dim%2;
545  m_valid = true;
546  m_age = 0; // -1 and 0 are exact, no roundoff error
547 
548 
549  return *this;
550 }
551 
552 bool _MatrixInverseImpl(const int size, CoordType* in, CoordType* out);
553 
554 template<int dim>
556 {
557  // average the matrix with it's inverse transpose,
558  // that will clean up the error to linear order
559 
560  CoordType buf1[dim*dim], buf2[dim*dim];
561 
562  for(int i = 0; i < dim; ++i) {
563  for(int j = 0; j < dim; ++j) {
564  buf1[j*dim + i] = m_elem[i][j];
565  buf2[j*dim + i] = ((i == j) ? 1.f : 0.f);
566  }
567  }
568 
569  bool success = _MatrixInverseImpl(dim, buf1, buf2);
570  assert(success); // matrix can't be degenerate
571  if (!success) {
572  return;
573  }
574 
575  for(int i = 0; i < dim; ++i) {
576  for(int j = 0; j < dim; ++j) {
577  CoordType& elem = m_elem[i][j];
578  elem += buf2[i*dim + j];
579  elem /= 2;
580  }
581  }
582 
583  m_age = 1;
584 }
585 
586 } // namespace WFMath
587 
588 #endif // WFMATH_ROTMATRIX_FUNCS_H
Generic library namespace.
Definition: atlasconv.h:45
Vector< dim > row(const int i) const
Get a copy of the i&#39;th row as a Vector.
Definition: rotmatrix_funcs.h:293
Vector< dim > column(const int i) const
Get a copy of the i&#39;th column as a Vector.
Definition: rotmatrix_funcs.h:306
void normalize()
normalize to remove accumulated round-off error
Definition: rotmatrix_funcs.h:555
RotMatrix< dim > InvProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1^-1 * m2^-1
Definition: rotmatrix_funcs.h:155
A dim dimensional rotation matrix. Technically, a member of the group O(dim).
Definition: const.h:53
CoordType sqrMag() const
The squared magnitude of a vector.
Definition: vector_funcs.h:237
RotMatrix< dim > InvProd(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1^-1 * m2
Definition: rotmatrix_funcs.h:133
void setValid(bool valid=true)
make isValid() return true if you&#39;ve initialized the vector by hand
Definition: vector.h:154
RotMatrix & rotate(const RotMatrix &m)
rotate the matrix using another matrix
Definition: rotmatrix.h:196
A dim dimensional vector.
Definition: const.h:55
RotMatrix & fromQuaternion(const Quaternion &q, const bool not_flip=true)
3D only: set a RotMatrix from a Quaternion
RotMatrix & rotation(const int i, const int j, CoordType theta)
set the matrix to a rotation by the angle theta in the (i, j) plane
Definition: rotmatrix_funcs.h:360
float CoordType
Basic floating point type.
Definition: const.h:140
bool setVals(const CoordType vals[dim][dim], CoordType precision=numeric_constants< CoordType >::epsilon())
Set the values of the elements of the matrix.
Definition: rotmatrix_funcs.h:241
RotMatrix & mirror()
set the matrix to mirror all axes
Definition: rotmatrix_funcs.h:538
RotMatrix & identity()
set the matrix to the identity matrix
Definition: rotmatrix_funcs.h:335
RotMatrix< dim > operator*(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
Definition: rotmatrix_funcs.h:223
An error thrown by certain functions when passed parallel vectors.
Definition: error.h:37
RotMatrix< dim > ProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2^-1
Definition: rotmatrix_funcs.h:111
A normalized quaterion.
Definition: quaternion.h:35
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
Definition: rotmatrix_funcs.h:89
CoordType trace() const
Get the trace of the matrix.
Definition: rotmatrix_funcs.h:349
RotMatrix inverse() const
Get the inverse of the matrix.
Definition: rotmatrix_funcs.h:319