Skip to content

Commit

Permalink
experement with psd factorization, but can't do it since that block m…
Browse files Browse the repository at this point in the history 
…atrices aren't PDS
  • Loading branch information
@JulioJerez
JulioJerez committed Nov 4, 2024
1 parent e8599a7 commit e1841a3
Show file tree
Hide file tree
Showing 5 changed files with 129 additions and 23 deletions.
Binary file modified newton-4.00/applications/media/tippeTop.fbx
View file
Binary file not shown.
Binary file added newton-4.00/applications/media/tippeTopCap.fbx
View file
Binary file not shown.
142 changes: 119 additions & 23 deletions newton-4.00/sdk/dCore/ndSpatialMatrix.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,13 +24,69 @@
#include "ndGeneralMatrix.h"
#include "ndSpatialMatrix.h"

ndSpatialMatrix ndSpatialMatrix::CholeskyFactorization(ndInt32 rows) const
{
#ifdef _DEBUG
for (ndInt32 i = 0; i < rows; ++i)
{
ndAssert(m_rows[i][i] > ndFloat64(0.0f));
for (ndInt32 j = i + 1; j < rows; ++j)
{
ndFloat64 a = m_rows[i][j];
ndFloat64 b = m_rows[j][i];
ndAssert(ndAbs(a - b) < ndFloat64(1.0e-6f));
}
}
#endif

ndSpatialMatrix psdMatrix(*this);
psdMatrix[0][0] = ndFloat64(sqrt(psdMatrix[0][0]));
for (ndInt32 j = 1; j < rows; ++j)
{
ndFloat64 s = ndFloat64(0.0f);
for (ndInt32 k = 0; k < j; ++k)
{
s += psdMatrix[j][k] * psdMatrix[j][k];
}
ndFloat64 d = psdMatrix[j][j];
psdMatrix[j][j] = ndFloat64(sqrt(d - s));
for (ndInt32 i = j + 1; i < rows; ++i)
{
s = ndFloat64(0.0f);
for (ndInt32 k = 0; k < j; ++k)
{
s += psdMatrix[i][k] * psdMatrix[j][k];
}
psdMatrix[i][j] = (psdMatrix[i][j] - s) / psdMatrix[j][j];
psdMatrix[j][i] = ndFloat64(0.0f);
}
}

#ifdef _DEBUG
ndSpatialMatrix psdMatrix1(*this);
ndCholeskyFactorization(rows, 8, &psdMatrix1[0][0]);

for (ndInt32 i = 0; i < rows; ++i)
{
for (ndInt32 j = 0; j <= i; ++j)
{
ndFloat64 a = psdMatrix.m_rows[i][j];
ndFloat64 b = psdMatrix.m_rows[i][j];
ndAssert(ndAbs(a - b) < ndFloat64(1.0e-6f));
}
}
#endif

return psdMatrix;
}

ndSpatialMatrix ndSpatialMatrix::Inverse(ndInt32 rows) const
{
ndSpatialMatrix tmp;
ndSpatialMatrix inv;
for (ndInt32 i = 0; i < rows; ++i)
{
tmp[i] = (*this)[i];
tmp[i] = m_rows[i];
inv[i] = ndSpatialVector::m_zero;
inv[i][i] = ndFloat32(1.0f);
}
Expand All @@ -51,17 +107,6 @@ ndSpatialMatrix ndSpatialMatrix::Inverse(ndInt32 rows) const
}
ndAssert(pivot > ndFloat32(0.0f));
ndAssert((pivot > ndFloat32(1.0e-6f)) || (ndConditionNumber(rows, 6, (ndFloat64*)&m_rows[0]) < ndFloat32(1.0e5f)));
//if (!((pivot > ndFloat32(1.0e-6f)) || (ndConditionNumber(rows, 6, (dFloat64*)&m_rows[0]) < ndFloat32(1.0e5f))))
//{
// for (ndInt32 m = 0; m < rows; m++) {
// for (ndInt32 n = 0; n < rows; n++) {
// dTrace(("%f ", m_rows[m][n]));
// }
// dTrace(("\n"));
// }
// ndAssert(0);
//}

if (permute != i)
{
for (ndInt32 j = 0; j < rows; ++j)
Expand Down Expand Up @@ -105,29 +150,80 @@ ndSpatialMatrix ndSpatialMatrix::Inverse(ndInt32 rows) const
}
}


