OpenVDB: openvdb/math/Mat.h Source File

OpenVDB 11.0.0

Loading...
Searching...
No Matches
Mat.h
Go to the documentation of this file.
1// Copyright Contributors to the OpenVDB Project
2// SPDX-License-Identifier: MPL-2.0
3//
4/// @file Mat.h
5/// @author Joshua Schpok
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
18namespace openvdb {
19 OPENVDB_USE_VERSION_NAMESPACE
20namespace OPENVDB_VERSION_NAME {
21namespace math {
22
23/// @class Mat "Mat.h"
24/// A base class for square matrices.
25template<unsigned SIZE, typename T>
26 class Mat
27{
28public:
29 using value_type = T;
30 using ValueType = T;
31 enum SIZE_ { size = SIZE };
32
33 /// Trivial constructor, the matrix is NOT initialized
34 /// @note destructor, copy constructor, assignment operator and
35 /// move constructor are left to be defined by the compiler (default)
36 Mat() = default;
37
38 // Number of cols, rows, elements
39 static unsigned numRows() { return SIZE; }
40 static unsigned numColumns() { return SIZE; }
41 static unsigned numElements() { return SIZE*SIZE; }
42
43 /// @return string representation of matrix
44 /// Since output is multiline, optional indentation argument prefixes
45 /// each newline with that much white space. It does not indent
46 /// the first line, since you might be calling this inline:
47 ///
48 /// cout << "matrix: " << mat.str(7)
49 ///
50 /// matrix: [[1 2]
51 /// [3 4]]
52 std::string
53 str(unsigned indentation = 0) const {
54
55 std::string ret;
56 std::string indent;
57
58 // We add +1 since we're indenting one for the first '['
59 indent.append(indentation+1, ' ');
60
61 ret.append("[");
62
63 // For each row,
64 for (unsigned i(0); i < SIZE; i++) {
65
66 ret.append("[");
67
68 // For each column
69 for (unsigned j(0); j < SIZE; j++) {
70
71 // Put a comma after everything except the last
72 if (j) ret.append(", ");
73 ret.append(std::to_string(mm[(i*SIZE)+j]));
74 }
75
76 ret.append("]");
77
78 // At the end of every row (except the last)...
79 if (i < SIZE - 1) {
80 // ...suffix the row bracket with a comma, newline, and advance indentation.
81 ret.append(",\n");
82 ret.append(indent);
83 }
84 }
85
86 ret.append("]");
87
88 return ret;
89 }
90
91 /// Write a Mat to an output stream
92 friend std::ostream& operator<<(
93 std::ostream& ostr,
94 const Mat<SIZE, T>& m)
95 {
96 ostr << m.str();
97 return ostr;
98 }
99
100 /// Direct access to the internal data
101 T* asPointer() { return mm; }
102 const T* asPointer() const { return mm; }
103
104 //@{
105 /// Array style reference to ith row
106 T* operator[](int i) { return &(mm[i*SIZE]); }
107 const T* operator[](int i) const { return &(mm[i*SIZE]); }
108 //@}
109
110 void write(std::ostream& os) const {
111 os.write(reinterpret_cast<const char*>(&mm), sizeof(T)*SIZE*SIZE);
112 }
113
114 void read(std::istream& is) {
115 is.read(reinterpret_cast<char*>(&mm), sizeof(T)*SIZE*SIZE);
116 }
117
118 /// Return the maximum of the absolute of all elements in this matrix
119 T absMax() const {
120 T x = static_cast<T>(std::fabs(mm[0]));
121 for (unsigned i = 1; i < numElements(); ++i) {
122 x = std::max(x, static_cast<T>(std::fabs(mm[i])));
123 }
124 return x;
125 }
126
127 /// True if a Nan is present in this matrix
128 bool isNan() const {
129 for (unsigned i = 0; i < numElements(); ++i) {
130 if (math::isNan(mm[i])) return true;
131 }
132 return false;
133 }
134
135 /// True if an Inf is present in this matrix
136 bool isInfinite() const {
137 for (unsigned i = 0; i < numElements(); ++i) {
138 if (math::isInfinite(mm[i])) return true;
139 }
140 return false;
141 }
142
143 /// True if no Nan or Inf values are present
144 bool isFinite() const {
145 for (unsigned i = 0; i < numElements(); ++i) {
146 if (!math::isFinite(mm[i])) return false;
147 }
148 return true;
149 }
150
151 /// True if all elements are exactly zero
152 bool isZero() const {
153 for (unsigned i = 0; i < numElements(); ++i) {
154 if (!math::isZero(mm[i])) return false;
155 }
156 return true;
157 }
158
159protected:
160 T mm[SIZE*SIZE];
161};
162
163
164template<typename T> class Quat;
165template<typename T> class Vec3;
166
167/// @brief Return the rotation matrix specified by the given quaternion.
168/// @details The quaternion is normalized and used to construct the matrix.
169/// Note that the matrix is transposed to match post-multiplication semantics.
170template<class MatType>
171MatType
172rotation(const Quat<typename MatType::value_type> &q,
173 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
174{
175 using T = typename MatType::value_type;
176
177 T qdot(q.dot(q));
178 T s(0);
179
180 if (!isApproxEqual(qdot, T(0.0),eps)) {
181 s = T(2.0 / qdot);
182 }
183
184 T x = s*q.x();
185 T y = s*q.y();
186 T z = s*q.z();
187 T wx = x*q.w();
188 T wy = y*q.w();
189 T wz = z*q.w();
190 T xx = x*q.x();
191 T xy = y*q.x();
192 T xz = z*q.x();
193 T yy = y*q.y();
194 T yz = z*q.y();
195 T zz = z*q.z();
196
197 MatType r;
198 r[0][0]=T(1) - (yy+zz); r[0][1]=xy + wz; r[0][2]=xz - wy;
199 r[1][0]=xy - wz; r[1][1]=T(1) - (xx+zz); r[1][2]=yz + wx;
200 r[2][0]=xz + wy; r[2][1]=yz - wx; r[2][2]=T(1) - (xx+yy);
201
202 if(MatType::numColumns() == 4) padMat4(r);
203 return r;
204}
205
206
207
208/// @brief Return a matrix for rotation by @a angle radians about the given @a axis.
209/// @param axis The axis (one of X, Y, Z) to rotate about.
210/// @param angle The rotation angle, in radians.
211template<class MatType>
212MatType
213 rotation(Axis axis, typename MatType::value_type angle)
214{
215 using T = typename MatType::value_type;
216 T c = static_cast<T>(cos(angle));
217 T s = static_cast<T>(sin(angle));
218
219 MatType result;
220 result.setIdentity();
221
222 switch (axis) {
223 case X_AXIS:
224 result[1][1] = c;
225 result[1][2] = s;
226 result[2][1] = -s;
227 result[2][2] = c;
228 return result;
229 case Y_AXIS:
230 result[0][0] = c;
231 result[0][2] = -s;
232 result[2][0] = s;
233 result[2][2] = c;
234 return result;
235 case Z_AXIS:
236 result[0][0] = c;
237 result[0][1] = s;
238 result[1][0] = -s;
239 result[1][1] = c;
240 return result;
241 default:
242 throw ValueError("Unrecognized rotation axis");
243 }
244}
245
246
247/// @brief Return a matrix for rotation by @a angle radians about the given @a axis.
248/// @note The axis must be a unit vector.
249template<class MatType>
250MatType
251 rotation(const Vec3<typename MatType::value_type> &_axis, typename MatType::value_type angle)
252{
253 using T = typename MatType::value_type;
254 T txy, txz, tyz, sx, sy, sz;
255
256 Vec3<T> axis(_axis.unit());
257
258 // compute trig properties of angle:
259 T c(cos(double(angle)));
260 T s(sin(double(angle)));
261 T t(1 - c);
262
263 MatType result;
264 // handle diagonal elements
265 result[0][0] = axis[0]*axis[0] * t + c;
266 result[1][1] = axis[1]*axis[1] * t + c;
267 result[2][2] = axis[2]*axis[2] * t + c;
268
269 txy = axis[0]*axis[1] * t;
270 sz = axis[2] * s;
271
272 txz = axis[0]*axis[2] * t;
273 sy = axis[1] * s;
274
275 tyz = axis[1]*axis[2] * t;
276 sx = axis[0] * s;
277
278 // right handed space
279 // Contribution from rotation about 'z'
280 result[0][1] = txy + sz;
281 result[1][0] = txy - sz;
282 // Contribution from rotation about 'y'
283 result[0][2] = txz - sy;
284 result[2][0] = txz + sy;
285 // Contribution from rotation about 'x'
286 result[1][2] = tyz + sx;
287 result[2][1] = tyz - sx;
288
289 if(MatType::numColumns() == 4) padMat4(result);
290 return MatType(result);
291}
292
293
294/// @brief Return the Euler angles composing the given rotation matrix.
295/// @details Optional axes arguments describe in what order elementary rotations
296/// are applied. Note that in our convention, XYZ means Rz * Ry * Rx.
297/// Because we are using rows rather than columns to represent the
298/// local axes of a coordinate frame, the interpretation from a local
299/// reference point of view is to first rotate about the x axis, then
300/// about the newly rotated y axis, and finally by the new local z axis.
301/// From a fixed reference point of view, the interpretation is to
302/// rotate about the stationary world z, y, and x axes respectively.
303///
304/// Irrespective of the Euler angle convention, in the case of distinct
305/// axes, eulerAngles() returns the x, y, and z angles in the corresponding
306/// x, y, z components of the returned Vec3. For the XZX convention, the
307/// left X value is returned in Vec3.x, and the right X value in Vec3.y.
308/// For the ZXZ convention the left Z value is returned in Vec3.z and
309/// the right Z value in Vec3.y
310///
311/// Examples of reconstructing r from its Euler angle decomposition
312///
313/// v = eulerAngles(r, ZYX_ROTATION);
314/// rx.setToRotation(Vec3d(1,0,0), v[0]);
315/// ry.setToRotation(Vec3d(0,1,0), v[1]);
316/// rz.setToRotation(Vec3d(0,0,1), v[2]);
317/// r = rx * ry * rz;
318///
319/// v = eulerAngles(r, ZXZ_ROTATION);
320/// rz1.setToRotation(Vec3d(0,0,1), v[2]);
321/// rx.setToRotation (Vec3d(1,0,0), v[0]);
322/// rz2.setToRotation(Vec3d(0,0,1), v[1]);
323/// r = rz2 * rx * rz1;
324///
325/// v = eulerAngles(r, XZX_ROTATION);
326/// rx1.setToRotation (Vec3d(1,0,0), v[0]);
327/// rx2.setToRotation (Vec3d(1,0,0), v[1]);
328/// rz.setToRotation (Vec3d(0,0,1), v[2]);
329/// r = rx2 * rz * rx1;
330///
331template<class MatType>
332 Vec3<typename MatType::value_type>
333eulerAngles(
334 const MatType& mat,
335 RotationOrder rotationOrder,
336 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
337{
338 using ValueType = typename MatType::value_type;
339 using V = Vec3<ValueType>;
340 ValueType phi, theta, psi;
341
342 switch(rotationOrder)
343 {
344 case XYZ_ROTATION:
345 if (isApproxEqual(mat[2][0], ValueType(1.0), eps)) {
346 theta = ValueType(math::pi<double>() / 2.0);
347 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
348 psi = phi;
349 } else if (isApproxEqual(mat[2][0], ValueType(-1.0), eps)) {
350 theta = ValueType(-math::pi<double>() / 2.0);
351 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
352 psi = -phi;
353 } else {
354 psi = ValueType(atan2(-mat[1][0],mat[0][0]));
355 phi = ValueType(atan2(-mat[2][1],mat[2][2]));
356 theta = ValueType(atan2(mat[2][0],
357 sqrt( mat[2][1]*mat[2][1] +
358 mat[2][2]*mat[2][2])));
359 }
360 return V(phi, theta, psi);
361 case ZXY_ROTATION:
362 if (isApproxEqual(mat[1][2], ValueType(1.0), eps)) {
363 theta = ValueType(math::pi<double>() / 2.0);
364 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
365 psi = phi;
366 } else if (isApproxEqual(mat[1][2], ValueType(-1.0), eps)) {
367 theta = ValueType(-math::pi<double>() / 2.0);
368 phi = ValueType(0.5 * atan2(mat[0][1],mat[2][1]));
369 psi = -phi;
370 } else {
371 psi = ValueType(atan2(-mat[0][2], mat[2][2]));
372 phi = ValueType(atan2(-mat[1][0], mat[1][1]));
373 theta = ValueType(atan2(mat[1][2],
374 sqrt(mat[0][2] * mat[0][2] +
375 mat[2][2] * mat[2][2])));
376 }
377 return V(theta, psi, phi);
378
379 case YZX_ROTATION:
380 if (isApproxEqual(mat[0][1], ValueType(1.0), eps)) {
381 theta = ValueType(math::pi<double>() / 2.0);
382 phi = ValueType(0.5 * atan2(mat[2][0], mat[2][2]));
383 psi = phi;
384 } else if (isApproxEqual(mat[0][1], ValueType(-1.0), eps)) {
385 theta = ValueType(-math::pi<double>() / 2.0);
386 phi = ValueType(0.5 * atan2(mat[2][0], mat[1][0]));
387 psi = -phi;
388 } else {
389 psi = ValueType(atan2(-mat[2][1], mat[1][1]));
390 phi = ValueType(atan2(-mat[0][2], mat[0][0]));
391 theta = ValueType(atan2(mat[0][1],
392 sqrt(mat[0][0] * mat[0][0] +
393 mat[0][2] * mat[0][2])));
394 }
395 return V(psi, phi, theta);
396
397 case XZX_ROTATION:
398
399 if (isApproxEqual(mat[0][0], ValueType(1.0), eps)) {
400 theta = ValueType(0.0);
401 phi = ValueType(0.5 * atan2(mat[1][2], mat[1][1]));
402 psi = phi;
403 } else if (isApproxEqual(mat[0][0], ValueType(-1.0), eps)) {
404 theta = ValueType(math::pi<double>());
405 psi = ValueType(0.5 * atan2(mat[2][1], -mat[1][1]));
406 phi = - psi;
407 } else {
408 psi = ValueType(atan2(mat[2][0], -mat[1][0]));
409 phi = ValueType(atan2(mat[0][2], mat[0][1]));
410 theta = ValueType(atan2(sqrt(mat[0][1] * mat[0][1] +
411 mat[0][2] * mat[0][2]),
412 mat[0][0]));
413 }
414 return V(phi, psi, theta);
415
416 case ZXZ_ROTATION:
417
418 if (isApproxEqual(mat[2][2], ValueType(1.0), eps)) {
419 theta = ValueType(0.0);
420 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
421 psi = phi;
422 } else if (isApproxEqual(mat[2][2], ValueType(-1.0), eps)) {
423 theta = ValueType(math::pi<double>());
424 phi = ValueType(0.5 * atan2(mat[0][1], mat[0][0]));
425 psi = -phi;
426 } else {
427 psi = ValueType(atan2(mat[0][2], mat[1][2]));
428 phi = ValueType(atan2(mat[2][0], -mat[2][1]));
429 theta = ValueType(atan2(sqrt(mat[0][2] * mat[0][2] +
430 mat[1][2] * mat[1][2]),
431 mat[2][2]));
432 }
433 return V(theta, psi, phi);
434
435 case YXZ_ROTATION:
436
437 if (isApproxEqual(mat[2][1], ValueType(1.0), eps)) {
438 theta = ValueType(-math::pi<double>() / 2.0);
439 phi = ValueType(0.5 * atan2(-mat[1][0], mat[0][0]));
440 psi = phi;
441 } else if (isApproxEqual(mat[2][1], ValueType(-1.0), eps)) {
442 theta = ValueType(math::pi<double>() / 2.0);
443 phi = ValueType(0.5 * atan2(mat[1][0], mat[0][0]));
444 psi = -phi;
445 } else {
446 psi = ValueType(atan2(mat[0][1], mat[1][1]));
447 phi = ValueType(atan2(mat[2][0], mat[2][2]));
448 theta = ValueType(atan2(-mat[2][1],
449 sqrt(mat[0][1] * mat[0][1] +
450 mat[1][1] * mat[1][1])));
451 }
452 return V(theta, phi, psi);
453
454 case ZYX_ROTATION:
455
456 if (isApproxEqual(mat[0][2], ValueType(1.0), eps)) {
457 theta = ValueType(-math::pi<double>() / 2.0);
458 phi = ValueType(0.5 * atan2(-mat[1][0], mat[1][1]));
459 psi = phi;
460 } else if (isApproxEqual(mat[0][2], ValueType(-1.0), eps)) {
461 theta = ValueType(math::pi<double>() / 2.0);
462 phi = ValueType(0.5 * atan2(mat[2][1], mat[2][0]));
463 psi = -phi;
464 } else {
465 psi = ValueType(atan2(mat[1][2], mat[2][2]));
466 phi = ValueType(atan2(mat[0][1], mat[0][0]));
467 theta = ValueType(atan2(-mat[0][2],
468 sqrt(mat[0][1] * mat[0][1] +
469 mat[0][0] * mat[0][0])));
470 }
471 return V(psi, theta, phi);
472
473 case XZY_ROTATION:
474
475 if (isApproxEqual(mat[1][0], ValueType(-1.0), eps)) {
476 theta = ValueType(math::pi<double>() / 2.0);
477 psi = ValueType(0.5 * atan2(mat[2][1], mat[2][2]));
478 phi = -psi;
479 } else if (isApproxEqual(mat[1][0], ValueType(1.0), eps)) {
480 theta = ValueType(-math::pi<double>() / 2.0);
481 psi = ValueType(0.5 * atan2(- mat[2][1], mat[2][2]));
482 phi = psi;
483 } else {
484 psi = ValueType(atan2(mat[2][0], mat[0][0]));
485 phi = ValueType(atan2(mat[1][2], mat[1][1]));
486 theta = ValueType(atan2(- mat[1][0],
487 sqrt(mat[1][1] * mat[1][1] +
488 mat[1][2] * mat[1][2])));
489 }
490 return V(phi, psi, theta);
491 }
492
493 OPENVDB_THROW(NotImplementedError, "Euler extraction sequence not implemented");
494}
495
496
497/// @brief Return a rotation matrix that maps @a v1 onto @a v2
498/// about the cross product of @a v1 and @a v2.
499/// <a name="rotation_v1_v2"></a>
500template<typename MatType, typename ValueType1, typename ValueType2>
501inline MatType
502 rotation(
503 const Vec3<ValueType1>& _v1,
504 const Vec3<ValueType2>& _v2,
505 typename MatType::value_type eps = static_cast<typename MatType::value_type>(1.0e-8))
506{
507 using T = typename MatType::value_type;
508
509 Vec3<T> v1(_v1);
510 Vec3<T> v2(_v2);
511
512 // Check if v1 and v2 are unit length
513 if (!isApproxEqual(T(1), v1.dot(v1), eps)) {
514 v1.normalize();
515 }
516 if (!isApproxEqual(T(1), v2.dot(v2), eps)) {
517 v2.normalize();
518 }
519
520 Vec3<T> cross;
521 cross.cross(v1, v2);
522
523 if (isApproxEqual(cross[0], zeroVal<T>(), eps) &&
524 isApproxEqual(cross[1], zeroVal<T>(), eps) &&
525 isApproxEqual(cross[2], zeroVal<T>(), eps)) {
526
527
528 // Given two unit vectors v1 and v2 that are nearly parallel, build a
529 // rotation matrix that maps v1 onto v2. First find which principal axis
530 // p is closest to perpendicular to v1. Find a reflection that exchanges
531 // v1 and p, and find a reflection that exchanges p2 and v2. The desired
532 // rotation matrix is the composition of these two reflections. See the
533 // paper "Efficiently Building a Matrix to Rotate One Vector to
534 // Another" by Tomas Moller and John Hughes in Journal of Graphics
535 // Tools Vol 4, No 4 for details.
536
537 Vec3<T> u, v, p(0.0, 0.0, 0.0);
538
539 double x = Abs(v1[0]);
540 double y = Abs(v1[1]);
541 double z = Abs(v1[2]);
542
543 if (x < y) {
544 if (z < x) {
545 p[2] = 1;
546 } else {
547 p[0] = 1;
548 }
549 } else {
550 if (z < y) {
551 p[2] = 1;
552 } else {
553 p[1] = 1;
554 }
555 }
556 u = p - v1;
557 v = p - v2;
558
559 double udot = u.dot(u);
560 double vdot = v.dot(v);
561
562 double a = -2 / udot;
563 double b = -2 / vdot;
564 double c = 4 * u.dot(v) / (udot * vdot);
565
566 MatType result;
567 result.setIdentity();
568
569 for (int j = 0; j < 3; j++) {
570 for (int i = 0; i < 3; i++)
571 result[i][j] = static_cast<T>(
572 a * u[i] * u[j] + b * v[i] * v[j] + c * v[j] * u[i]);
573 }
574 result[0][0] += 1.0;
575 result[1][1] += 1.0;
576 result[2][2] += 1.0;
577
578 if(MatType::numColumns() == 4) padMat4(result);
579 return result;
580
581 } else {
582 double c = v1.dot(v2);
583 double a = (1.0 - c) / cross.dot(cross);
584
585 double a0 = a * cross[0];
586 double a1 = a * cross[1];
587 double a2 = a * cross[2];
588
589 double a01 = a0 * cross[1];
590 double a02 = a0 * cross[2];
591 double a12 = a1 * cross[2];
592
593 MatType r;
594
595 r[0][0] = static_cast<T>(c + a0 * cross[0]);
596 r[0][1] = static_cast<T>(a01 + cross[2]);
597 r[0][2] = static_cast<T>(a02 - cross[1]);
598 r[1][0] = static_cast<T>(a01 - cross[2]);
599 r[1][1] = static_cast<T>(c + a1 * cross[1]);
600 r[1][2] = static_cast<T>(a12 + cross[0]);
601 r[2][0] = static_cast<T>(a02 + cross[1]);
602 r[2][1] = static_cast<T>(a12 - cross[0]);
603 r[2][2] = static_cast<T>(c + a2 * cross[2]);
604
605 if(MatType::numColumns() == 4) padMat4(r);
606 return r;
607
608 }
609}
610
611
612/// Return a matrix that scales by @a s.
613template<class MatType>
614MatType
615scale(const Vec3<typename MatType::value_type>& s)
616{
617 // Gets identity, then sets top 3 diagonal
618 // Inefficient by 3 sets.
619
620 MatType result;
621 result.setIdentity();
622 result[0][0] = s[0];
623 result[1][1] = s[1];
624 result[2][2] = s[2];
625
626 return result;
627}
628
629
630/// Return a Vec3 representing the lengths of the passed matrix's upper 3&times;3's rows.
631template<class MatType>
632 Vec3<typename MatType::value_type>
633getScale(const MatType &mat)
634{
635 using V = Vec3<typename MatType::value_type>;
636 return V(
637 V(mat[0][0], mat[0][1], mat[0][2]).length(),
638 V(mat[1][0], mat[1][1], mat[1][2]).length(),
639 V(mat[2][0], mat[2][1], mat[2][2]).length());
640}
641
642
643/// @brief Return a copy of the given matrix with its upper 3&times;3 rows normalized.
644/// @details This can be geometrically interpreted as a matrix with no scaling
645/// along its major axes.
646template<class MatType>
647MatType
648unit(const MatType &mat, typename MatType::value_type eps = 1.0e-8)
649{
650 Vec3<typename MatType::value_type> dud;
651 return unit(mat, eps, dud);
652}
653
654
655/// @brief Return a copy of the given matrix with its upper 3&times;3 rows normalized,
656/// and return the length of each of these rows in @a scaling.
657/// @details This can be geometrically interpretted as a matrix with no scaling
658/// along its major axes, and the scaling in the input vector
659template<class MatType>
660MatType
661 unit(
662 const MatType &in,
663 typename MatType::value_type eps,
664 Vec3<typename MatType::value_type>& scaling)
665{
666 using T = typename MatType::value_type;
667 MatType result(in);
668
669 for (int i(0); i < 3; i++) {
670 try {
671 const Vec3<T> u(
672 Vec3<T>(in[i][0], in[i][1], in[i][2]).unit(eps, scaling[i]));
673 for (int j=0; j<3; j++) result[i][j] = u[j];
674 } catch (ArithmeticError&) {
675 for (int j=0; j<3; j++) result[i][j] = 0;
676 }
677 }
678 return result;
679}
680
681
682/// @brief Set the matrix to a shear along @a axis0 by a fraction of @a axis1.
683/// @param axis0 The fixed axis of the shear.
684/// @param axis1 The shear axis.
685/// @param shear The shear factor.
686template <class MatType>
687MatType
688shear(Axis axis0, Axis axis1, typename MatType::value_type shear)
689{
690 int index0 = static_cast<int>(axis0);
691 int index1 = static_cast<int>(axis1);
692
693 MatType result;
694 result.setIdentity();
695 if (axis0 == axis1) {
696 result[index1][index0] = shear + 1;
697 } else {
698 result[index1][index0] = shear;
699 }
700
701 return result;
702}
703
704
705/// Return a matrix as the cross product of the given vector.
706template<class MatType>
707MatType
708skew(const Vec3<typename MatType::value_type> &skew)
709{
710 using T = typename MatType::value_type;
711
712 MatType r;
713 r[0][0] = T(0); r[0][1] = skew.z(); r[0][2] = -skew.y();
714 r[1][0] = -skew.z(); r[1][1] = T(0); r[2][1] = skew.x();
715 r[2][0] = skew.y(); r[2][1] = -skew.x(); r[2][2] = T(0);
716
717 if(MatType::numColumns() == 4) padMat4(r);
718 return r;
719}
720
721
722/// @brief Return an orientation matrix such that z points along @a direction,
723/// and y is along the @a direction / @a vertical plane.
724template<class MatType>
725MatType
726aim(const Vec3<typename MatType::value_type>& direction,
727 const Vec3<typename MatType::value_type>& vertical)
728{
729 using T = typename MatType::value_type;
730 Vec3<T> forward(direction.unit());
731 Vec3<T> horizontal(vertical.unit().cross(forward).unit());
732 Vec3<T> up(forward.cross(horizontal).unit());
733
734 MatType r;
735
736 r[0][0]=horizontal.x(); r[0][1]=horizontal.y(); r[0][2]=horizontal.z();
737 r[1][0]=up.x(); r[1][1]=up.y(); r[1][2]=up.z();
738 r[2][0]=forward.x(); r[2][1]=forward.y(); r[2][2]=forward.z();
739
740 if(MatType::numColumns() == 4) padMat4(r);
741 return r;
742}
743
744/// @brief This function snaps a specific axis to a specific direction,
745/// preserving scaling.
746/// @details It does this using minimum energy, thus posing a unique solution if
747/// basis & direction aren't parallel.
748/// @note @a direction need not be unit.
749template<class MatType>
750inline MatType
751snapMatBasis(const MatType& source, Axis axis, const Vec3<typename MatType::value_type>& direction)
752{
753 using T = typename MatType::value_type;
754
755 Vec3<T> unitDir(direction.unit());
756 Vec3<T> ourUnitAxis(source.row(axis).unit());
757
758 // Are the two parallel?
759 T parallel = unitDir.dot(ourUnitAxis);
760
761 // Already snapped!
762 if (isApproxEqual(parallel, T(1.0))) return source;
763
764 if (isApproxEqual(parallel, T(-1.0))) {
765 OPENVDB_THROW(ValueError, "Cannot snap to inverse axis");
766 }
767
768 // Find angle between our basis and the one specified
769 T angleBetween(angle(unitDir, ourUnitAxis));
770 // Caclulate axis to rotate along
771 Vec3<T> rotationAxis = unitDir.cross(ourUnitAxis);
772
773 MatType rotation;
774 rotation.setToRotation(rotationAxis, angleBetween);
775
776 return source * rotation;
777}
778
779/// @brief Write 0s along Mat4's last row and column, and a 1 on its diagonal.
780/// @details Useful initialization when we're initializing just the 3&times;3 block.
781template<class MatType>
782inline MatType&
783padMat4(MatType& dest)
784{
785 dest[0][3] = dest[1][3] = dest[2][3] = 0;
786 dest[3][2] = dest[3][1] = dest[3][0] = 0;
787 dest[3][3] = 1;
788
789 return dest;
790}
791
792
793/// @brief Solve for A=B*B, given A.
794/// @details Denman-Beavers square root iteration
795template<typename MatType>
796inline void
797sqrtSolve(const MatType& aA, MatType& aB, double aTol=0.01)
798{
799 unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
800
801 MatType Y[2], Z[2];
802 Y[0] = aA;
803 Z[0] = MatType::identity();
804
805 unsigned int current = 0;
806 for (unsigned int iteration=0; iteration < iterations; iteration++) {
807 unsigned int last = current;
808 current = !current;
809
810 MatType invY = Y[last].inverse();
811 MatType invZ = Z[last].inverse();
812
813 Y[current] = 0.5 * (Y[last] + invZ);
814 Z[current] = 0.5 * (Z[last] + invY);
815 }
816 aB = Y[current];
817}
818
819
820template<typename MatType>
821inline void
822powSolve(const MatType& aA, MatType& aB, double aPower, double aTol=0.01)
823{
824 unsigned int iterations = static_cast<unsigned int>(log(aTol)/log(0.5));
825
826 const bool inverted = (aPower < 0.0);
827 if (inverted) { aPower = -aPower; }
828
829 unsigned int whole = static_cast<unsigned int>(aPower);
830 double fraction = aPower - whole;
831
832 MatType R = MatType::identity();
833 MatType partial = aA;
834
835 double contribution = 1.0;
836 for (unsigned int iteration = 0; iteration < iterations; iteration++) {
837 sqrtSolve(partial, partial, aTol);
838 contribution *= 0.5;
839 if (fraction >= contribution) {
840 R *= partial;
841 fraction -= contribution;
842 }
843 }
844
845 partial = aA;
846 while (whole) {
847 if (whole & 1) { R *= partial; }
848 whole >>= 1;
849 if (whole) { partial *= partial; }
850 }
851
852 if (inverted) { aB = R.inverse(); }
853 else { aB = R; }
854}
855
856
857/// @brief Determine if a matrix is an identity matrix.
858template<typename MatType>
859inline bool
860isIdentity(const MatType& m)
861{
862 return m.eq(MatType::identity());
863}
864
865
866/// @brief Determine if a matrix is invertible.
867template<typename MatType>
868inline bool
869isInvertible(const MatType& m)
870{
871 using ValueType = typename MatType::ValueType;
872 return !isApproxEqual(m.det(), ValueType(0));
873}
874
875
876/// @brief Determine if a matrix is symmetric.
877/// @details This implicitly uses math::isApproxEqual() to determine equality.
878template<typename MatType>
879inline bool
880isSymmetric(const MatType& m)
881{
882 return m.eq(m.transpose());
883}
884
885
886/// Determine if a matrix is unitary (i.e., rotation or reflection).
887template<typename MatType>
888inline bool
889isUnitary(const MatType& m)
890{
891 using ValueType = typename MatType::ValueType;
892 if (!isApproxEqual(std::abs(m.det()), ValueType(1.0))) return false;
893 // check that the matrix transpose is the inverse
894 MatType temp = m * m.transpose();
895 return temp.eq(MatType::identity());
896}
897
898
899/// Determine if a matrix is diagonal.
900template<typename MatType>
901inline bool
902isDiagonal(const MatType& mat)
903{
904 int n = MatType::size;
905 typename MatType::ValueType temp(0);
906 for (int i = 0; i < n; ++i) {
907 for (int j = 0; j < n; ++j) {
908 if (i != j) {
909 temp += std::abs(mat(i,j));
910 }
911 }
912 }
913 return isApproxEqual(temp, typename MatType::ValueType(0.0));
914}
915
916
917/// Return the <i>L</i><sub>&infin;</sub> norm of an <i>N</i>&times;<i>N</i> matrix.
918template<typename MatType>
919typename MatType::ValueType
920lInfinityNorm(const MatType& matrix)
921{
922 int n = MatType::size;
923 typename MatType::ValueType norm = 0;
924
925 for( int j = 0; j<n; ++j) {
926 typename MatType::ValueType column_sum = 0;
927
928 for (int i = 0; i<n; ++i) {
929 column_sum += std::fabs(matrix(i,j));
930 }
931 norm = std::max(norm, column_sum);
932 }
933
934 return norm;
935}
936
937
938/// Return the <i>L</i><sub>1</sub> norm of an <i>N</i>&times;<i>N</i> matrix.
939template<typename MatType>
940typename MatType::ValueType
941lOneNorm(const MatType& matrix)
942{
943 int n = MatType::size;
944 typename MatType::ValueType norm = 0;
945
946 for( int i = 0; i<n; ++i) {
947 typename MatType::ValueType row_sum = 0;
948
949 for (int j = 0; j<n; ++j) {
950 row_sum += std::fabs(matrix(i,j));
951 }
952 norm = std::max(norm, row_sum);
953 }
954
955 return norm;
956}
957
958
959/// @brief Decompose an invertible 3&times;3 matrix into a unitary matrix
960/// followed by a symmetric matrix (positive semi-definite Hermitian),
961/// i.e., M = U * S.
962/// @details If det(U) = 1 it is a rotation, otherwise det(U) = -1,
963/// meaning there is some part reflection.
964/// See "Computing the polar decomposition with applications"
965/// Higham, N.J. - SIAM J. Sc. Stat Comput 7(4):1160-1174
966template<typename MatType>
967bool
968polarDecomposition(const MatType& input, MatType& unitary,
969 MatType& positive_hermitian, unsigned int MAX_ITERATIONS=100)
970{
971 unitary = input;
972 MatType new_unitary(input);
973 MatType unitary_inv;
974
975 if (fabs(unitary.det()) < math::Tolerance<typename MatType::ValueType>::value()) return false;
976
977 unsigned int iteration(0);
978
979 typename MatType::ValueType linf_of_u;
980 typename MatType::ValueType l1nm_of_u;
981 typename MatType::ValueType linf_of_u_inv;
982 typename MatType::ValueType l1nm_of_u_inv;
983 typename MatType::ValueType l1_error = 100;
984 double gamma;
985
986 do {
987 unitary_inv = unitary.inverse();
988 linf_of_u = lInfinityNorm(unitary);
989 l1nm_of_u = lOneNorm(unitary);
990
991 linf_of_u_inv = lInfinityNorm(unitary_inv);
992 l1nm_of_u_inv = lOneNorm(unitary_inv);
993
994 gamma = sqrt( sqrt( (l1nm_of_u_inv * linf_of_u_inv ) / (l1nm_of_u * linf_of_u) ));
995
996 new_unitary = 0.5*(gamma * unitary + (1./gamma) * unitary_inv.transpose() );
997
998 l1_error = lInfinityNorm(unitary - new_unitary);
999 unitary = new_unitary;
1000
1001 /// this generally converges in less than ten iterations
1002 if (iteration > MAX_ITERATIONS) return false;
1003 iteration++;
1004 } while (l1_error > math::Tolerance<typename MatType::ValueType>::value());
1005
1006 positive_hermitian = unitary.transpose() * input;
1007 return true;
1008}
1009
1010////////////////////////////////////////
1011
1012/// @return true if m0 < m1, comparing components in order of significance.
1013template<unsigned SIZE, typename T>
1014inline bool
1015cwiseLessThan(const Mat<SIZE, T>& m0, const Mat<SIZE, T>& m1)
1016{
1017 const T* m0p = m0.asPointer();
1018 const T* m1p = m1.asPointer();
1019 constexpr unsigned size = SIZE*SIZE;
1020 for (unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
1021 if (!math::isExactlyEqual(*m0p, *m1p)) return *m0p < *m1p;
1022 }
1023 return *m0p < *m1p;
1024}
1025
1026/// @return true if m0 > m1, comparing components in order of significance.
1027template<unsigned SIZE, typename T>
1028inline bool
1029cwiseGreaterThan(const Mat<SIZE, T>& m0, const Mat<SIZE, T>& m1)
1030{
1031 const T* m0p = m0.asPointer();
1032 const T* m1p = m1.asPointer();
1033 constexpr unsigned size = SIZE*SIZE;
1034 for (unsigned i = 0; i < size-1; ++i, ++m0p, ++m1p) {
1035 if (!math::isExactlyEqual(*m0p, *m1p)) return *m0p > *m1p;
1036 }
1037 return *m0p > *m1p;
1038}
1039
1040} // namespace math
1041} // namespace OPENVDB_VERSION_NAME
1042} // namespace openvdb
1043
1044#endif // OPENVDB_MATH_MAT_HAS_BEEN_INCLUDED
Math.h
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
openvdb::v11_0::ArithmeticError
Definition Exceptions.h:56
openvdb::v11_0::NotImplementedError
Definition Exceptions.h:61
openvdb::v11_0::ValueError
Definition Exceptions.h:65
openvdb::v11_0::math::Mat
Definition Mat.h:27
openvdb::v11_0::math::Mat::isZero
bool isZero() const
True if all elements are exactly zero.
Definition Mat.h:152
openvdb::v11_0::math::Mat::operator[]
T * operator[](int i)
Array style reference to ith row.
Definition Mat.h:106
openvdb::v11_0::math::Mat::operator[]
const T * operator[](int i) const
Definition Mat.h:107
openvdb::v11_0::math::Mat::Mat
Mat()=default
openvdb::v11_0::math::Mat::write
void write(std::ostream &os) const
Definition Mat.h:110
openvdb::v11_0::math::Mat::numColumns
static unsigned numColumns()
Definition Mat.h:40
openvdb::v11_0::math::Mat::asPointer
T * asPointer()
Direct access to the internal data.
Definition Mat.h:101
openvdb::v11_0::math::Mat::numRows
static unsigned numRows()
Definition Mat.h:39
openvdb::v11_0::math::Mat::str
std::string str(unsigned indentation=0) const
Definition Mat.h:53
openvdb::v11_0::math::Mat::absMax
T absMax() const
Return the maximum of the absolute of all elements in this matrix.
Definition Mat.h:119
openvdb::v11_0::math::Mat::isInfinite
bool isInfinite() const
True if an Inf is present in this matrix.
Definition Mat.h:136
openvdb::v11_0::math::Mat::asPointer
const T * asPointer() const
Definition Mat.h:102
openvdb::v11_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:92
openvdb::v11_0::math::Mat::SIZE_
SIZE_
Definition Mat.h:31
openvdb::v11_0::math::Mat::numElements
static unsigned numElements()
Definition Mat.h:41
openvdb::v11_0::math::Mat::ValueType
T ValueType
Definition Mat.h:30
openvdb::v11_0::math::Mat::isNan
bool isNan() const
True if a Nan is present in this matrix.
Definition Mat.h:128
openvdb::v11_0::math::Mat::value_type
T value_type
Definition Mat.h:29
openvdb::v11_0::math::Mat::read
void read(std::istream &is)
Definition Mat.h:114
openvdb::v11_0::math::Mat::isFinite
bool isFinite() const
True if no Nan or Inf values are present.
Definition Mat.h:144
openvdb::v11_0::math::Quat
Definition Quat.h:79
openvdb::v11_0::math::Quat::x
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition Quat.h:193
openvdb::v11_0::math::Quat::y
T & y()
Definition Quat.h:194
openvdb::v11_0::math::Quat::z
T & z()
Definition Quat.h:195
openvdb::v11_0::math::Quat::w
T & w()
Definition Quat.h:196
openvdb::v11_0::math::Quat::dot
T dot(const Quat &q) const
Dot product.
Definition Quat.h:451
openvdb::v11_0::math::Vec3
Definition Vec3.h:24
openvdb::v11_0::math::Vec3::x
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition Vec3.h:85
openvdb::v11_0::math::Vec3::dot
T dot(const Vec3< T > &v) const
Dot product.
Definition Vec3.h:191
openvdb::v11_0::math::Vec3::y
T & y()
Definition Vec3.h:86
openvdb::v11_0::math::Vec3::z
T & z()
Definition Vec3.h:87
openvdb::v11_0::math::Vec3::cross
Vec3< T > cross(const Vec3< T > &v) const
Return the cross product of "this" vector and v;.
Definition Vec3.h:220
openvdb::v11_0::math::Vec3::unit
Vec3< T > unit(T eps=0) const
return normalized this, throws if null vector
Definition Vec3.h:374
openvdb::v11_0::math::Vec3::normalize
bool normalize(T eps=T(1.0e-7))
this = normalized this
Definition Vec3.h:362
openvdb::v11_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:751
openvdb::v11_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:333
openvdb::v11_0::math::cwiseLessThan
bool cwiseLessThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition Mat.h:1015
openvdb::v11_0::math::isDiagonal
bool isDiagonal(const MatType &mat)
Determine if a matrix is diagonal.
Definition Mat.h:902
openvdb::v11_0::math::isApproxEqual
bool isApproxEqual(const Type &a, const Type &b, const Type &tolerance)
Return true if a is equal to b to within the given tolerance.
Definition Math.h:406
openvdb::v11_0::math::isInvertible
bool isInvertible(const MatType &m)
Determine if a matrix is invertible.
Definition Mat.h:869
openvdb::v11_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:783
openvdb::v11_0::math::lInfinityNorm
MatType::ValueType lInfinityNorm(const MatType &matrix)
Return the L∞ norm of an N×N matrix.
Definition Mat.h:920
openvdb::v11_0::math::scale
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition Mat.h:615
openvdb::v11_0::math::isUnitary
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition Mat.h:889
openvdb::v11_0::math::isSymmetric
bool isSymmetric(const MatType &m)
Determine if a matrix is symmetric.
Definition Mat.h:880
openvdb::v11_0::math::lOneNorm
MatType::ValueType lOneNorm(const MatType &matrix)
Return the L1 norm of an N×N matrix.
Definition Mat.h:941
openvdb::v11_0::math::unit
MatType unit(const MatType &mat, typename MatType::value_type eps=1.0e-8)
Return a copy of the given matrix with its upper 3×3 rows normalized.
Definition Mat.h:648
openvdb::v11_0::math::rotation
MatType rotation(const Quat< typename MatType::value_type > &q, typename MatType::value_type eps=static_cast< typename MatType::value_type >(1.0e-8))
Return the rotation matrix specified by the given quaternion.
Definition Mat.h:172
openvdb::v11_0::math::Abs
Coord Abs(const Coord &xyz)
Definition Coord.h:517
openvdb::v11_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:968
openvdb::v11_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:726
openvdb::v11_0::math::angle
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition Vec2.h:446
openvdb::v11_0::math::powSolve
void powSolve(const MatType &aA, MatType &aB, double aPower, double aTol=0.01)
Definition Mat.h:822
openvdb::v11_0::math::isIdentity
bool isIdentity(const MatType &m)
Determine if a matrix is an identity matrix.
Definition Mat.h:860
openvdb::v11_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:688
openvdb::v11_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:708
openvdb::v11_0::math::Axis
Axis
Definition Math.h:901
openvdb::v11_0::math::Z_AXIS
@ Z_AXIS
Definition Math.h:904
openvdb::v11_0::math::X_AXIS
@ X_AXIS
Definition Math.h:902
openvdb::v11_0::math::Y_AXIS
@ Y_AXIS
Definition Math.h:903
openvdb::v11_0::math::cwiseGreaterThan
bool cwiseGreaterThan(const Mat< SIZE, T > &m0, const Mat< SIZE, T > &m1)
Definition Mat.h:1029
openvdb::v11_0::math::sqrtSolve
void sqrtSolve(const MatType &aA, MatType &aB, double aTol=0.01)
Solve for A=B*B, given A.
Definition Mat.h:797
openvdb::v11_0::math::RotationOrder
RotationOrder
Definition Math.h:908
openvdb::v11_0::math::YXZ_ROTATION
@ YXZ_ROTATION
Definition Math.h:911
openvdb::v11_0::math::ZXY_ROTATION
@ ZXY_ROTATION
Definition Math.h:913
openvdb::v11_0::math::YZX_ROTATION
@ YZX_ROTATION
Definition Math.h:912
openvdb::v11_0::math::ZXZ_ROTATION
@ ZXZ_ROTATION
Definition Math.h:916
openvdb::v11_0::math::XZX_ROTATION
@ XZX_ROTATION
Definition Math.h:915
openvdb::v11_0::math::XYZ_ROTATION
@ XYZ_ROTATION
Definition Math.h:909
openvdb::v11_0::math::ZYX_ROTATION
@ ZYX_ROTATION
Definition Math.h:914
openvdb::v11_0::math::XZY_ROTATION
@ XZY_ROTATION
Definition Math.h:910
openvdb::v11_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:633
openvdb
Definition Exceptions.h:13
Exceptions.h
OPENVDB_THROW
#define OPENVDB_THROW(exception, message)
Definition Exceptions.h:74
openvdb::v11_0::math::Tolerance
Tolerance for floating-point comparison.
Definition Math.h:148
OPENVDB_VERSION_NAME
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition version.h.in:121
OPENVDB_USE_VERSION_NAMESPACE
#define OPENVDB_USE_VERSION_NAMESPACE
Definition version.h.in:212