8#ifndef INCLUDE_RML_MATRIXOPERATIONS_H_
9#define INCLUDE_RML_MATRIXOPERATIONS_H_
11#include <eigen3/Eigen/Dense>
19inline void Double2Matrix(
double dmat[],
int rows,
int cols, Eigen::MatrixXd& MatT)
21 MatT = Eigen::Map<Eigen::MatrixXd>(dmat, rows, cols);
24inline void Matrix2Double(
const Eigen::MatrixXd& MatT,
int rows,
int cols,
double dmat[])
26 Eigen::Map<Eigen::MatrixXd>(dmat, rows, cols) = MatT;
29inline void Double2Vector(
double dmat[],
int rows, Eigen::VectorXd& MatT)
31 MatT = Eigen::Map<Eigen::VectorXd>(dmat, rows, 1);
34inline void Vector2Double(
const Eigen::VectorXd& MatT,
int rows,
double dmat[])
36 Eigen::Map<Eigen::VectorXd>(dmat, rows, 1) = MatT;
41 Eigen::ArrayXd wdiag = Eigen::Map<Eigen::ArrayXd>(diag, MatT.cols());
42 MatT.diagonal() = wdiag;
45inline Eigen::MatrixXd
RightJuxtapose(
const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B)
47 Eigen::MatrixXd res = A;
48 if (res.rows() == 0 && res.cols() == 0) {
49 res.resize(B.rows(), Eigen::NoChange);
51 eigen_assert(res.rows() == B.rows());
52 res.conservativeResize(Eigen::NoChange, A.cols() + B.cols());
53 res.block(0, A.cols(), B.rows(), B.cols()) = B;
57inline Eigen::MatrixXd
UnderJuxtapose(
const Eigen::MatrixXd& A,
const Eigen::MatrixXd& B)
59 Eigen::MatrixXd res = A;
60 if (res.rows() == 0 && res.cols() == 0) {
61 res.resize(Eigen::NoChange, B.cols());
63 eigen_assert(res.cols() == B.cols());
64 res.conservativeResize(A.rows() + B.rows(), Eigen::NoChange);
65 res.block(A.rows(), 0, B.rows(), B.cols()) = B;
Types and algorithms for robotic mobile manipulation.
Definition ArmModel.h:19
void Vector2Double(const Eigen::VectorXd &MatT, int rows, double dmat[])
Definition MatrixOperations.h:34
Eigen::MatrixXd UnderJuxtapose(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B)
Definition MatrixOperations.h:57
void SetDiagonalFromDouble(Eigen::MatrixXd &MatT, double diag[])
Definition MatrixOperations.h:39
void Double2Vector(double dmat[], int rows, Eigen::VectorXd &MatT)
Definition MatrixOperations.h:29
void Double2Matrix(double dmat[], int rows, int cols, Eigen::MatrixXd &MatT)
Definition MatrixOperations.h:19
void Matrix2Double(const Eigen::MatrixXd &MatT, int rows, int cols, double dmat[])
Definition MatrixOperations.h:24
Eigen::MatrixXd RightJuxtapose(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B)
Definition MatrixOperations.h:45