#ifdef _DEBUG
for (ndInt32 i = 0; i < rows; ++i)
for (ndInt32 i = 0; i < rows; ++i)
{
for (ndInt32 j = 0; j < rows; ++j)
ndFloat64 sum = ndFloat32(0.0f);
for (ndInt32 k = 0; k < rows; ++k)
{
tmp[i][j] = m_rows[j][i];
sum += m_rows[i][k] * inv[k][i];
}
ndAssert(ndAbs(sum - ndFloat64(1.0f)) < ndFloat64(1.0e-6f));

for (ndInt32 j = i + 1; j < rows; ++j)
{
sum = ndFloat32(0.0f);
for (ndInt32 k = 0; k < rows; ++k)
{
sum += m_rows[i][k] * inv[k][j];
}
ndAssert(ndAbs(sum) < ndFloat64(1.0e-6f));
}
}
#endif

for (ndInt32 i = 0; i < rows; ++i)
return inv;
}


ndSpatialMatrix ndSpatialMatrix::InversePositiveDefinite(ndInt32 rows) const
{
const ndSpatialMatrix lower(CholeskyFactorization(rows));
const ndSpatialMatrix lowerInverse(lower.Inverse(rows));

ndSpatialMatrix inv;
for (ndInt32 i = 0; i < rows; ++i)
{
ndSpatialVector v(inv.VectorTimeMatrix(tmp[i], rows));
ndAssert(ndAbs(v[i] - ndFloat64(1.0f)) < ndFloat64(1.0e-6f));
for (ndInt32 j = 0; j < rows; ++j)
ndFloat64 sum = ndFloat32(0.0f);
for (ndInt32 k = i; k < rows; ++k)
{
if (j != i)
sum += lowerInverse[k][i] * lowerInverse[k][i];
}
inv[i][i] = sum;

for (ndInt32 j = i + 1; j < rows; ++j)
{
sum = ndFloat32(0.0f);
for (ndInt32 k = j; k < rows; ++k)
{
sum += lowerInverse[k][i] * lowerInverse[k][j];
}
inv[i][j] = sum;
inv[j][i] = sum;
}
}

#ifdef _DEBUG
for (ndInt32 i = 0; i < rows; ++i)
{
ndFloat64 sum = ndFloat32(0.0f);
for (ndInt32 k = 0; k < rows; ++k)
{
sum += m_rows[i][k] * inv[k][i];
}
ndAssert(ndAbs(sum - ndFloat64(1.0f)) < ndFloat64(1.0e-6f));

for (ndInt32 j = i + 1; j < rows; ++j)
{
sum = ndFloat32(0.0f);
for (ndInt32 k = 0; k < rows; ++k)
{
ndAssert(ndAbs(v[j]) < ndFloat64(1.0e-6f));
sum += m_rows[i][k] * inv[k][j];
}
ndAssert(ndAbs(sum) < ndFloat64(1.0e-6f));
}
}
#endif

return inv;
}
}
2 changes: 2 additions & 0 deletions newton-4.00/sdk/dCore/ndSpatialMatrix.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,8 @@ class ndSpatialMatrix
}

D_CORE_API ndSpatialMatrix Inverse(ndInt32 rows) const;
D_CORE_API ndSpatialMatrix CholeskyFactorization(ndInt32 rows) const;
D_CORE_API ndSpatialMatrix InversePositiveDefinite(ndInt32 rows) const;

inline ndSpatialVector VectorTimeMatrix(const ndSpatialVector& jacobian) const
{
Expand Down
8 changes: 8 additions & 0 deletions newton-4.00/sdk/dNewton/ndSkeletonContainer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,6 +196,9 @@ void ndSkeletonContainer::ndNode::CalculateJointDiagonal(const ndSpatialMatrix*
}

ndSpatialMatrix& jointInvMass = m_data.m_joint.m_invMass;

// can't use CholeskyFactorization because neither bodyMass or jointMass
// are guarantee to be PSD
jointInvMass = jointMass.Inverse(m_dof);
}

Expand Down Expand Up @@ -266,6 +269,11 @@ ndInt32 ndSkeletonContainer::ndNode::Factorize(const ndLeftHandSide* const leftH
{
CalculateBodyDiagonal(child, bodyMassArray, jointMassArray);
}

// can't use CholeskyFactorization because neither bodyMass or jointMass
// are guarantee to be PSD
//ndSpatialMatrix xxx0(bodyMass.Inverse(6));
//ndSpatialMatrix xxx1(bodyMass.InversePositiveDefinite(6));
bodyInvMass = bodyMass.Inverse(6);
}
else
Expand Down

0 comments on commit e1841a3

Please sign in to comment.