TAMSVIZ
Visualization and annotation tool for ROS
All Classes Pages
transformations.cpp
1 // TAMSVIZ
2 // (c) 2020 Philipp Ruppel
3 
4 #include "transformations.h"
5 
6 Eigen::Matrix4d projectionMatrix(double fov_y, double aspect, double near,
7  double far) {
8  double s = 1.0f / std::tan((double)fov_y * 0.5);
9  Eigen::Matrix4d matrix = Eigen::Matrix4d::Zero();
10  matrix(0, 0) = s * std::sqrt(aspect);
11  matrix(1, 1) = s / std::sqrt(aspect);
12  matrix(2, 2) = -far / (far - near);
13  matrix(2, 3) = -far * near / (far - near);
14  matrix(3, 2) = -1;
15  return matrix;
16 }
17 
18 Eigen::Matrix4d lookatMatrix(const Eigen::Vector3d &eye,
19  const Eigen::Vector3d &at,
20  const Eigen::Vector3d &up) {
21  Eigen::Vector3d z = (eye - at).normalized();
22  Eigen::Vector3d x = up.cross(z).normalized();
23  Eigen::Vector3d y = z.cross(x).normalized();
24  Eigen::Matrix4d matrix = Eigen::Matrix4d::Identity();
25  matrix(0, 0) = x[0];
26  matrix(0, 1) = x[1];
27  matrix(0, 2) = x[2];
28  matrix(1, 0) = y[0];
29  matrix(1, 1) = y[1];
30  matrix(1, 2) = y[2];
31  matrix(2, 0) = z[0];
32  matrix(2, 1) = z[1];
33  matrix(2, 2) = z[2];
34  matrix(0, 3) = -x.dot(eye);
35  matrix(1, 3) = -y.dot(eye);
36  matrix(2, 3) = -z.dot(eye);
37  return matrix;
38 }