19 #ifndef SURGSIM_MATH_MATRIX_H
20 #define SURGSIM_MATH_MATRIX_H
25 #include <Eigen/Geometry>
35 typedef Eigen::Matrix<float, 2, 2, Eigen::RowMajor>
Matrix22f;
39 typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor>
Matrix33f;
43 typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor>
Matrix44f;
47 typedef Eigen::Matrix<double, 2, 2, Eigen::RowMajor>
Matrix22d;
51 typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor>
Matrix33d;
55 typedef Eigen::Matrix<double, 4, 4, Eigen::RowMajor>
Matrix44d;
59 typedef Eigen::Matrix<double, 6, 6, Eigen::RowMajor>
Matrix66d;
65 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
Matrix;
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)
76 return Eigen::AngleAxis<T>(angle, axis).toRotationMatrix();
85 template <
typename T,
int VOpt>
88 Eigen::Matrix<T, 3, 3> result;
91 result(0, 1) = -vector(2);
92 result(0, 2) = vector(1);
94 result(1, 0) = vector(2);
96 result(1, 2) = -vector(0);
98 result(2, 0) = -vector(1);
99 result(2, 1) = vector(0);
112 template <
typename T,
int MOpt>
113 inline Eigen::Matrix<T, 3, 1>
skew(
const Eigen::Matrix<T, 3, 3, MOpt>& matrix)
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));
126 template <
typename T,
int MOpt,
int VOpt>
128 T* angle, Eigen::Matrix<T, 3, 1, VOpt>* axis)
130 Eigen::AngleAxis<T> angleAxis(matrix);
131 *angle = angleAxis.angle();
132 *axis = angleAxis.axis();
141 template <
typename T,
int MOpt>
145 Eigen::AngleAxis<T> angleAxis(matrix);
146 return angleAxis.angle();
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)
160 matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol) += subMatrix;
170 template <
class Matrix,
class SubMatrix>
171 void addSubMatrix(
const SubMatrix& subMatrix,
const std::vector<size_t> blockIds,
size_t blockSize,
Matrix* matrix)
173 const size_t numBlocks = blockIds.size();
175 for (
size_t block0 = 0; block0 < numBlocks; block0++)
177 size_t blockId0 = blockIds[block0];
179 for (
size_t block1 = 0; block1 < numBlocks; block1++)
181 size_t blockId1 = blockIds[block1];
183 matrix->block(blockSize * blockId0, blockSize * blockId1, blockSize, blockSize) +=
184 subMatrix.block(blockSize * block0, blockSize * block1, blockSize, blockSize);
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)
200 matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol,
201 blockSizeRow, blockSizeCol) = subMatrix;
213 template <
class Matrix>
215 size_t blockSizeRow,
size_t blockSizeCol)
217 return matrix.block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol);
224 template <
class Derived>
225 void zeroRow(
size_t row, Eigen::DenseBase<Derived>* matrix)
227 matrix->middleRows(row, 1).setZero();
234 template <
class Derived>
235 void zeroColumn(
size_t column, Eigen::DenseBase<Derived>* matrix)
237 (*matrix).middleCols(column, 1).setZero();
243 #endif // SURGSIM_MATH_MATRIX_H