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