OpenVDB: Mat.h Source File

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 {
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
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
202 rotation(const Quat<typename MatType::value_type> &q,
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>
362 Vec3<typename MatType::value_type>
363 eulerAngles(
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
531 rotation(
532 const Vec3<typename MatType::value_type>& _v1,
533 const Vec3<typename MatType::value_type>& _v2,
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
643 scale(const Vec3<typename MatType::value_type>& s)
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>
660 Vec3<typename MatType::value_type>
661 getScale(const MatType &mat)
662 {
663 typedef Vec3<typename MatType::value_type> V;
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 {
678 Vec3<typename MatType::value_type> dud;
679 return unit(mat, eps, dud);
680 }
681
682
687 template<class MatType>
688 MatType
689 unit(
690 const MatType &in,
691 typename MatType::value_type eps,
692 Vec3<typename MatType::value_type>& scaling)
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
736 skew(const Vec3<typename MatType::value_type> &skew)
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
754 aim(const Vec3<typename MatType::value_type>& direction,
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++;
1032 } while (l1_error > math::Tolerance<typename MatType::ValueType>::value());
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/ )
openvdb::v4_0_2::math::Quat::dot
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:492
openvdb::v4_0_2::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:996
openvdb::v4_0_2::math::Tolerance
Tolerance for floating-point comparison.
Definition: Math.h:117
openvdb::v4_0_2::math::YXZ_ROTATION
Definition: Math.h:862
openvdb::v4_0_2::math::isInvertible
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition: Mat.h:897
openvdb::v4_0_2::math::lOneNorm
MatType::ValueType lOneNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:969
openvdb::v4_0_2::math::Quat::w
T & w()
Definition: Quat.h:226
Math.h
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
openvdb::v4_0_2::math::Quat::y
T & y()
Definition: Quat.h:224
openvdb::v4_0_2::math::angle
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:471
openvdb::v4_0_2::NotImplementedError
Definition: Exceptions.h:88
openvdb::v4_0_2::math::Vec3::dot
T dot(const Vec3< T > &v) const
Dot product.
Definition: Vec3.h:215
openvdb::v4_0_2::math::isIdentity
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition: Mat.h:888
openvdb::v4_0_2::math::Mat::isZero
bool isZero() const
True if all elements are exactly zero.
Definition: Mat.h:182
openvdb::v4_0_2::math::Mat::value_type
T value_type
Definition: Mat.h:55
openvdb::v4_0_2::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:754
openvdb::v4_0_2::math::Vec3::z
T & z()
Definition: Vec3.h:111
OPENVDB_THROW
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:101
openvdb::v4_0_2::math::Mat< 3, T >::SIZE_
SIZE_
Definition: Mat.h:57
i
tbb::atomic< Index32 > i
Definition: LeafBuffer.h:71
openvdb::v4_0_2::ArithmeticError
Definition: Exceptions.h:82
openvdb::v4_0_2::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:354
openvdb::v4_0_2::math::Mat::read
void read(std::istream &is)
Definition: Mat.h:144
openvdb::v4_0_2::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:661
openvdb::v4_0_2::math::Vec3::unit
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition: Vec3.h:388
openvdb::v4_0_2::math::Mat::isFinite
bool isFinite() const
True if no Nan or Inf values are present.
Definition: Mat.h:174
openvdb::v4_0_2::math::Vec3
Definition: Mat.h:195
openvdb::v4_0_2::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:811
openvdb::v4_0_2::math::Y_AXIS
Definition: Math.h:854
openvdb::v4_0_2::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:531
openvdb::v4_0_2::math::ZXZ_ROTATION
Definition: Math.h:867
openvdb::v4_0_2::math::scale
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:643
openvdb::v4_0_2::math::sqrtSolve
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition: Mat.h:825
openvdb::v4_0_2::math::Mat::absMax
T absMax() const
Return the maximum of the absolute of all elements in this matrix.
Definition: Mat.h:149
openvdb::v4_0_2::tools::composite::max
const std::enable_if<!VecTraits< T >::IsVec, T >::type & max(const T &a, const T &b)
Definition: Composite.h:133
openvdb::v4_0_2::math::isDiagonal
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition: Mat.h:930
openvdb::v4_0_2::math::Mat::numColumns
static unsigned numColumns()
Definition: Mat.h:61
openvdb::v4_0_2::math::X_AXIS
Definition: Math.h:853
openvdb::v4_0_2::math::powSolve
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition: Mat.h:850
openvdb::v4_0_2::math::Mat::str
std::string str(unsigned indentation=0) const
Definition: Mat.h:94
Exceptions.h
openvdb::v4_0_2::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:779
openvdb::v4_0_2::math::Mat::numElements
static unsigned numElements()
Definition: Mat.h:62
OPENVDB_VERSION_NAME
#define OPENVDB_VERSION_NAME
Definition: version.h:43
openvdb::v4_0_2::math::Mat::mm
T mm[SIZE *SIZE]
Definition: Mat.h:190
openvdb::v4_0_2::math::RotationOrder
RotationOrder
Definition: Math.h:859
openvdb::v4_0_2::math::isZero
bool isZero(const Type &x)
Return true if x is exactly equal to zero.
Definition: Math.h:308
openvdb::v4_0_2::math::Quat
Definition: Mat.h:194
openvdb::v4_0_2::math::Quat::x
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:223
openvdb::v4_0_2::math::Abs
Coord Abs(const Coord &xyz)
Definition: Coord.h:509
openvdb::v4_0_2::math::Mat
Definition: Mat.h:52
openvdb::v4_0_2::math::Vec3::normalize
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition: Vec3.h:376
openvdb
Definition: Exceptions.h:39
openvdb::v4_0_2::math::Vec3::cross
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition: Vec3.h:244
openvdb::v4_0_2::math::Mat::isInfinite
bool isInfinite() const
True if an Inf is present in this matrix.
Definition: Mat.h:166
openvdb::v4_0_2::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:132
openvdb::v4_0_2::math::isSymmetric
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition: Mat.h:908
openvdb::v4_0_2::math::Vec3::x
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:109
openvdb::v4_0_2::math::YZX_ROTATION
Definition: Math.h:863
openvdb::v4_0_2::math::Vec3::y
T & y()
Definition: Vec3.h:110
openvdb::v4_0_2::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:736
openvdb::v4_0_2::math::ZXY_ROTATION
Definition: Math.h:864
openvdb::v4_0_2::math::Z_AXIS
Definition: Math.h:855
openvdb::v4_0_2::ValueError
Definition: Exceptions.h:92
openvdb::v4_0_2::math::XZX_ROTATION
Definition: Math.h:866
openvdb::v4_0_2::math::Mat::write
void write(std::ostream &os) const
Definition: Mat.h:140
openvdb::v4_0_2::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:716
openvdb::v4_0_2::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:689
openvdb::v4_0_2::math::Mat::Mat
Mat()
Definition: Mat.h:66
openvdb::v4_0_2::math::lInfinityNorm
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the norm of an N x N matrix.
Definition: Mat.h:948
openvdb::v4_0_2::math::Mat::Mat
Mat(Mat const &src)
Copy constructor. Used when the class signature matches exactly.
Definition: Mat.h:69
openvdb::v4_0_2::math::XYZ_ROTATION
Definition: Math.h:860
openvdb::v4_0_2::math::Mat::numRows
static unsigned numRows()
Definition: Mat.h:60
OPENVDB_USE_VERSION_NAMESPACE
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:71
openvdb::v4_0_2::math::Axis
Axis
Definition: Math.h:852
openvdb::v4_0_2::math::Mat::operator=
Mat & operator=(Mat const &src)
Definition: Mat.h:75
openvdb::v4_0_2::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:363
openvdb::v4_0_2::math::Quat::z
T & z()
Definition: Quat.h:225
openvdb::v4_0_2::math::ZYX_ROTATION
Definition: Math.h:865
size
static constexpr size_t size
The size of a LeafBuffer when LeafBuffer::mOutOfCore is atomic.
Definition: LeafBuffer.h:85
openvdb::v4_0_2::math::XZY_ROTATION
Definition: Math.h:861
openvdb::v4_0_2::math::Mat::ValueType
T ValueType
Definition: Mat.h:56
openvdb::v4_0_2::math::isUnitary
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:917
openvdb::v4_0_2::math::Mat::isNan
bool isNan() const
True if a Nan is present in this matrix.
Definition: Mat.h:158