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
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
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
00066 const btMatrix3x3 delta = (u * (gamma - 2.0) + h.transpose() * inv_gamma) * 0.5;
00067
00068
00069 u += delta;
00070 h = u.inverse();
00071
00072
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
00082
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