RML  1.0
Robotics Mathematical Library
Loading...
Searching...
No Matches
MatrixOperations.h
Go to the documentation of this file.
1
8#ifndef INCLUDE_RML_MATRIXOPERATIONS_H_
9#define INCLUDE_RML_MATRIXOPERATIONS_H_
10
11#include <eigen3/Eigen/Dense>
12#include <iostream>
13#include <string>
14
15#include "Types.h"
16
17namespace rml {
18
19inline void Double2Matrix(double dmat[], int rows, int cols, Eigen::MatrixXd& MatT)
20{
21 MatT = Eigen::Map<Eigen::MatrixXd>(dmat, rows, cols);
22}
23
24inline void Matrix2Double(const Eigen::MatrixXd& MatT, int rows, int cols, double dmat[])
25{
26 Eigen::Map<Eigen::MatrixXd>(dmat, rows, cols) = MatT;
27}
28
29inline void Double2Vector(double dmat[], int rows, Eigen::VectorXd& MatT)
30{
31 MatT = Eigen::Map<Eigen::VectorXd>(dmat, rows, 1);
32}
33
34inline void Vector2Double(const Eigen::VectorXd& MatT, int rows, double dmat[])
35{
36 Eigen::Map<Eigen::VectorXd>(dmat, rows, 1) = MatT;
37}
38
39inline void SetDiagonalFromDouble(Eigen::MatrixXd& MatT, double diag[])
40{
41 Eigen::ArrayXd wdiag = Eigen::Map<Eigen::ArrayXd>(diag, MatT.cols());
42 MatT.diagonal() = wdiag;
43}
44
45inline Eigen::MatrixXd RightJuxtapose(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B)
46{
47 Eigen::MatrixXd res = A;
48 if (res.rows() == 0 && res.cols() == 0) {
49 res.resize(B.rows(), Eigen::NoChange);
50 }
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;
54 return res;
55}
56
57inline Eigen::MatrixXd UnderJuxtapose(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B)
58{
59 Eigen::MatrixXd res = A;
60 if (res.rows() == 0 && res.cols() == 0) {
61 res.resize(Eigen::NoChange, B.cols());
62 }
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;
66 return res;
67}
68}
69
70#endif /* INCLUDE_RML_MATRIXOPERATIONS_H_ */
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