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
39namespace WFMath {
40
41template<int dim>
42inline 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
71template<int dim> // m1 * m2
73{
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
93template<int dim> // m1 * m2^-1
95{
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
115template<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}
137template<int dim> // m1^-1 * m2^-1
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 }
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}
159template<int dim> // m * v
160inline Vector<dim> Prod(const RotMatrix<dim>& m, const Vector<dim>& v)
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];
169 }
170
171 out.m_valid = m.m_valid && v.m_valid;
172
173 return out;
174}
175
176template<int dim> // m^-1 * 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];
186 }
187
188 out.m_valid = m.m_valid && v.m_valid;
189
190 return out;
191}
192
193template<int dim> // v * m
194inline 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
199template<int dim> // v * m^-1
201{
202 return Prod(m, v); // Since transpose() and inverse() are the same
203}
204
205template<int dim>
207{
208 return Prod(m1, m2);
209}
210
211template<int dim>
212inline Vector<dim> operator*(const RotMatrix<dim>& m, const Vector<dim>& v)
213{
214 return Prod(m, v);
215}
216
217template<int dim>
218inline 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
223template<int dim>
224inline 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
236template<int dim>
237inline 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
248bool _MatrixSetValsImpl(int size, CoordType* vals, bool& flip,
249 CoordType* buf1, CoordType* buf2, CoordType precision);
250
251template<int dim>
252inline 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
275template<int dim>
276inline 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
288template<int dim>
289inline 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
301template<int dim>
303{
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
317template<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
331template<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
342template<int dim>
343RotMatrix<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
376template<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
421template<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
483template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis,
484 CoordType theta);
485template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis);
487 const bool not_flip);
488
490
491template<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
520template<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
535bool _MatrixInverseImpl(int size, CoordType* in, CoordType* out);
536
537template<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
A normalized quaternion.
Definition: quaternion.h:36
A dim dimensional rotation matrix. Technically, a member of the group O(dim).
Definition: rotmatrix.h:87
RotMatrix & rotation(int i, int j, CoordType theta)
set the matrix to a rotation by the angle theta in the (i, j) plane
Vector< dim > row(int i) const
Get a copy of the i'th row as a Vector.
Vector< dim > column(int i) const
Get a copy of the i'th column as a Vector.
RotMatrix & rotate(const RotMatrix &m)
rotate the matrix using another matrix
Definition: rotmatrix.h:196
RotMatrix & identity()
set the matrix to the identity matrix
CoordType trace() const
Get the trace of the matrix.
void normalize()
normalize to remove accumulated round-off error
RotMatrix & mirror()
set the matrix to mirror all axes
RotMatrix & fromQuaternion(const Quaternion &q, bool not_flip=true)
3D only: set a RotMatrix from a Quaternion
RotMatrix inverse() const
Get the inverse of the matrix.
bool setVals(const CoordType vals[dim][dim], CoordType precision=numeric_constants< CoordType >::epsilon())
Set the values of the elements of the matrix.
void setValid(bool valid=true)
make isValid() return true if you've initialized the vector by hand
Definition: vector.h:154
CoordType sqrMag() const
The squared magnitude of a vector.
Definition: vector_funcs.h:220
Generic library namespace.
Definition: shape.h:41
double CoordType
Basic floating point type.
Definition: const.h:140
RotMatrix< dim > InvProd(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1^-1 * m2
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
RotMatrix< dim > InvProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1^-1 * m2^-1
RotMatrix< dim > ProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2^-1
RotMatrix< dim > operator*(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
An error thrown by certain functions when passed parallel vectors.
Definition: error.h:37