SparseMatrix.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2012-2015, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
18 
19 #ifndef SURGSIM_MATH_SPARSEMATRIX_H
20 #define SURGSIM_MATH_SPARSEMATRIX_H
21 
22 #include <Eigen/Sparse>
23 
25 #include "SurgSim/Math/Matrix.h"
26 
27 namespace SurgSim
28 {
29 namespace Math
30 {
32 typedef Eigen::SparseMatrix<double> SparseMatrix;
33 
38 template < class DerivedSub, class SparseType,
39  int StorageOrder =
40  ((SparseType::Flags & Eigen::RowMajorBit) == Eigen::RowMajorBit) ? Eigen::RowMajor : Eigen::ColMajor >
41 class Operation
42 {
43 public :
44  typedef typename SparseType::Scalar T;
45  typedef typename SparseType::Index Index;
46 
53  void assign(T* ptr, size_t start, size_t n, Index m, const DerivedSub& subMatrix, size_t colRowId) {}
54 
58  void assign(T* ptr, const T& value) {}
59 
66  void add(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t colRowId) {}
67 
71  void add(T* ptr, const T& value) {}
72 };
73 
75 template <class DerivedSub, class SparseType>
76 class Operation<DerivedSub, SparseType, Eigen::ColMajor>
77 {
78 public:
79  typedef typename SparseType::Scalar T;
80  typedef typename SparseType::Index Index;
81 
82  void assign(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t colId)
83  {
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;
87 
88  // ptr[start] is the 1st element in the column
89  // The elements exists and are contiguous in memory, we use Eigen::Map functionality to optimize the operation
90  Eigen::Map<ColVector>(&ptr[start], static_cast<IndexVector>(n)).operator = (
91  subMatrix.col(static_cast<IndexSub>(colId)).segment(0, static_cast<IndexSub>(n)));
92  }
93 
94  void assign(T* ptr, const T& value)
95  {
96  *ptr = value;
97  }
98 
99  void add(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t colId)
100  {
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;
104 
105  // ptr[start] is the 1st element in the column
106  // The elements exists and are contiguous in memory, we use Eigen::Map functionality to optimize the operation
107  Eigen::Map<ColVector>(&ptr[start], static_cast<IndexVector>(n)).operator += (
108  subMatrix.col(static_cast<IndexSub>(colId)).segment(0, static_cast<IndexSub>(n)));
109  }
110 
111  void add(T* ptr, const T& value)
112  {
113  *ptr += value;
114  }
115 };
116 
118 template <class DerivedSub, class SparseType>
119 class Operation<DerivedSub, SparseType, Eigen::RowMajor>
120 {
121 public:
122  typedef typename SparseType::Scalar T;
123  typedef typename SparseType::Index Index;
124 
125  void assign(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t rowId)
126  {
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;
130 
131  // ptr[start] is the 1st element in the row
132  // The elements exists and are contiguous in memory, we use Eigen::Map functionality to optimize the operation
133  Eigen::Map<RowVector>(&ptr[start], static_cast<IndexVector>(m)).operator = (
134  subMatrix.row(static_cast<IndexSub>(rowId)).segment(0, static_cast<IndexSub>(m)));
135  }
136 
137  void assign(T* ptr, const T& value)
138  {
139  *ptr = value;
140  }
141 
142  void add(T* ptr, size_t start, size_t n, size_t m, const DerivedSub& subMatrix, size_t rowId)
143  {
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;
147 
148  // ptr[start] is the 1st element in the row
149  // The elements exists and are contiguous in memory, we use Eigen::Map functionality to optimize the operation
150  Eigen::Map<RowVector>(&ptr[start], static_cast<IndexVector>(m)).operator += (
151  subMatrix.row(static_cast<IndexSub>(rowId)).segment(0, static_cast<IndexSub>(m)));
152  }
153 
154  void add(T* ptr, const T& value)
155  {
156  *ptr += value;
157  }
158 };
159 
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,
187  void (Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::*op)
188  (double*, size_t, size_t, size_t, const Matrix&, size_t))
189 {
191  SURGSIM_ASSERT(nullptr != matrix) << "Invalid recipient matrix, nullptr found";
192 
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";
195 
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";
199 
200  double* ptr = matrix->valuePtr();
201  const Index* innerIndices = matrix->innerIndexPtr();
202  const Index* outerIndices = matrix->outerIndexPtr();
203 
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);
208 
209  for (Index outerLoop = 0; outerLoop < outerSize; ++outerLoop)
210  {
211  // outerIndices[outerStart + outerLoop] is the index in ptr and innerIndices of the first non-zero element in
212  // the outer element (outerStart + outerLoop)
213  const Index innerStartIdInCurrentOuter = outerIndices[outerStart + outerLoop];
214  const Index innerStartIdInNextOuter = outerIndices[outerStart + outerLoop + 1];
215 
216  // Look for the index of innerStart in this outer (the column/row may contain elements before)
217  Index innerFirstElement;
218  if (innerIndices[innerStartIdInCurrentOuter] == innerStart)
219  {
220  innerFirstElement = innerStartIdInCurrentOuter;
221  }
222  else
223  {
224  innerFirstElement = static_cast<Index>(matrix->data().searchLowerIndex(
225  innerStartIdInCurrentOuter, innerStartIdInNextOuter - 1, innerStart));
226  }
227 
228  // Make sure we actually found the 1st element of the block in this outer
229  SURGSIM_ASSERT(innerIndices[innerFirstElement] == innerStart) <<
230  "matrix is missing an element of the block (1st element on a row/column)";
231 
232  // Make sure that we are not going to write out of the range...
233  // i.e. The column/row (starting at the beginning of the block) has at least innerSize elements
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)";
236 
237  // Make sure that the last element corresponding to the block size has the expected index
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)";
242 
243  (operation.*op)(ptr, innerFirstElement, n, m, subMatrix, outerLoop);
244  }
245 }
246 
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&))
273 {
275  SURGSIM_ASSERT(nullptr != matrix) << "Invalid recipient matrix, nullptr found";
276 
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";
281 
282  for (Index row = 0; row < n; ++row)
283  {
284  for (Index column = 0; column < m; ++column)
285  {
286  (operation.*op)(
287  &matrix->coeffRef(static_cast<Index>(rowStart) + row, static_cast<Index>(columnStart) + column),
288  subMatrix(row, column));
289  }
290  }
291 }
292 
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)
304 {
305  if (initialize)
306  {
307  blockOperation(subMatrix, static_cast<size_t>(subMatrix.rows() * blockIdRow),
308  static_cast<size_t>(subMatrix.cols() * blockIdCol), matrix,
309  &Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::add);
310  }
311  else
312  {
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,
316  &Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::add);
317  }
318 }
319 
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)
330 {
331  if (initialize)
332  {
333  blockOperation(subMatrix, (subMatrix.rows() * blockIdRow),
334  (subMatrix.cols() * blockIdCol), matrix,
335  &Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::assign);
336  }
337  else
338  {
339  blockWithSearch(subMatrix, (subMatrix.rows() * blockIdRow),
340  (subMatrix.cols() * blockIdCol),
341  subMatrix.rows(), subMatrix.cols(), matrix,
342  &Operation<Matrix, Eigen::SparseMatrix<double, Opt, Index>>::assign);
343  }
344 }
345 
349 template <typename T, int Opt, typename Index>
350 void zeroRow(size_t row, Eigen::SparseMatrix<T, Opt, Index>* matrix)
351 {
352  for (Index column = 0; column < matrix->cols(); ++column)
353  {
354  if (matrix->coeff(static_cast<Index>(row), column))
355  {
356  matrix->coeffRef(static_cast<Index>(row), column) = 0;
357  }
358  }
359 }
360 
364 template <typename T, int Opt, typename Index>
365 inline void zeroColumn(size_t column, Eigen::SparseMatrix<T, Opt, Index>* matrix)
366 {
367  for (Index row = 0; row < matrix->rows(); ++row)
368  {
369  if (matrix->coeff(row, static_cast<Index>(column)))
370  {
371  matrix->coeffRef(row, static_cast<Index>(column)) = 0;
372  }
373  }
374 }
375 
380 template <typename T, int Opt, typename Index>
381 inline void clearMatrix(Eigen::SparseMatrix<T, Opt, Index>* matrix)
382 {
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)
386  {
387  *ptr = 0;
388  ++ptr;
389  }
390 }
391 
392 }; // namespace Math
393 }; // namespace SurgSim
394 
395 #endif // SURGSIM_MATH_SPARSEMATRIX_H
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Math::assignSubMatrix
void assignSubMatrix(const Eigen::Ref< const Matrix > &subMatrix, size_t blockIdRow, size_t blockIdCol, Eigen::SparseMatrix< double, Opt, Index > *matrix, bool initialize=true)
Helper method to assign a sub-matrix into a matrix, for the sake of clarity.
Definition: SparseMatrix.h:328
Eigen
Definition: MathUtilities.h:95
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::ColMajor >::Index
SparseType::Index Index
Definition: SparseMatrix.h:80
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::RowMajor >::assign
void assign(T *ptr, const T &value)
Definition: SparseMatrix.h:137
SurgSim::Math::Operation::add
void add(T *ptr, size_t start, size_t n, size_t m, const DerivedSub &subMatrix, size_t colRowId)
Do the addition of a row/column of a matrix to a chunk of memory.
Definition: SparseMatrix.h:66
SurgSim::Math::clearMatrix
void clearMatrix(Eigen::SparseMatrix< T, Opt, Index > *matrix)
Helper method to zero all entries of a matrix specialized for Sparse Matrices.
Definition: SparseMatrix.h:381
Matrix.h
Definitions of small fixed-size square matrix types.
SurgSim::Math::addSubMatrix
void addSubMatrix(const SubMatrix &subMatrix, size_t blockIdRow, size_t blockIdCol, size_t blockSizeRow, size_t blockSizeCol, Matrix *matrix)
Helper method to add a sub-matrix into a matrix, for the sake of clarity.
Definition: Matrix.h:157
SurgSim::Math::Operation::add
void add(T *ptr, const T &value)
Do the addition of a single matrix element (operator +=)
Definition: SparseMatrix.h:71
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::ColMajor >::add
void add(T *ptr, size_t start, size_t n, size_t m, const DerivedSub &subMatrix, size_t colId)
Definition: SparseMatrix.h:99
Assert.h
The header that provides the assertion API.
SurgSim
Definition: CompoundShapeToGraphics.cpp:30
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::ColMajor >::T
SparseType::Scalar T
Definition: SparseMatrix.h:79
SurgSim::Math::zeroColumn
void zeroColumn(size_t column, Eigen::DenseBase< Derived > *matrix)
Helper method to zero a column of a matrix.
Definition: Matrix.h:235
SurgSim::Math::Operation::Index
SparseType::Index Index
Definition: SparseMatrix.h:45
SurgSim::Math::blockWithSearch
void blockWithSearch(const Eigen::Ref< const Matrix > &subMatrix, size_t rowStart, size_t columnStart, size_t n, size_t m, Eigen::SparseMatrix< double, Opt, Index > *matrix, void(Operation< Matrix, Eigen::SparseMatrix< double, Opt, Index >>::*op)(double *, size_t, size_t, size_t, const Matrix &, size_t))
Runs a given operation on a SparseMatrix block(i, j, n, m) from a (n x m) 'subMatrix' with searching ...
Definition: SparseMatrix.h:185
SurgSim::Math::blockOperation
void blockOperation(const Eigen::Ref< const Matrix > &subMatrix, size_t rowStart, size_t columnStart, Eigen::SparseMatrix< double, Opt, Index > *matrix, void(Operation< Matrix, Eigen::SparseMatrix< double, Opt, Index >>::*op)(double *, const double &))
Runs a given operation on a SparseMatrix block(i, j, n, m) from a (n x m) 'subMatrix' with searching ...
Definition: SparseMatrix.h:270
SurgSim::Math::SparseMatrix
Eigen::SparseMatrix< double > SparseMatrix
A sparse matrix.
Definition: SparseMatrix.h:32
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::RowMajor >::assign
void assign(T *ptr, size_t start, size_t n, size_t m, const DerivedSub &subMatrix, size_t rowId)
Definition: SparseMatrix.h:125
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::RowMajor >::add
void add(T *ptr, size_t start, size_t n, size_t m, const DerivedSub &subMatrix, size_t rowId)
Definition: SparseMatrix.h:142
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::RowMajor >::add
void add(T *ptr, const T &value)
Definition: SparseMatrix.h:154
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::ColMajor >::add
void add(T *ptr, const T &value)
Definition: SparseMatrix.h:111
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::RowMajor >::T
SparseType::Scalar T
Definition: SparseMatrix.h:122
SurgSim::Math::Operation::assign
void assign(T *ptr, size_t start, size_t n, Index m, const DerivedSub &subMatrix, size_t colRowId)
Do the assignment of a row/column of a matrix to a chunk of memory.
Definition: SparseMatrix.h:53
SurgSim::Math::Operation
Helper class to run operation a column/row of a matrix to a chunk of memory where the size is dynamic...
Definition: SparseMatrix.h:42
SurgSim::Math::Operation::T
SparseType::Scalar T
Definition: SparseMatrix.h:44
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::RowMajor >::Index
SparseType::Index Index
Definition: SparseMatrix.h:123
SurgSim::Math::zeroRow
void zeroRow(size_t row, Eigen::DenseBase< Derived > *matrix)
Helper method to zero a row of a matrix.
Definition: Matrix.h:225
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::ColMajor >::assign
void assign(T *ptr, const T &value)
Definition: SparseMatrix.h:94
SurgSim::Math::Operation::assign
void assign(T *ptr, const T &value)
Do the assignment of a single matrix element (operator =)
Definition: SparseMatrix.h:58
SurgSim::Math::Operation< DerivedSub, SparseType, Eigen::ColMajor >::assign
void assign(T *ptr, size_t start, size_t n, size_t m, const DerivedSub &subMatrix, size_t colId)
Definition: SparseMatrix.h:82
SurgSim::Math::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dynamic size matrix.
Definition: Matrix.h:65