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
39namespace WFMath {
40
41template<int dim>
42inline 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
50template<int dim>
51inline 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
64template<int dim>
65inline 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
88template<int dim> // m1 * m2
90{
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}
110template<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 }
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}
132template<int dim> // m1^-1 * m2
135 RotMatrix<dim> out;
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
154template<int dim> // m1^-1 * m2^-1
156{
157 RotMatrix<dim> out;
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
176template<int dim> // m * v
177inline 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];
186 }
187
188 out.m_valid = m.m_valid && v.m_valid;
189
190 return out;
191}
192
193template<int dim> // m^-1 * 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
210template<int dim> // v * m
211inline 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
216template<int dim> // v * m^-1
218{
219 return Prod(m, v); // Since transpose() and inverse() are the same
220}
221
222template<int dim>
224{
225 return Prod(m1, m2);
226}
227
228template<int dim>
229inline Vector<dim> operator*(const RotMatrix<dim>& m, const Vector<dim>& v)
230{
231 return Prod(m, v);
232}
233
234template<int dim>
235inline 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
240template<int dim>
241inline 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
253template<int dim>
254inline 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
265bool _MatrixSetValsImpl(const int size, CoordType* vals, bool& flip,
266 CoordType* buf1, CoordType* buf2, CoordType precision);
267
268template<int dim>
269inline 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
292template<int dim>
293inline 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
305template<int dim>
306inline 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
318template<int dim>
320{
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
334template<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
348template<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
359template<int dim>
360RotMatrix<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
393template<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
438template<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
500template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis,
501 CoordType theta);
502template<> RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis);
504 const bool not_flip);
505
507
508template<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
537template<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
552bool _MatrixInverseImpl(const int size, CoordType* in, CoordType* out);
553
554template<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
A normalized quaterion.
Definition quaternion.h:36
A dim dimensional rotation matrix. Technically, a member of the group O(dim).
Definition rotmatrix.h:87
A dim dimensional vector.
Definition vector.h:121
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:237
Generic library namespace.
Definition atlasconv.h:45
RotMatrix< dim > InvProd(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1^-1 * m2
Definition rotmatrix_funcs.h:133
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2
Definition rotmatrix_funcs.h:89
RotMatrix< dim > InvProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1^-1 * m2^-1
Definition rotmatrix_funcs.h:155
float CoordType
Basic floating point type.
Definition const.h:140
RotMatrix< dim > ProdInv(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2^-1
Definition rotmatrix_funcs.h:111
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