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