Matrix.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2012-2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_MATH_MATRIX_H
#define SURGSIM_MATH_MATRIX_H
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU> // needed for determinant() and inverse()
namespace SurgSim
{
namespace Math
{
typedef Eigen::Matrix<float, 2, 2, Eigen::RowMajor> Matrix22f;
typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Matrix33f;
typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> Matrix44f;
typedef Eigen::Matrix<double, 2, 2, Eigen::RowMajor> Matrix22d;
typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> Matrix33d;
typedef Eigen::Matrix<double, 4, 4, Eigen::RowMajor> Matrix44d;
typedef Eigen::Matrix<double, 6, 6, Eigen::RowMajor> Matrix66d;
typedef Eigen::DiagonalMatrix<double, Eigen::Dynamic> DiagonalMatrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
template <typename T, int VOpt>
inline Eigen::Matrix<T, 3, 3> makeRotationMatrix(const T& angle, const Eigen::Matrix<T, 3, 1, VOpt>& axis)
{
return Eigen::AngleAxis<T>(angle, axis).toRotationMatrix();
}
template <typename T, int VOpt>
inline Eigen::Matrix<T, 3, 3> makeSkewSymmetricMatrix(const Eigen::Matrix<T, 3, 1, VOpt>& vector)
{
Eigen::Matrix<T, 3, 3> result;
result(0, 0) = 0.0;
result(0, 1) = -vector(2);
result(0, 2) = vector(1);
result(1, 0) = vector(2);
result(1, 1) = 0.0;
result(1, 2) = -vector(0);
result(2, 0) = -vector(1);
result(2, 1) = vector(0);
result(2, 2) = 0.0;
return result;
}
template <typename T, int MOpt>
inline Eigen::Matrix<T, 3, 1> skew(const Eigen::Matrix<T, 3, 3, MOpt>& matrix)
{
Eigen::Matrix<T, 3, 3, MOpt> skewSymmetricPart = (matrix - matrix.transpose()) / 2.0;
return Eigen::Matrix<T, 3, 1>(skewSymmetricPart(2, 1), skewSymmetricPart(0, 2), skewSymmetricPart(1, 0));
}
template <typename T, int MOpt, int VOpt>
inline void computeAngleAndAxis(const Eigen::Matrix<T, 3, 3, MOpt>& matrix,
T* angle, Eigen::Matrix<T, 3, 1, VOpt>* axis)
{
Eigen::AngleAxis<T> angleAxis(matrix);
*angle = angleAxis.angle();
*axis = angleAxis.axis();
}
template <typename T, int MOpt>
inline T computeAngle(const Eigen::Matrix<T, 3, 3, MOpt>& matrix)
{
// TODO(bert): there has to be a more efficient way...
Eigen::AngleAxis<T> angleAxis(matrix);
return angleAxis.angle();
}
template <class Matrix, class SubMatrix>
void addSubMatrix(const SubMatrix& subMatrix, size_t blockIdRow, size_t blockIdCol,
size_t blockSizeRow, size_t blockSizeCol, Matrix* matrix)
{
matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol) += subMatrix;
}
template <class Matrix, class SubMatrix>
void addSubMatrix(const SubMatrix& subMatrix, const std::vector<size_t> blockIds, size_t blockSize, Matrix* matrix)
{
const size_t numBlocks = blockIds.size();
for (size_t block0 = 0; block0 < numBlocks; block0++)
{
size_t blockId0 = blockIds[block0];
for (size_t block1 = 0; block1 < numBlocks; block1++)
{
size_t blockId1 = blockIds[block1];
matrix->block(blockSize * blockId0, blockSize * blockId1, blockSize, blockSize) +=
subMatrix.block(blockSize * block0, blockSize * block1, blockSize, blockSize);
}
}
}
template <class Matrix, class SubMatrix>
void setSubMatrix(const SubMatrix& subMatrix, size_t blockIdRow, size_t blockIdCol,
size_t blockSizeRow, size_t blockSizeCol, Matrix* matrix)
{
matrix->block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol,
blockSizeRow, blockSizeCol) = subMatrix;
}
template <class Matrix>
Eigen::Block<Matrix> getSubMatrix(Matrix& matrix, size_t blockIdRow, size_t blockIdCol, // NOLINT
size_t blockSizeRow, size_t blockSizeCol)
{
return matrix.block(blockSizeRow * blockIdRow, blockSizeCol * blockIdCol, blockSizeRow, blockSizeCol);
}
template <class Derived>
void zeroRow(size_t row, Eigen::DenseBase<Derived>* matrix)
{
matrix->middleRows(row, 1).setZero();
}
template <class Derived>
void zeroColumn(size_t column, Eigen::DenseBase<Derived>* matrix)
{
(*matrix).middleCols(column, 1).setZero();
}
}; // namespace Math
}; // namespace SurgSim
#endif // SURGSIM_MATH_MATRIX_H
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::Matrix66d
Eigen::Matrix< double, 6, 6, Eigen::RowMajor > Matrix66d
A 6x6 matrix of doubles.
Definition: Matrix.h:59
SurgSim::Math::computeAngle
T computeAngle(const Eigen::Matrix< T, 3, 3, MOpt > &matrix)
Get the angle corresponding to a quaternion&#39;s rotation, in radians.
Definition: Matrix.h:142
SurgSim::Math::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dynamic size matrix.
Definition: Matrix.h:65
SurgSim::Math::Matrix44d
Eigen::Matrix< double, 4, 4, Eigen::RowMajor > Matrix44d
A 4x4 matrix of doubles.
Definition: Matrix.h:55
SurgSim::Math::makeSkewSymmetricMatrix
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
SurgSim::Math::getSubMatrix
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
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::Matrix33f
Eigen::Matrix< float, 3, 3, Eigen::RowMajor > Matrix33f
A 3x3 matrix of floats.
Definition: Matrix.h:39
SurgSim::Math::DiagonalMatrix
Eigen::DiagonalMatrix< double, Eigen::Dynamic > DiagonalMatrix
A dynamic size diagonal matrix.
Definition: Matrix.h:62
SurgSim::Math::makeRotationMatrix
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
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::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::Matrix44f
Eigen::Matrix< float, 4, 4, Eigen::RowMajor > Matrix44f
A 4x4 matrix of floats.
Definition: Matrix.h:43
SurgSim::Math::skew
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
SurgSim::Math::Matrix22f
Eigen::Matrix< float, 2, 2, Eigen::RowMajor > Matrix22f
A 2x2 matrix of floats.
Definition: Matrix.h:35
SurgSim::Math::computeAngleAndAxis
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
SurgSim::Math::Matrix22d
Eigen::Matrix< double, 2, 2, Eigen::RowMajor > Matrix22d
A 2x2 matrix of doubles.
Definition: Matrix.h:47
SurgSim::Math::Matrix33d
Eigen::Matrix< double, 3, 3, Eigen::RowMajor > Matrix33d
A 3x3 matrix of doubles.
Definition: Matrix.h:51
SurgSim::Math::setSubMatrix
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