UnitTests/BulletUnitTests/TestPolarDecomposition.cpp

   1 #include "TestPolarDecomposition.h"
   2 #include "BulletSoftBody/btSoftBodyInternals.h"
   3 #include "LinearMath/btPolarDecomposition.h"
   4 #include "btCholeskyDecomposition.h"
   5
   6 namespace
   7 {
   8   const int MAX_ITERATIONS = btPolarDecomposition::DEFAULT_MAX_ITERATIONS;
   9   const btScalar TOLERANCE = btPolarDecomposition::DEFAULT_TOLERANCE;
  10 }
  11
  12 void TestPolarDecomposition::testZeroMatrix()
  13 {
  14   const btMatrix3x3 A(0,0,0,0,0,0,0,0,0);
  15   const int iterations = PolarDecompose(A, U, H);
  16
  17   // The decomposition should fail because the zero matrix is singular
  18   CPPUNIT_ASSERT(iterations == MAX_ITERATIONS);
  19 }
  20
  21 void TestPolarDecomposition::testSingularMatrix()
  22 {
  23   const btMatrix3x3 A(1,0,0,1,0,0,0,0,1);
  24
  25   const int iterations = PolarDecompose(A, U, H);
  26
  27   // The cdecomposition should fail because the matrix is singular.
  28   CPPUNIT_ASSERT(iterations == MAX_ITERATIONS);
  29 }
  30
  31 void TestPolarDecomposition::testPoorlyConditionedMatrix()
  32 {
  33   const btScalar e = btScalar(TOLERANCE);
  34   const btMatrix3x3 A(1,0,0,1,e,0,0,0,1);
  35
  36   const int iterations = PolarDecompose(A, U, H);
  37
  38   // The decomposition should succeed, however, the matrix is poorly
  39   // conditioned, i.e. as 'e' approaches zero, the matrix approaches a singular
  40   // matrix (no inverse).
  41   CPPUNIT_ASSERT(iterations < MAX_ITERATIONS);
  42   CPPUNIT_ASSERT(equal(A, U * H));
  43   CPPUNIT_ASSERT(isOrthogonal(U));
  44   CPPUNIT_ASSERT(isSymmetric(H));
  45   CPPUNIT_ASSERT(isPositiveDefinite(H));
  46 }
  47
  48 void TestPolarDecomposition::testIdentityMatrix()
  49 {
  50   const btMatrix3x3 A = I;
  51   const int iterations = PolarDecompose(A, U, H);
  52
  53   // The identity is a special case. The decomposition should succeed and both
  54   // the U and H matrices should be equal to the identity.
  55   CPPUNIT_ASSERT(iterations < MAX_ITERATIONS);
  56   CPPUNIT_ASSERT(equal(A, U * H));
  57   CPPUNIT_ASSERT(equal(U, I));
  58   CPPUNIT_ASSERT(equal(H, I));
  59 }
  60
  61 void TestPolarDecomposition::testNonSingularMatrix()
  62 {
  63   const btMatrix3x3 A(4, 6, 6, 9, 2, 0, 1, 6, 0);
  64   const int iterations = PolarDecompose(A, U, H);
  65
  66   // The decomposition should succeed so that A = U*H, where U is orthogonal and
  67   // H is symmetric and positive definite.
  68   CPPUNIT_ASSERT(iterations < MAX_ITERATIONS);
  69   CPPUNIT_ASSERT(equal(A, U * H));
  70   CPPUNIT_ASSERT(isOrthogonal(U));
  71   CPPUNIT_ASSERT(isSymmetric(H));
  72   CPPUNIT_ASSERT(isPositiveDefinite(H));
  73 }
  74
  75 bool TestPolarDecomposition::equal(const btMatrix3x3& A, const btMatrix3x3& B) const
  76 {
  77   for (unsigned int i = 0; i < 3; ++i)
  78     for (unsigned int j = 0; j < 3; ++j)
  79       if (btFabs(A[i][j] - B[i][j]) > TOLERANCE)
  80         return false;
  81
  82   return true;
  83 }
  84
  85 bool TestPolarDecomposition::isOrthogonal(const btMatrix3x3& A) const
  86 {
  87   return equal(A.transpose() * A, I);
  88 }
  89
  90 bool TestPolarDecomposition::isPositiveDefinite(const btMatrix3x3& A) const
  91 {
  92   static btMatrix3x3 storage;
  93   return (0 == choleskyDecompose(A, storage));
  94 }
  95
  96 bool TestPolarDecomposition::isSymmetric(const btMatrix3x3& A) const
  97 {
  98   return
  99     (btFabs(A[0][1] - A[1][0]) < TOLERANCE) &&
 100     (btFabs(A[0][2] - A[2][0]) < TOLERANCE) &&
 101     (btFabs(A[1][2] - A[2][1]) < TOLERANCE);
 102 }
 103