OpenVDB  11.0.0
Mat.h
Go to the documentation of this file.
1 // Copyright Contributors to the OpenVDB Project
2 // SPDX-License-Identifier: MPL-2.0
3 //
4 /// @file Mat.h
5 /// @author Joshua Schpok
6 
7 #ifndef OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
8 #define OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
9 
10 #include "Math.h"
11 #include <openvdb/Exceptions.h>
12 #include <algorithm> // for std::max()
13 #include <cmath>
14 #include <iostream>
15 #include <string>
16 
17 
18 namespace openvdb {
20 namespace OPENVDB_VERSION_NAME {
21 namespace math {
22 
23 /// @class Mat "Mat.h"
24 /// A base class for square matrices.
25 template<unsigned SIZE, typename T>
26 class Mat
27 {
28 public:
29  using value_type = T;
30  using ValueType = T;
31  enum SIZE_ { size = SIZE };
32 
33  /// Trivial constructor, the matrix is NOT initialized
34  /// @note destructor, copy constructor, assignment operator and
35  /// move constructor are left to be defined by the compiler (default)
36  Mat() = default;
37 
38  // Number of cols, rows, elements
39  static unsigned numRows() { return SIZE; }
40  static unsigned numColumns() { return SIZE; }
41  static unsigned numElements() { return SIZE*SIZE; }
42 
43  /// @return string representation of matrix
44  /// Since output is multiline, optional indentation argument prefixes
45  /// each newline with that much white space. It does not indent
46  /// the first line, since you might be calling this inline:
47  ///
48  /// cout << "matrix: " << mat.str(7)
49  ///
50  /// matrix: [[1 2]
51  /// [3 4]]
52  std::string
53  str(unsigned indentation = 0) const {
54 
55  std::string ret;
56  std::string indent;
57 
58  // We add +1 since we're indenting one for the first '['
59  indent.append(indentation+1, ' ');
60 
61  ret.append("[");
62 
63  // For each row,
64  for (unsigned i(0); i < SIZE; i++) {
65 
66  ret.append("[");
67 
68  // For each column
69  for (unsigned j(0); j < SIZE; j++) {
70 
71  // Put a comma after everything except the last
72  if (j) ret.append(", ");
73  ret.append(std::to_string(mm[(i*SIZE)+j]));
74  }
75 
76  ret.append("]");
77 
78  // At the end of every row (except the last)...
79  if (i < SIZE - 1) {
80  // ...suffix the row bracket with a comma, newline, and advance indentation.
81  ret.append(",\n");
82  ret.append(indent);
83  }
84  }
85 
86  ret.append("]");
87 
88  return ret;
89  }
90 
91  /// Write a Mat to an output stream
92  friend std::ostream& operator<<(
93  std::ostream& ostr,
94  const Mat<SIZE, T>& m)
95  {
96  ostr << m.str();
97  return ostr;
98  }
99 
100  /// Direct access to the internal data
101  T* asPointer() { return mm; }
102  const T* asPointer() const { return mm; }
103 
104  //@{
105  /// Array style reference to ith row
106  T* operator[](int i) { return &(mm[i*SIZE]); }
107  const T* operator[](int i) const { return &(mm[i*SIZE]); }
108  //@}
109 
110  void write(std::ostream& os) const {
111  os.write(reinterpret_cast<const char*>(&mm), sizeof(T)*SIZE*SIZE);
112  }
113 
114  void read(std::istream& is) {
115  is.read(reinterpret_cast<char*>(&mm), sizeof(T)*SIZE*SIZE);
116  }
117 
118  /// Return the maximum of the absolute of all elements in this matrix
119  T absMax() const {
120  T x = static_cast<T>(std::fabs(mm[0]));
121  for (unsigned i = 1; i < numElements(); ++i) {
122  x = std::max(x, static_cast<T>(std::fabs(mm[i])));
123  }
124  return x;
125  }
126 
127  /// True if a Nan is present in this matrix
128  bool isNan() const {
129  for (unsigned i = 0; i < numElements(); ++i) {
130  if (math::isNan(mm[i])) return true;
131  }
132  return false;
133  }
134 
135  /// True if an Inf is present in this matrix
136  bool isInfinite() const {
137  for (unsigned i = 0; i < numElements(); ++i) {
138  if (math::isInfinite(mm[i])) return true;
139  }
140  return false;
141  }
142 
143  /// True if no Nan or Inf values are present
144  bool isFinite() const {
145  for (unsigned i = 0; i < numElements(); ++i) {
146  if (!math::isFinite(mm[i])) return false;
147  }
148  return true;
149  }
150 
151  /// True if all elements are exactly zero
152  bool isZero() const {
153  for (unsigned i = 0; i < numElements(); ++i) {
154  if (!math::isZero(mm[i])) return false;
155  }
156  return true;
157  }
158 
159 protected:
160  T mm[SIZE*SIZE];
161 };
162 
163 
164 template<typename T> class Quat;
165 template<typename T> class Vec3;
166 
167 /// @brief Return the rotation matrix specified by the given quaternion.
168 /// @details The quaternion is normalized and used to construct the matrix.
169 /// Note that the matrix is transposed to match post-multiplication semantics.
170 template<class MatType>
171 MatType
173  typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
174 {
175  using T = typename MatType::value_type;
176 
177  T qdot(q.dot(q));
178  T s(0);
179 
180  if (!isApproxEqual(qdot, T(0.0),eps)) {
181  s = T(2.0 / qdot);
182  }
183 
184  T x = s*q.x();
185  T y = s*q.y();
186  T z = s*q.z();
187  T wx = x*q.w();
188  T wy = y*q.w();
189  T wz = z*q.w();
190  T xx = x*q.x();
191  T xy = y*q.x();
192  T xz = z*q.x();
193  T yy = y*q.y();
194  T yz = z*q.y();
195  T zz = z*q.z();
196 
197  MatType r;
198  r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
199  r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
200  r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
201 
202  if(MatType::numColumns() == 4) padMat4(r);
203  return r;
204 }
205 
206 
207 
208 /// @brief Return a matrix for rotation by @a angle radians about the given @a axis.
209 /// @param axis The axis (one of X, Y, Z) to rotate about.
210 /// @param angle The rotation angle, in radians.
211 template<class MatType>
212 MatType
213 rotation(Axis axis, typename MatType::value_type angle)
214 {
215  using T = typename MatType::value_type;
216  T c = static_cast<T>(cos(angle));
217  T s = static_cast<T>(sin(angle));
218 
219  MatType result;
220  result.setIdentity();
221 
222  switch (axis) {
223  case X_AXIS:
224  result[1][1] = c;
225  result[1][2] = s;
226  result[2][1] = -s;
227  result[2][2] = c;
228  return result;
229  case Y_AXIS:
230  result[0][0] = c;
231  result[0][2] = -s;
232  result[2][0] = s;
233  result[2][2] = c;
234  return result;
235  case Z_AXIS:
236  result[0][0] = c;
237  result[0][1] = s;
238  result[1][0] = -s;
239  result[1][1] = c;
240  return result;
241  default:
242  throw ValueError("Unrecognized rotation axis");
243  }
244 }
245 
246 
247 /// @brief Return a matrix for rotation by @a angle radians about the given @a axis.
248 /// @note The axis must be a unit vector.
249 template<class MatType>
250 MatType
251 rotation(const Vec3<typename MatType::value_type> &_axis, typename MatType::value_type angle)
252 {
253  using T = typename MatType::value_type;
254  T txy, txz, tyz, sx, sy, sz;
255 
256  Vec3<T> axis(_axis.unit());
257 
258  // compute trig properties of angle:
259  T c(cos(double(angle)));
260  T s(sin(double(angle)));
261  T t(1 - c);
262 
263  MatType result;
264  // handle diagonal elements
265  result[0][0] = axis[0]*axis[0] * t + c;
266  result[1][1] = axis[1]*axis[1] * t + c;
267  result[2][2] = axis[2]*axis[2] * t + c;
268 
269  txy = axis[0]*axis[1] * t;
270  sz = axis[2] * s;
271 
272  txz = axis[0]*axis[2] * t;
273  sy = axis[1] * s;
274 
275  tyz = axis[1]*axis[2] * t;
276  sx = axis[0] * s;
277 
278  // right handed space
279  // Contribution from rotation about 'z'
280  result[0][1] = txy + sz;
281  result[1][0] = txy - sz;
282  // Contribution from rotation about 'y'
283  result[0][2] = txz - sy;
284  result[2][0] = txz + sy;
285  // Contribution from rotation about 'x'
286  result[1][2] = tyz + sx;
287  result[2][1] = tyz - sx;
288 
289  if(MatType::numColumns() == 4) padMat4(result);
290  return MatType(result);
291 }
292 
293 
294 /// @brief Return the Euler angles composing the given rotation matrix.
295 /// @details Optional axes arguments describe in what order elementary rotations
296 /// are applied. Note that in our convention, XYZ means Rz * Ry * Rx.
297 /// Because we are using rows rather than columns to represent the
298 /// local axes of a coordinate frame, the interpretation from a local
299 /// reference point of view is to first rotate about the x axis, then
300 /// about the newly rotated y axis, and finally by the new local z axis.
301 /// From a fixed reference point of view, the interpretation is to
302 /// rotate about the stationary world z, y, and x axes respectively.
303 ///
304 /// Irrespective of the Euler angle convention, in the case of distinct
305 /// axes, eulerAngles() returns the x, y, and z angles in the corresponding
306 /// x, y, z components of the returned Vec3. For the XZX convention, the
307 /// left X value is returned in Vec3.x, and the right X value in Vec3.y.
308 /// For the ZXZ convention the left Z value is returned in Vec3.z and
309 /// the right Z value in Vec3.y
310 ///
311 /// Examples of reconstructing r from its Euler angle decomposition
312 ///
313 /// v = eulerAngles(r, ZYX_ROTATION);
314 /// rx.setToRotation(Vec3d(1,0,0), v[0]);
315 /// ry.setToRotation(Vec3d(0,1,0), v[1]);
316 /// rz.setToRotation(Vec3d(0,0,1), v[2]);
317 /// r = rx * ry * rz;
318 ///
319 /// v = eulerAngles(r, ZXZ_ROTATION);
320 /// rz1.setToRotation(Vec3d(0,0,1), v[2]);
321 /// rx.setToRotation (Vec3d(1,0,0), v[0]);
322 /// rz2.setToRotation(Vec3d(0,0,1), v[1]);
323 /// r = rz2 * rx * rz1;
324 ///
325 /// v = eulerAngles(r, XZX_ROTATION);
326 /// rx1.setToRotation (Vec3d(1,0,0), v[0]);
327 /// rx2.setToRotation (Vec3d(1,0,0), v[1]);
328 /// rz.setToRotation (Vec3d(0,0,1), v[2]);
329 /// r = rx2 * rz * rx1;
330 ///
331 template<class MatType>
334  const MatType& mat,
335  RotationOrder rotationOrder,
336  typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
337 {
338  using ValueType = typename MatType::value_type;
339  using V = Vec3<ValueType>;
340  ValueType phi, theta, psi;
341 
342  switch(rotationOrder)
343  {
344  case XYZ_ROTATION:
345  if (isApproxEqual(mat[2][0], ValueType(1.0), eps)) {
346  theta = ValueType(math::pi<double>() / 2.0);
347  phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
348  psi = phi;
349  } else if (isApproxEqual(mat[2][0], ValueType(-1.0), eps)) {
350  theta = ValueType(-math::pi<double>() / 2.0);
351  phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
352  psi = -phi;
353  } else {
354  psi = ValueType(atan2(-mat[1][0],mat[0][0]));
355  phi = ValueType(atan2(-mat[2][1],mat[2][2]));
356  theta = ValueType(atan2(mat[2][0],
357  sqrt( mat[2][1]*mat[2][1] +
358  mat[2][2]*mat[2][2])));
359  }
360  return V(phi, theta, psi);
361  case ZXY_ROTATION:
362  if (isApproxEqual(mat[1][2], ValueType(1.0), eps)) {
363  theta = ValueType(math::pi<double>() / 2.0);
364  phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
365  psi = phi;
366  } else if (isApproxEqual(mat[1][2], ValueType(-1.0), eps)) {
367  theta = ValueType(-math::pi<double>() / 2.0);
368  phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
369  psi = -phi;
370  } else {
371  psi = ValueType(atan2(-mat[0][2], mat[2][2]));
372  phi = ValueType(atan2(-mat[1][0], mat[1][1]));
373  theta = ValueType(atan2(mat[1][2],
374  sqrt(mat[0][2] * mat[0][2] +
375  mat[2][2] * mat[2][2])));
376  }
377  return V(theta, psi, phi);
378 
379  case YZX_ROTATION:
380  if (isApproxEqual(mat[0][1], ValueType(1.0), eps)) {
381  theta = ValueType(math::pi<double>() / 2.0);
382  phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
383  psi = phi;
384  } else if (isApproxEqual(mat[0][1], ValueType(-1.0), eps)) {
385  theta = ValueType(-math::pi<double>() / 2.0);
386  phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
387  psi = -phi;
388  } else {
389  psi = ValueType(atan2(-mat[2][1], mat[1][1]));
390  phi = ValueType(atan2(-mat[0][2], mat[0][0]));
391  theta = ValueType(atan2(mat[0][1],
392  sqrt(mat[0][0] * mat[0][0] +
393  mat[0][2] * mat[0][2])));
394  }
395  return V(psi, phi, theta);
396 
397  case XZX_ROTATION:
398 
399  if (isApproxEqual(mat[0][0], ValueType(1.0), eps)) {
400  theta = ValueType(0.0);
401  phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
402  psi = phi;
403  } else if (isApproxEqual(mat[0][0], ValueType(-1.0), eps)) {
404  theta = ValueType(math::pi<double>());
405  psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
406  phi = - psi;
407  } else {
408  psi = ValueType(atan2(mat[2][0], -mat[1][0]));
409  phi = ValueType(atan2(mat[0][2], mat[0][1]));
410  theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
411  mat[0][2] * mat[0][2]),
412  mat[0][0]));
413  }
414  return V(phi, psi, theta);
415 
416  case ZXZ_ROTATION:
417 
418  if (isApproxEqual(mat[2][2], ValueType(1.0), eps)) {
419  theta = ValueType(0.0);
420  phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
421  psi = phi;
422  } else if (isApproxEqual(mat[2][2], ValueType(-1.0), eps)) {
423  theta = ValueType(math::pi<double>());
424  phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
425  psi = -phi;
426  } else {
427  psi = ValueType(atan2(mat[0][2], mat[1][2]));
428  phi = ValueType(atan2(mat[2][0], -mat[2][1]));
429  theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
430  mat[1][2] * mat[1][2]),
431  mat[2][2]));
432  }
433  return V(theta, psi, phi);
434 
435  case YXZ_ROTATION:
436 
437  if (isApproxEqual(mat[2][1], ValueType(1.0), eps)) {
438  theta = ValueType(-math::pi<double>() / 2.0);
439  phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
440  psi = phi;
441  } else if (isApproxEqual(mat[2][1], ValueType(-1.0), eps)) {
442  theta = ValueType(math::pi<double>() / 2.0);
443  phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
444  psi = -phi;
445  } else {
446  psi = ValueType(atan2(mat[0][1], mat[1][1]));
447  phi = ValueType(atan2(mat[2][0], mat[2][2]));
448  theta = ValueType(atan2(-mat[2][1],
449  sqrt(mat[0][1] * mat[0][1] +
450  mat[1][1] * mat[1][1])));
451  }
452  return V(theta, phi, psi);
453 
454  case ZYX_ROTATION:
455 
456  if (isApproxEqual(mat[0][2], ValueType(1.0), eps)) {
457  theta = ValueType(-math::pi<double>() / 2.0);
458  phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
459  psi = phi;
460  } else if (isApproxEqual(mat[0][2], ValueType(-1.0), eps)) {
461  theta = ValueType(math::pi<double>() / 2.0);
462  phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
463  psi = -phi;
464  } else {
465  psi = ValueType(atan2(mat[1][2], mat[2][2]));
466  phi = ValueType(atan2(mat[0][1], mat[0][0]));
467  theta = ValueType(atan2(-mat[0][2],
468  sqrt(mat[0][1] * mat[0][1] +
469  mat[0][0] * mat[0][0])));
470  }
471  return V(psi, theta, phi);
472 
473  case XZY_ROTATION:
474 
475  if (isApproxEqual(mat[1][0], ValueType(-1.0), eps)) {
476  theta = ValueType(math::pi<double>() / 2.0);
477  psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
478  phi = -psi;
479  } else if (isApproxEqual(mat[1][0], ValueType(1.0), eps)) {
480  theta = ValueType(-math::pi<double>() / 2.0);
481  psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
482  phi = psi;
483  } else {
484  psi = ValueType(atan2(mat[2][0], mat[0][0]));
485  phi = ValueType(atan2(mat[1][2], mat[1][1]));
486  theta = ValueType(atan2(- mat[1][0],
487  sqrt(mat[1][1] * mat[1][1] +
488  mat[1][2] * mat[1][2])));
489  }
490  return V(phi, psi, theta);
491  }
492 
493  OPENVDB_THROW(NotImplementedError, "Euler extraction sequence not implemented");
494 }
495 
496 
497 /// @brief Return a rotation matrix that maps @a v1 onto @a v2
498 /// about the cross product of @a v1 and @a v2.
499 /// <a name="rotation_v1_v2"></a>
500 template<typename MatType, typename ValueType1, typename ValueType2>
501 inline MatType
503  const Vec3<ValueType1>& _v1,
504  const Vec3<ValueType2>& _v2,
505  typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
506 {
507  using T = typename MatType::value_type;
508 
509  Vec3<T> v1(_v1);
510  Vec3<T> v2(_v2);
511 
512  // Check if v1 and v2 are unit length
513  if (!isApproxEqual(T(1), v1.dot(v1), eps)) {
514  v1.normalize();
515  }
516  if (!isApproxEqual(T(1), v2.dot(v2), eps)) {
517  v2.normalize();
518  }
519 
520  Vec3<T> cross;
521  cross.cross(v1, v2);
522 
523  if (isApproxEqual(cross[0], zeroVal<T>(), eps) &&
524  isApproxEqual(cross[1], zeroVal<T>(), eps) &&
525  isApproxEqual(cross[2], zeroVal<T>(), eps)) {
526 
527 
528  // Given two unit vectors v1 and v2 that are nearly parallel, build a
529  // rotation matrix that maps v1 onto v2. First find which principal axis
530  // p is closest to perpendicular to v1. Find a reflection that exchanges
531  // v1 and p, and find a reflection that exchanges p2 and v2. The desired
532  // rotation matrix is the composition of these two reflections. See the
533  // paper "Efficiently Building a Matrix to Rotate One Vector to
534  // Another" by Tomas Moller and John Hughes in Journal of Graphics
535  // Tools Vol 4, No 4 for details.
536 
537  Vec3<T> u, v, p(0.0, 0.0, 0.0);
538 
539  double x = Abs(v1[0]);
540  double y = Abs(v1[1]);
541  double z = Abs(v1[2]);
542 
543  if (x < y) {
544  if (z < x) {
545  p[2] = 1;
546  } else {
547  p[0] = 1;
548  }
549  } else {
550  if (z < y) {
551  p[2] = 1;
552  } else {
553  p[1] = 1;
554  }
555  }
556  u = p - v1;
557  v = p - v2;
558 
559  double udot = u.dot(u);
560  double vdot = v.dot(v);
561 
562  double a = -2 / udot;
563  double b = -2 / vdot;
564  double c = 4 * u.dot(v) / (udot * vdot);
565 
566  MatType result;
567  result.setIdentity();
568 
569  for (int j = 0; j < 3; j++) {
570  for (int i = 0; i < 3; i++)
571  result[i][j] = static_cast<T>(
572  a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i]);
573  }
574  result[0][0] += 1.0;
575  result[1][1] += 1.0;
576  result[2][2] += 1.0;
577 
578  if(MatType::numColumns() == 4) padMat4(result);
579  return result;
580 
581  } else {
582  double c = v1.dot(v2);
583  double a = (1.0 - c) / cross.dot(cross);
584 
585  double a0 = a * cross[0];
586  double a1 = a * cross[1];
587  double a2 = a * cross[2];
588 
589  double a01 = a0 * cross[1];
590  double a02 = a0 * cross[2];
591  double a12 = a1 * cross[2];
592 
593  MatType r;
594 
595  r[0][0] = static_cast<T>(c + a0 * cross[0]);
596  r[0][1] = static_cast<T>(a01 + cross[2]);
597  r[0][2] = static_cast<T>(a02 - cross[1]);
598  r[1][0] = static_cast<T>(a01 - cross[2]);
599  r[1][1] = static_cast<T>(c + a1 * cross[1]);
600  r[1][2] = static_cast<T>(a12 + cross[0]);
601  r[2][0] = static_cast<T>(a02 + cross[1]);
602  r[2][1] = static_cast<T>(a12 - cross[0]);
603  r[2][2] = static_cast<T>(c + a2 * cross[2]);
604 
605  if(MatType::numColumns() == 4) padMat4(r);
606  return r;
607 
608  }
609 }
610 
611 
612 /// Return a matrix that scales by @a s.
613 template<class MatType>
614 MatType
616 {
617  // Gets identity, then sets top 3 diagonal
618  // Inefficient by 3 sets.
619 
620  MatType result;
621  result.setIdentity();
622  result[0][0] = s[0];
623  result[1][1] = s[1];
624  result[2][2] = s[2];
625 
626  return result;
627 }
628 
629 
630 /// Return a Vec3 representing the lengths of the passed matrix's upper 3&times;3's rows.
631 template<class MatType>
633 getScale(const MatType &mat)
634 {
636  return V(
637  V(mat[0][0], mat[0][1], mat[0][2]).length(),
638  V(mat[1][0], mat[1][1], mat[1][2]).length(),
639  V(mat[2][0], mat[2][1], mat[2][2]).length());
640 }
641 
642 
643 /// @brief Return a copy of the given matrix with its upper 3&times;3 rows normalized.
644 /// @details This can be geometrically interpreted as a matrix with no scaling
645 /// along its major axes.
646 template<class MatType>
647 MatType
648 unit(const MatType &mat, typename MatType::value_type eps = 1.0e-8)
649 {
651  return unit(mat, eps, dud);
652 }
653 
654 
655 /// @brief Return a copy of the given matrix with its upper 3&times;3 rows normalized,
656 /// and return the length of each of these rows in @a scaling.
657 /// @details This can be geometrically interpretted as a matrix with no scaling
658 /// along its major axes, and the scaling in the input vector
659 template<class MatType>
660 MatType
662  const MatType &in,
663  typename MatType::value_type eps,
665 {
666  using T = typename MatType::value_type;
667  MatType result(in);
668 
669  for (int i(0); i < 3; i++) {
670  try {
671  const Vec3<T> u(
672  Vec3<T>(in[i][0], in[i][1], in[i][2]).unit(eps, scaling[i]));
673  for (int j=0; j<3; j++) result[i][j] = u[j];
674  } catch (ArithmeticError&) {
675  for (int j=0; j<3; j++) result[i][j] = 0;
676  }
677  }
678  return result;
679 }
680 
681 
682 /// @brief Set the matrix to a shear along @a axis0 by a fraction of @a axis1.
683 /// @param axis0 The fixed axis of the shear.
684 /// @param axis1 The shear axis.
685 /// @param shear The shear factor.
686 template <class MatType>
687 MatType
688 shear(Axis axis0, Axis axis1, typename MatType::value_type shear)
689 {
690  int index0 = static_cast<int>(axis0);
691  int index1 = static_cast<int>(axis1);
692 
693  MatType result;
694  result.setIdentity();
695  if (axis0 == axis1) {
696  result[index1][index0] = shear + 1;
697  } else {
698  result[index1][index0] = shear;
699  }
700 
701  return result;
702 }
703 
704 
705 /// Return a matrix as the cross product of the given vector.
706 template<class MatType>
707 MatType
709 {
710  using T = typename MatType::value_type;
711 
712  MatType r;
713  r[0][0] = T(0); r[0][1] = skew.z(); r[0][2] = -skew.y();
714  r[1][0] = -skew.z(); r[1][1] = T(0); r[2][1] = skew.x();
715  r[2][0] = skew.y(); r[2][1] = -skew.x(); r[2][2] = T(0);
716 
717  if(MatType::numColumns() == 4) padMat4(r);
718  return r;
719 }
720 
721 
722 /// @brief Return an orientation matrix such that z points along @a direction,
723 /// and y is along the @a direction / @a vertical plane.
724 template<class MatType>
725 MatType
727  const Vec3<typename MatType::value_type>& vertical)
728 {
729  using T = typename MatType::value_type;
730  Vec3<T> forward(direction.unit());
731  Vec3<T> horizontal(vertical.unit().cross(forward).unit());
732  Vec3<T> up(forward.cross(horizontal).unit());
733 
734  MatType r;
735 
736  r[0][0]=horizontal.x(); r[0][1]=horizontal.y(); r[0][2]=horizontal.z();
737  r[1][0]=up.x(); r[1][1]=up.y(); r[1][2]=up.z();
738  r[2][0]=forward.x(); r[2][1]=forward.y(); r[2][2]=forward.z();
739 
740  if(MatType::numColumns() == 4) padMat4(r);
741  return r;
742 }
743 
744 /// @brief This function snaps a specific axis to a specific direction,
745 /// preserving scaling.
746 /// @details It does this using minimum energy, thus posing a unique solution if
747 /// basis & direction aren't parallel.
748 /// @note @a direction need not be unit.
749 template<class MatType>
750 inline MatType
751 snapMatBasis(const MatType& source, Axis axis, const Vec3<typename MatType::value_type>& direction)
752 {
753  using T = typename MatType::value_type;
754 
755  Vec3<T> unitDir(direction.unit());
756  Vec3<T> ourUnitAxis(source.row(axis).unit());
757 
758  // Are the two parallel?
759  T parallel = unitDir.dot(ourUnitAxis);
760 
761  // Already snapped!
762  if (isApproxEqual(parallel, T(1.0))) return source;
763 
764  if (isApproxEqual(parallel, T(-1.0))) {
765  OPENVDB_THROW(ValueError, "Cannot snap to inverse axis");
766  }
767 
768  // Find angle between our basis and the one specified
769  T angleBetween(angle(unitDir, ourUnitAxis));
770  // Caclulate axis to rotate along
771  Vec3<T> rotationAxis = unitDir.cross(ourUnitAxis);
772 
773  MatType rotation;
774  rotation.setToRotation(rotationAxis, angleBetween);
775 
776  return source * rotation;
777 }
778 
779 /// @brief Write 0s along Mat4's last row and column, and a 1 on its diagonal.
780 /// @details Useful initialization when we're initializing just the 3&times;3 block.
781 template<class MatType>
782 inline MatType&
783 padMat4(MatType& dest)
784 {
785  dest[0][3] = dest[1][3] = dest[2][3] = 0;
786  dest[3][2] = dest[3][1] = dest[3][0] = 0;
787  dest[3][3] = 1;
788 
789  return dest;
790 }
791 
792 
793 /// @brief Solve for A=B*B, given A.
794 /// @details Denman-Beavers square root iteration
795 template<typename MatType>
796 inline void
797 sqrtSolve(const MatType& aA, MatType& aB, double aTol=0.01)
798 {
799  unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
800 
801  MatType Y[2], Z[2];
802  Y[0] = aA;
803  Z[0] = MatType::identity();
804 
805  unsigned int current = 0;
806  for (unsigned int iteration=0; iteration < iterations; iteration++) {
807  unsigned int last = current;
808  current = !current;
809 
810  MatType invY = Y[last].inverse();
811  MatType invZ = Z[last].inverse();
812 
813  Y[current] = 0.5 * (Y[last] + invZ);
814  Z[current] = 0.5 * (Z[last] + invY);
815  }
816  aB = Y[current];
817 }
818 
819 
820 template<typename MatType>
821 inline void
822 powSolve(const MatType& aA, MatType& aB, double aPower, double aTol=0.01)
823 {
824  unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
825 
826  const bool inverted = (aPower < 0.0);
827  if (inverted) { aPower = -aPower; }
828 
829  unsigned int whole = static_cast<unsigned int>(aPower);
830  double fraction = aPower - whole;
831 
832  MatType R = MatType::identity();
833  MatType partial = aA;
834 
835  double contribution = 1.0;
836  for (unsigned int iteration = 0; iteration < iterations; iteration++) {
837  sqrtSolve(partial, partial, aTol);
838  contribution *= 0.5;
839  if (fraction >= contribution) {
840  R *= partial;
841  fraction -= contribution;
842  }
843  }
844 
845  partial = aA;
846  while (whole) {
847  if (whole & 1) { R *= partial; }
848  whole >>= 1;
849  if (whole) { partial *= partial; }
850  }
851 
852  if (inverted) { aB = R.inverse(); }
853  else { aB = R; }
854 }
855 
856 
857 /// @brief Determine if a matrix is an identity matrix.
858 template<typename MatType>
859 inline bool
860 isIdentity(const MatType& m)
861 {
862  return m.eq(MatType::identity());
863 }
864 
865 
866 /// @brief Determine if a matrix is invertible.
867 template<typename MatType>
868 inline bool
869 isInvertible(const MatType& m)
870 {
871  using ValueType = typename MatType::ValueType;
872  return !isApproxEqual(m.det(), ValueType(0));
873 }
874 
875 
876 /// @brief Determine if a matrix is symmetric.
877 /// @details This implicitly uses math::isApproxEqual() to determine equality.
878 template<typename MatType>
879 inline bool
880 isSymmetric(const MatType& m)
881 {
882  return m.eq(m.transpose());
883 }
884 
885 
886 /// Determine if a matrix is unitary (i.e., rotation or reflection).
887 template<typename MatType>
888 inline bool
889 isUnitary(const MatType& m)
890 {
891  using ValueType = typename MatType::ValueType;
892  if (!isApproxEqual(std::abs(m.det()), ValueType(1.0))) return false;
893  // check that the matrix transpose is the inverse
894  MatType temp = m * m.transpose();
895  return temp.eq(MatType::identity());
896 }
897 
898 
899 /// Determine if a matrix is diagonal.
900 template<typename MatType>
901 inline bool
902 isDiagonal(const MatType& mat)
903 {
904  int n = MatType::size;
905  typename MatType::ValueType temp(0);
906  for (int i = 0; i < n; ++i) {
907  for (int j = 0; j < n; ++j) {
908  if (i != j) {
909  temp += std::abs(mat(i,j));
910  }
911  }
912  }
913  return isApproxEqual(temp, typename MatType::ValueType(0.0));
914 }
915 
916 
917 /// Return the <i>L</i><sub>&infin;</sub> norm of an <i>N</i>&times;<i>N</i> matrix.
918 template<typename MatType>
919 typename MatType::ValueType
920 lInfinityNorm(const MatType& matrix)
921 {
922  int n = MatType::size;
923  typename MatType::ValueType norm = 0;
924 
925  for( int j = 0; j<n; ++j) {
926  typename MatType::ValueType column_sum = 0;
927 
928  for (int i = 0; i<n; ++i) {
929  column_sum += std::fabs(matrix(i,j));
930  }
931  norm = std::max(norm, column_sum);
932  }
933 
934  return norm;
935 }
936 
937 
938 /// Return the <i>L</i><sub>1</sub> norm of an <i>N</i>&times;<i>N</i> matrix.
939 template<typename MatType>
940 typename MatType::ValueType
941 lOneNorm(const MatType& matrix)
942 {
943  int n = MatType::size;
944  typename MatType::ValueType norm = 0;
945 
946  for( int i = 0; i<n; ++i) {
947  typename MatType::ValueType row_sum = 0;
948 
949  for (int j = 0; j<n; ++j) {
950  row_sum += std::fabs(matrix(i,j));
951  }
952  norm = std::max(norm, row_sum);
953  }
954 
955  return norm;
956 }
957 
958 
959 /// @brief Decompose an invertible 3&times;3 matrix into a unitary matrix
960 /// followed by a symmetric matrix (positive semi-definite Hermitian),
961 /// i.e., M = U * S.
962 /// @details If det(U) = 1 it is a rotation, otherwise det(U) = -1,
963 /// meaning there is some part reflection.
964 /// See "Computing the polar decomposition with applications"
965 /// Higham, N.J. - SIAM J. Sc. Stat Comput 7(4):1160-1174
966 template<typename MatType>
967 bool
968 polarDecomposition(const MatType& input, MatType& unitary,
969  MatType& positive_hermitian, unsigned int MAX_ITERATIONS=100)
970 {
971  unitary = input;
972  MatType new_unitary(input);
973  MatType unitary_inv;
974 
975  if (fabs(unitary.det()) < math::Tolerance<typename MatType::ValueType>::value()) return false;
976 
977  unsigned int iteration(0);
978 
979  typename MatType::ValueType linf_of_u;
980  typename MatType::ValueType l1nm_of_u;
981  typename MatType::ValueType linf_of_u_inv;
982  typename MatType::ValueType l1nm_of_u_inv;
983  typename MatType::ValueType l1_error = 100;
984  double gamma;
985 
986  do {
987  unitary_inv = unitary.inverse();
988  linf_of_u = lInfinityNorm(unitary);
989  l1nm_of_u = lOneNorm(unitary);
990 
991  linf_of_u_inv = lInfinityNorm(unitary_inv);
992  l1nm_of_u_inv = lOneNorm(unitary_inv);
993 
994  gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
995 
996  new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
997 
998  l1_error = lInfinityNorm(unitary - new_unitary);
999  unitary = new_unitary;
1000 
1001  /// this generally converges in less than ten iterations
1002  if (iteration > MAX_ITERATIONS) return false;
1003  iteration++;
1005 
1006  positive_hermitian = unitary.transpose() * input;
1007  return true;
1008 }
1009 
1010 ////////////////////////////////////////
1011 
1012 /// @return true if m0 < m1, comparing components in order of significance.
1013 template<unsigned SIZE, typename T>
1014 inline bool
1016 {
1017  const T* m0p = m0.asPointer();
1018  const T* m1p = m1.asPointer();
1019  constexpr unsigned size = SIZE*SIZE;
1020  for (unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
1021  if (!math::isExactlyEqual(*m0p, *m1p)) return *m0p < *m1p;
1022  }
1023  return *m0p < *m1p;
1024 }
1025 
1026 /// @return true if m0 > m1, comparing components in order of significance.
1027 template<unsigned SIZE, typename T>
1028 inline bool
1030 {
1031  const T* m0p = m0.asPointer();
1032  const T* m1p = m1.asPointer();
1033  constexpr unsigned size = SIZE*SIZE;
1034  for (unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
1035  if (!math::isExactlyEqual(*m0p, *m1p)) return *m0p > *m1p;
1036  }
1037  return *m0p > *m1p;
1038 }
1039 
1040 } // namespace math
1041 } // namespace OPENVDB_VERSION_NAME
1042 } // namespace openvdb
1043 
1044 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:220
Definition: Exceptions.h:65
static unsigned numElements()
Definition: Mat.h:41
Definition: Math.h:904
RotationOrder
Definition: Math.h:908
Definition: Mat.h:26
bool isNan(const float x)
Return true if x is a NaN (Not-A-Number) value.
Definition: Math.h:395
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:708
bool isInfinite() const
True if an Inf is present in this matrix.
Definition: Mat.h:136
const T * asPointer() const
Definition: Mat.h:102
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:74
bool isFinite(const float x)
Return true if x is finite.
Definition: Math.h:375
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:443
Definition: Exceptions.h:56
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:615
T value_type
Definition: Mat.h:29
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:902
bool isNan() const
True if a Nan is present in this matrix.
Definition: Mat.h:128
bool isFinite() const
True if no Nan or Inf values are present.
Definition: Mat.h:144
T absMax() const
Return the maximum of the absolute of all elements in this matrix.
Definition: Mat.h:119
Definition: Mat.h:165
T * operator[](int i)
Array style reference to ith row.
Definition: Mat.h:106
const T * operator[](int i) const
Array style reference to ith row.
Definition: Mat.h:107
static unsigned numColumns()
Definition: Mat.h:40
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:446
Coord Abs(const Coord &xyz)
Definition: Coord.h:517
bool isZero() const
True if all elements are exactly zero.
Definition: Mat.h:152
bool cwiseGreaterThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition: Mat.h:1029
bool polarDecomposition(const MatType &input, MatType &unitary, MatType &positive_hermitian, unsigned int MAX_ITERATIONS=100)
Decompose an invertible 3×3 matrix into a unitary matrix followed by a symmetric matrix (positive sem...
Definition: Mat.h:968
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the L∞ norm of an N×N matrix.
Definition: Mat.h:920
bool isZero(const Type &x)
Return true if x is exactly equal to zero.
Definition: Math.h:337
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:860
T & y()
Definition: Vec3.h:86
bool cwiseLessThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition: Mat.h:1015
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:374
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:889
Definition: Math.h:902
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:191
Axis
Definition: Math.h:901
MatType snapMatBasis(const MatType &source, Axis axis, const Vec3< typename MatType::value_type > &direction)
This function snaps a specific axis to a specific direction, preserving scaling.
Definition: Mat.h:751
Vec3< typename MatType::value_type > eulerAngles(const MatType &mat, RotationOrder rotationOrder, typename MatType::value_type eps=static_cast< typename MatType::value_type >(1.0e-8))
Return the Euler angles composing the given rotation matrix.
Definition: Mat.h:333
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:822
Definition: Math.h:903
bool isApproxEqual(const Type &a, const Type &b, const Type &tolerance)
Return true if a is equal to b to within the given tolerance.
Definition: Math.h:406
Definition: Exceptions.h:13
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:451
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:797
MatType unit(const MatType &in, typename MatType::value_type eps, Vec3< typename MatType::value_type > &scaling)
Return a copy of the given matrix with its upper 3×3 rows normalized, and return the length of each o...
Definition: Mat.h:661
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:869
T ValueType
Definition: Mat.h:30
T & z()
Definition: Vec3.h:87
void write(std::ostream &os) const
Definition: Mat.h:110
Definition: Mat.h:164
T & w()
Definition: Quat.h:196
MatType aim(const Vec3< typename MatType::value_type > &direction, const Vec3< typename MatType::value_type > &vertical)
Return an orientation matrix such that z points along direction, and y is along the direction / verti...
Definition: Mat.h:726
Definition: Exceptions.h:61
MatType & padMat4(MatType &dest)
Write 0s along Mat4&#39;s last row and column, and a 1 on its diagonal.
Definition: Mat.h:783
MatType::ValueType lOneNorm(const MatType &matrix)
Return the L1 norm of an N×N matrix.
Definition: Mat.h:941
Vec3< typename MatType::value_type > getScale(const MatType &mat)
Return a Vec3 representing the lengths of the passed matrix&#39;s upper 3×3&#39;s rows.
Definition: Mat.h:633
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition: Mat.h:92
static unsigned numRows()
Definition: Mat.h:39
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:193
Tolerance for floating-point comparison.
Definition: Math.h:148
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:880
const std::enable_if<!VecTraits< T >::IsVec, T >::type & max(const T &a, const T &b)
Definition: Composite.h:110
void read(std::istream &is)
Definition: Mat.h:114
T * asPointer()
Direct access to the internal data.
Definition: Mat.h:101
bool isInfinite(const float x)
Return true if x is an infinity value (either positive infinity or negative infinity).
Definition: Math.h:385
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h.in:121
MatType rotation(const Vec3< ValueType1 > &_v1, const Vec3< ValueType2 > &_v2, typename MatType::value_type eps=static_cast< typename MatType::value_type >(1.0e-8))
Return a rotation matrix that maps v1 onto v2 about the cross product of v1 and v2.
Definition: Mat.h:502
std::string str(unsigned indentation=0) const
Definition: Mat.h:53
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:362
T & y()
Definition: Quat.h:194
T & z()
Definition: Quat.h:195
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:85
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h.in:212
MatType shear(Axis axis0, Axis axis1, typename MatType::value_type shear)
Set the matrix to a shear along axis0 by a fraction of axis1.
Definition: Mat.h:688