All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Matrix.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2012-2013, 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_MATRIX_H
20 #define SURGSIM_MATH_MATRIX_H
21 
22 #include <vector>
23 
24 #include <Eigen/Core>
25 #include <Eigen/Geometry>
26 #include <Eigen/LU> // needed for determinant() and inverse()
27 
28 namespace SurgSim
29 {
30 namespace Math
31 {
32 
35 typedef Eigen::Matrix<float, 2, 2, Eigen::RowMajor> Matrix22f;
36 
39 typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Matrix33f;
40 
43 typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> Matrix44f;
44 
47 typedef Eigen::Matrix<double, 2, 2, Eigen::RowMajor> Matrix22d;
48 
51 typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Matrix33d;
52 
55 typedef Eigen::Matrix<double, 4, 4, Eigen::RowMajor> Matrix44d;
56 
59 typedef Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Matrix66d;
60 
62 typedef Eigen::DiagonalMatrix<double, Eigen::Dynamic> DiagonalMatrix;
63 
65 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
66 
73 template <typename T, int VOpt>
74 inline Eigen::Matrix<T, 3, 3> makeRotationMatrix(const T& angle, const Eigen::Matrix<T, 3, 1, VOpt>& axis)
75 {
76  return Eigen::AngleAxis<T>(angle, axis).toRotationMatrix();
77 }
78 
85 template <typename T, int VOpt>
86 inline Eigen::Matrix<T, 3, 3> makeSkewSymmetricMatrix(const Eigen::Matrix<T, 3, 1, VOpt>& vector)
87 {
88  Eigen::Matrix<T, 3, 3> result;
89 
90  result(0, 0) = 0.0;
91  result(0, 1) = -vector(2);
92  result(0, 2) = vector(1);
93 
94  result(1, 0) = vector(2);
95  result(1, 1) = 0.0;
96  result(1, 2) = -vector(0);
97 
98  result(2, 0) = -vector(1);
99  result(2, 1) = vector(0);
100  result(2, 2) = 0.0;
101 
102  return result;
103 }
104 
112 template <typename T, int MOpt>
113 inline Eigen::Matrix<T, 3, 1> skew(const Eigen::Matrix<T, 3, 3, MOpt>& matrix)
114 {
115  Eigen::Matrix<T, 3, 3, MOpt> skewSymmetricPart = (matrix - matrix.transpose()) / 2.0;
116  return Eigen::Matrix<T, 3, 1>(skewSymmetricPart(2, 1), skewSymmetricPart(0, 2), skewSymmetricPart(1, 0));
117 }
118 
126 template <typename T, int MOpt, int VOpt>
127 inline void computeAngleAndAxis(const Eigen::Matrix<T, 3, 3, MOpt>& matrix,
128  T* angle, Eigen::Matrix<T, 3, 1, VOpt>* axis)
129 {
130  Eigen::AngleAxis<T> angleAxis(matrix);
131  *angle = angleAxis.angle();
132  *axis = angleAxis.axis();
133 }
134 
141 template <typename T, int MOpt>
142 inline T computeAngle(const Eigen::Matrix<T, 3, 3, MOpt>& matrix)
143 {
144  // TODO(bert): there has to be a more efficient way...
145  Eigen::AngleAxis<T> angleAxis(matrix);
146  return angleAxis.angle();
147 }
148 
156 template <class Matrix, class SubMatrix>
157 void addSubMatrix(const SubMatrix& subMatrix, size_t blockIdRow, size_t blockIdCol,
158  size_t blockSizeRow, size_t blockSizeCol, Matrix* matrix)
159 {
160  matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol) += subMatrix;
161 }
162 
170 template <class Matrix, class SubMatrix>
171 void addSubMatrix(const SubMatrix& subMatrix, const std::vector<size_t> blockIds, size_t blockSize, Matrix* matrix)
172 {
173  const size_t numBlocks = blockIds.size();
174 
175  for (size_t block0 = 0; block0 < numBlocks; block0++)
176  {
177  size_t blockId0 = blockIds[block0];
178 
179  for (size_t block1 = 0; block1 < numBlocks; block1++)
180  {
181  size_t blockId1 = blockIds[block1];
182 
183  matrix->block(blockSize * blockId0, blockSize * blockId1, blockSize, blockSize)
184  += subMatrix.block(blockSize * block0, blockSize * block1, blockSize, blockSize);
185  }
186  }
187 }
188 
196 template <class Matrix, class SubMatrix>
197 void setSubMatrix(const SubMatrix& subMatrix, size_t blockIdRow, size_t blockIdCol,
198  size_t blockSizeRow, size_t blockSizeCol, Matrix* matrix)
199 {
200  matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol,
201  blockSizeRow, blockSizeCol) = subMatrix;
202 }
203 
213 template <class Matrix>
214 Eigen::Block<Matrix> getSubMatrix(Matrix& matrix, size_t blockIdRow, size_t blockIdCol, // NOLINT
215  size_t blockSizeRow, size_t blockSizeCol)
216 {
217  return matrix.block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol);
218 }
219 
220 }; // namespace Math
221 }; // namespace SurgSim
222 
223 #endif // SURGSIM_MATH_MATRIX_H
Eigen::Matrix< double, 6, 6, Eigen::RowMajor > Matrix66d
A 6x6 matrix of doubles.
Definition: Matrix.h:59
T computeAngle(const Eigen::Matrix< T, 3, 3, MOpt > &matrix)
Get the angle corresponding to a quaternion's rotation, in radians.
Definition: Matrix.h:142
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dynamic size matrix.
Definition: Matrix.h:65
Eigen::Matrix< double, 4, 4, Eigen::RowMajor > Matrix44d
A 4x4 matrix of doubles.
Definition: Matrix.h:55
Eigen::Matrix< T, 3, 3 > makeSkewSymmetricMatrix(const Eigen::Matrix< T, 3, 1, VOpt > &vector)
Create a skew-symmetric matrix corresponding to the specified vector.
Definition: Matrix.h:86
Eigen::Block< Matrix > getSubMatrix(Matrix &matrix, size_t blockIdRow, size_t blockIdCol, size_t blockSizeRow, size_t blockSizeCol)
Helper method to access a sub-matrix from a matrix, for the sake of clarity.
Definition: Matrix.h:214
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
Eigen::Matrix< float, 3, 3, Eigen::RowMajor > Matrix33f
A 3x3 matrix of floats.
Definition: Matrix.h:39
Eigen::DiagonalMatrix< double, Eigen::Dynamic > DiagonalMatrix
A dynamic size diagonal matrix.
Definition: Matrix.h:62
Eigen::Matrix< T, 3, 3 > makeRotationMatrix(const T &angle, const Eigen::Matrix< T, 3, 1, VOpt > &axis)
Create a rotation matrix corresponding to the specified angle (in radians) and axis.
Definition: Matrix.h:74
Eigen::Matrix< float, 4, 4, Eigen::RowMajor > Matrix44f
A 4x4 matrix of floats.
Definition: Matrix.h:43
Eigen::Matrix< T, 3, 1 > skew(const Eigen::Matrix< T, 3, 3, MOpt > &matrix)
Extract the unique vector from the skew-symmetric part of a given matrix.
Definition: Matrix.h:113
Eigen::Matrix< float, 2, 2, Eigen::RowMajor > Matrix22f
A 2x2 matrix of floats.
Definition: Matrix.h:35
void computeAngleAndAxis(const Eigen::Matrix< T, 3, 3, MOpt > &matrix, T *angle, Eigen::Matrix< T, 3, 1, VOpt > *axis)
Get the angle (in radians) and axis corresponding to a rotation matrix.
Definition: Matrix.h:127
Eigen::Matrix< double, 2, 2, Eigen::RowMajor > Matrix22d
A 2x2 matrix of doubles.
Definition: Matrix.h:47
Eigen::Matrix< double, 3, 3, Eigen::RowMajor > Matrix33d
A 3x3 matrix of doubles.
Definition: Matrix.h:51
void setSubMatrix(const SubMatrix &subMatrix, size_t blockIdRow, size_t blockIdCol, size_t blockSizeRow, size_t blockSizeCol, Matrix *matrix)
Helper method to set a sub-matrix into a matrix, for the sake of clarity.
Definition: Matrix.h:197