TAMSVIZ
Visualization and annotation tool for ROS
interaction.cpp
1 // TAMSVIZ
2 // (c) 2020 Philipp Ruppel
3 
4 #include "interaction.h"
5 
6 Eigen::Vector3d
7 InteractionRay::projectToAxis(const Eigen::Vector3d &axis_start,
8  const Eigen::Vector3d &axis_direction) const {
9  Eigen::Vector3d camera_direction =
10  view_matrix.inverse().col(2).head(3).normalized();
11  Eigen::Vector3d side_vector =
12  camera_direction.cross(axis_direction).normalized();
13  Eigen::Vector3d plane_normal = side_vector.cross(axis_direction).normalized();
14  Eigen::Hyperplane<double, 3> plane(plane_normal, axis_start);
15  Eigen::ParametrizedLine<double, 3> ray(center, direction);
16  Eigen::Vector3d intersection = ray.intersectionPoint(plane);
17  Eigen::Vector3d d = intersection - axis_start;
18  return d - side_vector * d.dot(side_vector) + axis_start;
19 }
20 
21 Eigen::Vector3d
22 Interaction::projectToAxis(const Eigen::Vector3d &direction) const {
23  return current.projectToAxis(begin.point, direction) -
24  previous.projectToAxis(begin.point, direction);
25 }
26 
27 Eigen::Vector3d InteractionRay::projectPlane(const Eigen::Vector3d &p,
28  const Eigen::Vector3d &n) const {
29  Eigen::Hyperplane<double, 3> plane(n, p);
30  Eigen::ParametrizedLine<double, 3> ray(center, direction);
31  return ray.intersectionPoint(plane);
32 }
33 
34 Eigen::Vector3d Interaction::projectPlane(const Eigen::Vector3d &normal) const {
35  return current.projectPlane(begin.point, normal) -
36  previous.projectPlane(begin.point, normal);
37 }