wfmath 1.0.3
A math library for the Worldforge system.
rotmatrix.cpp
1// rotmatrix.cpp (RotMatrix<> implementation)
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#ifdef HAVE_CONFIG_H
27#include "config.h"
28#endif
29
30#include "rotmatrix_funcs.h"
31#include "quaternion.h"
32
33#include <limits>
34
35namespace WFMath {
36
37static CoordType _MatrixDeterminantImpl(int size, CoordType* m);
38
39template<> RotMatrix<3>& RotMatrix<3>::fromQuaternion(const Quaternion& q,
40 const bool not_flip)
41{
42 CoordType xx, yy, zz, xy, xz, yz;
43 const Vector<3> &vref = q.vector();
44
45 xx = vref[0] * vref[0];
46 xy = vref[0] * vref[1];
47 xz = vref[0] * vref[2];
48 yy = vref[1] * vref[1];
49 yz = vref[1] * vref[2];
50 zz = vref[2] * vref[2];
51
52 Vector<3> wvec = vref * q.scalar();
53
54 m_elem[0][0] = 1 - 2 * (yy + zz);
55 m_elem[1][1] = 1 - 2 * (xx + zz);
56 m_elem[2][2] = 1 - 2 * (xx + yy);
57
58 m_elem[0][1] = 2 * (xy - wvec[2]);
59 m_elem[0][2] = 2 * (xz + wvec[1]);
60
61 m_elem[1][0] = 2 * (xy + wvec[2]);
62 m_elem[1][2] = 2 * (yz - wvec[0]);
63
64 m_elem[2][0] = 2 * (xz - wvec[1]);
65 m_elem[2][1] = 2 * (yz + wvec[0]);
66
67 m_flip = !not_flip;
68 m_age = q.age();
69 if(!not_flip)
70 *this = Prod(*this, RotMatrix<3>().mirror(0));
71
72 m_valid = true;
73 return *this;
74}
75
76template<> RotMatrix<3>& RotMatrix<3>::rotate(const Quaternion& q)
77{
78 Vector<3> vec;
79 vec.setValid();
80 m_valid = m_valid && q.isValid();
81 m_age += q.age();
82
83 // rotate both sides by q
84
85 for(auto & vec_num : m_elem) {
86 for(int elem_num = 0; elem_num < 3; ++elem_num)
87 vec[elem_num] = vec_num[elem_num];
88 vec.rotate(q);
89 for(int elem_num = 0; elem_num < 3; ++elem_num)
90 vec_num[elem_num] = vec[elem_num];
91 }
92
93 checkNormalization();
94
95 return *this;
96}
97
98template<>
99RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis,
100 CoordType theta)
101{
102 CoordType max = 0;
103 int main_comp = -1;
104
105 for(int i = 0; i < 3; ++i) {
106 CoordType val = std::fabs(axis[i]);
107 if(val > max) {
108 max = val;
109 main_comp = i;
110 }
111 }
112
113 assert("Must pass a nonzero length vector as axis to avoid this" && main_comp != -1);
114
115 Vector<3> tmp, v1, v2;
116
117 int new_comp = main_comp ? main_comp - 1 : 2; // Not parallel to axis
118 for(int i = 0; i < 3; ++i)
119 tmp[i] = ((i == new_comp) ? 1.f : 0.f);
120
121 v1 = Cross(axis, tmp); // 3D specific part
122 v2 = Cross(axis, v1);
123
124 return rotation(v1, v2, theta);
125}
126
127template<>
128RotMatrix<3>& RotMatrix<3>::rotation (const Vector<3>& axis)
129{
130 CoordType max = 0;
131 int main_comp = -1;
132 CoordType angle = axis.mag();
133
134 if(angle == 0) {
135 return identity();
136 }
137
138 for(int i = 0; i < 3; ++i) {
139 CoordType val = std::fabs(axis[i]);
140 if(val > max) {
141 max = val;
142 main_comp = i;
143 }
144 }
145
146 assert("Can't happen with nonzero angle" && main_comp != -1);
147
148 Vector<3> tmp, v1, v2;
149
150 int new_comp = main_comp ? main_comp - 1 : 2; // Not parallel to axis
151 for(int i = 0; i < 3; ++i)
152 tmp[i] = ((i == new_comp) ? 1.f : 0.f);
153
154 v1 = Cross(axis, tmp); // 3D specific part
155 v2 = Cross(axis, v1);
156
157 return rotation(v1, v2, angle);
158}
159
160bool _MatrixSetValsImpl(const int size, CoordType* vals, bool& flip,
161 CoordType* buf1, CoordType* buf2, CoordType precision)
162{
163 precision = std::fabs(precision);
164
165 if(precision >= .9) // Can get an infinite loop for precision == 1
166 return false;
167
168 // Check that vals form an orthogonal matrix, also increase their
169 // precision to WFMATH_EPSILON
170
171 while(true) {
172 CoordType try_prec = 0;
173
174 for(int i = 0; i < size; ++i) {
175 for(int j = 0; j < size; ++j) {
176 CoordType ans = 0;
177 for(int k = 0; k < size; ++k) {
178 ans += vals[i*size+k] * vals[j*size+k];
179 }
180
181 if(i == j) // Subtract identity matrix
182 --ans;
183 ans = std::fabs(ans);
184 if(ans >= try_prec)
185 try_prec = ans;
186 }
187 }
188
189 if(try_prec > precision)
190 return false;
191
192 if(try_prec <= numeric_constants<CoordType>::epsilon())
193 break;
194
195 // Precision needs improvement, use linear approximation scheme.
196
197 // This scheme takes the original matrix (call it A)
198 // and subtracts another matrix (delta), where
199 //
200 // delta = (A - (A^T)^-1) / 2
201 //
202 // This is correct, up to errors of order delta^2,
203 // if you assume you can choose a delta such that
204 // A^T * delta is symmetric (delta is underdetermined
205 // by the linear approximation scheme)
206
207 // This procedure will not increase the precision of
208 // the parameters which determine the matrix (i.e. the
209 // Euler angles), but it will increase the precision
210 // to which the matrix satisfies the condition
211 // A^T * A == (identity matrix).
212
213 // The symmetry condition on A^T * delta represents an
214 // arbitrary condition which will determine the higher
215 // precision components of the Euler angles. This
216 // makes the problem of increasing the precision of
217 // the matrix well determined, and solvable by this
218 // perturbative method.
219
220 for(int i = 0; i < size; ++i) {
221 for(int j = 0; j < size; ++j) {
222 buf1[i*size+j] = vals[j*size+i];
223 buf2[i*size+j] = ((i == j) ? 1.f : 0.f);
224 }
225 }
226
227 bool ans = _MatrixInverseImpl(size, buf1, buf2);
228
229 if(!ans) { // Degenerate matrix, something badly wrong
230 return false;
231 }
232
233 for(int i = 0; i < size; ++i) {
234 for(int j = 0; j < size; ++j) {
235 CoordType& elem = vals[i*size+j];
236 elem += buf2[i*size+j];
237 elem /= 2;
238 }
239 }
240
241 // The above scheme should approx. square the precision.
242 // That is, try_prec -> try_prec * try_prec * (fudge factor)
243 // in the next iteration.
244 }
245
246 // The determinant is either 1 or -1, depending on the parity.
247 // Use that to calculate flip.
248
249 for(int i = 0; i < size; ++i)
250 for(int j = 0; j < size; ++j)
251 buf1[i*size+j] = vals[i*size+j];
252
253 flip = _MatrixDeterminantImpl(size, buf1) < 0;
254
255 return true;
256}
257
258static CoordType _MatrixDeterminantImpl(const int size, CoordType* m)
259{
260 // First, construct an upper triangular matrix with the
261 // same determinant as the original matrix. Then just
262 // multiply the diagonal terms to get the determinant.
263
264 for(int i = 0; i < size - 1; ++i) {
265 CoordType minval = 0;
266 for(int j = 0; j < size; ++j)
267 minval += m[j*size+i] * m[j*size+i];
268 minval /= std::numeric_limits<CoordType>::max();
269 if(minval < std::numeric_limits<CoordType>::min())
270 minval = std::numeric_limits<CoordType>::min();
271 if(m[i*size+i] * m[i*size+i] < minval) { // Find a row with nonzero element
272 int j;
273 for(j = i + 1; j < size; ++j)
274 if(m[j*size+i] * m[j*size+i] >= minval)
275 break;
276 if(j == size) // No nonzero element found, degenerate matrix, det == 0
277 return 0;
278 m[i*size+i] = m[j*size+i];
279 for(int k = i + 1; k < size; ++k) // For k < i, m[j*size+k] == 0
280 m[i*size+k] += m[j*size+k];
281 }
282 for(int j = i + 1; j < size; ++j) {
283 CoordType factor = m[j*size+i] / m[i*size+i];
284 // We know factor isn't bigger than about sqrt(WFMATH_MAX), due to
285 // the comparison with minval done above.
286 m[j*size+i] = 0;
287 if(factor != 0) {
288 for(int k = i + 1; k < size; ++k) // For k < i, m[k*size+j] == 0
289 m[j*size+k] -= m[i*size+k] * factor;
290 }
291 }
292 }
293
294 CoordType out = 1;
295
296 for(int i = 0; i < size; ++i)
297 out *= m[i*size+i];
298
299 return out;
300}
301
302bool _MatrixInverseImpl(const int size, CoordType* in, CoordType* out)
303{
304 // Invert using row operations. First, make m upper triangular,
305 // with 1's on the diagonal
306
307 for(int i = 0; i < size; ++i) {
308
309 // Make sure in[i*size+i] is nonzero
310 CoordType minval = 0;
311 for(int j = 0; j < size; ++j)
312 minval += in[j*size+i] * in[j*size+i];
313 minval /= std::numeric_limits<CoordType>::max();
314 if(minval < std::numeric_limits<CoordType>::min())
315 minval = std::numeric_limits<CoordType>::min();
316 if(in[i*size+i] * in[i*size+i] < minval) { // Find a nonzero element
317 int j;
318 for(j = i + 1; j < size; ++j)
319 if(in[j*size+i] * in[j*size+i] >= minval)
320 break;
321 if(j == size) // degenerate matrix
322 return false;
323 for(int k = 0; k < size; ++k) {
324 out[i*size+k] += out[j*size+k];
325 in[i*size+k] += in[j*size+k];
326 }
327 }
328 // We now know in[i*size+i] / in[j*size+i] >= sqrt(WFMATH_MIN) for any j
329
330 // Normalize the row, so in[i*size+i] == 1
331 CoordType tmp = in[i*size+i];
332 in[i*size+i] = 1;
333 for(int j = 0; j < size; ++j) {
334 out[i*size+j] /= tmp;
335 if(j > i) // in[i*size+j] == 0 for j < i
336 in[i*size+j] /= tmp;
337 }
338
339 // Do row subtraction to make in[j*size+i] zero for j > i
340 for(int j = i + 1; j < size; ++j) {
341 CoordType tmp2 = in[j*size+i];
342 in[j*size+i] = 0;
343 if(tmp2 != 0) {
344 for(int k = 0; k < size; ++k) {
345 out[j*size+k] -= out[i*size+k] * tmp2;
346 in[j*size+k] -= in[i*size+k] * tmp2;
347 }
348 }
349 }
350 }
351
352 // Now perform row operations on "out" which would make "m"
353 // into the identity matrix
354
355 for(int i = size - 1; i >= 1; --i) {
356 for(int j = i - 1; j >= 0; --j) {
357 CoordType tmp = in[j*size+i];
358 if(tmp != 0)
359 for(int k = 0; k < size; ++k)
360 out[j*size+k] -= out[i*size+k] * tmp;
361 // Don't bother modifying in[j*size+k], we never use it again.
362 }
363 }
364
365 return true;
366}
367
368template <>
369RotMatrix<3>::RotMatrix(const Quaternion& q,
370 const bool not_flip) : m_flip(false), m_valid(false),
371 m_age(0)
372{
373 fromQuaternion(q, not_flip);
374}
375
376template class RotMatrix<2>;
377template class RotMatrix<3>;
378
379static_assert(std::is_standard_layout<RotMatrix<2>>::value, "RotMatrix should be standard layout.");
380static_assert(std::is_trivially_copyable<RotMatrix<2>>::value, "RotMatrix should be trivially copyable.");
381
382static_assert(std::is_standard_layout<RotMatrix<3>>::value, "RotMatrix should be standard layout.");
383static_assert(std::is_trivially_copyable<RotMatrix<3>>::value, "RotMatrix should be trivially copyable.");
384
385template RotMatrix<2> operator*<2>(RotMatrix<2> const&, RotMatrix<2> const&);
386template RotMatrix<3> operator*<3>(RotMatrix<3> const&, RotMatrix<3> const&);
387
388template RotMatrix<2> ProdInv<2>(RotMatrix<2> const&, RotMatrix<2> const&);
389template RotMatrix<3> ProdInv<3>(RotMatrix<3> const&, RotMatrix<3> const&);
390
391template Vector<2> operator*<2>(Vector<2> const&, RotMatrix<2> const&);
392template Vector<3> operator*<3>(Vector<3> const&, RotMatrix<3> const&);
393
394template Vector<2> operator*<2>(RotMatrix<2> const&, Vector<2> const&);
395template Vector<3> operator*<3>(RotMatrix<3> const&, Vector<3> const&);
396
397template Vector<2> ProdInv<2>(Vector<2> const&, RotMatrix<2> const&);
398template Vector<3> ProdInv<3>(Vector<3> const&, RotMatrix<3> const&);
399
400template Vector<3> Prod<3>(Vector<3> const&, RotMatrix<3> const&);
401template Vector<2> Prod<2>(Vector<2> const&, RotMatrix<2> const&);
402
403template RotMatrix<3> Prod<3>(RotMatrix<3> const&, RotMatrix<3> const&);
404template RotMatrix<2> Prod<2>(RotMatrix<2> const&, RotMatrix<2> const&);
405
406}
RotMatrix & rotation(int i, int j, CoordType theta)
set the matrix to a rotation by the angle theta in the (i, j) plane
RotMatrix & rotate(const RotMatrix &m)
rotate the matrix using another matrix
Definition: rotmatrix.h:196
RotMatrix & fromQuaternion(const Quaternion &q, bool not_flip=true)
3D only: set a RotMatrix from a Quaternion
Generic library namespace.
Definition: shape.h:41
double CoordType
Basic floating point type.
Definition: const.h:140
CoordType Cross(const Vector< 2 > &v1, const Vector< 2 > &v2)
2D only: get the z component of the cross product of two vectors
Definition: vector.cpp:102
RotMatrix< dim > Prod(const RotMatrix< dim > &m1, const RotMatrix< dim > &m2)
returns m1 * m2