1 #include "btPolarDecomposition.h" 2 #include "btMinMax.h" 3 4 namespace 5 { 6 btScalar abs_column_sum(const btMatrix3x3& a, int i) 7 { 8 return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]); 9 } 10 11 btScalar abs_row_sum(const btMatrix3x3& a, int i) 12 { 13 return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]); 14 } 15 16 btScalar p1_norm(const btMatrix3x3& a) 17 { 18 const btScalar sum0 = abs_column_sum(a,0); 19 const btScalar sum1 = abs_column_sum(a,1); 20 const btScalar sum2 = abs_column_sum(a,2); 21 return btMax(btMax(sum0, sum1), sum2); 22 } 23 24 btScalar pinf_norm(const btMatrix3x3& a) 25 { 26 const btScalar sum0 = abs_row_sum(a,0); 27 const btScalar sum1 = abs_row_sum(a,1); 28 const btScalar sum2 = abs_row_sum(a,2); 29 return btMax(btMax(sum0, sum1), sum2); 30 } 31 } 32 33 const btScalar btPolarDecomposition::DEFAULT_TOLERANCE = btScalar(0.0001); 34 const unsigned int btPolarDecomposition::DEFAULT_MAX_ITERATIONS = 16; 35 36 btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations) 37 : m_tolerance(tolerance) 38 , m_maxIterations(maxIterations) 39 { 40 } 41 42 unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const 43 { 44 // Use the 'u' and 'h' matrices for intermediate calculations 45 u = a; 46 h = a.inverse(); 47 48 for (unsigned int i = 0; i < m_maxIterations; ++i) 49 { 50 const btScalar h_1 = p1_norm(h); 51 const btScalar h_inf = pinf_norm(h); 52 const btScalar u_1 = p1_norm(u); 53 const btScalar u_inf = pinf_norm(u); 54 55 const btScalar h_norm = h_1 * h_inf; 56 const btScalar u_norm = u_1 * u_inf; 57 58 // The matrix is effectively singular so we cannot invert it 59 if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm)) 60 break; 61 62 const btScalar gamma = btPow(h_norm / u_norm, 0.25f); 63 const btScalar inv_gamma = btScalar(1.0) / gamma; 64 65 // Determine the delta to 'u' 66 const btMatrix3x3 delta = (u * (gamma - btScalar(2.0)) + h.transpose() * inv_gamma) * btScalar(0.5); 67 68 // Update the matrices 69 u += delta; 70 h = u.inverse(); 71 72 // Check for convergence 73 if (p1_norm(delta) <= m_tolerance * u_1) 74 { 75 h = u.transpose() * a; 76 h = (h + h.transpose()) * 0.5; 77 return i; 78 } 79 } 80 81 // The algorithm has failed to converge to the specified tolerance, but we 82 // want to make sure that the matrices returned are in the right form. 83 h = u.transpose() * a; 84 h = (h + h.transpose()) * 0.5; 85 86 return m_maxIterations; 87 } 88 89 unsigned int btPolarDecomposition::maxIterations() const 90 { 91 return m_maxIterations; 92 } 93 94 unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) 95 { 96 static btPolarDecomposition polar; 97 return polar.decompose(a, u, h); 98 } 99 100
