v01

test/ivan-tests/euler2RotTest.cpp

@@ -0,0 +1,71 @@
+//
+// 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;
+}