| //##################################################################### |
| // Copyright 2003-2004, Ronald Fedkiw, Geoffrey Irving. |
| // This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt. |
| //##################################################################### |
| // Class SYMMETRIC_MATRIX_3X3 |
| //##################################################################### |
| #include "SYMMETRIC_MATRIX_3X3.h" |
| #include "../Math_Tools/max.h" |
| #include "../Math_Tools/abs.h" |
| #include "../Math_Tools/exchange_sort.h" |
| #include "../Utilities/DEBUG_UTILITIES.h" |
| using namespace PhysBAM; |
| //##################################################################### |
| // Function Fast_Eigenvalues_Double |
| //##################################################################### |
| // lambda_x > lambda_y > lambda_z |
| template<class T> DIAGONAL_MATRIX_3X3<double> SYMMETRIC_MATRIX_3X3<T>:: |
| Fast_Eigenvalues_Double() const |
| { |
| double m = one_third * ( (double) x11 + x22 + x33); |
| double a11 = x11 - m, a22 = x22 - m, a33 = x33 - m, a12_sqr = (double) x21 * x21, a13_sqr = (double) x31 * x31, a23_sqr = (double) x32 * x32; |
| double p = one_sixth * (a11 * a11 + a22 * a22 + a33 * a33 + 2 * (a12_sqr + a13_sqr + a23_sqr)); |
| double q = .5 * (a11 * (a22 * a33 - a23_sqr) - a22 * a13_sqr - a33 * a12_sqr) + (double) x21 * x31 * x32; |
| double sqrt_p = sqrt (p), disc = p * p * p - q * q; |
| double phi = one_third * atan2 (sqrt (max ( (double) 0, disc)), q), c = cos (phi), s = sin (phi); |
| double sqrt_p_cos = sqrt_p * c, root_three_sqrt_p_sin = root_three * sqrt_p * s; |
| DIAGONAL_MATRIX_3X3<double> lambda (m + 2 * sqrt_p_cos, m - sqrt_p_cos - root_three_sqrt_p_sin, m - sqrt_p_cos + root_three_sqrt_p_sin); |
| exchange_sort (lambda.x33, lambda.x22, lambda.x11); |
| return lambda; |
| } |
| //##################################################################### |
| // Function Fast_Eigenvectors_Double |
| //##################################################################### |
| template<class T> void SYMMETRIC_MATRIX_3X3<T>:: |
| Fast_Eigenvectors_Double (const DIAGONAL_MATRIX_3X3<double>& lambda, MATRIX_3X3<double>& eigenvectors, const double tolerance) const |
| { |
| double tiny = tolerance * max (abs (lambda.x11), abs (lambda.x33)); |
| SYMMETRIC_MATRIX_3X3<double> A (*this); |
| |
| if (lambda.x11 - lambda.x22 <= tiny) |
| { |
| if (lambda.x22 - lambda.x33 <= tiny) eigenvectors = MATRIX_3X3<double>::Identity_Matrix(); |
| else |
| { |
| VECTOR_3D<double> v3 = (A - lambda.x33).Cofactor_Matrix().Largest_Column_Normalized(); |
| VECTOR_3D<double> v2 = v3.Orthogonal_Vector().Normalized(); |
| eigenvectors = MATRIX_3X3<double> (VECTOR_3D<double>::Cross_Product (v2, v3), v2, v3); |
| } |
| } |
| else if (lambda.x22 - lambda.x33 <= tiny) |
| { |
| VECTOR_3D<double> v1 = (A - lambda.x11).Cofactor_Matrix().Largest_Column_Normalized(); |
| VECTOR_3D<double> v2 = v1.Orthogonal_Vector().Normalized(); |
| eigenvectors = MATRIX_3X3<double> (v1, v2, VECTOR_3D<double>::Cross_Product (v1, v2)); |
| } |
| else |
| { |
| VECTOR_3D<double> v1 = (A - lambda.x11).Cofactor_Matrix().Largest_Column_Normalized(); |
| VECTOR_3D<double> v3 = (A - lambda.x33).Cofactor_Matrix().Largest_Column_Normalized(); |
| eigenvectors = MATRIX_3X3<double> (v1, VECTOR_3D<double>::Cross_Product (v3, v1), v3); |
| } |
| } |
| //##################################################################### |
| // Function Fast_Solve_Eigenproblem_Double |
| //##################################################################### |
| template<class T> void SYMMETRIC_MATRIX_3X3<T>:: |
| Fast_Solve_Eigenproblem_Double (DIAGONAL_MATRIX_3X3<double>& eigenvalues, MATRIX_3X3<double>& eigenvectors, const double tolerance) const |
| { |
| eigenvalues = Fast_Eigenvalues_Double(); |
| Fast_Eigenvectors_Double (eigenvalues, eigenvectors, tolerance); |
| } |
| //##################################################################### |
| // Function Solve_Eigenproblem |
| //#################################################################### |
| template<class T> void SYMMETRIC_MATRIX_3X3<T>:: |
| Solve_Eigenproblem (DIAGONAL_MATRIX_3X3<T>& eigenvalues, MATRIX_3X3<T>& eigenvectors) const |
| { |
| eigenvectors = MATRIX_3X3<T>::Identity_Matrix(); |
| T a11 = x11, a12 = x21, a13 = x31, a22 = x22, a23 = x32, a33 = x33; |
| T v11 = 1, v12 = 0, v13 = 0, v21 = 0, v22 = 1, v23 = 0, v31 = 0, v32 = 0, v33 = 1; |
| int sweep; |
| |
| for (sweep = 1; sweep <= 50; sweep++) |
| { |
| T sum = (T) fabs (a12) + (T) fabs (a13) + (T) fabs (a23); |
| |
| if (sum == 0) break; |
| |
| T threshold = sweep < 4 ? (T) (1. / 45) * sum : 0; |
| Jacobi_Transform (sweep, threshold, a11, a12, a22, a13, a23, v11, v12, v21, v22, v31, v32); |
| Jacobi_Transform (sweep, threshold, a11, a13, a33, a12, a23, v11, v13, v21, v23, v31, v33); |
| Jacobi_Transform (sweep, threshold, a22, a23, a33, a12, a13, v12, v13, v22, v23, v32, v33); |
| } |
| |
| assert (sweep <= 50); |
| eigenvalues = DIAGONAL_MATRIX_3X3<T> (a11, a22, a33); |
| eigenvectors = MATRIX_3X3<T> (v11, v21, v31, v12, v22, v32, v13, v23, v33); |
| } |
| //##################################################################### |
| // Function Jacobi_Transform |
| //##################################################################### |
| template<class T> inline void SYMMETRIC_MATRIX_3X3<T>:: |
| Jacobi_Transform (const int sweep, const T threshold, T& app, T& apq, T& aqq, T& arp, T& arq, T& v1p, T& v1q, T& v2p, T& v2q, T& v3p, T& v3q) |
| { |
| T g = 100 * apq; |
| |
| if (sweep > 4 && app + g == app && aqq + g == aqq) apq = 0; |
| else if (fabs (apq) > threshold) |
| { |
| T h = aqq - app, t; |
| |
| if (h + g == h) t = apq / h; |
| else |
| { |
| T theta = (T).5 * h / apq; |
| t = 1 / (fabs (theta) + sqrt (1 + sqr (theta))); |
| |
| if (theta < 0) t = -t; |
| } |
| |
| T cosine = 1 / sqrt (1 + t * t), sine = t * cosine, tau = sine / (1 + cosine), da = t * apq; |
| app -= da; |
| aqq += da; |
| apq = 0; |
| T arp_tmp = arp - sine * (arq + tau * arp); |
| arq = arq + sine * (arp - tau * arq); |
| arp = arp_tmp; |
| T v1p_tmp = v1p - sine * (v1q + tau * v1p); |
| v1q = v1q + sine * (v1p - tau * v1q); |
| v1p = v1p_tmp; |
| T v2p_tmp = v2p - sine * (v2q + tau * v2p); |
| v2q = v2q + sine * (v2p - tau * v2q); |
| v2p = v2p_tmp; |
| T v3p_tmp = v3p - sine * (v3q + tau * v3p); |
| v3q = v3q + sine * (v3p - tau * v3q); |
| v3p = v3p_tmp; |
| } |
| } |
| //##################################################################### |
| template class SYMMETRIC_MATRIX_3X3<float>; |
| template class SYMMETRIC_MATRIX_3X3<double>; |