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