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