| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright Contributors to the OpenVDB Project | ||
| 2 | // SPDX-License-Identifier: MPL-2.0 | ||
| 3 | |||
| 4 | #ifndef OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED | ||
| 5 | #define OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED | ||
| 6 | |||
| 7 | #include <openvdb/Exceptions.h> | ||
| 8 | #include "Vec3.h" | ||
| 9 | #include "Mat.h" | ||
| 10 | #include <algorithm> // for std::copy() | ||
| 11 | #include <cassert> | ||
| 12 | #include <cmath> | ||
| 13 | #include <iomanip> | ||
| 14 | |||
| 15 | |||
| 16 | namespace openvdb { | ||
| 17 | OPENVDB_USE_VERSION_NAMESPACE | ||
| 18 | namespace OPENVDB_VERSION_NAME { | ||
| 19 | namespace math { | ||
| 20 | |||
| 21 | template<typename T> class Vec3; | ||
| 22 | template<typename T> class Mat4; | ||
| 23 | template<typename T> class Quat; | ||
| 24 | |||
| 25 | /// @class Mat3 Mat3.h | ||
| 26 | /// @brief 3x3 matrix class. | ||
| 27 | template<typename T> | ||
| 28 | class Mat3: public Mat<3, T> | ||
| 29 | { | ||
| 30 | public: | ||
| 31 | /// Data type held by the matrix. | ||
| 32 | using value_type = T; | ||
| 33 | using ValueType = T; | ||
| 34 | using MyBase = Mat<3, T>; | ||
| 35 | |||
| 36 | /// Trivial constructor, the matrix is NOT initialized | ||
| 37 | #if OPENVDB_ABI_VERSION_NUMBER >= 8 | ||
| 38 | /// @note destructor, copy constructor, assignment operator and | ||
| 39 | /// move constructor are left to be defined by the compiler (default) | ||
| 40 | Mat3() = default; | ||
| 41 | #else | ||
| 42 | Mat3() {} | ||
| 43 | |||
| 44 | /// Copy constructor | ||
| 45 | Mat3(const Mat<3, T> &m) | ||
| 46 | { | ||
| 47 | for (int i=0; i<3; ++i) { | ||
| 48 | for (int j=0; j<3; ++j) { | ||
| 49 | MyBase::mm[i*3 + j] = m[i][j]; | ||
| 50 | } | ||
| 51 | } | ||
| 52 | } | ||
| 53 | #endif | ||
| 54 | |||
| 55 | /// Constructor given the quaternion rotation, e.g. Mat3f m(q); | ||
| 56 | /// The quaternion is normalized and used to construct the matrix | ||
| 57 | 4 | Mat3(const Quat<T> &q) | |
| 58 | { setToRotation(q); } | ||
| 59 | |||
| 60 | |||
| 61 | /// Constructor given array of elements, the ordering is in row major form: | ||
| 62 | /** @verbatim | ||
| 63 | a b c | ||
| 64 | d e f | ||
| 65 | g h i | ||
| 66 | @endverbatim */ | ||
| 67 | template<typename Source> | ||
| 68 | 168379382 | Mat3(Source a, Source b, Source c, | |
| 69 | Source d, Source e, Source f, | ||
| 70 | Source g, Source h, Source i) | ||
| 71 | { | ||
| 72 | 168379382 | MyBase::mm[0] = static_cast<T>(a); | |
| 73 | 168379382 | MyBase::mm[1] = static_cast<T>(b); | |
| 74 | 168379382 | MyBase::mm[2] = static_cast<T>(c); | |
| 75 | 168379382 | MyBase::mm[3] = static_cast<T>(d); | |
| 76 | 168379382 | MyBase::mm[4] = static_cast<T>(e); | |
| 77 | 168379382 | MyBase::mm[5] = static_cast<T>(f); | |
| 78 | 168379382 | MyBase::mm[6] = static_cast<T>(g); | |
| 79 | 168379382 | MyBase::mm[7] = static_cast<T>(h); | |
| 80 |
14/24✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 1 times.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
|
4721241 | MyBase::mm[8] = static_cast<T>(i); |
| 81 | } // constructor1Test | ||
| 82 | |||
| 83 | /// Construct matrix from rows or columns vectors (defaults to rows | ||
| 84 | /// for historical reasons) | ||
| 85 | template<typename Source> | ||
| 86 | Mat3(const Vec3<Source> &v1, const Vec3<Source> &v2, const Vec3<Source> &v3, bool rows = true) | ||
| 87 | { | ||
| 88 | if (rows) { | ||
| 89 | this->setRows(v1, v2, v3); | ||
| 90 | } else { | ||
| 91 | this->setColumns(v1, v2, v3); | ||
| 92 | } | ||
| 93 | } | ||
| 94 | |||
| 95 | /// Constructor given array of elements, the ordering is in row major form:\n | ||
| 96 | /// a[0] a[1] a[2]\n | ||
| 97 | /// a[3] a[4] a[5]\n | ||
| 98 | /// a[6] a[7] a[8]\n | ||
| 99 | template<typename Source> | ||
| 100 | Mat3(Source *a) | ||
| 101 | { | ||
| 102 | MyBase::mm[0] = static_cast<T>(a[0]); | ||
| 103 | MyBase::mm[1] = static_cast<T>(a[1]); | ||
| 104 | MyBase::mm[2] = static_cast<T>(a[2]); | ||
| 105 | MyBase::mm[3] = static_cast<T>(a[3]); | ||
| 106 | MyBase::mm[4] = static_cast<T>(a[4]); | ||
| 107 | MyBase::mm[5] = static_cast<T>(a[5]); | ||
| 108 | MyBase::mm[6] = static_cast<T>(a[6]); | ||
| 109 | MyBase::mm[7] = static_cast<T>(a[7]); | ||
| 110 | MyBase::mm[8] = static_cast<T>(a[8]); | ||
| 111 | } // constructor1Test | ||
| 112 | |||
| 113 | /// Conversion constructor | ||
| 114 | template<typename Source> | ||
| 115 | ✗ | explicit Mat3(const Mat3<Source> &m) | |
| 116 | { | ||
| 117 | ✗ | for (int i=0; i<3; ++i) { | |
| 118 | ✗ | for (int j=0; j<3; ++j) { | |
| 119 | ✗ | MyBase::mm[i*3 + j] = static_cast<T>(m[i][j]); | |
| 120 | } | ||
| 121 | } | ||
| 122 | } | ||
| 123 | |||
| 124 | /// Conversion from Mat4 (copies top left) | ||
| 125 | explicit Mat3(const Mat4<T> &m) | ||
| 126 | { | ||
| 127 | for (int i=0; i<3; ++i) { | ||
| 128 | for (int j=0; j<3; ++j) { | ||
| 129 | MyBase::mm[i*3 + j] = m[i][j]; | ||
| 130 | } | ||
| 131 | } | ||
| 132 | } | ||
| 133 | |||
| 134 | /// Predefined constant for identity matrix | ||
| 135 | 111 | static const Mat3<T>& identity() { | |
| 136 |
3/4✓ Branch 0 taken 2 times.
✓ Branch 1 taken 109 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
111 | static const Mat3<T> sIdentity = Mat3<T>( |
| 137 | 1, 0, 0, | ||
| 138 | 0, 1, 0, | ||
| 139 | 0, 0, 1 | ||
| 140 | ); | ||
| 141 | 111 | return sIdentity; | |
| 142 | } | ||
| 143 | |||
| 144 | /// Predefined constant for zero matrix | ||
| 145 | 6428481 | static const Mat3<T>& zero() { | |
| 146 |
3/4✓ Branch 0 taken 4 times.
✓ Branch 1 taken 3214244 times.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
6428481 | static const Mat3<T> sZero = Mat3<T>( |
| 147 | 0, 0, 0, | ||
| 148 | 0, 0, 0, | ||
| 149 | 0, 0, 0 | ||
| 150 | ); | ||
| 151 | 6428481 | return sZero; | |
| 152 | } | ||
| 153 | |||
| 154 | /// Set ith row to vector v | ||
| 155 | void setRow(int i, const Vec3<T> &v) | ||
| 156 | { | ||
| 157 | // assert(i>=0 && i<3); | ||
| 158 | int i3 = i * 3; | ||
| 159 | |||
| 160 | 18 | MyBase::mm[i3+0] = v[0]; | |
| 161 | 18 | MyBase::mm[i3+1] = v[1]; | |
| 162 | 18 | MyBase::mm[i3+2] = v[2]; | |
| 163 | } // rowColumnTest | ||
| 164 | |||
| 165 | /// Get ith row, e.g. Vec3d v = m.row(1); | ||
| 166 | Vec3<T> row(int i) const | ||
| 167 | { | ||
| 168 | // assert(i>=0 && i<3); | ||
| 169 | return Vec3<T>((*this)(i,0), (*this)(i,1), (*this)(i,2)); | ||
| 170 | } // rowColumnTest | ||
| 171 | |||
| 172 | /// Set jth column to vector v | ||
| 173 | void setCol(int j, const Vec3<T>& v) | ||
| 174 | { | ||
| 175 | // assert(j>=0 && j<3); | ||
| 176 | 18 | MyBase::mm[0+j] = v[0]; | |
| 177 | 18 | MyBase::mm[3+j] = v[1]; | |
| 178 | 18 | MyBase::mm[6+j] = v[2]; | |
| 179 | } // rowColumnTest | ||
| 180 | |||
| 181 | /// Get jth column, e.g. Vec3d v = m.col(0); | ||
| 182 | Vec3<T> col(int j) const | ||
| 183 | { | ||
| 184 | // assert(j>=0 && j<3); | ||
| 185 | return Vec3<T>((*this)(0,j), (*this)(1,j), (*this)(2,j)); | ||
| 186 | } // rowColumnTest | ||
| 187 | |||
| 188 | /// Alternative indexed reference to the elements | ||
| 189 | /// Note that the indices are row first and column second. | ||
| 190 | /// e.g. m(0,0) = 1; | ||
| 191 | T& operator()(int i, int j) | ||
| 192 | { | ||
| 193 | // assert(i>=0 && i<3); | ||
| 194 | // assert(j>=0 && j<3); | ||
| 195 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 9 times.
|
114 | return MyBase::mm[3*i+j]; |
| 196 | } // trivial | ||
| 197 | |||
| 198 | /// Alternative indexed constant reference to the elements, | ||
| 199 | /// Note that the indices are row first and column second. | ||
| 200 | /// e.g. float f = m(1,0); | ||
| 201 | T operator()(int i, int j) const | ||
| 202 | { | ||
| 203 | // assert(i>=0 && i<3); | ||
| 204 | // assert(j>=0 && j<3); | ||
| 205 | 3399052074 | return MyBase::mm[3*i+j]; | |
| 206 | } // trivial | ||
| 207 | |||
| 208 | /// Set the rows of this matrix to the vectors v1, v2, v3 | ||
| 209 | void setRows(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3) | ||
| 210 | { | ||
| 211 | MyBase::mm[0] = v1[0]; | ||
| 212 | MyBase::mm[1] = v1[1]; | ||
| 213 | MyBase::mm[2] = v1[2]; | ||
| 214 | MyBase::mm[3] = v2[0]; | ||
| 215 | MyBase::mm[4] = v2[1]; | ||
| 216 | MyBase::mm[5] = v2[2]; | ||
| 217 | MyBase::mm[6] = v3[0]; | ||
| 218 | MyBase::mm[7] = v3[1]; | ||
| 219 | MyBase::mm[8] = v3[2]; | ||
| 220 | } // setRows | ||
| 221 | |||
| 222 | /// Set the columns of this matrix to the vectors v1, v2, v3 | ||
| 223 | void setColumns(const Vec3<T> &v1, const Vec3<T> &v2, const Vec3<T> &v3) | ||
| 224 | { | ||
| 225 | MyBase::mm[0] = v1[0]; | ||
| 226 | MyBase::mm[1] = v2[0]; | ||
| 227 | MyBase::mm[2] = v3[0]; | ||
| 228 | MyBase::mm[3] = v1[1]; | ||
| 229 | MyBase::mm[4] = v2[1]; | ||
| 230 | MyBase::mm[5] = v3[1]; | ||
| 231 | MyBase::mm[6] = v1[2]; | ||
| 232 | MyBase::mm[7] = v2[2]; | ||
| 233 | MyBase::mm[8] = v3[2]; | ||
| 234 | } // setColumns | ||
| 235 | |||
| 236 | /// Set diagonal and symmetric triangular components | ||
| 237 | void setSymmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri) | ||
| 238 | { | ||
| 239 | MyBase::mm[0] = vdiag[0]; | ||
| 240 | MyBase::mm[1] = vtri[0]; | ||
| 241 | MyBase::mm[2] = vtri[1]; | ||
| 242 | MyBase::mm[3] = vtri[0]; | ||
| 243 | MyBase::mm[4] = vdiag[1]; | ||
| 244 | MyBase::mm[5] = vtri[2]; | ||
| 245 | MyBase::mm[6] = vtri[1]; | ||
| 246 | MyBase::mm[7] = vtri[2]; | ||
| 247 | MyBase::mm[8] = vdiag[2]; | ||
| 248 | } // setSymmetricTest | ||
| 249 | |||
| 250 | /// Return a matrix with the prescribed diagonal and symmetric triangular components. | ||
| 251 | static Mat3 symmetric(const Vec3<T> &vdiag, const Vec3<T> &vtri) | ||
| 252 | { | ||
| 253 | return Mat3( | ||
| 254 | vdiag[0], vtri[0], vtri[1], | ||
| 255 | vtri[0], vdiag[1], vtri[2], | ||
| 256 | vtri[1], vtri[2], vdiag[2] | ||
| 257 | ); | ||
| 258 | } | ||
| 259 | |||
| 260 | /// Set the matrix as cross product of the given vector | ||
| 261 | void setSkew(const Vec3<T> &v) | ||
| 262 | {*this = skew(v);} | ||
| 263 | |||
| 264 | /// @brief Set this matrix to the rotation matrix specified by the quaternion | ||
| 265 | /// @details The quaternion is normalized and used to construct the matrix. | ||
| 266 | /// Note that the matrix is transposed to match post-multiplication semantics. | ||
| 267 | void setToRotation(const Quat<T> &q) | ||
| 268 | 4 | {*this = rotation<Mat3<T> >(q);} | |
| 269 | |||
| 270 | /// @brief Set this matrix to the rotation specified by @a axis and @a angle | ||
| 271 | /// @details The axis must be unit vector | ||
| 272 | void setToRotation(const Vec3<T> &axis, T angle) | ||
| 273 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | {*this = rotation<Mat3<T> >(axis, angle);} |
| 274 | |||
| 275 | /// Set this matrix to zero | ||
| 276 | void setZero() | ||
| 277 | { | ||
| 278 | MyBase::mm[0] = 0; | ||
| 279 | 10 | MyBase::mm[1] = 0; | |
| 280 | 10 | MyBase::mm[2] = 0; | |
| 281 | 10 | MyBase::mm[3] = 0; | |
| 282 | MyBase::mm[4] = 0; | ||
| 283 | 10 | MyBase::mm[5] = 0; | |
| 284 | 10 | MyBase::mm[6] = 0; | |
| 285 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
|
10 | MyBase::mm[7] = 0; |
| 286 | MyBase::mm[8] = 0; | ||
| 287 | } // trivial | ||
| 288 | |||
| 289 | /// Set this matrix to identity | ||
| 290 | void setIdentity() | ||
| 291 | { | ||
| 292 | 7106692 | MyBase::mm[0] = 1; | |
| 293 | 7106692 | MyBase::mm[1] = 0; | |
| 294 | 7106692 | MyBase::mm[2] = 0; | |
| 295 | 7106692 | MyBase::mm[3] = 0; | |
| 296 | 7106692 | MyBase::mm[4] = 1; | |
| 297 | 7106692 | MyBase::mm[5] = 0; | |
| 298 | 7106692 | MyBase::mm[6] = 0; | |
| 299 | 7106692 | MyBase::mm[7] = 0; | |
| 300 | 7106692 | MyBase::mm[8] = 1; | |
| 301 | } // trivial | ||
| 302 | |||
| 303 | /// Assignment operator | ||
| 304 | template<typename Source> | ||
| 305 | const Mat3& operator=(const Mat3<Source> &m) | ||
| 306 | { | ||
| 307 | const Source *src = m.asPointer(); | ||
| 308 | |||
| 309 | // don't suppress type conversion warnings | ||
| 310 | std::copy(src, (src + this->numElements()), MyBase::mm); | ||
| 311 | return *this; | ||
| 312 | } // opEqualToTest | ||
| 313 | |||
| 314 | /// Return @c true if this matrix is equivalent to @a m within a tolerance of @a eps. | ||
| 315 |
2/2✓ Branch 0 taken 1473997 times.
✓ Branch 1 taken 912385 times.
|
2386382 | bool eq(const Mat3 &m, T eps=1.0e-8) const |
| 316 | { | ||
| 317 |
2/2✓ Branch 0 taken 765517 times.
✓ Branch 1 taken 708480 times.
|
1473997 | return (isApproxEqual(MyBase::mm[0],m.mm[0],eps) && |
| 318 |
2/2✓ Branch 0 taken 523597 times.
✓ Branch 1 taken 241920 times.
|
765517 | isApproxEqual(MyBase::mm[1],m.mm[1],eps) && |
| 319 |
2/2✓ Branch 0 taken 509773 times.
✓ Branch 1 taken 13824 times.
|
523597 | isApproxEqual(MyBase::mm[2],m.mm[2],eps) && |
| 320 |
2/2✓ Branch 0 taken 416461 times.
✓ Branch 1 taken 93312 times.
|
509773 | isApproxEqual(MyBase::mm[3],m.mm[3],eps) && |
| 321 |
2/2✓ Branch 0 taken 326605 times.
✓ Branch 1 taken 89856 times.
|
416461 | isApproxEqual(MyBase::mm[4],m.mm[4],eps) && |
| 322 |
1/2✓ Branch 0 taken 326605 times.
✗ Branch 1 not taken.
|
326605 | isApproxEqual(MyBase::mm[5],m.mm[5],eps) && |
| 323 |
1/2✓ Branch 0 taken 326605 times.
✗ Branch 1 not taken.
|
326605 | isApproxEqual(MyBase::mm[6],m.mm[6],eps) && |
| 324 |
3/4✓ Branch 0 taken 1473997 times.
✓ Branch 1 taken 912385 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 326605 times.
|
2712987 | isApproxEqual(MyBase::mm[7],m.mm[7],eps) && |
| 325 | 2386382 | isApproxEqual(MyBase::mm[8],m.mm[8],eps)); | |
| 326 | } // trivial | ||
| 327 | |||
| 328 | /// Negation operator, for e.g. m1 = -m2; | ||
| 329 | Mat3<T> operator-() const | ||
| 330 | { | ||
| 331 | return Mat3<T>( | ||
| 332 | 2 | -MyBase::mm[0], -MyBase::mm[1], -MyBase::mm[2], | |
| 333 | 2 | -MyBase::mm[3], -MyBase::mm[4], -MyBase::mm[5], | |
| 334 | 2 | -MyBase::mm[6], -MyBase::mm[7], -MyBase::mm[8] | |
| 335 |
0/36✗ Branch 0 not taken.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
|
2 | ); |
| 336 | } // trivial | ||
| 337 | |||
| 338 | /// Multiplication operator, e.g. M = scalar * M; | ||
| 339 | // friend Mat3 operator*(T scalar, const Mat3& m) { | ||
| 340 | // return m*scalar; | ||
| 341 | // } | ||
| 342 | |||
| 343 | /// Multiply each element of this matrix by @a scalar. | ||
| 344 | template <typename S> | ||
| 345 | 306858620 | const Mat3<T>& operator*=(S scalar) | |
| 346 | { | ||
| 347 | 306858620 | MyBase::mm[0] *= scalar; | |
| 348 | 306858620 | MyBase::mm[1] *= scalar; | |
| 349 | 306858620 | MyBase::mm[2] *= scalar; | |
| 350 | 306858620 | MyBase::mm[3] *= scalar; | |
| 351 | 306858620 | MyBase::mm[4] *= scalar; | |
| 352 | 306858620 | MyBase::mm[5] *= scalar; | |
| 353 | 306858620 | MyBase::mm[6] *= scalar; | |
| 354 | 306858620 | MyBase::mm[7] *= scalar; | |
| 355 | 306858620 | MyBase::mm[8] *= scalar; | |
| 356 | 306858620 | return *this; | |
| 357 | } | ||
| 358 | |||
| 359 | /// Add each element of the given matrix to the corresponding element of this matrix. | ||
| 360 | template <typename S> | ||
| 361 | 151068972 | const Mat3<T> &operator+=(const Mat3<S> &m1) | |
| 362 | { | ||
| 363 | const S *s = m1.asPointer(); | ||
| 364 | |||
| 365 | 151068972 | MyBase::mm[0] += s[0]; | |
| 366 | 151068972 | MyBase::mm[1] += s[1]; | |
| 367 | 151068972 | MyBase::mm[2] += s[2]; | |
| 368 | 151068972 | MyBase::mm[3] += s[3]; | |
| 369 | 151068972 | MyBase::mm[4] += s[4]; | |
| 370 | 151068972 | MyBase::mm[5] += s[5]; | |
| 371 | 151068972 | MyBase::mm[6] += s[6]; | |
| 372 | 151068972 | MyBase::mm[7] += s[7]; | |
| 373 | 151068972 | MyBase::mm[8] += s[8]; | |
| 374 | 151068972 | return *this; | |
| 375 | } | ||
| 376 | |||
| 377 | /// Subtract each element of the given matrix from the corresponding element of this matrix. | ||
| 378 | template <typename S> | ||
| 379 | 151282158 | const Mat3<T> &operator-=(const Mat3<S> &m1) | |
| 380 | { | ||
| 381 | const S *s = m1.asPointer(); | ||
| 382 | |||
| 383 | 151282158 | MyBase::mm[0] -= s[0]; | |
| 384 | 151282158 | MyBase::mm[1] -= s[1]; | |
| 385 | 151282158 | MyBase::mm[2] -= s[2]; | |
| 386 | 151282158 | MyBase::mm[3] -= s[3]; | |
| 387 | 151282158 | MyBase::mm[4] -= s[4]; | |
| 388 | 151282158 | MyBase::mm[5] -= s[5]; | |
| 389 | 151282158 | MyBase::mm[6] -= s[6]; | |
| 390 | 151282158 | MyBase::mm[7] -= s[7]; | |
| 391 | 151282158 | MyBase::mm[8] -= s[8]; | |
| 392 | 151282158 | return *this; | |
| 393 | } | ||
| 394 | |||
| 395 | /// Multiply this matrix by the given matrix. | ||
| 396 | template <typename S> | ||
| 397 | 24468596 | const Mat3<T> &operator*=(const Mat3<S> &m1) | |
| 398 | { | ||
| 399 | 24468596 | Mat3<T> m0(*this); | |
| 400 | const T* s0 = m0.asPointer(); | ||
| 401 | const S* s1 = m1.asPointer(); | ||
| 402 | |||
| 403 | 24468596 | MyBase::mm[0] = static_cast<T>(s0[0] * s1[0] + | |
| 404 | 24468596 | s0[1] * s1[3] + | |
| 405 | 24468596 | s0[2] * s1[6]); | |
| 406 | 24468596 | MyBase::mm[1] = static_cast<T>(s0[0] * s1[1] + | |
| 407 | 24468596 | s0[1] * s1[4] + | |
| 408 | 24468596 | s0[2] * s1[7]); | |
| 409 | 24468596 | MyBase::mm[2] = static_cast<T>(s0[0] * s1[2] + | |
| 410 | 24468596 | s0[1] * s1[5] + | |
| 411 | 24468596 | s0[2] * s1[8]); | |
| 412 | |||
| 413 | 24468596 | MyBase::mm[3] = static_cast<T>(s0[3] * s1[0] + | |
| 414 | 24468596 | s0[4] * s1[3] + | |
| 415 | 24468596 | s0[5] * s1[6]); | |
| 416 | 24468596 | MyBase::mm[4] = static_cast<T>(s0[3] * s1[1] + | |
| 417 | 24468596 | s0[4] * s1[4] + | |
| 418 | 24468596 | s0[5] * s1[7]); | |
| 419 | 24468596 | MyBase::mm[5] = static_cast<T>(s0[3] * s1[2] + | |
| 420 | 24468596 | s0[4] * s1[5] + | |
| 421 | 24468596 | s0[5] * s1[8]); | |
| 422 | |||
| 423 | 24468596 | MyBase::mm[6] = static_cast<T>(s0[6] * s1[0] + | |
| 424 | 24468596 | s0[7] * s1[3] + | |
| 425 | 24468596 | s0[8] * s1[6]); | |
| 426 | 24468596 | MyBase::mm[7] = static_cast<T>(s0[6] * s1[1] + | |
| 427 | 24468596 | s0[7] * s1[4] + | |
| 428 | 24468596 | s0[8] * s1[7]); | |
| 429 | 24468596 | MyBase::mm[8] = static_cast<T>(s0[6] * s1[2] + | |
| 430 | 24468596 | s0[7] * s1[5] + | |
| 431 | 24468596 | s0[8] * s1[8]); | |
| 432 | |||
| 433 | 24468596 | return *this; | |
| 434 | } | ||
| 435 | |||
| 436 | /// @brief Return the cofactor matrix of this matrix. | ||
| 437 | 108 | Mat3 cofactor() const | |
| 438 | { | ||
| 439 | return Mat3<T>( | ||
| 440 | 108 | MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7], | |
| 441 | 108 | MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8], | |
| 442 | 108 | MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6], | |
| 443 | 108 | MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8], | |
| 444 | 108 | MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6], | |
| 445 | 108 | MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7], | |
| 446 | 108 | MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4], | |
| 447 | 108 | MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5], | |
| 448 | 108 | MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]); | |
| 449 | } | ||
| 450 | |||
| 451 | /// Return the adjoint of this matrix, i.e., the transpose of its cofactor. | ||
| 452 | 155789973 | Mat3 adjoint() const | |
| 453 | { | ||
| 454 | return Mat3<T>( | ||
| 455 | 155789973 | MyBase::mm[4] * MyBase::mm[8] - MyBase::mm[5] * MyBase::mm[7], | |
| 456 | 155789973 | MyBase::mm[2] * MyBase::mm[7] - MyBase::mm[1] * MyBase::mm[8], | |
| 457 | 155789973 | MyBase::mm[1] * MyBase::mm[5] - MyBase::mm[2] * MyBase::mm[4], | |
| 458 | 155789973 | MyBase::mm[5] * MyBase::mm[6] - MyBase::mm[3] * MyBase::mm[8], | |
| 459 | 155789973 | MyBase::mm[0] * MyBase::mm[8] - MyBase::mm[2] * MyBase::mm[6], | |
| 460 | 155789973 | MyBase::mm[2] * MyBase::mm[3] - MyBase::mm[0] * MyBase::mm[5], | |
| 461 | 155789973 | MyBase::mm[3] * MyBase::mm[7] - MyBase::mm[4] * MyBase::mm[6], | |
| 462 | 155789973 | MyBase::mm[1] * MyBase::mm[6] - MyBase::mm[0] * MyBase::mm[7], | |
| 463 | 155789973 | MyBase::mm[0] * MyBase::mm[4] - MyBase::mm[1] * MyBase::mm[3]); | |
| 464 | |||
| 465 | } // adjointTest | ||
| 466 | |||
| 467 | /// returns transpose of this | ||
| 468 | Mat3 transpose() const | ||
| 469 | { | ||
| 470 | return Mat3<T>( | ||
| 471 | 88123950 | MyBase::mm[0], MyBase::mm[3], MyBase::mm[6], | |
| 472 | 88123950 | MyBase::mm[1], MyBase::mm[4], MyBase::mm[7], | |
| 473 | 88123865 | MyBase::mm[2], MyBase::mm[5], MyBase::mm[8]); | |
| 474 | |||
| 475 | } // transposeTest | ||
| 476 | |||
| 477 | /// returns inverse of this | ||
| 478 | /// @throws ArithmeticError if singular | ||
| 479 | 155789865 | Mat3 inverse(T tolerance = 0) const | |
| 480 | { | ||
| 481 | 155789865 | Mat3<T> inv(this->adjoint()); | |
| 482 | |||
| 483 |
2/2✓ Branch 0 taken 84 times.
✓ Branch 1 taken 80255143 times.
|
155789865 | const T det = inv.mm[0]*MyBase::mm[0] + inv.mm[1]*MyBase::mm[3] + inv.mm[2]*MyBase::mm[6]; |
| 484 | |||
| 485 | // If the determinant is 0, m was singular and the result will be invalid. | ||
| 486 |
2/2✓ Branch 0 taken 84 times.
✓ Branch 1 taken 80255143 times.
|
155789865 | if (isApproxEqual(det,T(0.0),tolerance)) { |
| 487 |
2/6✓ Branch 1 taken 84 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 84 times.
✗ Branch 5 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
|
672 | OPENVDB_THROW(ArithmeticError, "Inversion of singular 3x3 matrix"); |
| 488 | } | ||
| 489 | 155789697 | return inv * (T(1)/det); | |
| 490 | } // invertTest | ||
| 491 | |||
| 492 | /// Determinant of matrix | ||
| 493 | 17310540 | T det() const | |
| 494 | { | ||
| 495 | 17310540 | const T co00 = MyBase::mm[4]*MyBase::mm[8] - MyBase::mm[5]*MyBase::mm[7]; | |
| 496 | 17310540 | const T co10 = MyBase::mm[5]*MyBase::mm[6] - MyBase::mm[3]*MyBase::mm[8]; | |
| 497 | 17310540 | const T co20 = MyBase::mm[3]*MyBase::mm[7] - MyBase::mm[4]*MyBase::mm[6]; | |
| 498 | 17310540 | return MyBase::mm[0]*co00 + MyBase::mm[1]*co10 + MyBase::mm[2]*co20; | |
| 499 | } // determinantTest | ||
| 500 | |||
| 501 | /// Trace of matrix | ||
| 502 | T trace() const | ||
| 503 | { | ||
| 504 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | return MyBase::mm[0]+MyBase::mm[4]+MyBase::mm[8]; |
| 505 | } | ||
| 506 | |||
| 507 | /// This function snaps a specific axis to a specific direction, | ||
| 508 | /// preserving scaling. It does this using minimum energy, thus | ||
| 509 | /// posing a unique solution if basis & direction arent parralel. | ||
| 510 | /// Direction need not be unit. | ||
| 511 | Mat3 snapBasis(Axis axis, const Vec3<T> &direction) | ||
| 512 | { | ||
| 513 | return snapMatBasis(*this, axis, direction); | ||
| 514 | } | ||
| 515 | |||
| 516 | /// Return the transformed vector by this matrix. | ||
| 517 | /// This function is equivalent to post-multiplying the matrix. | ||
| 518 | template<typename T0> | ||
| 519 | Vec3<T0> transform(const Vec3<T0> &v) const | ||
| 520 | { | ||
| 521 | 31250 | return static_cast< Vec3<T0> >(v * *this); | |
| 522 | } // xformVectorTest | ||
| 523 | |||
| 524 | /// Return the transformed vector by transpose of this matrix. | ||
| 525 | /// This function is equivalent to pre-multiplying the matrix. | ||
| 526 | template<typename T0> | ||
| 527 | Vec3<T0> pretransform(const Vec3<T0> &v) const | ||
| 528 | { | ||
| 529 | return static_cast< Vec3<T0> >(*this * v); | ||
| 530 | } // xformTVectorTest | ||
| 531 | |||
| 532 | |||
| 533 | /// @brief Treat @a diag as a diagonal matrix and return the product | ||
| 534 | /// of this matrix with @a diag (from the right). | ||
| 535 | Mat3 timesDiagonal(const Vec3<T>& diag) const | ||
| 536 | { | ||
| 537 | Mat3 ret(*this); | ||
| 538 | |||
| 539 | ret.mm[0] *= diag(0); | ||
| 540 | ret.mm[1] *= diag(1); | ||
| 541 | ret.mm[2] *= diag(2); | ||
| 542 | ret.mm[3] *= diag(0); | ||
| 543 | ret.mm[4] *= diag(1); | ||
| 544 | ret.mm[5] *= diag(2); | ||
| 545 | ret.mm[6] *= diag(0); | ||
| 546 | ret.mm[7] *= diag(1); | ||
| 547 | ret.mm[8] *= diag(2); | ||
| 548 | return ret; | ||
| 549 | } | ||
| 550 | }; // class Mat3 | ||
| 551 | |||
| 552 | |||
| 553 | /// @relates Mat3 | ||
| 554 | /// @brief Equality operator, does exact floating point comparisons | ||
| 555 | template <typename T0, typename T1> | ||
| 556 | 109106015 | bool operator==(const Mat3<T0> &m0, const Mat3<T1> &m1) | |
| 557 | { | ||
| 558 | const T0 *t0 = m0.asPointer(); | ||
| 559 | const T1 *t1 = m1.asPointer(); | ||
| 560 | |||
| 561 |
2/2✓ Branch 0 taken 490204524 times.
✓ Branch 1 taken 54454390 times.
|
1089317818 | for (int i=0; i<9; ++i) { |
| 562 |
2/2✓ Branch 0 taken 490105906 times.
✓ Branch 1 taken 98618 times.
|
980409039 | if (!isExactlyEqual(t0[i], t1[i])) return false; |
| 563 | } | ||
| 564 | return true; | ||
| 565 | } | ||
| 566 | |||
| 567 | /// @relates Mat3 | ||
| 568 | /// @brief Inequality operator, does exact floating point comparisons | ||
| 569 | template <typename T0, typename T1> | ||
| 570 | 109105204 | bool operator!=(const Mat3<T0> &m0, const Mat3<T1> &m1) { return !(m0 == m1); } | |
| 571 | |||
| 572 | /// @relates Mat3 | ||
| 573 | /// @brief Multiply each element of the given matrix by @a scalar and return the result. | ||
| 574 | template <typename S, typename T> | ||
| 575 | 226603430 | Mat3<typename promote<S, T>::type> operator*(S scalar, const Mat3<T> &m) | |
| 576 | 226603430 | { return m*scalar; } | |
| 577 | |||
| 578 | /// @relates Mat3 | ||
| 579 | /// @brief Multiply each element of the given matrix by @a scalar and return the result. | ||
| 580 | template <typename S, typename T> | ||
| 581 | 306858616 | Mat3<typename promote<S, T>::type> operator*(const Mat3<T> &m, S scalar) | |
| 582 | { | ||
| 583 | 306858616 | Mat3<typename promote<S, T>::type> result(m); | |
| 584 | 306858616 | result *= scalar; | |
| 585 | 306858616 | return result; | |
| 586 | } | ||
| 587 | |||
| 588 | /// @relates Mat3 | ||
| 589 | /// @brief Add corresponding elements of @a m0 and @a m1 and return the result. | ||
| 590 | template <typename T0, typename T1> | ||
| 591 | 151068972 | Mat3<typename promote<T0, T1>::type> operator+(const Mat3<T0> &m0, const Mat3<T1> &m1) | |
| 592 | { | ||
| 593 | 151068972 | Mat3<typename promote<T0, T1>::type> result(m0); | |
| 594 | 151068972 | result += m1; | |
| 595 | 151068972 | return result; | |
| 596 | } | ||
| 597 | |||
| 598 | /// @relates Mat3 | ||
| 599 | /// @brief Subtract corresponding elements of @a m0 and @a m1 and return the result. | ||
| 600 | template <typename T0, typename T1> | ||
| 601 | 151282158 | Mat3<typename promote<T0, T1>::type> operator-(const Mat3<T0> &m0, const Mat3<T1> &m1) | |
| 602 | { | ||
| 603 | 151282158 | Mat3<typename promote<T0, T1>::type> result(m0); | |
| 604 | 151282158 | result -= m1; | |
| 605 | 151282158 | return result; | |
| 606 | } | ||
| 607 | |||
| 608 | |||
| 609 | /// @brief Multiply @a m0 by @a m1 and return the resulting matrix. | ||
| 610 | template <typename T0, typename T1> | ||
| 611 | 24468596 | Mat3<typename promote<T0, T1>::type>operator*(const Mat3<T0> &m0, const Mat3<T1> &m1) | |
| 612 | { | ||
| 613 | 24468596 | Mat3<typename promote<T0, T1>::type> result(m0); | |
| 614 | 24468596 | result *= m1; | |
| 615 | 24468596 | return result; | |
| 616 | } | ||
| 617 | |||
| 618 | /// @relates Mat3 | ||
| 619 | /// @brief Multiply @a _m by @a _v and return the resulting vector. | ||
| 620 | template<typename T, typename MT> | ||
| 621 | inline Vec3<typename promote<T, MT>::type> | ||
| 622 | 296 | operator*(const Mat3<MT> &_m, const Vec3<T> &_v) | |
| 623 | { | ||
| 624 | MT const *m = _m.asPointer(); | ||
| 625 | return Vec3<typename promote<T, MT>::type>( | ||
| 626 | 296 | _v[0]*m[0] + _v[1]*m[1] + _v[2]*m[2], | |
| 627 | 296 | _v[0]*m[3] + _v[1]*m[4] + _v[2]*m[5], | |
| 628 | 296 | _v[0]*m[6] + _v[1]*m[7] + _v[2]*m[8]); | |
| 629 | } | ||
| 630 | |||
| 631 | /// @relates Mat3 | ||
| 632 | /// @brief Multiply @a _v by @a _m and return the resulting vector. | ||
| 633 | template<typename T, typename MT> | ||
| 634 | inline Vec3<typename promote<T, MT>::type> | ||
| 635 | 2337060 | operator*(const Vec3<T> &_v, const Mat3<MT> &_m) | |
| 636 | { | ||
| 637 | MT const *m = _m.asPointer(); | ||
| 638 | return Vec3<typename promote<T, MT>::type>( | ||
| 639 | 2337060 | _v[0]*m[0] + _v[1]*m[3] + _v[2]*m[6], | |
| 640 | 2337060 | _v[0]*m[1] + _v[1]*m[4] + _v[2]*m[7], | |
| 641 | 2337060 | _v[0]*m[2] + _v[1]*m[5] + _v[2]*m[8]); | |
| 642 | } | ||
| 643 | |||
| 644 | /// @relates Mat3 | ||
| 645 | /// @brief Multiply @a _v by @a _m and replace @a _v with the resulting vector. | ||
| 646 | template<typename T, typename MT> | ||
| 647 | inline Vec3<T> &operator *= (Vec3<T> &_v, const Mat3<MT> &_m) | ||
| 648 | { | ||
| 649 | Vec3<T> mult = _v * _m; | ||
| 650 | _v = mult; | ||
| 651 | return _v; | ||
| 652 | } | ||
| 653 | |||
| 654 | /// Returns outer product of v1, v2, i.e. v1 v2^T if v1 and v2 are | ||
| 655 | /// column vectors, e.g. M = Mat3f::outerproduct(v1,v2); | ||
| 656 | template <typename T> | ||
| 657 | Mat3<T> outerProduct(const Vec3<T>& v1, const Vec3<T>& v2) | ||
| 658 | { | ||
| 659 | return Mat3<T>(v1[0]*v2[0], v1[0]*v2[1], v1[0]*v2[2], | ||
| 660 | v1[1]*v2[0], v1[1]*v2[1], v1[1]*v2[2], | ||
| 661 | v1[2]*v2[0], v1[2]*v2[1], v1[2]*v2[2]); | ||
| 662 | }// outerProduct | ||
| 663 | |||
| 664 | |||
| 665 | /// Interpolate the rotation between m1 and m2 using Mat::powSolve. | ||
| 666 | /// Unlike slerp, translation is not treated independently. | ||
| 667 | /// This results in smoother animation results. | ||
| 668 | template<typename T, typename T0> | ||
| 669 | Mat3<T> powLerp(const Mat3<T0> &m1, const Mat3<T0> &m2, T t) | ||
| 670 | { | ||
| 671 | Mat3<T> x = m1.inverse() * m2; | ||
| 672 | powSolve(x, x, t); | ||
| 673 | Mat3<T> m = m1 * x; | ||
| 674 | return m; | ||
| 675 | } | ||
| 676 | |||
| 677 | |||
| 678 | namespace mat3_internal { | ||
| 679 | |||
| 680 | template<typename T> | ||
| 681 | inline void | ||
| 682 | 10 | pivot(int i, int j, Mat3<T>& S, Vec3<T>& D, Mat3<T>& Q) | |
| 683 | { | ||
| 684 | const int& n = Mat3<T>::size; // should be 3 | ||
| 685 | T temp; | ||
| 686 | /// scratch variables used in pivoting | ||
| 687 | double cotan_of_2_theta; | ||
| 688 | double tan_of_theta; | ||
| 689 | double cosin_of_theta; | ||
| 690 | double sin_of_theta; | ||
| 691 | double z; | ||
| 692 | |||
| 693 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
|
10 | double Sij = S(i,j); |
| 694 | |||
| 695 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
|
10 | double Sjj_minus_Sii = D[j] - D[i]; |
| 696 | |||
| 697 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
|
10 | if (fabs(Sjj_minus_Sii) * (10*math::Tolerance<T>::value()) > fabs(Sij)) { |
| 698 | ✗ | tan_of_theta = Sij / Sjj_minus_Sii; | |
| 699 | } else { | ||
| 700 | /// pivot on Sij | ||
| 701 | 10 | cotan_of_2_theta = 0.5*Sjj_minus_Sii / Sij ; | |
| 702 | |||
| 703 |
2/2✓ Branch 0 taken 5 times.
✓ Branch 1 taken 5 times.
|
10 | if (cotan_of_2_theta < 0.) { |
| 704 | 5 | tan_of_theta = | |
| 705 | 5 | -1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) - cotan_of_2_theta); | |
| 706 | } else { | ||
| 707 | 5 | tan_of_theta = | |
| 708 | 5 | 1./(sqrt(1. + cotan_of_2_theta*cotan_of_2_theta) + cotan_of_2_theta); | |
| 709 | } | ||
| 710 | } | ||
| 711 | |||
| 712 | 10 | cosin_of_theta = 1./sqrt( 1. + tan_of_theta * tan_of_theta); | |
| 713 | 10 | sin_of_theta = cosin_of_theta * tan_of_theta; | |
| 714 | 10 | z = tan_of_theta * Sij; | |
| 715 | 10 | S(i,j) = 0; | |
| 716 | 10 | D[i] -= z; | |
| 717 | 10 | D[j] += z; | |
| 718 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 10 times.
|
14 | for (int k = 0; k < i; ++k) { |
| 719 | 4 | temp = S(k,i); | |
| 720 | 4 | S(k,i) = cosin_of_theta * temp - sin_of_theta * S(k,j); | |
| 721 | 4 | S(k,j)= sin_of_theta * temp + cosin_of_theta * S(k,j); | |
| 722 | } | ||
| 723 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 10 times.
|
13 | for (int k = i+1; k < j; ++k) { |
| 724 | 3 | temp = S(i,k); | |
| 725 | 3 | S(i,k) = cosin_of_theta * temp - sin_of_theta * S(k,j); | |
| 726 | 3 | S(k,j) = sin_of_theta * temp + cosin_of_theta * S(k,j); | |
| 727 | } | ||
| 728 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 3 times.
|
13 | for (int k = j+1; k < n; ++k) { |
| 729 | 3 | temp = S(i,k); | |
| 730 | 3 | S(i,k) = cosin_of_theta * temp - sin_of_theta * S(j,k); | |
| 731 | 3 | S(j,k) = sin_of_theta * temp + cosin_of_theta * S(j,k); | |
| 732 | } | ||
| 733 |
2/2✓ Branch 0 taken 30 times.
✓ Branch 1 taken 10 times.
|
40 | for (int k = 0; k < n; ++k) |
| 734 | { | ||
| 735 | 30 | temp = Q(k,i); | |
| 736 | 30 | Q(k,i) = cosin_of_theta * temp - sin_of_theta*Q(k,j); | |
| 737 | 30 | Q(k,j) = sin_of_theta * temp + cosin_of_theta*Q(k,j); | |
| 738 | } | ||
| 739 | 10 | } | |
| 740 | |||
| 741 | } // namespace mat3_internal | ||
| 742 | |||
| 743 | |||
| 744 | /// @brief Use Jacobi iterations to decompose a symmetric 3x3 matrix | ||
| 745 | /// (diagonalize and compute eigenvectors) | ||
| 746 | /// @details This is based on the "Efficient numerical diagonalization of Hermitian 3x3 matrices" | ||
| 747 | /// Joachim Kopp. arXiv.org preprint: physics/0610206 | ||
| 748 | /// with the addition of largest pivot | ||
| 749 | template<typename T> | ||
| 750 | inline bool | ||
| 751 | 3 | diagonalizeSymmetricMatrix(const Mat3<T>& input, Mat3<T>& Q, Vec3<T>& D, | |
| 752 | unsigned int MAX_ITERATIONS=250) | ||
| 753 | { | ||
| 754 | /// use Givens rotation matrix to eliminate off-diagonal entries. | ||
| 755 | /// initialize the rotation matrix as idenity | ||
| 756 | 3 | Q = Mat3<T>::identity(); | |
| 757 | int n = Mat3<T>::size; // should be 3 | ||
| 758 | |||
| 759 | /// temp matrix. Assumed to be symmetric | ||
| 760 | 3 | Mat3<T> S(input); | |
| 761 | |||
| 762 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
|
12 | for (int i = 0; i < n; ++i) { |
| 763 | 9 | D[i] = S(i,i); | |
| 764 | } | ||
| 765 | |||
| 766 | unsigned int iterations(0); | ||
| 767 | /// Just iterate over all the non-diagonal enteries | ||
| 768 | /// using the largest as a pivot. | ||
| 769 | do { | ||
| 770 | /// check for absolute convergence | ||
| 771 | /// are symmetric off diagonals all zero | ||
| 772 | double er = 0; | ||
| 773 |
2/2✓ Branch 0 taken 39 times.
✓ Branch 1 taken 13 times.
|
52 | for (int i = 0; i < n; ++i) { |
| 774 |
2/2✓ Branch 0 taken 39 times.
✓ Branch 1 taken 39 times.
|
78 | for (int j = i+1; j < n; ++j) { |
| 775 | 39 | er += fabs(S(i,j)); | |
| 776 | } | ||
| 777 | } | ||
| 778 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 3 times.
|
13 | if (std::abs(er) < math::Tolerance<T>::value()) { |
| 779 | return true; | ||
| 780 | } | ||
| 781 | 10 | iterations++; | |
| 782 | |||
| 783 | T max_element = 0; | ||
| 784 | int ip = 0; | ||
| 785 | int jp = 0; | ||
| 786 | /// loop over all the off-diagonals above the diagonal | ||
| 787 |
2/2✓ Branch 0 taken 30 times.
✓ Branch 1 taken 10 times.
|
40 | for (int i = 0; i < n; ++i) { |
| 788 |
2/2✓ Branch 0 taken 30 times.
✓ Branch 1 taken 30 times.
|
60 | for (int j = i+1; j < n; ++j){ |
| 789 | |||
| 790 |
2/2✓ Branch 0 taken 11 times.
✓ Branch 1 taken 19 times.
|
30 | if ( fabs(D[i]) * (10*math::Tolerance<T>::value()) > fabs(S(i,j))) { |
| 791 | /// value too small to pivot on | ||
| 792 | 11 | S(i,j) = 0; | |
| 793 | } | ||
| 794 |
2/2✓ Branch 0 taken 17 times.
✓ Branch 1 taken 13 times.
|
30 | if (fabs(S(i,j)) > max_element) { |
| 795 | max_element = fabs(S(i,j)); | ||
| 796 | ip = i; | ||
| 797 | jp = j; | ||
| 798 | } | ||
| 799 | } | ||
| 800 | } | ||
| 801 | 10 | mat3_internal::pivot(ip, jp, S, D, Q); | |
| 802 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | } while (iterations < MAX_ITERATIONS); |
| 803 | |||
| 804 | return false; | ||
| 805 | } | ||
| 806 | |||
| 807 | template<typename T> | ||
| 808 | inline Mat3<T> | ||
| 809 | Abs(const Mat3<T>& m) | ||
| 810 | { | ||
| 811 | Mat3<T> out; | ||
| 812 | const T* ip = m.asPointer(); | ||
| 813 | T* op = out.asPointer(); | ||
| 814 |
4/4✓ Branch 0 taken 369108 times.
✓ Branch 1 taken 41012 times.
✓ Branch 2 taken 590301 times.
✓ Branch 3 taken 65589 times.
|
1066010 | for (unsigned i = 0; i < 9; ++i, ++op, ++ip) *op = math::Abs(*ip); |
| 815 | return out; | ||
| 816 | } | ||
| 817 | |||
| 818 | template<typename Type1, typename Type2> | ||
| 819 | inline Mat3<Type1> | ||
| 820 | cwiseAdd(const Mat3<Type1>& m, const Type2 s) | ||
| 821 | { | ||
| 822 | Mat3<Type1> out; | ||
| 823 | const Type1* ip = m.asPointer(); | ||
| 824 | Type1* op = out.asPointer(); | ||
| 825 | ✗ | for (unsigned i = 0; i < 9; ++i, ++op, ++ip) { | |
| 826 | OPENVDB_NO_TYPE_CONVERSION_WARNING_BEGIN | ||
| 827 | ✗ | *op = *ip + s; | |
| 828 | OPENVDB_NO_TYPE_CONVERSION_WARNING_END | ||
| 829 | } | ||
| 830 | return out; | ||
| 831 | } | ||
| 832 | |||
| 833 | template<typename T> | ||
| 834 | inline bool | ||
| 835 | cwiseLessThan(const Mat3<T>& m0, const Mat3<T>& m1) | ||
| 836 | { | ||
| 837 | return cwiseLessThan<3, T>(m0, m1); | ||
| 838 | } | ||
| 839 | |||
| 840 | template<typename T> | ||
| 841 | inline bool | ||
| 842 | cwiseGreaterThan(const Mat3<T>& m0, const Mat3<T>& m1) | ||
| 843 | { | ||
| 844 | return cwiseGreaterThan<3, T>(m0, m1); | ||
| 845 | } | ||
| 846 | |||
| 847 | using Mat3s = Mat3<float>; | ||
| 848 | using Mat3d = Mat3<double>; | ||
| 849 | using Mat3f = Mat3d; | ||
| 850 | |||
| 851 | #if OPENVDB_ABI_VERSION_NUMBER >= 8 | ||
| 852 | OPENVDB_IS_POD(Mat3s) | ||
| 853 | OPENVDB_IS_POD(Mat3d) | ||
| 854 | #endif | ||
| 855 | |||
| 856 | } // namespace math | ||
| 857 | |||
| 858 | |||
| 859 |
1/43✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 60 not taken.
✗ Branch 61 not taken.
✗ Branch 64 not taken.
✗ Branch 65 not taken.
✗ Branch 66 not taken.
✗ Branch 67 not taken.
✓ Branch 70 taken 100 times.
✗ Branch 71 not taken.
✗ Branch 72 not taken.
✗ Branch 73 not taken.
✗ Branch 74 not taken.
✗ Branch 75 not taken.
|
1286040 | template<> inline math::Mat3s zeroVal<math::Mat3s>() { return math::Mat3s::zero(); } |
| 860 |
0/25✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 48 not taken.
✗ Branch 49 not taken.
✗ Branch 53 not taken.
✗ Branch 54 not taken.
✗ Branch 55 not taken.
✗ Branch 57 not taken.
✗ Branch 58 not taken.
|
1928178 | template<> inline math::Mat3d zeroVal<math::Mat3d>() { return math::Mat3d::zero(); } |
| 861 | |||
| 862 | } // namespace OPENVDB_VERSION_NAME | ||
| 863 | } // namespace openvdb | ||
| 864 | |||
| 865 | #endif // OPENVDB_MATH_MAT3_H_HAS_BEEN_INCLUDED | ||
| 866 |