OpenVDB  4.0.2
Mat.h
Go to the documentation of this file.
1 //
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 //
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 <openvdb/Exceptions.h>
39 #include <boost/format.hpp>
40 #include <cmath>
41 #include <iostream>
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 
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 
93  std::string
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((boost::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((boost::format(",\n%1%") % indent).str());
124  }
125 
126  ret.append("]");
127 
128  return ret;
129  }
130 
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 
149  T absMax() const {
150  T x = static_cast<T>(std::fabs(mm[0]));
151  for (unsigned i = 1; i < numElements(); ++i) {
152  x = std::max(x, static_cast<T>(std::fabs(mm[i])));
153  }
154  return x;
155  }
156 
158  bool isNan() const {
159  for (unsigned i = 0; i < numElements(); ++i) {
160  if (std::isnan(mm[i])) return true;
161  }
162  return false;
163  }
164 
166  bool isInfinite() const {
167  for (unsigned i = 0; i < numElements(); ++i) {
168  if (std::isinf(mm[i])) return true;
169  }
170  return false;
171  }
172 
174  bool isFinite() const {
175  for (unsigned i = 0; i < numElements(); ++i) {
176  if (!std::isfinite(mm[i])) return false;
177  }
178  return true;
179  }
180 
182  bool isZero() const {
183  for (unsigned i = 0; i < numElements(); ++i) {
184  if (!isZero(mm[i])) return false;
185  }
186  return true;
187  }
188 
189 protected:
190  T mm[SIZE*SIZE];
191 };
192 
193 
194 template<typename T> class Quat;
195 template<typename T> class Vec3;
196 
200 template<class MatType>
201 MatType
203  typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
204 {
205  typedef typename MatType::value_type T;
206 
207  T qdot(q.dot(q));
208  T s(0);
209 
210  if (!isApproxEqual(qdot, T(0.0),eps)) {
211  s = T(2.0 / qdot);
212  }
213 
214  T x = s*q.x();
215  T y = s*q.y();
216  T z = s*q.z();
217  T wx = x*q.w();
218  T wy = y*q.w();
219  T wz = z*q.w();
220  T xx = x*q.x();
221  T xy = y*q.x();
222  T xz = z*q.x();
223  T yy = y*q.y();
224  T yz = z*q.y();
225  T zz = z*q.z();
226 
227  MatType r;
228  r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
229  r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
230  r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
231 
232  if(MatType::numColumns() == 4) padMat4(r);
233  return r;
234 }
235 
236 
237 
241 template<class MatType>
242 MatType
243 rotation(Axis axis, typename MatType::value_type angle)
244 {
245  typedef typename MatType::value_type T;
246  T c = static_cast<T>(cos(angle));
247  T s = static_cast<T>(sin(angle));
248 
249  MatType result;
250  result.setIdentity();
251 
252  switch (axis) {
253  case X_AXIS:
254  result[1][1] = c;
255  result[1][2] = s;
256  result[2][1] = -s;
257  result[2][2] = c;
258  return result;
259  case Y_AXIS:
260  result[0][0] = c;
261  result[0][2] = -s;
262  result[2][0] = s;
263  result[2][2] = c;
264  return result;
265  case Z_AXIS:
266  result[0][0] = c;
267  result[0][1] = s;
268  result[1][0] = -s;
269  result[1][1] = c;
270  return result;
271  default:
272  throw ValueError("Unrecognized rotation axis");
273  }
274 }
275 
276 
279 template<class MatType>
280 MatType
281 rotation(const Vec3<typename MatType::value_type> &_axis, typename MatType::value_type angle)
282 {
283  typedef typename MatType::value_type T;
284  T txy, txz, tyz, sx, sy, sz;
285 
286  Vec3<T> axis(_axis.unit());
287 
288  // compute trig properties of angle:
289  T c(cos(double(angle)));
290  T s(sin(double(angle)));
291  T t(1 - c);
292 
293  MatType result;
294  // handle diagonal elements
295  result[0][0] = axis[0]*axis[0] * t + c;
296  result[1][1] = axis[1]*axis[1] * t + c;
297  result[2][2] = axis[2]*axis[2] * t + c;
298 
299  txy = axis[0]*axis[1] * t;
300  sz = axis[2] * s;
301 
302  txz = axis[0]*axis[2] * t;
303  sy = axis[1] * s;
304 
305  tyz = axis[1]*axis[2] * t;
306  sx = axis[0] * s;
307 
308  // right handed space
309  // Contribution from rotation about 'z'
310  result[0][1] = txy + sz;
311  result[1][0] = txy - sz;
312  // Contribution from rotation about 'y'
313  result[0][2] = txz - sy;
314  result[2][0] = txz + sy;
315  // Contribution from rotation about 'x'
316  result[1][2] = tyz + sx;
317  result[2][1] = tyz - sx;
318 
319  if(MatType::numColumns() == 4) padMat4(result);
320  return MatType(result);
321 }
322 
323 
361 template<class MatType>
364  const MatType& mat,
365  RotationOrder rotationOrder,
366  typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
367 {
368  typedef typename MatType::value_type ValueType;
369  typedef Vec3<ValueType> V;
370  ValueType phi, theta, psi;
371 
372  switch(rotationOrder)
373  {
374  case XYZ_ROTATION:
375  if (isApproxEqual(mat[2][0], ValueType(1.0), eps)) {
376  theta = ValueType(M_PI_2);
377  phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
378  psi = phi;
379  } else if (isApproxEqual(mat[2][0], ValueType(-1.0), eps)) {
380  theta = ValueType(-M_PI_2);
381  phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
382  psi = -phi;
383  } else {
384  psi = ValueType(atan2(-mat[1][0],mat[0][0]));
385  phi = ValueType(atan2(-mat[2][1],mat[2][2]));
386  theta = ValueType(atan2(mat[2][0],
387  sqrt( mat[2][1]*mat[2][1] +
388  mat[2][2]*mat[2][2])));
389  }
390  return V(phi, theta, psi);
391  case ZXY_ROTATION:
392  if (isApproxEqual(mat[1][2], ValueType(1.0), eps)) {
393  theta = ValueType(M_PI_2);
394  phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
395  psi = phi;
396  } else if (isApproxEqual(mat[1][2], ValueType(-1.0), eps)) {
397  theta = ValueType(-M_PI/2);
398  phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
399  psi = -phi;
400  } else {
401  psi = ValueType(atan2(-mat[0][2], mat[2][2]));
402  phi = ValueType(atan2(-mat[1][0], mat[1][1]));
403  theta = ValueType(atan2(mat[1][2],
404  sqrt(mat[0][2] * mat[0][2] +
405  mat[2][2] * mat[2][2])));
406  }
407  return V(theta, psi, phi);
408 
409  case YZX_ROTATION:
410  if (isApproxEqual(mat[0][1], ValueType(1.0), eps)) {
411  theta = ValueType(M_PI_2);
412  phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
413  psi = phi;
414  } else if (isApproxEqual(mat[0][1], ValueType(-1.0), eps)) {
415  theta = ValueType(-M_PI/2);
416  phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
417  psi = -phi;
418  } else {
419  psi = ValueType(atan2(-mat[2][1], mat[1][1]));
420  phi = ValueType(atan2(-mat[0][2], mat[0][0]));
421  theta = ValueType(atan2(mat[0][1],
422  sqrt(mat[0][0] * mat[0][0] +
423  mat[0][2] * mat[0][2])));
424  }
425  return V(psi, phi, theta);
426 
427  case XZX_ROTATION:
428 
429  if (isApproxEqual(mat[0][0], ValueType(1.0), eps)) {
430  theta = ValueType(0.0);
431  phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
432  psi = phi;
433  } else if (isApproxEqual(mat[0][0], ValueType(-1.0), eps)) {
434  theta = ValueType(M_PI);
435  psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
436  phi = - psi;
437  } else {
438  psi = ValueType(atan2(mat[2][0], -mat[1][0]));
439  phi = ValueType(atan2(mat[0][2], mat[0][1]));
440  theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
441  mat[0][2] * mat[0][2]),
442  mat[0][0]));
443  }
444  return V(phi, psi, theta);
445 
446  case ZXZ_ROTATION:
447 
448  if (isApproxEqual(mat[2][2], ValueType(1.0), eps)) {
449  theta = ValueType(0.0);
450  phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
451  psi = phi;
452  } else if (isApproxEqual(mat[2][2], ValueType(-1.0), eps)) {
453  theta = ValueType(M_PI);
454  phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
455  psi = -phi;
456  } else {
457  psi = ValueType(atan2(mat[0][2], mat[1][2]));
458  phi = ValueType(atan2(mat[2][0], -mat[2][1]));
459  theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
460  mat[1][2] * mat[1][2]),
461  mat[2][2]));
462  }
463  return V(theta, psi, phi);
464 
465  case YXZ_ROTATION:
466 
467  if (isApproxEqual(mat[2][1], ValueType(1.0), eps)) {
468  theta = ValueType(-M_PI_2);
469  phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
470  psi = phi;
471  } else if (isApproxEqual(mat[2][1], ValueType(-1.0), eps)) {
472  theta = ValueType(M_PI_2);
473  phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
474  psi = -phi;
475  } else {
476  psi = ValueType(atan2(mat[0][1], mat[1][1]));
477  phi = ValueType(atan2(mat[2][0], mat[2][2]));
478  theta = ValueType(atan2(-mat[2][1],
479  sqrt(mat[0][1] * mat[0][1] +
480  mat[1][1] * mat[1][1])));
481  }
482  return V(theta, phi, psi);
483 
484  case ZYX_ROTATION:
485 
486  if (isApproxEqual(mat[0][2], ValueType(1.0), eps)) {
487  theta = ValueType(-M_PI_2);
488  phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
489  psi = phi;
490  } else if (isApproxEqual(mat[0][2], ValueType(-1.0), eps)) {
491  theta = ValueType(M_PI_2);
492  phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
493  psi = -phi;
494  } else {
495  psi = ValueType(atan2(mat[1][2], mat[2][2]));
496  phi = ValueType(atan2(mat[0][1], mat[0][0]));
497  theta = ValueType(atan2(-mat[0][2],
498  sqrt(mat[0][1] * mat[0][1] +
499  mat[0][0] * mat[0][0])));
500  }
501  return V(psi, theta, phi);
502 
503  case XZY_ROTATION:
504 
505  if (isApproxEqual(mat[1][0], ValueType(-1.0), eps)) {
506  theta = ValueType(M_PI_2);
507  psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
508  phi = -psi;
509  } else if (isApproxEqual(mat[1][0], ValueType(1.0), eps)) {
510  theta = ValueType(-M_PI_2);
511  psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
512  phi = psi;
513  } else {
514  psi = ValueType(atan2(mat[2][0], mat[0][0]));
515  phi = ValueType(atan2(mat[1][2], mat[1][1]));
516  theta = ValueType(atan2(- mat[1][0],
517  sqrt(mat[1][1] * mat[1][1] +
518  mat[1][2] * mat[1][2])));
519  }
520  return V(phi, psi, theta);
521  }
522 
523  OPENVDB_THROW(NotImplementedError, "Euler extraction sequence not implemented");
524 }
525 
526 
529 template<class MatType>
530 MatType
534  typename MatType::value_type eps=1.0e-8)
535 {
536  typedef typename MatType::value_type T;
537  Vec3<T> v1(_v1);
538  Vec3<T> v2(_v2);
539 
540  // Check if v1 and v2 are unit length
541  if (!isApproxEqual(1.0, v1.dot(v1), eps)) {
542  v1.normalize();
543  }
544  if (!isApproxEqual(1.0, v2.dot(v2), eps)) {
545  v2.normalize();
546  }
547 
548  Vec3<T> cross;
549  cross.cross(v1, v2);
550 
551  if (isApproxEqual(cross[0], 0.0, eps) &&
552  isApproxEqual(cross[1], 0.0, eps) &&
553  isApproxEqual(cross[2], 0.0, eps)) {
554 
555 
556  // Given two unit vectors v1 and v2 that are nearly parallel, build a
557  // rotation matrix that maps v1 onto v2. First find which principal axis
558  // p is closest to perpendicular to v1. Find a reflection that exchanges
559  // v1 and p, and find a reflection that exchanges p2 and v2. The desired
560  // rotation matrix is the composition of these two reflections. See the
561  // paper "Efficiently Building a Matrix to Rotate One Vector to
562  // Another" by Tomas Moller and John Hughes in Journal of Graphics
563  // Tools Vol 4, No 4 for details.
564 
565  Vec3<T> u, v, p(0.0, 0.0, 0.0);
566 
567  double x = Abs(v1[0]);
568  double y = Abs(v1[1]);
569  double z = Abs(v1[2]);
570 
571  if (x < y) {
572  if (z < x) {
573  p[2] = 1;
574  } else {
575  p[0] = 1;
576  }
577  } else {
578  if (z < y) {
579  p[2] = 1;
580  } else {
581  p[1] = 1;
582  }
583  }
584  u = p - v1;
585  v = p - v2;
586 
587  double udot = u.dot(u);
588  double vdot = v.dot(v);
589 
590  double a = -2 / udot;
591  double b = -2 / vdot;
592  double c = 4 * u.dot(v) / (udot * vdot);
593 
594  MatType result;
595  result.setIdentity();
596 
597  for (int j = 0; j < 3; j++) {
598  for (int i = 0; i < 3; i++)
599  result[i][j] =
600  a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i];
601  }
602  result[0][0] += 1.0;
603  result[1][1] += 1.0;
604  result[2][2] += 1.0;
605 
606  if(MatType::numColumns() == 4) padMat4(result);
607  return result;
608 
609  } else {
610  double c = v1.dot(v2);
611  double a = (1.0 - c) / cross.dot(cross);
612 
613  double a0 = a * cross[0];
614  double a1 = a * cross[1];
615  double a2 = a * cross[2];
616 
617  double a01 = a0 * cross[1];
618  double a02 = a0 * cross[2];
619  double a12 = a1 * cross[2];
620 
621  MatType r;
622 
623  r[0][0] = c + a0 * cross[0];
624  r[0][1] = a01 + cross[2];
625  r[0][2] = a02 - cross[1],
626  r[1][0] = a01 - cross[2];
627  r[1][1] = c + a1 * cross[1];
628  r[1][2] = a12 + cross[0];
629  r[2][0] = a02 + cross[1];
630  r[2][1] = a12 - cross[0];
631  r[2][2] = c + a2 * cross[2];
632 
633  if(MatType::numColumns() == 4) padMat4(r);
634  return r;
635 
636  }
637 }
638 
639 
641 template<class MatType>
642 MatType
644 {
645  // Gets identity, then sets top 3 diagonal
646  // Inefficient by 3 sets.
647 
648  MatType result;
649  result.setIdentity();
650  result[0][0] = s[0];
651  result[1][1] = s[1];
652  result[2][2] = s[2];
653 
654  return result;
655 }
656 
657 
659 template<class MatType>
661 getScale(const MatType &mat)
662 {
664  return V(
665  V(mat[0][0], mat[0][1], mat[0][2]).length(),
666  V(mat[1][0], mat[1][1], mat[1][2]).length(),
667  V(mat[2][0], mat[2][1], mat[2][2]).length());
668 }
669 
670 
674 template<class MatType>
675 MatType
676 unit(const MatType &mat, typename MatType::value_type eps = 1.0e-8)
677 {
679  return unit(mat, eps, dud);
680 }
681 
682 
687 template<class MatType>
688 MatType
690  const MatType &in,
691  typename MatType::value_type eps,
693 {
694  typedef typename MatType::value_type T;
695  MatType result(in);
696 
697  for (int i(0); i < 3; i++) {
698  try {
699  const Vec3<T> u(
700  Vec3<T>(in[i][0], in[i][1], in[i][2]).unit(eps, scaling[i]));
701  for (int j=0; j<3; j++) result[i][j] = u[j];
702  } catch (ArithmeticError&) {
703  for (int j=0; j<3; j++) result[i][j] = 0;
704  }
705  }
706  return result;
707 }
708 
709 
714 template <class MatType>
715 MatType
716 shear(Axis axis0, Axis axis1, typename MatType::value_type shear)
717 {
718  int index0 = static_cast<int>(axis0);
719  int index1 = static_cast<int>(axis1);
720 
721  MatType result;
722  result.setIdentity();
723  if (axis0 == axis1) {
724  result[index1][index0] = shear + 1;
725  } else {
726  result[index1][index0] = shear;
727  }
728 
729  return result;
730 }
731 
732 
734 template<class MatType>
735 MatType
737 {
738  typedef typename MatType::value_type T;
739 
740  MatType r;
741  r[0][0] = T(0); r[0][1] = skew.z(); r[0][2] = -skew.y();
742  r[1][0] = -skew.z(); r[1][1] = T(0); r[2][1] = skew.x();
743  r[2][0] = skew.y(); r[2][1] = -skew.x(); r[2][2] = T(0);
744 
745  if(MatType::numColumns() == 4) padMat4(r);
746  return r;
747 }
748 
749 
752 template<class MatType>
753 MatType
755  const Vec3<typename MatType::value_type>& vertical)
756 {
757  typedef typename MatType::value_type T;
758  Vec3<T> forward(direction.unit());
759  Vec3<T> horizontal(vertical.unit().cross(forward).unit());
760  Vec3<T> up(forward.cross(horizontal).unit());
761 
762  MatType r;
763 
764  r[0][0]=horizontal.x(); r[0][1]=horizontal.y(); r[0][2]=horizontal.z();
765  r[1][0]=up.x(); r[1][1]=up.y(); r[1][2]=up.z();
766  r[2][0]=forward.x(); r[2][1]=forward.y(); r[2][2]=forward.z();
767 
768  if(MatType::numColumns() == 4) padMat4(r);
769  return r;
770 }
771 
777 template<class MatType>
778 inline MatType
779 snapMatBasis(const MatType& source, Axis axis, const Vec3<typename MatType::value_type>& direction)
780 {
781  typedef typename MatType::value_type T;
782 
783  Vec3<T> unitDir(direction.unit());
784  Vec3<T> ourUnitAxis(source.row(axis).unit());
785 
786  // Are the two parallel?
787  T parallel = unitDir.dot(ourUnitAxis);
788 
789  // Already snapped!
790  if (isApproxEqual(parallel, T(1.0))) return source;
791 
792  if (isApproxEqual(parallel, T(-1.0))) {
793  OPENVDB_THROW(ValueError, "Cannot snap to inverse axis");
794  }
795 
796  // Find angle between our basis and the one specified
797  T angleBetween(angle(unitDir, ourUnitAxis));
798  // Caclulate axis to rotate along
799  Vec3<T> rotationAxis = unitDir.cross(ourUnitAxis);
800 
801  MatType rotation;
802  rotation.setToRotation(rotationAxis, angleBetween);
803 
804  return source * rotation;
805 }
806 
809 template<class MatType>
810 static MatType&
811 padMat4(MatType& dest)
812 {
813  dest[0][3] = dest[1][3] = dest[2][3] = 0;
814  dest[3][2] = dest[3][1] = dest[3][0] = 0;
815  dest[3][3] = 1;
816 
817  return dest;
818 }
819 
820 
823 template<typename MatType>
824 inline void
825 sqrtSolve(const MatType& aA, MatType& aB, double aTol=0.01)
826 {
827  unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
828 
829  MatType Y[2], Z[2];
830  Y[0] = aA;
831  Z[0] = MatType::identity();
832 
833  unsigned int current = 0;
834  for (unsigned int iteration=0; iteration < iterations; iteration++) {
835  unsigned int last = current;
836  current = !current;
837 
838  MatType invY = Y[last].inverse();
839  MatType invZ = Z[last].inverse();
840 
841  Y[current] = 0.5 * (Y[last] + invZ);
842  Z[current] = 0.5 * (Z[last] + invY);
843  }
844  aB = Y[current];
845 }
846 
847 
848 template<typename MatType>
849 inline void
850 powSolve(const MatType& aA, MatType& aB, double aPower, double aTol=0.01)
851 {
852  unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
853 
854  const bool inverted = (aPower < 0.0);
855  if (inverted) { aPower = -aPower; }
856 
857  unsigned int whole = static_cast<unsigned int>(aPower);
858  double fraction = aPower - whole;
859 
860  MatType R = MatType::identity();
861  MatType partial = aA;
862 
863  double contribution = 1.0;
864  for (unsigned int iteration = 0; iteration < iterations; iteration++) {
865  sqrtSolve(partial, partial, aTol);
866  contribution *= 0.5;
867  if (fraction >= contribution) {
868  R *= partial;
869  fraction -= contribution;
870  }
871  }
872 
873  partial = aA;
874  while (whole) {
875  if (whole & 1) { R *= partial; }
876  whole >>= 1;
877  if (whole) { partial *= partial; }
878  }
879 
880  if (inverted) { aB = R.inverse(); }
881  else { aB = R; }
882 }
883 
884 
886 template<typename MatType>
887 inline bool
888 isIdentity(const MatType& m)
889 {
890  return m.eq(MatType::identity());
891 }
892 
893 
895 template<typename MatType>
896 inline bool
897 isInvertible(const MatType& m)
898 {
899  typedef typename MatType::ValueType ValueType;
900  return !isApproxEqual(m.det(), ValueType(0));
901 }
902 
903 
906 template<typename MatType>
907 inline bool
908 isSymmetric(const MatType& m)
909 {
910  return m.eq(m.transpose());
911 }
912 
913 
915 template<typename MatType>
916 inline bool
917 isUnitary(const MatType& m)
918 {
919  typedef typename MatType::ValueType value_type;
920  if (!isApproxEqual(std::abs(m.det()), value_type(1.0))) return false;
921  // check that the matrix transpose is the inverse
922  MatType temp = m * m.transpose();
923  return temp.eq(MatType::identity());
924 }
925 
926 
928 template<typename MatType>
929 inline bool
930 isDiagonal(const MatType& mat)
931 {
932  int n = MatType::size;
933  typename MatType::ValueType temp(0);
934  for (int i = 0; i < n; ++i) {
935  for (int j = 0; j < n; ++j) {
936  if (i != j) {
937  temp+=std::abs(mat(i,j));
938  }
939  }
940  }
941  return isApproxEqual(temp, typename MatType::ValueType(0.0));
942 }
943 
944 
946 template<typename MatType>
947 typename MatType::ValueType
948 lInfinityNorm(const MatType& matrix)
949 {
950  int n = MatType::size;
951  typename MatType::ValueType norm = 0;
952 
953  for( int j = 0; j<n; ++j) {
954  typename MatType::ValueType column_sum = 0;
955 
956  for (int i = 0; i<n; ++i) {
957  column_sum += fabs(matrix(i,j));
958  }
959  norm = std::max(norm, column_sum);
960  }
961 
962  return norm;
963 }
964 
965 
967 template<typename MatType>
968 typename MatType::ValueType
969 lOneNorm(const MatType& matrix)
970 {
971  int n = MatType::size;
972  typename MatType::ValueType norm = 0;
973 
974  for( int i = 0; i<n; ++i) {
975  typename MatType::ValueType row_sum = 0;
976 
977  for (int j = 0; j<n; ++j) {
978  row_sum += fabs(matrix(i,j));
979  }
980  norm = std::max(norm, row_sum);
981  }
982 
983  return norm;
984 }
985 
986 
994 template<typename MatType>
995 bool
996 polarDecomposition(const MatType& input, MatType& unitary,
997  MatType& positive_hermitian, unsigned int MAX_ITERATIONS=100)
998 {
999  unitary = input;
1000  MatType new_unitary(input);
1001  MatType unitary_inv;
1002 
1003  if (fabs(unitary.det()) < math::Tolerance<typename MatType::ValueType>::value()) return false;
1004 
1005  unsigned int iteration(0);
1006 
1007  typename MatType::ValueType linf_of_u;
1008  typename MatType::ValueType l1nm_of_u;
1009  typename MatType::ValueType linf_of_u_inv;
1010  typename MatType::ValueType l1nm_of_u_inv;
1011  typename MatType::ValueType l1_error = 100;
1012  double gamma;
1013 
1014  do {
1015  unitary_inv = unitary.inverse();
1016  linf_of_u = lInfinityNorm(unitary);
1017  l1nm_of_u = lOneNorm(unitary);
1018 
1019  linf_of_u_inv = lInfinityNorm(unitary_inv);
1020  l1nm_of_u_inv = lOneNorm(unitary_inv);
1021 
1022  gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
1023 
1024  new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
1025 
1026  l1_error = lInfinityNorm(unitary - new_unitary);
1027  unitary = new_unitary;
1028 
1030  if (iteration > MAX_ITERATIONS) return false;
1031  iteration++;
1033 
1034  positive_hermitian = unitary.transpose() * input;
1035  return true;
1036 }
1037 
1038 } // namespace math
1039 } // namespace OPENVDB_VERSION_NAME
1040 } // namespace openvdb
1041 
1042 #endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
1043 
1044 // Copyright (c) 2012-2017 DreamWorks Animation LLC
1045 // All rights reserved. This software is distributed under the
1046 // Mozilla Public License 2.0 ( http://www.mozilla.org/MPL/2.0/ )
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:492
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:996
Tolerance for floating-point comparison.
Definition: Math.h:117
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:897
MatType::ValueType lOneNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:969
T & w()
Definition: Quat.h:226
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
T & y()
Definition: Quat.h:224
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:471
Definition: Exceptions.h:88
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:215
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:888
bool isZero() const
True if all elements are exactly zero.
Definition: Mat.h:182
T value_type
Definition: Mat.h:55
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:754
T & z()
Definition: Vec3.h:111
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:101
tbb::atomic< Index32 > i
Definition: LeafBuffer.h:71
Definition: Exceptions.h:82
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:354
void read(std::istream &is)
Definition: Mat.h:144
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:661
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:388
bool isFinite() const
True if no Nan or Inf values are present.
Definition: Mat.h:174
Definition: Mat.h:195
static MatType & padMat4(MatType &dest)
Write 0s along Mat4&#39;s last row and column, and a 1 on its diagonal.
Definition: Mat.h:811
Definition: Math.h:854
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:531
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:643
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:825
T absMax() const
Return the maximum of the absolute of all elements in this matrix.
Definition: Mat.h:149
const std::enable_if<!VecTraits< T >::IsVec, T >::type & max(const T &a, const T &b)
Definition: Composite.h:133
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:930
static unsigned numColumns()
Definition: Mat.h:61
Definition: Math.h:853
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:850
std::string str(unsigned indentation=0) const
Definition: Mat.h:94
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:779
static unsigned numElements()
Definition: Mat.h:62
#define OPENVDB_VERSION_NAME
Definition: version.h:43
T mm[SIZE *SIZE]
Definition: Mat.h:190
RotationOrder
Definition: Math.h:859
bool isZero(const Type &x)
Return true if x is exactly equal to zero.
Definition: Math.h:308
Definition: Mat.h:194
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:223
Coord Abs(const Coord &xyz)
Definition: Coord.h:509
Definition: Mat.h:52
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:376
Definition: Exceptions.h:39
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:244
bool isInfinite() const
True if an Inf is present in this matrix.
Definition: Mat.h:166
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition: Mat.h:132
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:908
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:109
T & y()
Definition: Vec3.h:110
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:736
Definition: Math.h:855
Definition: Exceptions.h:92
void write(std::ostream &os) const
Definition: Mat.h:140
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:716
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:689
Mat()
Definition: Mat.h:66
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:948
Mat(Mat const &src)
Copy constructor. Used when the class signature matches exactly.
Definition: Mat.h:69
static unsigned numRows()
Definition: Mat.h:60
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:71
Axis
Definition: Math.h:852
Mat & operator=(Mat const &src)
Definition: Mat.h:75
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:363
T & z()
Definition: Quat.h:225
static constexpr size_t size
The size of a LeafBuffer when LeafBuffer::mOutOfCore is atomic.
Definition: LeafBuffer.h:85
T ValueType
Definition: Mat.h:56
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:917
bool isNan() const
True if a Nan is present in this matrix.
Definition: Mat.h:158