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 {
45 OPENVDB_USE_VERSION_NAMESPACE
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,
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>
313 Vec3<typename MatType::value_type>
314 eulerAngles(
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
482 rotation(
483 const Vec3<typename MatType::value_type>& _v1,
484 const Vec3<typename MatType::value_type>& _v2,
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
594 scale(const Vec3<typename MatType::value_type>& s)
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>
611 Vec3<typename MatType::value_type>
612 getScale(const MatType &mat)
613 {
614 typedef Vec3<typename MatType::value_type> V;
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 {
629 Vec3<typename MatType::value_type> dud;
630 return unit(mat, eps, dud);
631 }
632
633
638 template<class MatType>
639 MatType
640 unit(
641 const MatType &in,
642 typename MatType::value_type eps,
643 Vec3<typename MatType::value_type>& scaling)
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
687 skew(const Vec3<typename MatType::value_type> &skew)
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
705 aim(const Vec3<typename MatType::value_type>& direction,
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++;
1012 } while (l1_error > math::Tolerance<typename MatType::ValueType>::value());
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/ )
openvdb::v3_2_0::math::Mat::Mat
Mat(Mat const &src)
Copy constructor. Used when the class signature matches exactly.
Definition: Mat.h:69
openvdb::v3_2_0::math::Quat::z
T & z()
Definition: Quat.h:225
openvdb::v3_2_0::math::Quat::y
T & y()
Definition: Quat.h:224
openvdb::v3_2_0::math::sqrtSolve
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:776
openvdb::v3_2_0::math::Abs
Coord Abs(const Coord &xyz)
Definition: Coord.h:247
openvdb::v3_2_0::math::Vec3::dot
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:203
openvdb::v3_2_0::math::Mat::str
std::string str(unsigned indentation=0) const
Definition: Mat.h:85
openvdb::v3_2_0::math::Vec3::z
T & z()
Definition: Vec3.h:99
openvdb::v3_2_0::math::rotation
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
Math.h
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
openvdb::v3_2_0::math::X_AXIS
Definition: Math.h:857
openvdb::v3_2_0::math::XZX_ROTATION
Definition: Math.h:870
openvdb::v3_2_0::math::Vec3::unit
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:360
openvdb::v3_2_0::math::Vec3
Definition: Mat.h:146
openvdb::v3_2_0::math::Quat
Definition: Mat.h:145
OPENVDB_THROW
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:97
openvdb::v3_2_0::math::Mat::mm
T mm[SIZE *SIZE]
Definition: Mat.h:141
openvdb::v3_2_0::math::isUnitary
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:897
openvdb::v3_2_0::math::Quat::w
T & w()
Definition: Quat.h:226
openvdb::v3_2_0::ArithmeticError
Definition: Exceptions.h:78
openvdb::v3_2_0::math::Mat::ValueType
T ValueType
Definition: Mat.h:56
openvdb::v3_2_0::math::Y_AXIS
Definition: Math.h:858
openvdb::v3_2_0::math::lInfinityNorm
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:928
openvdb::v3_2_0::math::shear
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
openvdb::v3_2_0::tools::composite::max
const boost::disable_if_c< VecTraits< T >::IsVec, T >::type & max(const T &a, const T &b)
Definition: Composite.h:132
openvdb::v3_2_0::math::Mat::operator<<
friend std::ostream & operator<<(std::ostream &ostr, const Mat< SIZE, T > &m)
Write a Mat to an output stream.
Definition: Mat.h:123
openvdb::v3_2_0::math::XZY_ROTATION
Definition: Math.h:865
openvdb::v3_2_0::math::ZXY_ROTATION
Definition: Math.h:868
openvdb::v3_2_0::math::ZXZ_ROTATION
Definition: Math.h:871
openvdb::v3_2_0::math::YXZ_ROTATION
Definition: Math.h:866
openvdb::v3_2_0::math::isApproxEqual
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
openvdb::v3_2_0::math::Mat::numRows
static unsigned numRows()
Definition: Mat.h:60
openvdb::v3_2_0::math::XYZ_ROTATION
Definition: Math.h:864
openvdb::v3_2_0::math::RotationOrder
RotationOrder
Definition: Math.h:863
openvdb::v3_2_0::math::unit
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
Exceptions.h
OPENVDB_VERSION_NAME
#define OPENVDB_VERSION_NAME
Definition: version.h:43
openvdb::v3_2_0::math::Vec3::x
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:97
openvdb::v3_2_0::math::Mat::value_type
T value_type
Definition: Mat.h:55
openvdb
Definition: Exceptions.h:39
openvdb::v3_2_0::math::Tolerance
Tolerance for floating-point comparison.
Definition: Math.h:125
openvdb::v3_2_0::math::scale
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:594
openvdb::v3_2_0::math::angle
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:446
openvdb::v3_2_0::math::Quat::x
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:223
openvdb::v3_2_0::math::Mat::Mat
Mat()
Definition: Mat.h:66
openvdb::v3_2_0::math::lOneNorm
MatType::ValueType lOneNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:949
openvdb::v3_2_0::math::Vec3::normalize
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:348
openvdb::v3_2_0::math::Z_AXIS
Definition: Math.h:859
openvdb::v3_2_0::math::getScale
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
openvdb::v3_2_0::math::Vec3::cross
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:232
openvdb::v3_2_0::math::Mat::read
void read(std::istream &is)
Definition: Mat.h:135
openvdb::v3_2_0::math::isInvertible
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:877
openvdb::v3_2_0::math::Vec3::y
T & y()
Definition: Vec3.h:98
openvdb::v3_2_0::math::aim
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
openvdb::v3_2_0::math::Mat::numColumns
static unsigned numColumns()
Definition: Mat.h:61
openvdb::v3_2_0::math::eulerAngles
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
openvdb::v3_2_0::NotImplementedError
Definition: Exceptions.h:84
openvdb::v3_2_0::ValueError
Definition: Exceptions.h:88
openvdb::v3_2_0::math::snapMatBasis
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
openvdb::v3_2_0::math::YZX_ROTATION
Definition: Math.h:867
openvdb::v3_2_0::math::Axis
Axis
Definition: Math.h:856
openvdb::v3_2_0::math::Mat< 3, T >::SIZE_
SIZE_
Definition: Mat.h:57
openvdb::v3_2_0::math::skew
MatType skew(const Vec3< typename MatType::value_type > &skew)
Return a matrix as the cross product of the given vector.
Definition: Mat.h:687
openvdb::v3_2_0::math::ZYX_ROTATION
Definition: Math.h:869
openvdb::v3_2_0::math::powSolve
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:810
OPENVDB_USE_VERSION_NAMESPACE
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:71
openvdb::v3_2_0::math::Mat
Definition: Mat.h:52
openvdb::v3_2_0::math::padMat4
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
openvdb::v3_2_0::math::isSymmetric
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:888
openvdb::v3_2_0::math::Mat::write
void write(std::ostream &os) const
Definition: Mat.h:131
openvdb::v3_2_0::math::Quat::dot
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:492
openvdb::v3_2_0::math::isDiagonal
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:910
openvdb::v3_2_0::math::Mat::numElements
static unsigned numElements()
Definition: Mat.h:62
openvdb::v3_2_0::math::isIdentity
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:868
openvdb::v3_2_0::math::polarDecomposition
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