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