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