71 lines
1.5 KiB
C++
71 lines
1.5 KiB
C++
//
|
|
// Created by ivan on 28.03.2022.
|
|
//
|
|
|
|
#include <iostream>
|
|
#include <cmath>
|
|
#include <Eigen/Dense>
|
|
|
|
void euler2Rot(Eigen::Matrix<double, 3, 3>& Rot, double const rx, double const ry, double const rz){
|
|
Eigen::Matrix<double, 3, 3> Rx = Eigen::Matrix<double, 3, 3>::Zero();
|
|
Eigen::Matrix<double, 3, 3> Ry = Eigen::Matrix<double, 3, 3>::Zero();
|
|
Eigen::Matrix<double, 3, 3> Rz = Eigen::Matrix<double, 3, 3>::Zero();
|
|
|
|
Rx(0, 0) = 1;
|
|
Rx(1, 1) = cos(rx);
|
|
Rx(2, 1) = sin(rx);
|
|
Rx(1, 2) = -sin(rx);
|
|
Rx(2, 2) = cos(rx);
|
|
|
|
std::cout << Rx << std::endl;
|
|
std::cout << " " << std::endl;
|
|
|
|
Ry(0, 0) = cos(ry);
|
|
Ry(0, 2) = sin(ry);
|
|
Ry(1, 1) = 1;
|
|
Ry(2, 0) = -sin(ry);
|
|
Ry(2, 2) = cos(ry);
|
|
|
|
std::cout << Ry << std::endl;
|
|
std::cout << " " << std::endl;
|
|
|
|
Rz(0, 0) = cos(rz);
|
|
Rz(0, 1) = -sin(rz);
|
|
Rz(1, 0) = sin(rz);
|
|
Rz(1, 1) = cos(rz);
|
|
Rz(2, 2) = 1;
|
|
|
|
std::cout << Rz << std::endl;
|
|
std::cout << " " << std::endl;
|
|
|
|
auto Rxy = Rx * Ry;
|
|
Rot = Rxy * Rz;
|
|
|
|
}
|
|
|
|
int main(int argc, char** argv){
|
|
double rx;
|
|
double ry;
|
|
double rz;
|
|
rx = 1.1;
|
|
ry = 1.2;
|
|
rz = 1.1;
|
|
Eigen::Matrix<double, 3, 3> Rot;
|
|
|
|
euler2Rot(Rot, rx, ry, rz);
|
|
std::cout << Rot << std::endl;
|
|
|
|
std::cout << "" << std::endl;
|
|
std::cout << "The eigen matrix multiplication" << std::endl;
|
|
Eigen::Matrix<int, 3, 3> M1;
|
|
Eigen::Matrix<int, 3, 3> M2;
|
|
M1 << 1, 2, 3,
|
|
4, 5, 6,
|
|
7, 8, 9;
|
|
|
|
M2 << 1, 2, 3,
|
|
4, 5, 6,
|
|
7, 8, 9;
|
|
|
|
std::cout << M1 * M2 << std::endl;
|
|
} |