| 1 |
// |
| 2 |
// SPDX-License-Identifier: BSD-3-Clause |
| 3 |
// Copyright Contributors to the OpenEXR Project. |
| 4 |
// |
| 5 |
|
| 6 |
// |
| 7 |
// |
| 8 |
// Functions operating on Matrix22, Matrix33, and Matrix44 types |
| 9 |
// |
| 10 |
// This file also defines a few predefined constant matrices. |
| 11 |
// |
| 12 |
|
| 13 |
#ifndef INCLUDED_IMATHMATRIXALGO_H |
| 14 |
#define INCLUDED_IMATHMATRIXALGO_H |
| 15 |
|
| 16 |
#include "ImathEuler.h" |
| 17 |
#include "ImathExport.h" |
| 18 |
#include "ImathMatrix.h" |
| 19 |
#include "ImathNamespace.h" |
| 20 |
#include "ImathQuat.h" |
| 21 |
#include "ImathVec.h" |
| 22 |
#include <math.h> |
| 23 |
|
| 24 |
IMATH_INTERNAL_NAMESPACE_HEADER_ENTER |
| 25 |
|
| 26 |
//------------------ |
| 27 |
// Identity matrices |
| 28 |
//------------------ |
| 29 |
|
| 30 |
/// M22f identity matrix |
| 31 |
IMATH_EXPORT_CONST M22f identity22f; |
| 32 |
/// M33f identity matrix |
| 33 |
IMATH_EXPORT_CONST M33f identity33f; |
| 34 |
/// M44f identity matrix |
| 35 |
IMATH_EXPORT_CONST M44f identity44f; |
| 36 |
/// M22d identity matrix |
| 37 |
IMATH_EXPORT_CONST M22d identity22d; |
| 38 |
/// M33d identity matrix |
| 39 |
IMATH_EXPORT_CONST M33d identity33d; |
| 40 |
/// M44d identity matrix |
| 41 |
IMATH_EXPORT_CONST M44d identity44d; |
| 42 |
|
| 43 |
//---------------------------------------------------------------------- |
| 44 |
// Extract scale, shear, rotation, and translation values from a matrix: |
| 45 |
// |
| 46 |
// Notes: |
| 47 |
// |
| 48 |
// This implementation follows the technique described in the paper by |
| 49 |
// Spencer W. Thomas in the Graphics Gems II article: "Decomposing a |
| 50 |
// Matrix into Simple Transformations", p. 320. |
| 51 |
// |
| 52 |
// - Some of the functions below have an optional exc parameter |
| 53 |
// that determines the functions' behavior when the matrix' |
| 54 |
// scaling is very close to zero: |
| 55 |
// |
| 56 |
// If exc is true, the functions throw a std::domain_error exception. |
| 57 |
// |
| 58 |
// If exc is false: |
| 59 |
// |
| 60 |
// extractScaling (m, s) returns false, s is invalid |
| 61 |
// sansScaling (m) returns m |
| 62 |
// removeScaling (m) returns false, m is unchanged |
| 63 |
// sansScalingAndShear (m) returns m |
| 64 |
// removeScalingAndShear (m) returns false, m is unchanged |
| 65 |
// extractAndRemoveScalingAndShear (m, s, h) |
| 66 |
// returns false, m is unchanged, |
| 67 |
// (sh) are invalid |
| 68 |
// checkForZeroScaleInRow () returns false |
| 69 |
// extractSHRT (m, s, h, r, t) returns false, (shrt) are invalid |
| 70 |
// |
| 71 |
// - Functions extractEuler(), extractEulerXYZ() and extractEulerZYX() |
| 72 |
// assume that the matrix does not include shear or non-uniform scaling, |
| 73 |
// but they do not examine the matrix to verify this assumption. |
| 74 |
// Matrices with shear or non-uniform scaling are likely to produce |
| 75 |
// meaningless results. Therefore, you should use the |
| 76 |
// removeScalingAndShear() routine, if necessary, prior to calling |
| 77 |
// extractEuler...() . |
| 78 |
// |
| 79 |
// - All functions assume that the matrix does not include perspective |
| 80 |
// transformation(s), but they do not examine the matrix to verify |
| 81 |
// this assumption. Matrices with perspective transformations are |
| 82 |
// likely to produce meaningless results. |
| 83 |
// |
| 84 |
//---------------------------------------------------------------------- |
| 85 |
|
| 86 |
// |
| 87 |
// Declarations for 4x4 matrix. |
| 88 |
// |
| 89 |
|
| 90 |
/// Extract the scaling component of the given 4x4 matrix. |
| 91 |
/// |
| 92 |
/// @param[in] mat The input matrix |
| 93 |
/// @param[out] scl The extracted scale, i.e. the output value |
| 94 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 95 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 96 |
template <class T> bool extractScaling (const Matrix44<T>& mat, Vec3<T>& scl, bool exc = true); |
| 97 |
|
| 98 |
/// Return the given 4x4 matrix with scaling removed. |
| 99 |
/// |
| 100 |
/// @param[in] mat The input matrix |
| 101 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` |
| 102 |
template <class T> Matrix44<T> sansScaling (const Matrix44<T>& mat, bool exc = true); |
| 103 |
|
| 104 |
/// Remove scaling from the given 4x4 matrix in place. Return true if the |
| 105 |
/// scale could be successfully extracted, false if the matrix is |
| 106 |
/// degenerate. |
| 107 |
// |
| 108 |
/// @param[in] mat The matrix to operate on |
| 109 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 110 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 111 |
template <class T> bool removeScaling (Matrix44<T>& mat, bool exc = true); |
| 112 |
|
| 113 |
/// Extract the scaling and shear components of the given 4x4 matrix. |
| 114 |
/// Return true if the scale could be successfully extracted, false if |
| 115 |
/// the matrix is degenerate. |
| 116 |
/// |
| 117 |
/// @param[in] mat The input matrix |
| 118 |
/// @param[out] scl The extracted scale |
| 119 |
/// @param[out] shr The extracted shear |
| 120 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 121 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 122 |
template <class T> bool extractScalingAndShear (const Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc = true); |
| 123 |
|
| 124 |
/// Return the given 4x4 matrix with scaling and shear removed. |
| 125 |
/// |
| 126 |
/// @param[in] mat The input matrix |
| 127 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 128 |
template <class T> Matrix44<T> sansScalingAndShear (const Matrix44<T>& mat, bool exc = true); |
| 129 |
|
| 130 |
/// Extract scaling and shear from the given 4x4 matrix in-place. |
| 131 |
/// |
| 132 |
/// @param[in,out] result The output matrix |
| 133 |
/// @param[in] mat The return value if `result` is degenerate |
| 134 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 135 |
template <class T> |
| 136 |
void sansScalingAndShear (Matrix44<T>& result, const Matrix44<T>& mat, bool exc = true); |
| 137 |
|
| 138 |
/// Remove scaling and shear from the given 4x4 matrix in place. |
| 139 |
// |
| 140 |
/// @param[in,out] mat The matrix to operate on |
| 141 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 142 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 143 |
template <class T> bool removeScalingAndShear (Matrix44<T>& mat, bool exc = true); |
| 144 |
|
| 145 |
/// Remove scaling and shear from the given 4x4 matrix in place, returning |
| 146 |
/// the extracted values. |
| 147 |
// |
| 148 |
/// @param[in,out] mat The matrix to operate on |
| 149 |
/// @param[out] scl The extracted scale |
| 150 |
/// @param[out] shr The extracted shear |
| 151 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 152 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 153 |
template <class T> |
| 154 |
bool |
| 155 |
extractAndRemoveScalingAndShear (Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc = true); |
| 156 |
|
| 157 |
/// Extract the rotation from the given 4x4 matrix in the form of XYZ |
| 158 |
/// euler angles. |
| 159 |
/// |
| 160 |
/// @param[in] mat The input matrix |
| 161 |
/// @param[out] rot The extracted XYZ euler angle vector |
| 162 |
template <class T> void extractEulerXYZ (const Matrix44<T>& mat, Vec3<T>& rot); |
| 163 |
|
| 164 |
/// Extract the rotation from the given 4x4 matrix in the form of ZYX |
| 165 |
/// euler angles. |
| 166 |
/// |
| 167 |
/// @param[in] mat The input matrix |
| 168 |
/// @param[out] rot The extracted ZYX euler angle vector |
| 169 |
template <class T> void extractEulerZYX (const Matrix44<T>& mat, Vec3<T>& rot); |
| 170 |
|
| 171 |
/// Extract the rotation from the given 4x4 matrix in the form of a quaternion. |
| 172 |
/// |
| 173 |
/// @param[in] mat The input matrix |
| 174 |
/// @return The extracted quaternion |
| 175 |
template <class T> Quat<T> extractQuat (const Matrix44<T>& mat); |
| 176 |
|
| 177 |
/// Extract the scaling, shear, rotation, and translation components |
| 178 |
/// of the given 4x4 matrix. The values are such that: |
| 179 |
/// |
| 180 |
/// M = S * H * R * T |
| 181 |
/// |
| 182 |
/// @param[in] mat The input matrix |
| 183 |
/// @param[out] s The extracted scale |
| 184 |
/// @param[out] h The extracted shear |
| 185 |
/// @param[out] r The extracted rotation |
| 186 |
/// @param[out] t The extracted translation |
| 187 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 188 |
/// @param[in] rOrder The order with which to extract the rotation |
| 189 |
/// @return True if the values could be extracted, false if the matrix is degenerate. |
| 190 |
template <class T> |
| 191 |
bool extractSHRT (const Matrix44<T>& mat, |
| 192 |
Vec3<T>& s, |
| 193 |
Vec3<T>& h, |
| 194 |
Vec3<T>& r, |
| 195 |
Vec3<T>& t, |
| 196 |
bool exc /*= true*/, |
| 197 |
typename Euler<T>::Order rOrder); |
| 198 |
|
| 199 |
/// Extract the scaling, shear, rotation, and translation components |
| 200 |
/// of the given 4x4 matrix. |
| 201 |
/// |
| 202 |
/// @param[in] mat The input matrix |
| 203 |
/// @param[out] s The extracted scale |
| 204 |
/// @param[out] h The extracted shear |
| 205 |
/// @param[out] r The extracted rotation, in XYZ euler angles |
| 206 |
/// @param[out] t The extracted translation |
| 207 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 208 |
/// @return True if the values could be extracted, false if the matrix is degenerate. |
| 209 |
template <class T> |
| 210 |
bool extractSHRT (const Matrix44<T>& mat, |
| 211 |
Vec3<T>& s, |
| 212 |
Vec3<T>& h, |
| 213 |
Vec3<T>& r, |
| 214 |
Vec3<T>& t, |
| 215 |
bool exc = true); |
| 216 |
|
| 217 |
/// Extract the scaling, shear, rotation, and translation components |
| 218 |
/// of the given 4x4 matrix. |
| 219 |
/// |
| 220 |
/// @param[in] mat The input matrix |
| 221 |
/// @param[out] s The extracted scale |
| 222 |
/// @param[out] h The extracted shear |
| 223 |
/// @param[out] r The extracted rotation, in Euler angles |
| 224 |
/// @param[out] t The extracted translation |
| 225 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 226 |
/// @return True if the values could be extracted, false if the matrix is degenerate. |
| 227 |
template <class T> |
| 228 |
bool extractSHRT (const Matrix44<T>& mat, |
| 229 |
Vec3<T>& s, |
| 230 |
Vec3<T>& h, |
| 231 |
Euler<T>& r, |
| 232 |
Vec3<T>& t, |
| 233 |
bool exc = true); |
| 234 |
|
| 235 |
/// Return true if the given scale can be removed from the given row |
| 236 |
/// matrix, false if `scl` is small enough that the operation would |
| 237 |
/// overflow. If `exc` is true, throw an exception on overflow. |
| 238 |
template <class T> bool checkForZeroScaleInRow (const T& scl, const Vec3<T>& row, bool exc = true); |
| 239 |
|
| 240 |
/// Return the 4x4 outer product two 4-vectors |
| 241 |
template <class T> Matrix44<T> outerProduct (const Vec4<T>& a, const Vec4<T>& b); |
| 242 |
|
| 243 |
/// |
| 244 |
/// Return a 4x4 matrix that rotates the vector `fromDirection` to `toDirection` |
| 245 |
/// |
| 246 |
template <class T> |
| 247 |
Matrix44<T> rotationMatrix (const Vec3<T>& fromDirection, const Vec3<T>& toDirection); |
| 248 |
|
| 249 |
/// |
| 250 |
/// Return a 4x4 matrix that rotates the `fromDir` vector |
| 251 |
/// so that it points towards `toDir1. You may also |
| 252 |
/// specify that you want the up vector to be pointing |
| 253 |
/// in a certain direction 1upDir`. |
| 254 |
template <class T> |
| 255 |
Matrix44<T> |
| 256 |
rotationMatrixWithUpDir (const Vec3<T>& fromDir, const Vec3<T>& toDir, const Vec3<T>& upDir); |
| 257 |
|
| 258 |
/// |
| 259 |
/// Construct a 4x4 matrix that rotates the z-axis so that it points |
| 260 |
/// towards `targetDir`. You must also specify that you want the up |
| 261 |
/// vector to be pointing in a certain direction `upDir`. |
| 262 |
/// |
| 263 |
/// Notes: The following degenerate cases are handled: |
| 264 |
/// (a) when the directions given by `toDir` and `upDir` |
| 265 |
/// are parallel or opposite (the direction vectors must have a non-zero cross product); |
| 266 |
/// (b) when any of the given direction vectors have zero length |
| 267 |
/// |
| 268 |
/// @param[out] result The output matrix |
| 269 |
/// @param[in] targetDir The target direction vector |
| 270 |
/// @param[in] upDir The up direction vector |
| 271 |
template <class T> |
| 272 |
void alignZAxisWithTargetDir (Matrix44<T>& result, Vec3<T> targetDir, Vec3<T> upDir); |
| 273 |
|
| 274 |
/// Compute an orthonormal direct 4x4 frame from a position, an x axis |
| 275 |
/// direction and a normal to the y axis. If the x axis and normal are |
| 276 |
/// perpendicular, then the normal will have the same direction as the |
| 277 |
/// z axis. |
| 278 |
/// |
| 279 |
/// @param[in] p The position of the frame |
| 280 |
/// @param[in] xDir The x axis direction of the frame |
| 281 |
/// @param[in] normal A normal to the y axis of the frame |
| 282 |
/// @return The orthonormal frame |
| 283 |
template <class T> |
| 284 |
Matrix44<T> computeLocalFrame (const Vec3<T>& p, const Vec3<T>& xDir, const Vec3<T>& normal); |
| 285 |
|
| 286 |
/// Add a translate/rotate/scale offset to a 4x4 input frame |
| 287 |
/// and put it in another frame of reference |
| 288 |
/// |
| 289 |
/// @param[in] inMat Input frame |
| 290 |
/// @param[in] tOffset Translation offset |
| 291 |
/// @param[in] rOffset Rotation offset in degrees |
| 292 |
/// @param[in] sOffset Scale offset |
| 293 |
/// @param[in] ref Frame of reference |
| 294 |
/// @return The offsetted frame |
| 295 |
template <class T> |
| 296 |
Matrix44<T> addOffset (const Matrix44<T>& inMat, |
| 297 |
const Vec3<T>& tOffset, |
| 298 |
const Vec3<T>& rOffset, |
| 299 |
const Vec3<T>& sOffset, |
| 300 |
const Vec3<T>& ref); |
| 301 |
|
| 302 |
/// Compute 4x4 translate/rotate/scale matrix from `A` with the |
| 303 |
/// rotate/scale of `B`. |
| 304 |
/// |
| 305 |
/// @param[in] keepRotateA If true, keep rotate from matrix `A`, use `B` otherwise |
| 306 |
/// @param[in] keepScaleA If true, keep scale from matrix `A`, use `B` otherwise |
| 307 |
/// @param[in] A Matrix A |
| 308 |
/// @param[in] B Matrix B |
| 309 |
/// @return Matrix `A` with tweaked rotation/scale |
| 310 |
template <class T> |
| 311 |
Matrix44<T> |
| 312 |
computeRSMatrix (bool keepRotateA, bool keepScaleA, const Matrix44<T>& A, const Matrix44<T>& B); |
| 313 |
|
| 314 |
// |
| 315 |
// Declarations for 3x3 matrix. |
| 316 |
// |
| 317 |
|
| 318 |
/// Extract the scaling component of the given 3x3 matrix. |
| 319 |
/// |
| 320 |
/// @param[in] mat The input matrix |
| 321 |
/// @param[out] scl The extracted scale, i.e. the output value |
| 322 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 323 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 324 |
template <class T> bool extractScaling (const Matrix33<T>& mat, Vec2<T>& scl, bool exc = true); |
| 325 |
|
| 326 |
/// Return the given 3x3 matrix with scaling removed. |
| 327 |
/// |
| 328 |
/// @param[in] mat The input matrix |
| 329 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` |
| 330 |
template <class T> Matrix33<T> sansScaling (const Matrix33<T>& mat, bool exc = true); |
| 331 |
|
| 332 |
/// Remove scaling from the given 3x3 matrix in place. Return true if the |
| 333 |
/// scale could be successfully extracted, false if the matrix is |
| 334 |
/// degenerate. |
| 335 |
// |
| 336 |
/// @param[in] mat The matrix to operate on |
| 337 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 338 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 339 |
template <class T> bool removeScaling (Matrix33<T>& mat, bool exc = true); |
| 340 |
|
| 341 |
/// Extract the scaling and shear components of the given 3x3 matrix. |
| 342 |
/// Return true if the scale could be successfully extracted, false if |
| 343 |
/// the matrix is degenerate. |
| 344 |
/// |
| 345 |
/// @param[in] mat The input matrix |
| 346 |
/// @param[out] scl The extracted scale |
| 347 |
/// @param[out] shr The extracted shear |
| 348 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 349 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 350 |
template <class T> |
| 351 |
bool extractScalingAndShear (const Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc = true); |
| 352 |
|
| 353 |
/// Return the given 3x3 matrix with scaling and shear removed. |
| 354 |
/// |
| 355 |
/// @param[in] mat The input matrix |
| 356 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 357 |
template <class T> Matrix33<T> sansScalingAndShear (const Matrix33<T>& mat, bool exc = true); |
| 358 |
|
| 359 |
|
| 360 |
/// Remove scaling and shear from the given 3x3e matrix in place. |
| 361 |
// |
| 362 |
/// @param[in,out] mat The matrix to operate on |
| 363 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 364 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 365 |
template <class T> bool removeScalingAndShear (Matrix33<T>& mat, bool exc = true); |
| 366 |
|
| 367 |
/// Remove scaling and shear from the given 3x3 matrix in place, returning |
| 368 |
/// the extracted values. |
| 369 |
// |
| 370 |
/// @param[in,out] mat The matrix to operate on |
| 371 |
/// @param[out] scl The extracted scale |
| 372 |
/// @param[out] shr The extracted shear |
| 373 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 374 |
/// @return True if the scale could be extracted, false if the matrix is degenerate. |
| 375 |
template <class T> |
| 376 |
bool extractAndRemoveScalingAndShear (Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc = true); |
| 377 |
|
| 378 |
/// Extract the rotation from the given 2x2 matrix |
| 379 |
/// |
| 380 |
/// @param[in] mat The input matrix |
| 381 |
/// @param[out] rot The extracted rotation value |
| 382 |
template <class T> void extractEuler (const Matrix22<T>& mat, T& rot); |
| 383 |
|
| 384 |
/// Extract the rotation from the given 3x3 matrix |
| 385 |
/// |
| 386 |
/// @param[in] mat The input matrix |
| 387 |
/// @param[out] rot The extracted rotation value |
| 388 |
template <class T> void extractEuler (const Matrix33<T>& mat, T& rot); |
| 389 |
|
| 390 |
/// Extract the scaling, shear, rotation, and translation components |
| 391 |
/// of the given 3x3 matrix. The values are such that: |
| 392 |
/// |
| 393 |
/// M = S * H * R * T |
| 394 |
/// |
| 395 |
/// @param[in] mat The input matrix |
| 396 |
/// @param[out] s The extracted scale |
| 397 |
/// @param[out] h The extracted shear |
| 398 |
/// @param[out] r The extracted rotation |
| 399 |
/// @param[out] t The extracted translation |
| 400 |
/// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero. |
| 401 |
/// @return True if the values could be extracted, false if the matrix is degenerate. |
| 402 |
template <class T> |
| 403 |
bool extractSHRT (const Matrix33<T>& mat, Vec2<T>& s, T& h, T& r, Vec2<T>& t, bool exc = true); |
| 404 |
|
| 405 |
/// Return true if the given scale can be removed from the given row |
| 406 |
/// matrix, false if `scl` is small enough that the operation would |
| 407 |
/// overflow. If `exc` is true, throw an exception on overflow. |
| 408 |
template <class T> bool checkForZeroScaleInRow (const T& scl, const Vec2<T>& row, bool exc = true); |
| 409 |
|
| 410 |
/// Return the 3xe outer product two 3-vectors |
| 411 |
template <class T> Matrix33<T> outerProduct (const Vec3<T>& a, const Vec3<T>& b); |
| 412 |
|
| 413 |
//------------------------------ |
| 414 |
// Implementation for 4x4 Matrix |
| 415 |
//------------------------------ |
| 416 |
|
| 417 |
template <class T> |
| 418 |
bool |
| 419 |
extractScaling (const Matrix44<T>& mat, Vec3<T>& scl, bool exc) |
| 420 |
{ |
| 421 |
Vec3<T> shr; |
| 422 |
Matrix44<T> M (mat); |
| 423 |
|
| 424 |
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc)) |
| 425 |
return false; |
| 426 |
|
| 427 |
return true; |
| 428 |
} |
| 429 |
|
| 430 |
template <class T> |
| 431 |
Matrix44<T> |
| 432 |
sansScaling (const Matrix44<T>& mat, bool exc) |
| 433 |
{ |
| 434 |
Vec3<T> scl; |
| 435 |
Vec3<T> shr; |
| 436 |
Vec3<T> rot; |
| 437 |
Vec3<T> tran; |
| 438 |
|
| 439 |
if (!extractSHRT (mat, scl, shr, rot, tran, exc)) |
| 440 |
return mat; |
| 441 |
|
| 442 |
Matrix44<T> M; |
| 443 |
|
| 444 |
M.translate (tran); |
| 445 |
M.rotate (rot); |
| 446 |
M.shear (shr); |
| 447 |
|
| 448 |
return M; |
| 449 |
} |
| 450 |
|
| 451 |
template <class T> |
| 452 |
bool |
| 453 |
removeScaling (Matrix44<T>& mat, bool exc) |
| 454 |
{ |
| 455 |
Vec3<T> scl; |
| 456 |
Vec3<T> shr; |
| 457 |
Vec3<T> rot; |
| 458 |
Vec3<T> tran; |
| 459 |
|
| 460 |
if (!extractSHRT (mat, scl, shr, rot, tran, exc)) |
| 461 |
return false; |
| 462 |
|
| 463 |
mat.makeIdentity(); |
| 464 |
mat.translate (tran); |
| 465 |
mat.rotate (rot); |
| 466 |
mat.shear (shr); |
| 467 |
|
| 468 |
return true; |
| 469 |
} |
| 470 |
|
| 471 |
template <class T> |
| 472 |
bool |
| 473 |
extractScalingAndShear (const Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc) |
| 474 |
{ |
| 475 |
Matrix44<T> M (mat); |
| 476 |
|
| 477 |
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc)) |
| 478 |
return false; |
| 479 |
|
| 480 |
return true; |
| 481 |
} |
| 482 |
|
| 483 |
template <class T> |
| 484 |
Matrix44<T> |
| 485 |
sansScalingAndShear (const Matrix44<T>& mat, bool exc) |
| 486 |
{ |
| 487 |
Vec3<T> scl; |
| 488 |
Vec3<T> shr; |
| 489 |
Matrix44<T> M (mat); |
| 490 |
|
| 491 |
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc)) |
| 492 |
return mat; |
| 493 |
|
| 494 |
return M; |
| 495 |
} |
| 496 |
|
| 497 |
template <class T> |
| 498 |
void |
| 499 |
sansScalingAndShear (Matrix44<T>& result, const Matrix44<T>& mat, bool exc) |
| 500 |
{ |
| 501 |
Vec3<T> scl; |
| 502 |
Vec3<T> shr; |
| 503 |
|
| 504 |
if (!extractAndRemoveScalingAndShear (result, scl, shr, exc)) |
| 505 |
result = mat; |
| 506 |
} |
| 507 |
|
| 508 |
template <class T> |
| 509 |
bool |
| 510 |
removeScalingAndShear (Matrix44<T>& mat, bool exc) |
| 511 |
{ |
| 512 |
Vec3<T> scl; |
| 513 |
Vec3<T> shr; |
| 514 |
|
| 515 |
if (!extractAndRemoveScalingAndShear (mat, scl, shr, exc)) |
| 516 |
return false; |
| 517 |
|
| 518 |
return true; |
| 519 |
} |
| 520 |
|
| 521 |
template <class T> |
| 522 |
bool |
| 523 |
extractAndRemoveScalingAndShear (Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc) |
| 524 |
{ |
| 525 |
// |
| 526 |
// This implementation follows the technique described in the paper by |
| 527 |
// Spencer W. Thomas in the Graphics Gems II article: "Decomposing a |
| 528 |
// Matrix into Simple Transformations", p. 320. |
| 529 |
// |
| 530 |
|
| 531 |
Vec3<T> row[3]; |
| 532 |
|
| 533 |
row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]); |
| 534 |
row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]); |
| 535 |
row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]); |
| 536 |
|
| 537 |
T maxVal = 0; |
| 538 |
for (int i = 0; i < 3; i++) |
| 539 |
for (int j = 0; j < 3; j++) |
| 540 |
if (IMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal) |
| 541 |
maxVal = IMATH_INTERNAL_NAMESPACE::abs (row[i][j]); |
| 542 |
|
| 543 |
// |
| 544 |
// We normalize the 3x3 matrix here. |
| 545 |
// It was noticed that this can improve numerical stability significantly, |
| 546 |
// especially when many of the upper 3x3 matrix's coefficients are very |
| 547 |
// close to zero; we correct for this step at the end by multiplying the |
| 548 |
// scaling factors by maxVal at the end (shear and rotation are not |
| 549 |
// affected by the normalization). |
| 550 |
|
| 551 |
if (maxVal != 0) |
| 552 |
{ |
| 553 |
for (int i = 0; i < 3; i++) |
| 554 |
if (!checkForZeroScaleInRow (maxVal, row[i], exc)) |
| 555 |
return false; |
| 556 |
else |
| 557 |
row[i] /= maxVal; |
| 558 |
} |
| 559 |
|
| 560 |
// Compute X scale factor. |
| 561 |
scl.x = row[0].length(); |
| 562 |
if (!checkForZeroScaleInRow (scl.x, row[0], exc)) |
| 563 |
return false; |
| 564 |
|
| 565 |
// Normalize first row. |
| 566 |
row[0] /= scl.x; |
| 567 |
|
| 568 |
// An XY shear factor will shear the X coord. as the Y coord. changes. |
| 569 |
// There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only |
| 570 |
// extract the first 3 because we can effect the last 3 by shearing in |
| 571 |
// XY, XZ, YZ combined rotations and scales. |
| 572 |
// |
| 573 |
// shear matrix < 1, YX, ZX, 0, |
| 574 |
// XY, 1, ZY, 0, |
| 575 |
// XZ, YZ, 1, 0, |
| 576 |
// 0, 0, 0, 1 > |
| 577 |
|
| 578 |
// Compute XY shear factor and make 2nd row orthogonal to 1st. |
| 579 |
shr[0] = row[0].dot (row[1]); |
| 580 |
row[1] -= shr[0] * row[0]; |
| 581 |
|
| 582 |
// Now, compute Y scale. |
| 583 |
scl.y = row[1].length(); |
| 584 |
if (!checkForZeroScaleInRow (scl.y, row[1], exc)) |
| 585 |
return false; |
| 586 |
|
| 587 |
// Normalize 2nd row and correct the XY shear factor for Y scaling. |
| 588 |
row[1] /= scl.y; |
| 589 |
shr[0] /= scl.y; |
| 590 |
|
| 591 |
// Compute XZ and YZ shears, orthogonalize 3rd row. |
| 592 |
shr[1] = row[0].dot (row[2]); |
| 593 |
row[2] -= shr[1] * row[0]; |
| 594 |
shr[2] = row[1].dot (row[2]); |
| 595 |
row[2] -= shr[2] * row[1]; |
| 596 |
|
| 597 |
// Next, get Z scale. |
| 598 |
scl.z = row[2].length(); |
| 599 |
if (!checkForZeroScaleInRow (scl.z, row[2], exc)) |
| 600 |
return false; |
| 601 |
|
| 602 |
// Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling. |
| 603 |
row[2] /= scl.z; |
| 604 |
shr[1] /= scl.z; |
| 605 |
shr[2] /= scl.z; |
| 606 |
|
| 607 |
// At this point, the upper 3x3 matrix in mat is orthonormal. |
| 608 |
// Check for a coordinate system flip. If the determinant |
| 609 |
// is less than zero, then negate the matrix and the scaling factors. |
| 610 |
if (row[0].dot (row[1].cross (row[2])) < 0) |
| 611 |
for (int i = 0; i < 3; i++) |
| 612 |
{ |
| 613 |
scl[i] *= -1; |
| 614 |
row[i] *= -1; |
| 615 |
} |
| 616 |
|
| 617 |
// Copy over the orthonormal rows into the returned matrix. |
| 618 |
// The upper 3x3 matrix in mat is now a rotation matrix. |
| 619 |
for (int i = 0; i < 3; i++) |
| 620 |
{ |
| 621 |
mat[i][0] = row[i][0]; |
| 622 |
mat[i][1] = row[i][1]; |
| 623 |
mat[i][2] = row[i][2]; |
| 624 |
} |
| 625 |
|
| 626 |
// Correct the scaling factors for the normalization step that we |
| 627 |
// performed above; shear and rotation are not affected by the |
| 628 |
// normalization. |
| 629 |
scl *= maxVal; |
| 630 |
|
| 631 |
return true; |
| 632 |
} |
| 633 |
|
| 634 |
template <class T> |
| 635 |
void |
| 636 |
extractEulerXYZ (const Matrix44<T>& mat, Vec3<T>& rot) |
| 637 |
{ |
| 638 |
// |
| 639 |
// Normalize the local x, y and z axes to remove scaling. |
| 640 |
// |
| 641 |
|
| 642 |
Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]); |
| 643 |
Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]); |
| 644 |
Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]); |
| 645 |
|
| 646 |
i.normalize(); |
| 647 |
j.normalize(); |
| 648 |
k.normalize(); |
| 649 |
|
| 650 |
Matrix44<T> M (i[0], i[1], i[2], 0, j[0], j[1], j[2], 0, k[0], k[1], k[2], 0, 0, 0, 0, 1); |
| 651 |
|
| 652 |
// |
| 653 |
// Extract the first angle, rot.x. |
| 654 |
// |
| 655 |
|
| 656 |
rot.x = std::atan2 (M[1][2], M[2][2]); |
| 657 |
|
| 658 |
// |
| 659 |
// Remove the rot.x rotation from M, so that the remaining |
| 660 |
// rotation, N, is only around two axes, and gimbal lock |
| 661 |
// cannot occur. |
| 662 |
// |
| 663 |
|
| 664 |
Matrix44<T> N; |
| 665 |
N.rotate (Vec3<T> (-rot.x, 0, 0)); |
| 666 |
N = N * M; |
| 667 |
|
| 668 |
// |
| 669 |
// Extract the other two angles, rot.y and rot.z, from N. |
| 670 |
// |
| 671 |
|
| 672 |
T cy = std::sqrt (N[0][0] * N[0][0] + N[0][1] * N[0][1]); |
| 673 |
rot.y = std::atan2 (-N[0][2], cy); |
| 674 |
rot.z = std::atan2 (-N[1][0], N[1][1]); |
| 675 |
} |
| 676 |
|
| 677 |
template <class T> |
| 678 |
void |
| 679 |
extractEulerZYX (const Matrix44<T>& mat, Vec3<T>& rot) |
| 680 |
{ |
| 681 |
// |
| 682 |
// Normalize the local x, y and z axes to remove scaling. |
| 683 |
// |
| 684 |
|
| 685 |
Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]); |
| 686 |
Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]); |
| 687 |
Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]); |
| 688 |
|
| 689 |
i.normalize(); |
| 690 |
j.normalize(); |
| 691 |
k.normalize(); |
| 692 |
|
| 693 |
Matrix44<T> M (i[0], i[1], i[2], 0, j[0], j[1], j[2], 0, k[0], k[1], k[2], 0, 0, 0, 0, 1); |
| 694 |
|
| 695 |
// |
| 696 |
// Extract the first angle, rot.x. |
| 697 |
// |
| 698 |
|
| 699 |
rot.x = -std::atan2 (M[1][0], M[0][0]); |
| 700 |
|
| 701 |
// |
| 702 |
// Remove the x rotation from M, so that the remaining |
| 703 |
// rotation, N, is only around two axes, and gimbal lock |
| 704 |
// cannot occur. |
| 705 |
// |
| 706 |
|
| 707 |
Matrix44<T> N; |
| 708 |
N.rotate (Vec3<T> (0, 0, -rot.x)); |
| 709 |
N = N * M; |
| 710 |
|
| 711 |
// |
| 712 |
// Extract the other two angles, rot.y and rot.z, from N. |
| 713 |
// |
| 714 |
|
| 715 |
T cy = std::sqrt (N[2][2] * N[2][2] + N[2][1] * N[2][1]); |
| 716 |
rot.y = -std::atan2 (-N[2][0], cy); |
| 717 |
rot.z = -std::atan2 (-N[1][2], N[1][1]); |
| 718 |
} |
| 719 |
|
| 720 |
template <class T> |
| 721 |
Quat<T> |
| 722 |
extractQuat (const Matrix44<T>& mat) |
| 723 |
{ |
| 724 |
T tr, s; |
| 725 |
T q[4]; |
| 726 |
int i, j, k; |
| 727 |
|
| 728 |
Quat<T> quat; |
| 729 |
|
| 730 |
int nxt[3] = { 1, 2, 0 }; |
| 731 |
tr = mat[0][0] + mat[1][1] + mat[2][2]; |
| 732 |
|
| 733 |
// check the diagonal |
| 734 |
if (tr > 0.0) |
| 735 |
{ |
| 736 |
s = std::sqrt (tr + T (1.0)); |
| 737 |
quat.r = s / T (2.0); |
| 738 |
s = T (0.5) / s; |
| 739 |
|
| 740 |
quat.v.x = (mat[1][2] - mat[2][1]) * s; |
| 741 |
quat.v.y = (mat[2][0] - mat[0][2]) * s; |
| 742 |
quat.v.z = (mat[0][1] - mat[1][0]) * s; |
| 743 |
} |
| 744 |
else |
| 745 |
{ |
| 746 |
// diagonal is negative |
| 747 |
i = 0; |
| 748 |
if (mat[1][1] > mat[0][0]) |
| 749 |
i = 1; |
| 750 |
if (mat[2][2] > mat[i][i]) |
| 751 |
i = 2; |
| 752 |
|
| 753 |
j = nxt[i]; |
| 754 |
k = nxt[j]; |
| 755 |
s = std::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + T (1.0)); |
| 756 |
|
| 757 |
q[i] = s * T (0.5); |
| 758 |
if (s != T (0.0)) |
| 759 |
s = T (0.5) / s; |
| 760 |
|
| 761 |
q[3] = (mat[j][k] - mat[k][j]) * s; |
| 762 |
q[j] = (mat[i][j] + mat[j][i]) * s; |
| 763 |
q[k] = (mat[i][k] + mat[k][i]) * s; |
| 764 |
|
| 765 |
quat.v.x = q[0]; |
| 766 |
quat.v.y = q[1]; |
| 767 |
quat.v.z = q[2]; |
| 768 |
quat.r = q[3]; |
| 769 |
} |
| 770 |
|
| 771 |
return quat; |
| 772 |
} |
| 773 |
|
| 774 |
template <class T> |
| 775 |
bool |
| 776 |
extractSHRT (const Matrix44<T>& mat, |
| 777 |
Vec3<T>& s, |
| 778 |
Vec3<T>& h, |
| 779 |
Vec3<T>& r, |
| 780 |
Vec3<T>& t, |
| 781 |
bool exc /* = true */, |
| 782 |
typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */) |
| 783 |
{ |
| 784 |
Matrix44<T> rot; |
| 785 |
|
| 786 |
rot = mat; |
| 787 |
if (!extractAndRemoveScalingAndShear (rot, s, h, exc)) |
| 788 |
return false; |
| 789 |
|
| 790 |
extractEulerXYZ (rot, r); |
| 791 |
|
| 792 |
t.x = mat[3][0]; |
| 793 |
t.y = mat[3][1]; |
| 794 |
t.z = mat[3][2]; |
| 795 |
|
| 796 |
if (rOrder != Euler<T>::XYZ) |
| 797 |
{ |
| 798 |
Euler<T> eXYZ (r, Euler<T>::XYZ); |
| 799 |
Euler<T> e (eXYZ, rOrder); |
| 800 |
r = e.toXYZVector(); |
| 801 |
} |
| 802 |
|
| 803 |
return true; |
| 804 |
} |
| 805 |
|
| 806 |
template <class T> |
| 807 |
bool |
| 808 |
extractSHRT (const Matrix44<T>& mat, Vec3<T>& s, Vec3<T>& h, Vec3<T>& r, Vec3<T>& t, bool exc) |
| 809 |
{ |
| 810 |
return extractSHRT (mat, s, h, r, t, exc, Euler<T>::XYZ); |
| 811 |
} |
| 812 |
|
| 813 |
template <class T> |
| 814 |
bool |
| 815 |
extractSHRT (const Matrix44<T>& mat, |
| 816 |
Vec3<T>& s, |
| 817 |
Vec3<T>& h, |
| 818 |
Euler<T>& r, |
| 819 |
Vec3<T>& t, |
| 820 |
bool exc /* = true */) |
| 821 |
{ |
| 822 |
return extractSHRT (mat, s, h, r, t, exc, r.order()); |
| 823 |
} |
| 824 |
|
| 825 |
template <class T> |
| 826 |
bool |
| 827 |
checkForZeroScaleInRow (const T& scl, const Vec3<T>& row, bool exc /* = true */) |
| 828 |
{ |
| 829 |
for (int i = 0; i < 3; i++) |
| 830 |
{ |
| 831 |
if ((abs (scl) < 1 && abs (row[i]) >= std::numeric_limits<T>::max() * abs (scl))) |
| 832 |
{ |
| 833 |
if (exc) |
| 834 |
throw std::domain_error ("Cannot remove zero scaling " |
| 835 |
"from matrix."); |
| 836 |
else |
| 837 |
return false; |
| 838 |
} |
| 839 |
} |
| 840 |
|
| 841 |
return true; |
| 842 |
} |
| 843 |
|
| 844 |
template <class T> |
| 845 |
Matrix44<T> |
| 846 |
outerProduct (const Vec4<T>& a, const Vec4<T>& b) |
| 847 |
{ |
| 848 |
return Matrix44<T> (a.x * b.x, |
| 849 |
a.x * b.y, |
| 850 |
a.x * b.z, |
| 851 |
a.x * b.w, |
| 852 |
a.y * b.x, |
| 853 |
a.y * b.y, |
| 854 |
a.y * b.z, |
| 855 |
a.x * b.w, |
| 856 |
a.z * b.x, |
| 857 |
a.z * b.y, |
| 858 |
a.z * b.z, |
| 859 |
a.x * b.w, |
| 860 |
a.w * b.x, |
| 861 |
a.w * b.y, |
| 862 |
a.w * b.z, |
| 863 |
a.w * b.w); |
| 864 |
} |
| 865 |
|
| 866 |
template <class T> |
| 867 |
Matrix44<T> |
| 868 |
rotationMatrix (const Vec3<T>& from, const Vec3<T>& to) |
| 869 |
{ |
| 870 |
Quat<T> q; |
| 871 |
q.setRotation (from, to); |
| 872 |
return q.toMatrix44(); |
| 873 |
} |
| 874 |
|
| 875 |
template <class T> |
| 876 |
Matrix44<T> |
| 877 |
rotationMatrixWithUpDir (const Vec3<T>& fromDir, const Vec3<T>& toDir, const Vec3<T>& upDir) |
| 878 |
{ |
| 879 |
// |
| 880 |
// The goal is to obtain a rotation matrix that takes |
| 881 |
// "fromDir" to "toDir". We do this in two steps and |
| 882 |
// compose the resulting rotation matrices; |
| 883 |
// (a) rotate "fromDir" into the z-axis |
| 884 |
// (b) rotate the z-axis into "toDir" |
| 885 |
// |
| 886 |
|
| 887 |
// The from direction must be non-zero; but we allow zero to and up dirs. |
| 888 |
if (fromDir.length() == 0) |
| 889 |
return Matrix44<T>(); |
| 890 |
|
| 891 |
else |
| 892 |
{ |
| 893 |
Matrix44<T> zAxis2FromDir (UNINITIALIZED); |
| 894 |
alignZAxisWithTargetDir (zAxis2FromDir, fromDir, Vec3<T> (0, 1, 0)); |
| 895 |
|
| 896 |
Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed(); |
| 897 |
|
| 898 |
Matrix44<T> zAxis2ToDir (UNINITIALIZED); |
| 899 |
alignZAxisWithTargetDir (zAxis2ToDir, toDir, upDir); |
| 900 |
|
| 901 |
return fromDir2zAxis * zAxis2ToDir; |
| 902 |
} |
| 903 |
} |
| 904 |
|
| 905 |
template <class T> |
| 906 |
void |
| 907 |
alignZAxisWithTargetDir (Matrix44<T>& result, Vec3<T> targetDir, Vec3<T> upDir) |
| 908 |
{ |
| 909 |
// |
| 910 |
// Ensure that the target direction is non-zero. |
| 911 |
// |
| 912 |
|
| 913 |
if (targetDir.length() == 0) |
| 914 |
targetDir = Vec3<T> (0, 0, 1); |
| 915 |
|
| 916 |
// |
| 917 |
// Ensure that the up direction is non-zero. |
| 918 |
// |
| 919 |
|
| 920 |
if (upDir.length() == 0) |
| 921 |
upDir = Vec3<T> (0, 1, 0); |
| 922 |
|
| 923 |
// |
| 924 |
// Check for degeneracies. If the upDir and targetDir are parallel |
| 925 |
// or opposite, then compute a new, arbitrary up direction that is |
| 926 |
// not parallel or opposite to the targetDir. |
| 927 |
// |
| 928 |
|
| 929 |
if (upDir.cross (targetDir).length() == 0) |
| 930 |
{ |
| 931 |
upDir = targetDir.cross (Vec3<T> (1, 0, 0)); |
| 932 |
if (upDir.length() == 0) |
| 933 |
upDir = targetDir.cross (Vec3<T> (0, 0, 1)); |
| 934 |
} |
| 935 |
|
| 936 |
// |
| 937 |
// Compute the x-, y-, and z-axis vectors of the new coordinate system. |
| 938 |
// |
| 939 |
|
| 940 |
Vec3<T> targetPerpDir = upDir.cross (targetDir); |
| 941 |
Vec3<T> targetUpDir = targetDir.cross (targetPerpDir); |
| 942 |
|
| 943 |
// |
| 944 |
// Rotate the x-axis into targetPerpDir (row 0), |
| 945 |
// rotate the y-axis into targetUpDir (row 1), |
| 946 |
// rotate the z-axis into targetDir (row 2). |
| 947 |
// |
| 948 |
|
| 949 |
Vec3<T> row[3]; |
| 950 |
row[0] = targetPerpDir.normalized(); |
| 951 |
row[1] = targetUpDir.normalized(); |
| 952 |
row[2] = targetDir.normalized(); |
| 953 |
|
| 954 |
result.x[0][0] = row[0][0]; |
| 955 |
result.x[0][1] = row[0][1]; |
| 956 |
result.x[0][2] = row[0][2]; |
| 957 |
result.x[0][3] = (T) 0; |
| 958 |
|
| 959 |
result.x[1][0] = row[1][0]; |
| 960 |
result.x[1][1] = row[1][1]; |
| 961 |
result.x[1][2] = row[1][2]; |
| 962 |
result.x[1][3] = (T) 0; |
| 963 |
|
| 964 |
result.x[2][0] = row[2][0]; |
| 965 |
result.x[2][1] = row[2][1]; |
| 966 |
result.x[2][2] = row[2][2]; |
| 967 |
result.x[2][3] = (T) 0; |
| 968 |
|
| 969 |
result.x[3][0] = (T) 0; |
| 970 |
result.x[3][1] = (T) 0; |
| 971 |
result.x[3][2] = (T) 0; |
| 972 |
result.x[3][3] = (T) 1; |
| 973 |
} |
| 974 |
|
| 975 |
// Compute an orthonormal direct frame from : a position, an x axis direction and a normal to the y axis |
| 976 |
// If the x axis and normal are perpendicular, then the normal will have the same direction as the z axis. |
| 977 |
// Inputs are : |
| 978 |
// -the position of the frame |
| 979 |
// -the x axis direction of the frame |
| 980 |
// -a normal to the y axis of the frame |
| 981 |
// Return is the orthonormal frame |
| 982 |
template <class T> |
| 983 |
Matrix44<T> |
| 984 |
computeLocalFrame (const Vec3<T>& p, const Vec3<T>& xDir, const Vec3<T>& normal) |
| 985 |
{ |
| 986 |
Vec3<T> _xDir (xDir); |
| 987 |
Vec3<T> x = _xDir.normalize(); |
| 988 |
Vec3<T> y = (normal % x).normalize(); |
| 989 |
Vec3<T> z = (x % y).normalize(); |
| 990 |
|
| 991 |
Matrix44<T> L; |
| 992 |
L[0][0] = x[0]; |
| 993 |
L[0][1] = x[1]; |
| 994 |
L[0][2] = x[2]; |
| 995 |
L[0][3] = 0.0; |
| 996 |
|
| 997 |
L[1][0] = y[0]; |
| 998 |
L[1][1] = y[1]; |
| 999 |
L[1][2] = y[2]; |
| 1000 |
L[1][3] = 0.0; |
| 1001 |
|
| 1002 |
L[2][0] = z[0]; |
| 1003 |
L[2][1] = z[1]; |
| 1004 |
L[2][2] = z[2]; |
| 1005 |
L[2][3] = 0.0; |
| 1006 |
|
| 1007 |
L[3][0] = p[0]; |
| 1008 |
L[3][1] = p[1]; |
| 1009 |
L[3][2] = p[2]; |
| 1010 |
L[3][3] = 1.0; |
| 1011 |
|
| 1012 |
return L; |
| 1013 |
} |
| 1014 |
|
| 1015 |
/// Add a translate/rotate/scale offset to an input frame and put it |
| 1016 |
/// in another frame of reference. |
| 1017 |
/// |
| 1018 |
/// @param inMat input frame |
| 1019 |
/// @param tOffset translate offset |
| 1020 |
/// @param rOffset rotate offset in degrees |
| 1021 |
/// @param sOffset scale offset |
| 1022 |
/// @param ref Frame of reference |
| 1023 |
/// @return The offsetted frame |
| 1024 |
template <class T> |
| 1025 |
Matrix44<T> |
| 1026 |
addOffset (const Matrix44<T>& inMat, |
| 1027 |
const Vec3<T>& tOffset, |
| 1028 |
const Vec3<T>& rOffset, |
| 1029 |
const Vec3<T>& sOffset, |
| 1030 |
const Matrix44<T>& ref) |
| 1031 |
{ |
| 1032 |
Matrix44<T> O; |
| 1033 |
|
| 1034 |
Vec3<T> _rOffset (rOffset); |
| 1035 |
_rOffset *= T(M_PI / 180.0); |
| 1036 |
O.rotate (_rOffset); |
| 1037 |
|
| 1038 |
O[3][0] = tOffset[0]; |
| 1039 |
O[3][1] = tOffset[1]; |
| 1040 |
O[3][2] = tOffset[2]; |
| 1041 |
|
| 1042 |
Matrix44<T> S; |
| 1043 |
S.scale (sOffset); |
| 1044 |
|
| 1045 |
Matrix44<T> X = S * O * inMat * ref; |
| 1046 |
|
| 1047 |
return X; |
| 1048 |
} |
| 1049 |
|
| 1050 |
// Compute Translate/Rotate/Scale matrix from matrix A with the Rotate/Scale of Matrix B |
| 1051 |
// Inputs are : |
| 1052 |
// -keepRotateA : if true keep rotate from matrix A, use B otherwise |
| 1053 |
// -keepScaleA : if true keep scale from matrix A, use B otherwise |
| 1054 |
// -Matrix A |
| 1055 |
// -Matrix B |
| 1056 |
// Return Matrix A with tweaked rotation/scale |
| 1057 |
template <class T> |
| 1058 |
Matrix44<T> |
| 1059 |
computeRSMatrix (bool keepRotateA, bool keepScaleA, const Matrix44<T>& A, const Matrix44<T>& B) |
| 1060 |
{ |
| 1061 |
Vec3<T> as, ah, ar, at; |
| 1062 |
if (!extractSHRT (A, as, ah, ar, at)) |
| 1063 |
throw std::domain_error ("degenerate A matrix in computeRSMatrix"); |
| 1064 |
|
| 1065 |
Vec3<T> bs, bh, br, bt; |
| 1066 |
if (!extractSHRT (B, bs, bh, br, bt)) |
| 1067 |
throw std::domain_error ("degenerate B matrix in computeRSMatrix"); |
| 1068 |
|
| 1069 |
if (!keepRotateA) |
| 1070 |
ar = br; |
| 1071 |
|
| 1072 |
if (!keepScaleA) |
| 1073 |
as = bs; |
| 1074 |
|
| 1075 |
Matrix44<T> mat; |
| 1076 |
mat.makeIdentity(); |
| 1077 |
mat.translate (at); |
| 1078 |
mat.rotate (ar); |
| 1079 |
mat.scale (as); |
| 1080 |
|
| 1081 |
return mat; |
| 1082 |
} |
| 1083 |
|
| 1084 |
//----------------------------------------------------------------------------- |
| 1085 |
// Implementation for 3x3 Matrix |
| 1086 |
//------------------------------ |
| 1087 |
|
| 1088 |
template <class T> |
| 1089 |
bool |
| 1090 |
extractScaling (const Matrix33<T>& mat, Vec2<T>& scl, bool exc) |
| 1091 |
{ |
| 1092 |
T shr; |
| 1093 |
Matrix33<T> M (mat); |
| 1094 |
|
| 1095 |
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc)) |
| 1096 |
return false; |
| 1097 |
|
| 1098 |
return true; |
| 1099 |
} |
| 1100 |
|
| 1101 |
template <class T> |
| 1102 |
Matrix33<T> |
| 1103 |
sansScaling (const Matrix33<T>& mat, bool exc) |
| 1104 |
{ |
| 1105 |
Vec2<T> scl; |
| 1106 |
T shr; |
| 1107 |
T rot; |
| 1108 |
Vec2<T> tran; |
| 1109 |
|
| 1110 |
if (!extractSHRT (mat, scl, shr, rot, tran, exc)) |
| 1111 |
return mat; |
| 1112 |
|
| 1113 |
Matrix33<T> M; |
| 1114 |
|
| 1115 |
M.translate (tran); |
| 1116 |
M.rotate (rot); |
| 1117 |
M.shear (shr); |
| 1118 |
|
| 1119 |
return M; |
| 1120 |
} |
| 1121 |
|
| 1122 |
template <class T> |
| 1123 |
bool |
| 1124 |
removeScaling (Matrix33<T>& mat, bool exc) |
| 1125 |
{ |
| 1126 |
Vec2<T> scl; |
| 1127 |
T shr; |
| 1128 |
T rot; |
| 1129 |
Vec2<T> tran; |
| 1130 |
|
| 1131 |
if (!extractSHRT (mat, scl, shr, rot, tran, exc)) |
| 1132 |
return false; |
| 1133 |
|
| 1134 |
mat.makeIdentity(); |
| 1135 |
mat.translate (tran); |
| 1136 |
mat.rotate (rot); |
| 1137 |
mat.shear (shr); |
| 1138 |
|
| 1139 |
return true; |
| 1140 |
} |
| 1141 |
|
| 1142 |
template <class T> |
| 1143 |
bool |
| 1144 |
extractScalingAndShear (const Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc) |
| 1145 |
{ |
| 1146 |
Matrix33<T> M (mat); |
| 1147 |
|
| 1148 |
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc)) |
| 1149 |
return false; |
| 1150 |
|
| 1151 |
return true; |
| 1152 |
} |
| 1153 |
|
| 1154 |
template <class T> |
| 1155 |
Matrix33<T> |
| 1156 |
sansScalingAndShear (const Matrix33<T>& mat, bool exc) |
| 1157 |
{ |
| 1158 |
Vec2<T> scl; |
| 1159 |
T shr; |
| 1160 |
Matrix33<T> M (mat); |
| 1161 |
|
| 1162 |
if (!extractAndRemoveScalingAndShear (M, scl, shr, exc)) |
| 1163 |
return mat; |
| 1164 |
|
| 1165 |
return M; |
| 1166 |
} |
| 1167 |
|
| 1168 |
template <class T> |
| 1169 |
bool |
| 1170 |
removeScalingAndShear (Matrix33<T>& mat, bool exc) |
| 1171 |
{ |
| 1172 |
Vec2<T> scl; |
| 1173 |
T shr; |
| 1174 |
|
| 1175 |
if (!extractAndRemoveScalingAndShear (mat, scl, shr, exc)) |
| 1176 |
return false; |
| 1177 |
|
| 1178 |
return true; |
| 1179 |
} |
| 1180 |
|
| 1181 |
template <class T> |
| 1182 |
bool |
| 1183 |
extractAndRemoveScalingAndShear (Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc) |
| 1184 |
{ |
| 1185 |
Vec2<T> row[2]; |
| 1186 |
|
| 1187 |
row[0] = Vec2<T> (mat[0][0], mat[0][1]); |
| 1188 |
row[1] = Vec2<T> (mat[1][0], mat[1][1]); |
| 1189 |
|
| 1190 |
T maxVal = 0; |
| 1191 |
for (int i = 0; i < 2; i++) |
| 1192 |
for (int j = 0; j < 2; j++) |
| 1193 |
if (IMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal) |
| 1194 |
maxVal = IMATH_INTERNAL_NAMESPACE::abs (row[i][j]); |
| 1195 |
|
| 1196 |
// |
| 1197 |
// We normalize the 2x2 matrix here. |
| 1198 |
// It was noticed that this can improve numerical stability significantly, |
| 1199 |
// especially when many of the upper 2x2 matrix's coefficients are very |
| 1200 |
// close to zero; we correct for this step at the end by multiplying the |
| 1201 |
// scaling factors by maxVal at the end (shear and rotation are not |
| 1202 |
// affected by the normalization). |
| 1203 |
|
| 1204 |
if (maxVal != 0) |
| 1205 |
{ |
| 1206 |
for (int i = 0; i < 2; i++) |
| 1207 |
if (!checkForZeroScaleInRow (maxVal, row[i], exc)) |
| 1208 |
return false; |
| 1209 |
else |
| 1210 |
row[i] /= maxVal; |
| 1211 |
} |
| 1212 |
|
| 1213 |
// Compute X scale factor. |
| 1214 |
scl.x = row[0].length(); |
| 1215 |
if (!checkForZeroScaleInRow (scl.x, row[0], exc)) |
| 1216 |
return false; |
| 1217 |
|
| 1218 |
// Normalize first row. |
| 1219 |
row[0] /= scl.x; |
| 1220 |
|
| 1221 |
// An XY shear factor will shear the X coord. as the Y coord. changes. |
| 1222 |
// There are 2 combinations (XY, YX), although we only extract the XY |
| 1223 |
// shear factor because we can effect the an YX shear factor by |
| 1224 |
// shearing in XY combined with rotations and scales. |
| 1225 |
// |
| 1226 |
// shear matrix < 1, YX, 0, |
| 1227 |
// XY, 1, 0, |
| 1228 |
// 0, 0, 1 > |
| 1229 |
|
| 1230 |
// Compute XY shear factor and make 2nd row orthogonal to 1st. |
| 1231 |
shr = row[0].dot (row[1]); |
| 1232 |
row[1] -= shr * row[0]; |
| 1233 |
|
| 1234 |
// Now, compute Y scale. |
| 1235 |
scl.y = row[1].length(); |
| 1236 |
if (!checkForZeroScaleInRow (scl.y, row[1], exc)) |
| 1237 |
return false; |
| 1238 |
|
| 1239 |
// Normalize 2nd row and correct the XY shear factor for Y scaling. |
| 1240 |
row[1] /= scl.y; |
| 1241 |
shr /= scl.y; |
| 1242 |
|
| 1243 |
// At this point, the upper 2x2 matrix in mat is orthonormal. |
| 1244 |
// Check for a coordinate system flip. If the determinant |
| 1245 |
// is -1, then flip the rotation matrix and adjust the scale(Y) |
| 1246 |
// and shear(XY) factors to compensate. |
| 1247 |
if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0) |
| 1248 |
{ |
| 1249 |
row[1][0] *= -1; |
| 1250 |
row[1][1] *= -1; |
| 1251 |
scl[1] *= -1; |
| 1252 |
shr *= -1; |
| 1253 |
} |
| 1254 |
|
| 1255 |
// Copy over the orthonormal rows into the returned matrix. |
| 1256 |
// The upper 2x2 matrix in mat is now a rotation matrix. |
| 1257 |
for (int i = 0; i < 2; i++) |
| 1258 |
{ |
| 1259 |
mat[i][0] = row[i][0]; |
| 1260 |
mat[i][1] = row[i][1]; |
| 1261 |
} |
| 1262 |
|
| 1263 |
scl *= maxVal; |
| 1264 |
|
| 1265 |
return true; |
| 1266 |
} |
| 1267 |
|
| 1268 |
template <class T> |
| 1269 |
void |
| 1270 |
extractEuler (const Matrix22<T>& mat, T& rot) |
| 1271 |
{ |
| 1272 |
// |
| 1273 |
// Normalize the local x and y axes to remove scaling. |
| 1274 |
// |
| 1275 |
|
| 1276 |
Vec2<T> i (mat[0][0], mat[0][1]); |
| 1277 |
Vec2<T> j (mat[1][0], mat[1][1]); |
| 1278 |
|
| 1279 |
i.normalize(); |
| 1280 |
j.normalize(); |
| 1281 |
|
| 1282 |
// |
| 1283 |
// Extract the angle, rot. |
| 1284 |
// |
| 1285 |
|
| 1286 |
rot = -std::atan2 (j[0], i[0]); |
| 1287 |
} |
| 1288 |
|
| 1289 |
template <class T> |
| 1290 |
void |
| 1291 |
extractEuler (const Matrix33<T>& mat, T& rot) |
| 1292 |
{ |
| 1293 |
// |
| 1294 |
// Normalize the local x and y axes to remove scaling. |
| 1295 |
// |
| 1296 |
|
| 1297 |
Vec2<T> i (mat[0][0], mat[0][1]); |
| 1298 |
Vec2<T> j (mat[1][0], mat[1][1]); |
| 1299 |
|
| 1300 |
i.normalize(); |
| 1301 |
j.normalize(); |
| 1302 |
|
| 1303 |
// |
| 1304 |
// Extract the angle, rot. |
| 1305 |
// |
| 1306 |
|
| 1307 |
rot = -std::atan2 (j[0], i[0]); |
| 1308 |
} |
| 1309 |
|
| 1310 |
template <class T> |
| 1311 |
bool |
| 1312 |
extractSHRT (const Matrix33<T>& mat, Vec2<T>& s, T& h, T& r, Vec2<T>& t, bool exc) |
| 1313 |
{ |
| 1314 |
Matrix33<T> rot; |
| 1315 |
|
| 1316 |
rot = mat; |
| 1317 |
if (!extractAndRemoveScalingAndShear (rot, s, h, exc)) |
| 1318 |
return false; |
| 1319 |
|
| 1320 |
extractEuler (rot, r); |
| 1321 |
|
| 1322 |
t.x = mat[2][0]; |
| 1323 |
t.y = mat[2][1]; |
| 1324 |
|
| 1325 |
return true; |
| 1326 |
} |
| 1327 |
|
| 1328 |
/// @cond Doxygen_Suppress |
| 1329 |
template <class T> |
| 1330 |
bool |
| 1331 |
checkForZeroScaleInRow (const T& scl, const Vec2<T>& row, bool exc /* = true */) |
| 1332 |
{ |
| 1333 |
for (int i = 0; i < 2; i++) |
| 1334 |
{ |
| 1335 |
if ((abs (scl) < 1 && abs (row[i]) >= std::numeric_limits<T>::max() * abs (scl))) |
| 1336 |
{ |
| 1337 |
if (exc) |
| 1338 |
throw std::domain_error ("Cannot remove zero scaling from matrix."); |
| 1339 |
else |
| 1340 |
return false; |
| 1341 |
} |
| 1342 |
} |
| 1343 |
|
| 1344 |
return true; |
| 1345 |
} |
| 1346 |
/// @endcond |
| 1347 |
|
| 1348 |
template <class T> |
| 1349 |
Matrix33<T> |
| 1350 |
outerProduct (const Vec3<T>& a, const Vec3<T>& b) |
| 1351 |
{ |
| 1352 |
return Matrix33<T> (a.x * b.x, |
| 1353 |
a.x * b.y, |
| 1354 |
a.x * b.z, |
| 1355 |
a.y * b.x, |
| 1356 |
a.y * b.y, |
| 1357 |
a.y * b.z, |
| 1358 |
a.z * b.x, |
| 1359 |
a.z * b.y, |
| 1360 |
a.z * b.z); |
| 1361 |
} |
| 1362 |
|
| 1363 |
/// Computes the translation and rotation that brings the 'from' points |
| 1364 |
/// as close as possible to the 'to' points under the Frobenius norm. |
| 1365 |
/// To be more specific, let x be the matrix of 'from' points and y be |
| 1366 |
/// the matrix of 'to' points, we want to find the matrix A of the form |
| 1367 |
/// [ R t ] |
| 1368 |
/// [ 0 1 ] |
| 1369 |
/// that minimizes |
| 1370 |
/// || (A*x - y)^T * W * (A*x - y) ||_F |
| 1371 |
/// If doScaling is true, then a uniform scale is allowed also. |
| 1372 |
/// @param A From points |
| 1373 |
/// @param B To points |
| 1374 |
/// @param weights Per-point weights |
| 1375 |
/// @param numPoints The number of points in `A`, `B`, and `weights` (must be equal) |
| 1376 |
/// @param doScaling If true, include a scaling transformation |
| 1377 |
/// @return The procrustes transformation |
| 1378 |
template <typename T> |
| 1379 |
M44d |
| 1380 |
procrustesRotationAndTranslation (const Vec3<T>* A, |
| 1381 |
const Vec3<T>* B, |
| 1382 |
const T* weights, |
| 1383 |
const size_t numPoints, |
| 1384 |
const bool doScaling = false); |
| 1385 |
|
| 1386 |
/// Computes the translation and rotation that brings the 'from' points |
| 1387 |
/// as close as possible to the 'to' points under the Frobenius norm. |
| 1388 |
/// To be more specific, let x be the matrix of 'from' points and y be |
| 1389 |
/// the matrix of 'to' points, we want to find the matrix A of the form |
| 1390 |
/// [ R t ] |
| 1391 |
/// [ 0 1 ] |
| 1392 |
/// that minimizes |
| 1393 |
/// || (A*x - y)^T * W * (A*x - y) ||_F |
| 1394 |
/// If doScaling is true, then a uniform scale is allowed also. |
| 1395 |
/// @param A From points |
| 1396 |
/// @param B To points |
| 1397 |
/// @param numPoints The number of points in `A` and `B` (must be equal) |
| 1398 |
/// @param doScaling If true, include a scaling transformation |
| 1399 |
/// @return The procrustes transformation |
| 1400 |
template <typename T> |
| 1401 |
M44d |
| 1402 |
procrustesRotationAndTranslation (const Vec3<T>* A, |
| 1403 |
const Vec3<T>* B, |
| 1404 |
const size_t numPoints, |
| 1405 |
const bool doScaling = false); |
| 1406 |
|
| 1407 |
/// Compute the SVD of a 3x3 matrix using Jacobi transformations. This method |
| 1408 |
/// should be quite accurate (competitive with LAPACK) even for poorly |
| 1409 |
/// conditioned matrices, and because it has been written specifically for the |
| 1410 |
/// 3x3/4x4 case it is much faster than calling out to LAPACK. |
| 1411 |
/// |
| 1412 |
/// The SVD of a 3x3/4x4 matrix A is defined as follows: |
| 1413 |
/// A = U * S * V^T |
| 1414 |
/// where S is the diagonal matrix of singular values and both U and V are |
| 1415 |
/// orthonormal. By convention, the entries S are all positive and sorted from |
| 1416 |
/// the largest to the smallest. However, some uses of this function may |
| 1417 |
/// require that the matrix U*V^T have positive determinant; in this case, we |
| 1418 |
/// may make the smallest singular value negative to ensure that this is |
| 1419 |
/// satisfied. |
| 1420 |
/// |
| 1421 |
/// Currently only available for single- and double-precision matrices. |
| 1422 |
template <typename T> |
| 1423 |
void jacobiSVD (const Matrix33<T>& A, |
| 1424 |
Matrix33<T>& U, |
| 1425 |
Vec3<T>& S, |
| 1426 |
Matrix33<T>& V, |
| 1427 |
const T tol = std::numeric_limits<T>::epsilon(), |
| 1428 |
const bool forcePositiveDeterminant = false); |
| 1429 |
|
| 1430 |
/// Compute the SVD of a 3x3 matrix using Jacobi transformations. This method |
| 1431 |
/// should be quite accurate (competitive with LAPACK) even for poorly |
| 1432 |
/// conditioned matrices, and because it has been written specifically for the |
| 1433 |
/// 3x3/4x4 case it is much faster than calling out to LAPACK. |
| 1434 |
/// |
| 1435 |
/// The SVD of a 3x3/4x4 matrix A is defined as follows: |
| 1436 |
/// A = U * S * V^T |
| 1437 |
/// where S is the diagonal matrix of singular values and both U and V are |
| 1438 |
/// orthonormal. By convention, the entries S are all positive and sorted from |
| 1439 |
/// the largest to the smallest. However, some uses of this function may |
| 1440 |
/// require that the matrix U*V^T have positive determinant; in this case, we |
| 1441 |
/// may make the smallest singular value negative to ensure that this is |
| 1442 |
/// satisfied. |
| 1443 |
/// |
| 1444 |
/// Currently only available for single- and double-precision matrices. |
| 1445 |
template <typename T> |
| 1446 |
void jacobiSVD (const Matrix44<T>& A, |
| 1447 |
Matrix44<T>& U, |
| 1448 |
Vec4<T>& S, |
| 1449 |
Matrix44<T>& V, |
| 1450 |
const T tol = std::numeric_limits<T>::epsilon(), |
| 1451 |
const bool forcePositiveDeterminant = false); |
| 1452 |
|
| 1453 |
/// Compute the eigenvalues (S) and the eigenvectors (V) of a real |
| 1454 |
/// symmetric matrix using Jacobi transformation, using a given |
| 1455 |
/// tolerance `tol`. |
| 1456 |
/// |
| 1457 |
/// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V: |
| 1458 |
/// A = V * S * V^T |
| 1459 |
/// where V is orthonormal and S is the diagonal matrix of eigenvalues. |
| 1460 |
/// Input matrix A must be symmetric. A is also modified during |
| 1461 |
/// the computation so that upper diagonal entries of A become zero. |
| 1462 |
template <typename T> |
| 1463 |
void jacobiEigenSolver (Matrix33<T>& A, Vec3<T>& S, Matrix33<T>& V, const T tol); |
| 1464 |
|
| 1465 |
/// Compute the eigenvalues (S) and the eigenvectors (V) of |
| 1466 |
/// a real symmetric matrix using Jacobi transformation. |
| 1467 |
/// |
| 1468 |
/// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V: |
| 1469 |
/// A = V * S * V^T |
| 1470 |
/// where V is orthonormal and S is the diagonal matrix of eigenvalues. |
| 1471 |
/// Input matrix A must be symmetric. A is also modified during |
| 1472 |
/// the computation so that upper diagonal entries of A become zero. |
| 1473 |
template <typename T> |
| 1474 |
inline void |
| 1475 |
jacobiEigenSolver (Matrix33<T>& A, Vec3<T>& S, Matrix33<T>& V) |
| 1476 |
{ |
| 1477 |
jacobiEigenSolver (A, S, V, std::numeric_limits<T>::epsilon()); |
| 1478 |
} |
| 1479 |
|
| 1480 |
/// Compute the eigenvalues (S) and the eigenvectors (V) of a real |
| 1481 |
/// symmetric matrix using Jacobi transformation, using a given |
| 1482 |
/// tolerance `tol`. |
| 1483 |
/// |
| 1484 |
/// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V: |
| 1485 |
/// A = V * S * V^T |
| 1486 |
/// where V is orthonormal and S is the diagonal matrix of eigenvalues. |
| 1487 |
/// Input matrix A must be symmetric. A is also modified during |
| 1488 |
/// the computation so that upper diagonal entries of A become zero. |
| 1489 |
template <typename T> |
| 1490 |
void jacobiEigenSolver (Matrix44<T>& A, Vec4<T>& S, Matrix44<T>& V, const T tol); |
| 1491 |
|
| 1492 |
/// Compute the eigenvalues (S) and the eigenvectors (V) of |
| 1493 |
/// a real symmetric matrix using Jacobi transformation. |
| 1494 |
/// |
| 1495 |
/// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V: |
| 1496 |
/// A = V * S * V^T |
| 1497 |
/// where V is orthonormal and S is the diagonal matrix of eigenvalues. |
| 1498 |
/// Input matrix A must be symmetric. A is also modified during |
| 1499 |
/// the computation so that upper diagonal entries of A become zero. |
| 1500 |
template <typename T> |
| 1501 |
inline void |
| 1502 |
jacobiEigenSolver (Matrix44<T>& A, Vec4<T>& S, Matrix44<T>& V) |
| 1503 |
{ |
| 1504 |
jacobiEigenSolver (A, S, V, std::numeric_limits<T>::epsilon()); |
| 1505 |
} |
| 1506 |
|
| 1507 |
/// Compute a eigenvector corresponding to the abs max eigenvalue |
| 1508 |
/// of a real symmetric matrix using Jacobi transformation. |
| 1509 |
template <typename TM, typename TV> void maxEigenVector (TM& A, TV& S); |
| 1510 |
|
| 1511 |
/// Compute a eigenvector corresponding to the abs min eigenvalue |
| 1512 |
/// of a real symmetric matrix using Jacobi transformation. |
| 1513 |
template <typename TM, typename TV> void minEigenVector (TM& A, TV& S); |
| 1514 |
|
| 1515 |
IMATH_INTERNAL_NAMESPACE_HEADER_EXIT |
| 1516 |
|
| 1517 |
#endif // INCLUDED_IMATHMATRIXALGO_H |