OpenVDB: Mat.h Source File

OpenVDB 5.2.0

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