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