OpenVDB 7.0.0

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