16 #ifndef SURGSIM_MATH_LINEARSOLVEANDINVERSE_INL_H 17 #define SURGSIM_MATH_LINEARSOLVEANDINVERSE_INL_H 27 template <
size_t BlockSize>
28 const Eigen::Block<const Matrix, BlockSize, BlockSize>
31 return A.block<BlockSize, BlockSize>(BlockSize * i, BlockSize * (i - 1));
34 template <
size_t BlockSize>
35 const Eigen::Block<const Matrix, BlockSize, BlockSize>
38 return A.block<BlockSize, BlockSize>(BlockSize * i, BlockSize * i);
41 template <
size_t BlockSize>
42 const Eigen::Block<const Matrix, BlockSize, BlockSize>
45 return A.block<BlockSize, BlockSize>(BlockSize * i, BlockSize * (i + 1));
48 template <
size_t BlockSize>
53 SURGSIM_ASSERT(inverse !=
nullptr) <<
"Null inverse matrix pointer";
56 "Cannot inverse a non square tri-diagonal block matrix ("<< A.rows() <<
" x "<< A.cols() <<
")";
58 const size_t size = A.rows();
59 const size_t numBlocks = size / BlockSize;
62 "Bad tri-diagonal block matrix structure, size = " << size <<
" block size = " << BlockSize <<
63 " and the number of blocks are " << numBlocks;
67 if (size <= 4 || numBlocks == static_cast<size_t>(1))
69 *inverse = A.inverse();
73 if (inverse->rows() < 0 ||
static_cast<size_t>(inverse->rows()) != size
74 || inverse->cols() < 0 ||
static_cast<size_t>(inverse->cols()) != size)
76 inverse->resize(size, size);
79 m_Bi_AiDiminus1_inv.resize(numBlocks);
80 m_Di.resize(numBlocks - 1);
81 m_Ei.resize(numBlocks);
86 m_Bi_AiDiminus1_inv[0] = Bi(A, 0).inverse();
87 m_Di[0] = m_Bi_AiDiminus1_inv[0] * (-minusCi(A, 0));
90 for(
size_t i = 1; i < numBlocks - 1; ++i)
92 m_Bi_AiDiminus1_inv[i] = (Bi(A, i) - (-minusAi(A, i)) * m_Di[i - 1]).inverse();
93 m_Di[i] = m_Bi_AiDiminus1_inv[i] * (-minusCi(A, i));
97 m_Bi_AiDiminus1_inv[numBlocks - 1] =
98 (Bi(A, numBlocks - 1) - (-minusAi(A, numBlocks - 1)) * m_Di[numBlocks - 2]).inverse();
103 m_Ei[numBlocks - 1] = Bi(A, numBlocks - 1).inverse() * (-minusAi(A, numBlocks - 1));
104 for(
size_t i = numBlocks - 2; i > 0; --i)
106 m_Ei[i] = (Bi(A, i) - (-minusCi(A, i)) * m_Ei[i + 1]).inverse() * (-minusAi(A, i));
111 for(
size_t i = 0; i < numBlocks - 1; ++i)
113 inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * i) =
114 (Block::Identity() - m_Di[i] * m_Ei[i + 1]).inverse() * m_Bi_AiDiminus1_inv[i];
117 inverse->block<BlockSize, BlockSize>(BlockSize * (numBlocks - 1), BlockSize * (numBlocks - 1)) =
118 m_Bi_AiDiminus1_inv[numBlocks - 1];
125 for(
size_t j = 1; j < numBlocks; ++j)
127 for(
size_t i = j; i > 0; --i)
129 inverse->block<BlockSize, BlockSize>(BlockSize * (i - 1), BlockSize * j) =
130 m_Di[i - 1] * inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * j);
131 inverse->block<BlockSize, BlockSize>(BlockSize * j, BlockSize * (i - 1)) =
132 inverse->block<BlockSize, BlockSize>(BlockSize * (i - 1), BlockSize * j).transpose();
138 for(
int j = 0; j < static_cast<int>(numBlocks); ++j)
140 for(
int i = j - 1; i >= 0; --i)
142 inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * j) =
143 m_Di[i] * inverse->block<BlockSize, BlockSize>(BlockSize * (i + 1), BlockSize * j);
145 for(
int i = j + 1; i < static_cast<int>(numBlocks); ++i)
147 inverse->block<BlockSize, BlockSize>(BlockSize * i, BlockSize * j) =
148 m_Ei[i] * inverse->block<BlockSize, BlockSize>(BlockSize * (i - 1), BlockSize * j);
154 template <
size_t BlockSize>
157 inverseTriDiagonalBlock(matrix, &m_inverse);
160 template <
size_t BlockSize>
163 return m_inverse * b;
166 template <
size_t BlockSize>
172 template <
size_t BlockSize>
175 inverseTriDiagonalBlock(matrix, &m_inverse,
true);
182 #endif // SURGSIM_MATH_LINEARSOLVEANDINVERSE_INL_H Definition: CompoundShapeToGraphics.cpp:29
const Eigen::Block< const Matrix, BlockSize, BlockSize > minusAi(const SurgSim::Math::Matrix &A, size_t i) const
Gets a lower-diagonal block element (named -Ai in the algorithm)
Definition: LinearSolveAndInverse-inl.h:29
Matrix getInverse() override
Definition: LinearSolveAndInverse-inl.h:167
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dynamic size matrix.
Definition: Matrix.h:65
const Eigen::Block< const Matrix, BlockSize, BlockSize > Bi(const SurgSim::Math::Matrix &A, size_t i) const
Gets a diagonal block element (named Bi in the algorithm)
Definition: LinearSolveAndInverse-inl.h:36
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A dynamic size column vector.
Definition: Vector.h:68
The header that provides the assertion API.
void inverseTriDiagonalBlock(const SurgSim::Math::Matrix &A, SurgSim::Math::Matrix *inv, bool isSymmetric=false)
Computes the inverse matrix.
Definition: LinearSolveAndInverse-inl.h:49
Vector solve(const Vector &b) override
Solve the linear system (matrix.x=b) using the matrix provided by the latest setMatrix call...
Definition: LinearSolveAndInverse-inl.h:161
const Eigen::Block< const Matrix, BlockSize, BlockSize > minusCi(const SurgSim::Math::Matrix &A, size_t i) const
Gets a upper-diagonal block element (named -Ci in the algorithm)
Definition: LinearSolveAndInverse-inl.h:43
void setMatrix(const Matrix &matrix) override
Set the linear solver matrix.
Definition: LinearSolveAndInverse-inl.h:155
void setMatrix(const Matrix &matrix) override
Set the linear solver matrix.
Definition: LinearSolveAndInverse-inl.h:173