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 
35 namespace WFMath {
36 
37 static CoordType _MatrixDeterminantImpl(int size, CoordType* m);
38 
39 template<> 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 
76 template<> 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 
98 template<>
99 RotMatrix<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 
127 template<>
128 RotMatrix<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 
160 bool _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 
258 static 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 
302 bool _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 
368 template <>
369 RotMatrix<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 
376 template class RotMatrix<2>;
377 template class RotMatrix<3>;
378 
379 static_assert(std::is_standard_layout<RotMatrix<2>>::value, "RotMatrix should be standard layout.");
380 static_assert(std::is_trivially_copyable<RotMatrix<2>>::value, "RotMatrix should be trivially copyable.");
381 
382 static_assert(std::is_standard_layout<RotMatrix<3>>::value, "RotMatrix should be standard layout.");
383 static_assert(std::is_trivially_copyable<RotMatrix<3>>::value, "RotMatrix should be trivially copyable.");
384 
385 template RotMatrix<2> operator*<2>(RotMatrix<2> const&, RotMatrix<2> const&);
386 template RotMatrix<3> operator*<3>(RotMatrix<3> const&, RotMatrix<3> const&);
387 
388 template RotMatrix<2> ProdInv<2>(RotMatrix<2> const&, RotMatrix<2> const&);
389 template RotMatrix<3> ProdInv<3>(RotMatrix<3> const&, RotMatrix<3> const&);
390 
391 template Vector<2> operator*<2>(Vector<2> const&, RotMatrix<2> const&);
392 template Vector<3> operator*<3>(Vector<3> const&, RotMatrix<3> const&);
393 
394 template Vector<2> operator*<2>(RotMatrix<2> const&, Vector<2> const&);
395 template Vector<3> operator*<3>(RotMatrix<3> const&, Vector<3> const&);
396 
397 template Vector<2> ProdInv<2>(Vector<2> const&, RotMatrix<2> const&);
398 template Vector<3> ProdInv<3>(Vector<3> const&, RotMatrix<3> const&);
399 
400 template Vector<3> Prod<3>(Vector<3> const&, RotMatrix<3> const&);
401 template Vector<2> Prod<2>(Vector<2> const&, RotMatrix<2> const&);
402 
403 template RotMatrix<3> Prod<3>(RotMatrix<3> const&, RotMatrix<3> const&);
404 template 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