104 lines
3.0 KiB
C++
104 lines
3.0 KiB
C++
#include "TestPolarDecomposition.h"
|
|
#include "BulletSoftBody/btSoftBodyInternals.h"
|
|
#include "LinearMath/btPolarDecomposition.h"
|
|
#include "btCholeskyDecomposition.h"
|
|
|
|
namespace
|
|
{
|
|
const int MAX_ITERATIONS = btPolarDecomposition::DEFAULT_MAX_ITERATIONS;
|
|
const btScalar TOLERANCE = btPolarDecomposition::DEFAULT_TOLERANCE;
|
|
}
|
|
|
|
void TestPolarDecomposition::testZeroMatrix()
|
|
{
|
|
const btMatrix3x3 A(0,0,0,0,0,0,0,0,0);
|
|
const int iterations = PolarDecompose(A, U, H);
|
|
|
|
// The decomposition should fail because the zero matrix is singular
|
|
CPPUNIT_ASSERT(iterations == MAX_ITERATIONS);
|
|
}
|
|
|
|
void TestPolarDecomposition::testSingularMatrix()
|
|
{
|
|
const btMatrix3x3 A(1,0,0,1,0,0,0,0,1);
|
|
|
|
const int iterations = PolarDecompose(A, U, H);
|
|
|
|
// The cdecomposition should fail because the matrix is singular.
|
|
CPPUNIT_ASSERT(iterations == MAX_ITERATIONS);
|
|
}
|
|
|
|
void TestPolarDecomposition::testPoorlyConditionedMatrix()
|
|
{
|
|
const btScalar e = btScalar(TOLERANCE);
|
|
const btMatrix3x3 A(1,0,0,1,e,0,0,0,1);
|
|
|
|
const int iterations = PolarDecompose(A, U, H);
|
|
|
|
// The decomposition should succeed, however, the matrix is poorly
|
|
// conditioned, i.e. as 'e' approaches zero, the matrix approaches a singular
|
|
// matrix (no inverse).
|
|
CPPUNIT_ASSERT(iterations < MAX_ITERATIONS);
|
|
CPPUNIT_ASSERT(equal(A, U * H));
|
|
CPPUNIT_ASSERT(isOrthogonal(U));
|
|
CPPUNIT_ASSERT(isSymmetric(H));
|
|
CPPUNIT_ASSERT(isPositiveDefinite(H));
|
|
}
|
|
|
|
void TestPolarDecomposition::testIdentityMatrix()
|
|
{
|
|
const btMatrix3x3 A = I;
|
|
const int iterations = PolarDecompose(A, U, H);
|
|
|
|
// The identity is a special case. The decomposition should succeed and both
|
|
// the U and H matrices should be equal to the identity.
|
|
CPPUNIT_ASSERT(iterations < MAX_ITERATIONS);
|
|
CPPUNIT_ASSERT(equal(A, U * H));
|
|
CPPUNIT_ASSERT(equal(U, I));
|
|
CPPUNIT_ASSERT(equal(H, I));
|
|
}
|
|
|
|
void TestPolarDecomposition::testNonSingularMatrix()
|
|
{
|
|
const btMatrix3x3 A(4, 6, 6, 9, 2, 0, 1, 6, 0);
|
|
const int iterations = PolarDecompose(A, U, H);
|
|
|
|
// The decomposition should succeed so that A = U*H, where U is orthogonal and
|
|
// H is symmetric and positive definite.
|
|
CPPUNIT_ASSERT(iterations < MAX_ITERATIONS);
|
|
CPPUNIT_ASSERT(equal(A, U * H));
|
|
CPPUNIT_ASSERT(isOrthogonal(U));
|
|
CPPUNIT_ASSERT(isSymmetric(H));
|
|
CPPUNIT_ASSERT(isPositiveDefinite(H));
|
|
}
|
|
|
|
bool TestPolarDecomposition::equal(const btMatrix3x3& A, const btMatrix3x3& B) const
|
|
{
|
|
for (unsigned int i = 0; i < 3; ++i)
|
|
for (unsigned int j = 0; j < 3; ++j)
|
|
if (btFabs(A[i][j] - B[i][j]) > TOLERANCE)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool TestPolarDecomposition::isOrthogonal(const btMatrix3x3& A) const
|
|
{
|
|
return equal(A.transpose() * A, I);
|
|
}
|
|
|
|
bool TestPolarDecomposition::isPositiveDefinite(const btMatrix3x3& A) const
|
|
{
|
|
static btMatrix3x3 storage;
|
|
return (0 == choleskyDecompose(A, storage));
|
|
}
|
|
|
|
bool TestPolarDecomposition::isSymmetric(const btMatrix3x3& A) const
|
|
{
|
|
return
|
|
(btFabs(A[0][1] - A[1][0]) < TOLERANCE) &&
|
|
(btFabs(A[0][2] - A[2][0]) < TOLERANCE) &&
|
|
(btFabs(A[1][2] - A[2][1]) < TOLERANCE);
|
|
}
|
|
|