btPolarDecomposition.cpp

Go to the documentation of this file.
00001 #include "btPolarDecomposition.h"
00002 #include "btMinMax.h"
00003 
00004 namespace
00005 {
00006   btScalar abs_column_sum(const btMatrix3x3& a, int i)
00007   {
00008     return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]);
00009   }
00010 
00011   btScalar abs_row_sum(const btMatrix3x3& a, int i)
00012   {
00013     return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]);
00014   }
00015 
00016   btScalar p1_norm(const btMatrix3x3& a)
00017   {
00018     const btScalar sum0 = abs_column_sum(a,0);
00019     const btScalar sum1 = abs_column_sum(a,1);
00020     const btScalar sum2 = abs_column_sum(a,2);
00021     return btMax(btMax(sum0, sum1), sum2);
00022   }
00023 
00024   btScalar pinf_norm(const btMatrix3x3& a)
00025   {
00026     const btScalar sum0 = abs_row_sum(a,0);
00027     const btScalar sum1 = abs_row_sum(a,1);
00028     const btScalar sum2 = abs_row_sum(a,2);
00029     return btMax(btMax(sum0, sum1), sum2);
00030   }
00031 }
00032 
00033 const btScalar btPolarDecomposition::DEFAULT_TOLERANCE = btScalar(0.0001);
00034 const unsigned int btPolarDecomposition::DEFAULT_MAX_ITERATIONS = 16;
00035 
00036 btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations)
00037 : m_tolerance(tolerance)
00038 , m_maxIterations(maxIterations)
00039 {
00040 }
00041 
00042 unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const
00043 {
00044   // Use the 'u' and 'h' matrices for intermediate calculations
00045   u = a;
00046   h = a.inverse();
00047 
00048   for (unsigned int i = 0; i < m_maxIterations; ++i)
00049   {
00050     const btScalar h_1 = p1_norm(h);
00051     const btScalar h_inf = pinf_norm(h);
00052     const btScalar u_1 = p1_norm(u);
00053     const btScalar u_inf = pinf_norm(u);
00054 
00055     const btScalar h_norm = h_1 * h_inf;
00056     const btScalar u_norm = u_1 * u_inf;
00057 
00058     // The matrix is effectively singular so we cannot invert it
00059     if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
00060       break;
00061 
00062     const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
00063     const btScalar inv_gamma = 1.0 / gamma;
00064 
00065     // Determine the delta to 'u'
00066     const btMatrix3x3 delta = (u * (gamma - 2.0) + h.transpose() * inv_gamma) * 0.5;
00067 
00068     // Update the matrices
00069     u += delta;
00070     h = u.inverse();
00071 
00072     // Check for convergence
00073     if (p1_norm(delta) <= m_tolerance * u_1)
00074     {
00075       h = u.transpose() * a;
00076       h = (h + h.transpose()) * 0.5;
00077       return i;
00078     }
00079   }
00080 
00081   // The algorithm has failed to converge to the specified tolerance, but we
00082   // want to make sure that the matrices returned are in the right form.
00083   h = u.transpose() * a;
00084   h = (h + h.transpose()) * 0.5;
00085 
00086   return m_maxIterations;
00087 }
00088 
00089 unsigned int btPolarDecomposition::maxIterations() const
00090 {
00091   return m_maxIterations;
00092 }
00093 
00094 unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h)
00095 {
00096   static btPolarDecomposition polar;
00097   return polar.decompose(a, u, h);
00098 }
00099