OpenVDB  3.2.0
Mat.h
Go to the documentation of this file.
1 //
3 // Copyright (c) 2012-2016 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 //
30 //
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 <boost/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 
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 
66  Mat() { }
67 
69  Mat(Mat const &src) {
70  for (unsigned i(0); i < numElements(); ++i) {
71  mm[i] = src.mm[i];
72  }
73  }
74 
84  std::string
85  str(unsigned indentation = 0) const {
86 
87  std::string ret;
88  std::string indent;
89 
90  // We add +1 since we're indenting one for the first '['
91  indent.append(indentation+1, ' ');
92 
93  ret.append("[");
94 
95  // For each row,
96  for (unsigned i(0); i < SIZE; i++) {
97 
98  ret.append("[");
99 
100  // For each column
101  for (unsigned j(0); j < SIZE; j++) {
102 
103  // Put a comma after everything except the last
104  if (j) ret.append(", ");
105  ret.append((boost::format("%1%") % mm[(i*SIZE)+j]).str());
106  }
107 
108  ret.append("]");
109 
110  // At the end of every row (except the last)...
111  if (i < SIZE-1 )
112  // ...suffix the row bracket with a comma, newline, and
113  // advance indentation
114  ret.append((boost::format(",\n%1%") % indent).str());
115  }
116 
117  ret.append("]");
118 
119  return ret;
120  }
121 
123  friend std::ostream& operator<<(
124  std::ostream& ostr,
125  const Mat<SIZE, T>& m)
126  {
127  ostr << m.str();
128  return ostr;
129  }
130 
131  void write(std::ostream& os) const {
132  os.write(reinterpret_cast<const char*>(&mm), sizeof(T)*SIZE*SIZE);
133  }
134 
135  void read(std::istream& is) {
136  is.read(reinterpret_cast<char*>(&mm), sizeof(T)*SIZE*SIZE);
137  }
138 
139 
140 protected:
141  T mm[SIZE*SIZE];
142 };
143 
144 
145 template<typename T> class Quat;
146 template<typename T> class Vec3;
147 
151 template<class MatType>
152 MatType
154  typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
155 {
156  typedef typename MatType::value_type T;
157 
158  T qdot(q.dot(q));
159  T s(0);
160 
161  if (!isApproxEqual(qdot, T(0.0),eps)) {
162  s = T(2.0 / qdot);
163  }
164 
165  T x = s*q.x();
166  T y = s*q.y();
167  T z = s*q.z();
168  T wx = x*q.w();
169  T wy = y*q.w();
170  T wz = z*q.w();
171  T xx = x*q.x();
172  T xy = y*q.x();
173  T xz = z*q.x();
174  T yy = y*q.y();
175  T yz = z*q.y();
176  T zz = z*q.z();
177 
178  MatType r;
179  r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
180  r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
181  r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
182 
183  if(MatType::numColumns() == 4) padMat4(r);
184  return r;
185 }
186 
187 
188 
192 template<class MatType>
193 MatType
194 rotation(Axis axis, typename MatType::value_type angle)
195 {
196  typedef typename MatType::value_type T;
197  T c = static_cast<T>(cos(angle));
198  T s = static_cast<T>(sin(angle));
199 
200  MatType result;
201  result.setIdentity();
202 
203  switch (axis) {
204  case X_AXIS:
205  result[1][1] = c;
206  result[1][2] = s;
207  result[2][1] = -s;
208  result[2][2] = c;
209  return result;
210  case Y_AXIS:
211  result[0][0] = c;
212  result[0][2] = -s;
213  result[2][0] = s;
214  result[2][2] = c;
215  return result;
216  case Z_AXIS:
217  result[0][0] = c;
218  result[0][1] = s;
219  result[1][0] = -s;
220  result[1][1] = c;
221  return result;
222  default:
223  throw ValueError("Unrecognized rotation axis");
224  }
225 }
226 
227 
230 template<class MatType>
231 MatType
232 rotation(const Vec3<typename MatType::value_type> &_axis, typename MatType::value_type angle)
233 {
234  typedef typename MatType::value_type T;
235  T txy, txz, tyz, sx, sy, sz;
236 
237  Vec3<T> axis(_axis.unit());
238 
239  // compute trig properties of angle:
240  T c(cos(double(angle)));
241  T s(sin(double(angle)));
242  T t(1 - c);
243 
244  MatType result;
245  // handle diagonal elements
246  result[0][0] = axis[0]*axis[0] * t + c;
247  result[1][1] = axis[1]*axis[1] * t + c;
248  result[2][2] = axis[2]*axis[2] * t + c;
249 
250  txy = axis[0]*axis[1] * t;
251  sz = axis[2] * s;
252 
253  txz = axis[0]*axis[2] * t;
254  sy = axis[1] * s;
255 
256  tyz = axis[1]*axis[2] * t;
257  sx = axis[0] * s;
258 
259  // right handed space
260  // Contribution from rotation about 'z'
261  result[0][1] = txy + sz;
262  result[1][0] = txy - sz;
263  // Contribution from rotation about 'y'
264  result[0][2] = txz - sy;
265  result[2][0] = txz + sy;
266  // Contribution from rotation about 'x'
267  result[1][2] = tyz + sx;
268  result[2][1] = tyz - sx;
269 
270  if(MatType::numColumns() == 4) padMat4(result);
271  return MatType(result);
272 }
273 
274 
312 template<class MatType>
315  const MatType& mat,
316  RotationOrder rotationOrder,
317  typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
318 {
319  typedef typename MatType::value_type ValueType;
320  typedef Vec3<ValueType> V;
321  ValueType phi, theta, psi;
322 
323  switch(rotationOrder)
324  {
325  case XYZ_ROTATION:
326  if (isApproxEqual(mat[2][0], ValueType(1.0), eps)) {
327  theta = ValueType(M_PI_2);
328  phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
329  psi = phi;
330  } else if (isApproxEqual(mat[2][0], ValueType(-1.0), eps)) {
331  theta = ValueType(-M_PI_2);
332  phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
333  psi = -phi;
334  } else {
335  psi = ValueType(atan2(-mat[1][0],mat[0][0]));
336  phi = ValueType(atan2(-mat[2][1],mat[2][2]));
337  theta = ValueType(atan2(mat[2][0],
338  sqrt( mat[2][1]*mat[2][1] +
339  mat[2][2]*mat[2][2])));
340  }
341  return V(phi, theta, psi);
342  case ZXY_ROTATION:
343  if (isApproxEqual(mat[1][2], ValueType(1.0), eps)) {
344  theta = ValueType(M_PI_2);
345  phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
346  psi = phi;
347  } else if (isApproxEqual(mat[1][2], ValueType(-1.0), eps)) {
348  theta = ValueType(-M_PI/2);
349  phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
350  psi = -phi;
351  } else {
352  psi = ValueType(atan2(-mat[0][2], mat[2][2]));
353  phi = ValueType(atan2(-mat[1][0], mat[1][1]));
354  theta = ValueType(atan2(mat[1][2],
355  sqrt(mat[0][2] * mat[0][2] +
356  mat[2][2] * mat[2][2])));
357  }
358  return V(theta, psi, phi);
359 
360  case YZX_ROTATION:
361  if (isApproxEqual(mat[0][1], ValueType(1.0), eps)) {
362  theta = ValueType(M_PI_2);
363  phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
364  psi = phi;
365  } else if (isApproxEqual(mat[0][1], ValueType(-1.0), eps)) {
366  theta = ValueType(-M_PI/2);
367  phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
368  psi = -phi;
369  } else {
370  psi = ValueType(atan2(-mat[2][1], mat[1][1]));
371  phi = ValueType(atan2(-mat[0][2], mat[0][0]));
372  theta = ValueType(atan2(mat[0][1],
373  sqrt(mat[0][0] * mat[0][0] +
374  mat[0][2] * mat[0][2])));
375  }
376  return V(psi, phi, theta);
377 
378  case XZX_ROTATION:
379 
380  if (isApproxEqual(mat[0][0], ValueType(1.0), eps)) {
381  theta = ValueType(0.0);
382  phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
383  psi = phi;
384  } else if (isApproxEqual(mat[0][0], ValueType(-1.0), eps)) {
385  theta = ValueType(M_PI);
386  psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
387  phi = - psi;
388  } else {
389  psi = ValueType(atan2(mat[2][0], -mat[1][0]));
390  phi = ValueType(atan2(mat[0][2], mat[0][1]));
391  theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
392  mat[0][2] * mat[0][2]),
393  mat[0][0]));
394  }
395  return V(phi, psi, theta);
396 
397  case ZXZ_ROTATION:
398 
399  if (isApproxEqual(mat[2][2], ValueType(1.0), eps)) {
400  theta = ValueType(0.0);
401  phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
402  psi = phi;
403  } else if (isApproxEqual(mat[2][2], ValueType(-1.0), eps)) {
404  theta = ValueType(M_PI);
405  phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
406  psi = -phi;
407  } else {
408  psi = ValueType(atan2(mat[0][2], mat[1][2]));
409  phi = ValueType(atan2(mat[2][0], -mat[2][1]));
410  theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
411  mat[1][2] * mat[1][2]),
412  mat[2][2]));
413  }
414  return V(theta, psi, phi);
415 
416  case YXZ_ROTATION:
417 
418  if (isApproxEqual(mat[2][1], ValueType(1.0), eps)) {
419  theta = ValueType(-M_PI_2);
420  phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
421  psi = phi;
422  } else if (isApproxEqual(mat[2][1], ValueType(-1.0), eps)) {
423  theta = ValueType(M_PI_2);
424  phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
425  psi = -phi;
426  } else {
427  psi = ValueType(atan2(mat[0][1], mat[1][1]));
428  phi = ValueType(atan2(mat[2][0], mat[2][2]));
429  theta = ValueType(atan2(-mat[2][1],
430  sqrt(mat[0][1] * mat[0][1] +
431  mat[1][1] * mat[1][1])));
432  }
433  return V(theta, phi, psi);
434 
435  case ZYX_ROTATION:
436 
437  if (isApproxEqual(mat[0][2], ValueType(1.0), eps)) {
438  theta = ValueType(-M_PI_2);
439  phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
440  psi = phi;
441  } else if (isApproxEqual(mat[0][2], ValueType(-1.0), eps)) {
442  theta = ValueType(M_PI_2);
443  phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
444  psi = -phi;
445  } else {
446  psi = ValueType(atan2(mat[1][2], mat[2][2]));
447  phi = ValueType(atan2(mat[0][1], mat[0][0]));
448  theta = ValueType(atan2(-mat[0][2],
449  sqrt(mat[0][1] * mat[0][1] +
450  mat[0][0] * mat[0][0])));
451  }
452  return V(psi, theta, phi);
453 
454  case XZY_ROTATION:
455 
456  if (isApproxEqual(mat[1][0], ValueType(-1.0), eps)) {
457  theta = ValueType(M_PI_2);
458  psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
459  phi = -psi;
460  } else if (isApproxEqual(mat[1][0], ValueType(1.0), eps)) {
461  theta = ValueType(-M_PI_2);
462  psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
463  phi = psi;
464  } else {
465  psi = ValueType(atan2(mat[2][0], mat[0][0]));
466  phi = ValueType(atan2(mat[1][2], mat[1][1]));
467  theta = ValueType(atan2(- mat[1][0],
468  sqrt(mat[1][1] * mat[1][1] +
469  mat[1][2] * mat[1][2])));
470  }
471  return V(phi, psi, theta);
472  }
473 
474  OPENVDB_THROW(NotImplementedError, "Euler extraction sequence not implemented");
475 }
476 
477 
480 template<class MatType>
481 MatType
485  typename MatType::value_type eps=1.0e-8)
486 {
487  typedef typename MatType::value_type T;
488  Vec3<T> v1(_v1);
489  Vec3<T> v2(_v2);
490 
491  // Check if v1 and v2 are unit length
492  if (!isApproxEqual(1.0, v1.dot(v1), eps)) {
493  v1.normalize();
494  }
495  if (!isApproxEqual(1.0, v2.dot(v2), eps)) {
496  v2.normalize();
497  }
498 
499  Vec3<T> cross;
500  cross.cross(v1, v2);
501 
502  if (isApproxEqual(cross[0], 0.0, eps) &&
503  isApproxEqual(cross[1], 0.0, eps) &&
504  isApproxEqual(cross[2], 0.0, eps)) {
505 
506 
507  // Given two unit vectors v1 and v2 that are nearly parallel, build a
508  // rotation matrix that maps v1 onto v2. First find which principal axis
509  // p is closest to perpendicular to v1. Find a reflection that exchanges
510  // v1 and p, and find a reflection that exchanges p2 and v2. The desired
511  // rotation matrix is the composition of these two reflections. See the
512  // paper "Efficiently Building a Matrix to Rotate One Vector to
513  // Another" by Tomas Moller and John Hughes in Journal of Graphics
514  // Tools Vol 4, No 4 for details.
515 
516  Vec3<T> u, v, p(0.0, 0.0, 0.0);
517 
518  double x = Abs(v1[0]);
519  double y = Abs(v1[1]);
520  double z = Abs(v1[2]);
521 
522  if (x < y) {
523  if (z < x) {
524  p[2] = 1;
525  } else {
526  p[0] = 1;
527  }
528  } else {
529  if (z < y) {
530  p[2] = 1;
531  } else {
532  p[1] = 1;
533  }
534  }
535  u = p - v1;
536  v = p - v2;
537 
538  double udot = u.dot(u);
539  double vdot = v.dot(v);
540 
541  double a = -2 / udot;
542  double b = -2 / vdot;
543  double c = 4 * u.dot(v) / (udot * vdot);
544 
545  MatType result;
546  result.setIdentity();
547 
548  for (int j = 0; j < 3; j++) {
549  for (int i = 0; i < 3; i++)
550  result[i][j] =
551  a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i];
552  }
553  result[0][0] += 1.0;
554  result[1][1] += 1.0;
555  result[2][2] += 1.0;
556 
557  if(MatType::numColumns() == 4) padMat4(result);
558  return result;
559 
560  } else {
561  double c = v1.dot(v2);
562  double a = (1.0 - c) / cross.dot(cross);
563 
564  double a0 = a * cross[0];
565  double a1 = a * cross[1];
566  double a2 = a * cross[2];
567 
568  double a01 = a0 * cross[1];
569  double a02 = a0 * cross[2];
570  double a12 = a1 * cross[2];
571 
572  MatType r;
573 
574  r[0][0] = c + a0 * cross[0];
575  r[0][1] = a01 + cross[2];
576  r[0][2] = a02 - cross[1],
577  r[1][0] = a01 - cross[2];
578  r[1][1] = c + a1 * cross[1];
579  r[1][2] = a12 + cross[0];
580  r[2][0] = a02 + cross[1];
581  r[2][1] = a12 - cross[0];
582  r[2][2] = c + a2 * cross[2];
583 
584  if(MatType::numColumns() == 4) padMat4(r);
585  return r;
586 
587  }
588 }
589 
590 
592 template<class MatType>
593 MatType
595 {
596  // Gets identity, then sets top 3 diagonal
597  // Inefficient by 3 sets.
598 
599  MatType result;
600  result.setIdentity();
601  result[0][0] = s[0];
602  result[1][1] = s[1];
603  result[2][2] = s[2];
604 
605  return result;
606 }
607 
608 
610 template<class MatType>
612 getScale(const MatType &mat)
613 {
615  return V(
616  V(mat[0][0], mat[0][1], mat[0][2]).length(),
617  V(mat[1][0], mat[1][1], mat[1][2]).length(),
618  V(mat[2][0], mat[2][1], mat[2][2]).length());
619 }
620 
621 
625 template<class MatType>
626 MatType
627 unit(const MatType &mat, typename MatType::value_type eps = 1.0e-8)
628 {
630  return unit(mat, eps, dud);
631 }
632 
633 
638 template<class MatType>
639 MatType
641  const MatType &in,
642  typename MatType::value_type eps,
644 {
645  typedef typename MatType::value_type T;
646  MatType result(in);
647 
648  for (int i(0); i < 3; i++) {
649  try {
650  const Vec3<T> u(
651  Vec3<T>(in[i][0], in[i][1], in[i][2]).unit(eps, scaling[i]));
652  for (int j=0; j<3; j++) result[i][j] = u[j];
653  } catch (ArithmeticError&) {
654  for (int j=0; j<3; j++) result[i][j] = 0;
655  }
656  }
657  return result;
658 }
659 
660 
665 template <class MatType>
666 MatType
667 shear(Axis axis0, Axis axis1, typename MatType::value_type shear)
668 {
669  int index0 = static_cast<int>(axis0);
670  int index1 = static_cast<int>(axis1);
671 
672  MatType result;
673  result.setIdentity();
674  if (axis0 == axis1) {
675  result[index1][index0] = shear + 1;
676  } else {
677  result[index1][index0] = shear;
678  }
679 
680  return result;
681 }
682 
683 
685 template<class MatType>
686 MatType
688 {
689  typedef typename MatType::value_type T;
690 
691  MatType r;
692  r[0][0] = T(0); r[0][1] = skew.z(); r[0][2] = -skew.y();
693  r[1][0] = -skew.z(); r[1][1] = T(0); r[2][1] = skew.x();
694  r[2][0] = skew.y(); r[2][1] = -skew.x(); r[2][2] = T(0);
695 
696  if(MatType::numColumns() == 4) padMat4(r);
697  return r;
698 }
699 
700 
703 template<class MatType>
704 MatType
706  const Vec3<typename MatType::value_type>& vertical)
707 {
708  typedef typename MatType::value_type T;
709  Vec3<T> forward(direction.unit());
710  Vec3<T> horizontal(vertical.unit().cross(forward).unit());
711  Vec3<T> up(forward.cross(horizontal).unit());
712 
713  MatType r;
714 
715  r[0][0]=horizontal.x(); r[0][1]=horizontal.y(); r[0][2]=horizontal.z();
716  r[1][0]=up.x(); r[1][1]=up.y(); r[1][2]=up.z();
717  r[2][0]=forward.x(); r[2][1]=forward.y(); r[2][2]=forward.z();
718 
719  if(MatType::numColumns() == 4) padMat4(r);
720  return r;
721 }
722 
728 template<class MatType>
729 inline MatType
730 snapMatBasis(const MatType& source, Axis axis, const Vec3<typename MatType::value_type>& direction)
731 {
732  typedef typename MatType::value_type T;
733 
734  Vec3<T> unitDir(direction.unit());
735  Vec3<T> ourUnitAxis(source.row(axis).unit());
736 
737  // Are the two parallel?
738  T parallel = unitDir.dot(ourUnitAxis);
739 
740  // Already snapped!
741  if (isApproxEqual(parallel, T(1.0))) return source;
742 
743  if (isApproxEqual(parallel, T(-1.0))) {
744  OPENVDB_THROW(ValueError, "Cannot snap to inverse axis");
745  }
746 
747  // Find angle between our basis and the one specified
748  T angleBetween(angle(unitDir, ourUnitAxis));
749  // Caclulate axis to rotate along
750  Vec3<T> rotationAxis = unitDir.cross(ourUnitAxis);
751 
752  MatType rotation;
753  rotation.setToRotation(rotationAxis, angleBetween);
754 
755  return source * rotation;
756 }
757 
760 template<class MatType>
761 static MatType&
762 padMat4(MatType& dest)
763 {
764  dest[0][3] = dest[1][3] = dest[2][3] = 0;
765  dest[3][2] = dest[3][1] = dest[3][0] = 0;
766  dest[3][3] = 1;
767 
768  return dest;
769 }
770 
771 
774 template <typename MatType>
775 inline void
776 sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
777 {
778  unsigned int iterations = (unsigned int)(log(aTol)/log(0.5));
779  MatType Y[2];
780  MatType Z[2];
781  MatType invY;
782  MatType invZ;
783 
784  unsigned int current = 0;
785 
786  Y[0]=aA;
787  Z[0] = MatType::identity();
788 
789  unsigned int iteration;
790  for (iteration=0; iteration<iterations; iteration++)
791  {
792  unsigned int last = current;
793  current = !current;
794 
795  invY = Y[last].inverse();
796  invZ = Z[last].inverse();
797 
798  Y[current]=0.5*(Y[last]+invZ);
799  Z[current]=0.5*(Z[last]+invY);
800  }
801 
802  MatType &R = Y[current];
803 
804  aB=R;
805 }
806 
807 
808 template <typename MatType>
809 inline void
810 powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
811 {
812  unsigned int iterations = (unsigned int)(log(aTol)/log(0.5));
813 
814  const bool inverted = ( aPower < 0.0 );
815 
816  if (inverted) {
817  aPower = -aPower;
818  }
819 
820  unsigned int whole = (unsigned int)aPower;
821  double fraction = aPower - whole;
822 
823  MatType R;
824  R = MatType::identity();
825 
826  MatType partial = aA;
827 
828  double contribution = 1.0;
829 
830  unsigned int iteration;
831 
832  for (iteration=0; iteration< iterations; iteration++)
833  {
834  sqrtSolve(partial, partial, aTol);
835  contribution *= 0.5;
836 
837  if (fraction>=contribution)
838  {
839  R *= partial;
840  fraction-=contribution;
841  }
842  }
843 
844  partial = aA;
845  while (whole)
846  {
847  if (whole & 1) {
848  R *= partial;
849  }
850  whole>>=1;
851  if(whole) {
852  partial*=partial;
853  }
854  }
855 
856  if (inverted) {
857  aB = R.inverse();
858  }
859  else {
860  aB = R;
861  }
862 }
863 
864 
866 template<typename MatType>
867 inline bool
868 isIdentity(const MatType& m)
869 {
870  return m.eq(MatType::identity());
871 }
872 
873 
875 template<typename MatType>
876 inline bool
877 isInvertible(const MatType& m)
878 {
879  typedef typename MatType::ValueType value_type;
880  return !isApproxEqual(m.det(), (value_type)0);
881 }
882 
883 
886 template<typename MatType>
887 inline bool
888 isSymmetric(const MatType& m)
889 {
890  return m.eq(m.transpose());
891 }
892 
893 
895 template<typename MatType>
896 inline bool
897 isUnitary(const MatType& m)
898 {
899  typedef typename MatType::ValueType value_type;
900  if (!isApproxEqual(std::abs(m.det()), value_type(1.0))) return false;
901  // check that the matrix transpose is the inverse
902  MatType temp = m * m.transpose();
903  return temp.eq(MatType::identity());
904 }
905 
906 
908 template<typename MatType>
909 inline bool
910 isDiagonal(const MatType& mat)
911 {
912  int n = MatType::size;
913  typename MatType::ValueType temp(0);
914  for (int i = 0; i < n; ++i) {
915  for (int j = 0; j < n; ++j) {
916  if (i != j) {
917  temp+=std::abs(mat(i,j));
918  }
919  }
920  }
921  return isApproxEqual(temp, typename MatType::ValueType(0.0));
922 }
923 
924 
926 template<typename MatType>
927 typename MatType::ValueType
928 lInfinityNorm(const MatType& matrix)
929 {
930  int n = MatType::size;
931  typename MatType::ValueType norm = 0;
932 
933  for( int j = 0; j<n; ++j) {
934  typename MatType::ValueType column_sum = 0;
935 
936  for (int i = 0; i<n; ++i) {
937  column_sum += fabs(matrix(i,j));
938  }
939  norm = std::max(norm, column_sum);
940  }
941 
942  return norm;
943 }
944 
945 
947 template<typename MatType>
948 typename MatType::ValueType
949 lOneNorm(const MatType& matrix)
950 {
951  int n = MatType::size;
952  typename MatType::ValueType norm = 0;
953 
954  for( int i = 0; i<n; ++i) {
955  typename MatType::ValueType row_sum = 0;
956 
957  for (int j = 0; j<n; ++j) {
958  row_sum += fabs(matrix(i,j));
959  }
960  norm = std::max(norm, row_sum);
961  }
962 
963  return norm;
964 }
965 
966 
974 template<typename MatType>
975 bool
976 polarDecomposition(const MatType& input, MatType& unitary,
977  MatType& positive_hermitian, unsigned int MAX_ITERATIONS=100)
978 {
979  unitary = input;
980  MatType new_unitary(input);
981  MatType unitary_inv;
982 
983  if (fabs(unitary.det()) < math::Tolerance<typename MatType::ValueType>::value()) return false;
984 
985  unsigned int iteration(0);
986 
987  typename MatType::ValueType linf_of_u;
988  typename MatType::ValueType l1nm_of_u;
989  typename MatType::ValueType linf_of_u_inv;
990  typename MatType::ValueType l1nm_of_u_inv;
991  typename MatType::ValueType l1_error = 100;
992  double gamma;
993 
994  do {
995  unitary_inv = unitary.inverse();
996  linf_of_u = lInfinityNorm(unitary);
997  l1nm_of_u = lOneNorm(unitary);
998 
999  linf_of_u_inv = lInfinityNorm(unitary_inv);
1000  l1nm_of_u_inv = lOneNorm(unitary_inv);
1001 
1002  gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
1003 
1004  new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
1005 
1006  l1_error = lInfinityNorm(unitary - new_unitary);
1007  unitary = new_unitary;
1008 
1010  if (iteration > MAX_ITERATIONS) return false;
1011  iteration++;
1013 
1014  positive_hermitian = unitary.transpose() * input;
1015  return true;
1016 }
1017 
1018 } // namespace math
1019 } // namespace OPENVDB_VERSION_NAME
1020 } // namespace openvdb
1021 
1022 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
1023 
1024 // Copyright (c) 2012-2016 DreamWorks Animation LLC
1025 // All rights reserved. This software is distributed under the
1026 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )
Mat(Mat const &src)
Copy constructor. Used when the class signature matches exactly.
Definition: Mat.h:69
T & z()
Definition: Quat.h:225
T & y()
Definition: Quat.h:224
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:776
Coord Abs(const Coord &xyz)
Definition: Coord.h:247
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:203
std::string str(unsigned indentation=0) const
Definition: Mat.h:85
T & z()
Definition: Vec3.h:99
MatType rotation(const Vec3< typename MatType::value_type > &_v1, const Vec3< typename MatType::value_type > &_v2, typename MatType::value_type eps=1.0e-8)
Return a rotation matrix that maps v1 onto v2 about the cross product of v1 and v2.
Definition: Mat.h:482
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
Definition: Math.h:857
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:360
Definition: Mat.h:146
Definition: Mat.h:145
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:97
T mm[SIZE *SIZE]
Definition: Mat.h:141
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:897
T & w()
Definition: Quat.h:226
Definition: Exceptions.h:78
T ValueType
Definition: Mat.h:56
Definition: Math.h:858
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:928
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:667
const boost::disable_if_c< VecTraits< T >::IsVec, T >::type & max(const T &a, const T &b)
Definition: Composite.h:132
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition: Mat.h:123
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
static unsigned numRows()
Definition: Mat.h:60
RotationOrder
Definition: Math.h:863
MatType unit(const MatType &in, typename MatType::value_type eps, Vec3< typename MatType::value_type > &scaling)
Return a copy of the given matrix with its upper 3x3 rows normalized, and return the length of each o...
Definition: Mat.h:640
#define OPENVDB_VERSION_NAME
Definition: version.h:43
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:97
T value_type
Definition: Mat.h:55
Definition: Exceptions.h:39
Tolerance for floating-point comparison.
Definition: Math.h:125
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:594
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:446
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:223
Mat()
Definition: Mat.h:66
MatType::ValueType lOneNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:949
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:348
Definition: Math.h:859
Vec3< typename MatType::value_type > getScale(const MatType &mat)
Return a Vec3 representing the lengths of the passed matrix&#39;s upper 3x3&#39;s rows.
Definition: Mat.h:612
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:232
void read(std::istream &is)
Definition: Mat.h:135
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:877
T & y()
Definition: Vec3.h:98
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:705
static unsigned numColumns()
Definition: Mat.h:61
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:314
Definition: Exceptions.h:84
Definition: Exceptions.h:88
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:730
Axis
Definition: Math.h:856
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:687
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:810
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:71
Definition: Mat.h:52
static MatType & padMat4(MatType &dest)
Write 0s along Mat4&#39;s last row and column, and a 1 on its diagonal.
Definition: Mat.h:762
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:888
void write(std::ostream &os) const
Definition: Mat.h:131
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:492
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:910
static unsigned numElements()
Definition: Mat.h:62
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:868
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:976