19 #ifndef SURGSIM_MATH_SPARSEMATRIX_H
20 #define SURGSIM_MATH_SPARSEMATRIX_H
22 #include <Eigen/Sparse>
38 template <
class DerivedSub,
class SparseType,
40 ((SparseType::Flags & Eigen::RowMajorBit) == Eigen::RowMajorBit) ? Eigen::RowMajor : Eigen::ColMajor >
44 typedef typename SparseType::Scalar
T;
45 typedef typename SparseType::Index
Index;
53 void assign(
T* ptr,
size_t start,
size_t n,
Index m,
const DerivedSub& subMatrix,
size_t colRowId) {}
66 void add(
T* ptr,
size_t start,
size_t n,
size_t m,
const DerivedSub& subMatrix,
size_t colRowId) {}
71 void add(
T* ptr,
const T& value) {}
75 template <
class DerivedSub,
class SparseType>
79 typedef typename SparseType::Scalar
T;
80 typedef typename SparseType::Index
Index;
82 void assign(
T* ptr,
size_t start,
size_t n,
size_t m,
const DerivedSub& subMatrix,
size_t colId)
84 typedef Eigen::Matrix < T, Eigen::Dynamic, 1, Eigen::DontAlign | Eigen::ColMajor > ColVector;
85 typedef typename Eigen::Matrix < T, Eigen::Dynamic, 1, Eigen::DontAlign | Eigen::ColMajor >::Index IndexVector;
86 typedef typename DerivedSub::Index IndexSub;
90 Eigen::Map<ColVector>(&ptr[start], static_cast<IndexVector>(n)).operator = (
91 subMatrix.col(static_cast<IndexSub>(colId)).segment(0, static_cast<IndexSub>(n)));
99 void add(
T* ptr,
size_t start,
size_t n,
size_t m,
const DerivedSub& subMatrix,
size_t colId)
101 typedef Eigen::Matrix < T, Eigen::Dynamic, 1, Eigen::DontAlign | Eigen::ColMajor > ColVector;
102 typedef typename Eigen::Matrix < T, Eigen::Dynamic, 1, Eigen::DontAlign | Eigen::ColMajor >::Index IndexVector;
103 typedef typename DerivedSub::Index IndexSub;
107 Eigen::Map<ColVector>(&ptr[start], static_cast<IndexVector>(n)).operator += (
108 subMatrix.col(static_cast<IndexSub>(colId)).segment(0, static_cast<IndexSub>(n)));
118 template <
class DerivedSub,
class SparseType>
122 typedef typename SparseType::Scalar
T;
123 typedef typename SparseType::Index
Index;
125 void assign(
T* ptr,
size_t start,
size_t n,
size_t m,
const DerivedSub& subMatrix,
size_t rowId)
127 typedef Eigen::Matrix < T, 1, Eigen::Dynamic, Eigen::DontAlign | Eigen::RowMajor > RowVector;
128 typedef typename Eigen::Matrix < T, 1, Eigen::Dynamic, Eigen::DontAlign | Eigen::RowMajor >::Index IndexVector;
129 typedef typename DerivedSub::Index IndexSub;
133 Eigen::Map<RowVector>(&ptr[start], static_cast<IndexVector>(m)).operator = (
134 subMatrix.row(static_cast<IndexSub>(rowId)).segment(0, static_cast<IndexSub>(m)));
142 void add(
T* ptr,
size_t start,
size_t n,
size_t m,
const DerivedSub& subMatrix,
size_t rowId)
144 typedef Eigen::Matrix < T, 1, Eigen::Dynamic, Eigen::DontAlign | Eigen::RowMajor > RowVector;
145 typedef typename Eigen::Matrix < T, 1, Eigen::Dynamic, Eigen::DontAlign | Eigen::RowMajor >::Index IndexVector;
146 typedef typename DerivedSub::Index IndexSub;
150 Eigen::Map<RowVector>(&ptr[start], static_cast<IndexVector>(m)).operator += (
151 subMatrix.row(static_cast<IndexSub>(rowId)).segment(0, static_cast<IndexSub>(m)));
184 template <
int Opt,
typename Index>
185 void blockWithSearch(
const Eigen::Ref<const Matrix>& subMatrix,
size_t rowStart,
size_t columnStart,
size_t n,
size_t m,
186 Eigen::SparseMatrix<double, Opt, Index>* matrix,
188 (
double*,
size_t,
size_t,
size_t,
const Matrix&,
size_t))
191 SURGSIM_ASSERT(
nullptr != matrix) <<
"Invalid recipient matrix, nullptr found";
193 SURGSIM_ASSERT(subMatrix.rows() >= static_cast<Matrix::Index>(n)) <<
"subMatrix doesn't have enough rows";
194 SURGSIM_ASSERT(subMatrix.cols() >= static_cast<Matrix::Index>(m)) <<
"subMatrix doesn't have enough columns";
196 SURGSIM_ASSERT(matrix->rows() >= static_cast<Index>(rowStart + n)) <<
"The block is out of range in matrix";
197 SURGSIM_ASSERT(matrix->cols() >= static_cast<Index>(columnStart + m)) <<
"The block is out of range in matrix";
198 SURGSIM_ASSERT(matrix->valuePtr() !=
nullptr) <<
"The matrix is not initialized correctly, null pointer to values";
200 double* ptr = matrix->valuePtr();
201 const Index* innerIndices = matrix->innerIndexPtr();
202 const Index* outerIndices = matrix->outerIndexPtr();
204 Index outerStart = static_cast<Index>(Opt == Eigen::ColMajor ? columnStart : rowStart);
205 Index innerStart = static_cast<Index>(Opt == Eigen::ColMajor ? rowStart : columnStart);
206 Index outerSize = static_cast<Index>(Opt == Eigen::ColMajor ? m : n);
207 Index innerSize = static_cast<Index>(Opt == Eigen::ColMajor ? n : m);
209 for (Index outerLoop = 0; outerLoop < outerSize; ++outerLoop)
213 const Index innerStartIdInCurrentOuter = outerIndices[outerStart + outerLoop];
214 const Index innerStartIdInNextOuter = outerIndices[outerStart + outerLoop + 1];
217 Index innerFirstElement;
218 if (innerIndices[innerStartIdInCurrentOuter] == innerStart)
220 innerFirstElement = innerStartIdInCurrentOuter;
224 innerFirstElement = static_cast<Index>(matrix->data().searchLowerIndex(
225 innerStartIdInCurrentOuter, innerStartIdInNextOuter - 1, innerStart));
230 "matrix is missing an element of the block (1st element on a row/column)";
234 SURGSIM_ASSERT(static_cast<Index>(innerSize) <= innerStartIdInNextOuter - innerFirstElement) <<
235 "matrix is missing elements of the block (but not the 1st element on a row/column)";
238 SURGSIM_ASSERT(innerStart + static_cast<Index>(innerSize) - 1 == \
239 innerIndices[innerFirstElement + static_cast<Index>(innerSize) - 1]) <<
240 "The last element of the block does not have the expected index. " <<
241 "The matrix is missing elements of the block (but not the 1st element on a row/column)";
243 (operation.*op)(ptr, innerFirstElement, n, m, subMatrix, outerLoop);
269 template <
int Opt,
typename Index>
270 void blockOperation(
const Eigen::Ref<const Matrix>& subMatrix,
size_t rowStart,
size_t columnStart,
271 Eigen::SparseMatrix<double, Opt, Index>* matrix,
272 void (
Operation<
Matrix, Eigen::SparseMatrix<double, Opt, Index>>::*op)(
double*,
const double&))
275 SURGSIM_ASSERT(
nullptr != matrix) <<
"Invalid recipient matrix, nullptr found";
277 Index n = static_cast<Index>(subMatrix.rows());
278 Index m = static_cast<Index>(subMatrix.cols());
279 SURGSIM_ASSERT(matrix->rows() >= static_cast<Index>(rowStart) + n) <<
"The block is out of range in matrix";
280 SURGSIM_ASSERT(matrix->cols() >= static_cast<Index>(columnStart) + m) <<
"The block is out of range in matrix";
282 for (Index row = 0; row < n; ++row)
284 for (Index column = 0; column < m; ++column)
287 &matrix->coeffRef(static_cast<Index>(rowStart) + row, static_cast<Index>(columnStart) + column),
288 subMatrix(row, column));
301 template <
int Opt,
typename Index>
302 void addSubMatrix(
const Eigen::Ref<const Matrix>& subMatrix,
size_t blockIdRow,
size_t blockIdCol,
303 Eigen::SparseMatrix<double, Opt, Index>* matrix,
bool initialize =
true)
307 blockOperation(subMatrix, static_cast<size_t>(subMatrix.rows() * blockIdRow),
308 static_cast<size_t>(subMatrix.cols() * blockIdCol), matrix,
313 blockWithSearch(subMatrix, static_cast<size_t>(subMatrix.rows() * blockIdRow),
314 static_cast<size_t>(subMatrix.cols() * blockIdCol),
315 static_cast<size_t>(subMatrix.rows()), static_cast<size_t>(subMatrix.cols()), matrix,
327 template <
int Opt,
typename Index>
328 void assignSubMatrix(
const Eigen::Ref<const Matrix>& subMatrix,
size_t blockIdRow,
size_t blockIdCol,
329 Eigen::SparseMatrix<double, Opt, Index>* matrix,
bool initialize =
true)
334 (subMatrix.cols() * blockIdCol), matrix,
340 (subMatrix.cols() * blockIdCol),
341 subMatrix.rows(), subMatrix.cols(), matrix,
349 template <
typename T,
int Opt,
typename Index>
350 void zeroRow(
size_t row, Eigen::SparseMatrix<T, Opt, Index>* matrix)
352 for (Index column = 0; column < matrix->cols(); ++column)
354 if (matrix->coeff(static_cast<Index>(row), column))
356 matrix->coeffRef(static_cast<Index>(row), column) = 0;
364 template <
typename T,
int Opt,
typename Index>
365 inline void zeroColumn(
size_t column, Eigen::SparseMatrix<T, Opt, Index>* matrix)
367 for (Index row = 0; row < matrix->rows(); ++row)
369 if (matrix->coeff(row, static_cast<Index>(column)))
371 matrix->coeffRef(row, static_cast<Index>(column)) = 0;
380 template <
typename T,
int Opt,
typename Index>
381 inline void clearMatrix(Eigen::SparseMatrix<T, Opt, Index>* matrix)
383 SURGSIM_ASSERT(matrix->isCompressed()) <<
"Invalid matrix. Matrix must be in compressed form.";
384 T* ptr = matrix->valuePtr();
385 for (Index value = 0; value < matrix->nonZeros(); ++value)
395 #endif // SURGSIM_MATH_SPARSEMATRIX_H