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