Blender  V3.3
btPolarDecomposition.cpp
Go to the documentation of this file.
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 } // namespace
32 
33 btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations)
34  : m_tolerance(tolerance), m_maxIterations(maxIterations)
35 {
36 }
37 
39 {
40  // Use the 'u' and 'h' matrices for intermediate calculations
41  u = a;
42  h = a.inverse();
43 
44  for (unsigned int i = 0; i < m_maxIterations; ++i)
45  {
46  const btScalar h_1 = p1_norm(h);
47  const btScalar h_inf = pinf_norm(h);
48  const btScalar u_1 = p1_norm(u);
49  const btScalar u_inf = pinf_norm(u);
50 
51  const btScalar h_norm = h_1 * h_inf;
52  const btScalar u_norm = u_1 * u_inf;
53 
54  // The matrix is effectively singular so we cannot invert it
55  if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
56  break;
57 
58  const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
59  const btScalar inv_gamma = btScalar(1.0) / gamma;
60 
61  // Determine the delta to 'u'
62  const btMatrix3x3 delta = (u * (gamma - btScalar(2.0)) + h.transpose() * inv_gamma) * btScalar(0.5);
63 
64  // Update the matrices
65  u += delta;
66  h = u.inverse();
67 
68  // Check for convergence
69  if (p1_norm(delta) <= m_tolerance * u_1)
70  {
71  h = u.transpose() * a;
72  h = (h + h.transpose()) * 0.5;
73  return i;
74  }
75  }
76 
77  // The algorithm has failed to converge to the specified tolerance, but we
78  // want to make sure that the matrices returned are in the right form.
79  h = u.transpose() * a;
80  h = (h + h.transpose()) * 0.5;
81 
82  return m_maxIterations;
83 }
84 
86 {
87  return m_maxIterations;
88 }
89 
90 unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h)
91 {
92  static btPolarDecomposition polar;
93  return polar.decompose(a, u, h);
94 }
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
unsigned int polarDecompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y)
Definition: btScalar.h:521
btPolarDecomposition(btScalar tolerance=btScalar(0.0001), unsigned int maxIterations=16)
unsigned int decompose(const btMatrix3x3 &a, btMatrix3x3 &u, btMatrix3x3 &h) const
unsigned int maxIterations() const
static unsigned a[3]
Definition: RandGen.cpp:78