5 #include <VirtualRobot/MathTools.h>
17 constexpr
T c =
M_PI / 180;
29 const Eigen::Vector2f v1_vec_normed = v1.normalized();
30 const Eigen::Vector2f v2_vec_normed = v2.normalized();
31 const float dot_product_vec =
32 v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
33 float yaw = acos(dot_product_vec);
42 points.push_back(p.front());
45 for (
size_t i = 1; i < (p.size() - 1); i++)
47 const Eigen::Vector2f& prev = p.at(i - 1);
48 const Eigen::Vector2f& at = p.at(i);
49 const Eigen::Vector2f& next = p.at(i + 1);
53 const float radius =
std::min(maxRadius, 300.
F);
55 const Eigen::Vector2f vIn = (prev - at).normalized();
56 const Eigen::Vector2f vOut = (next - at).normalized();
60 const float l = radius / std::tan(phi / 2);
62 points.push_back(at + Eigen::Vector2f(prev - at).normalized() * l);
63 points.push_back(at + Eigen::Vector2f(next - at).normalized() * l);
66 points.push_back(p.back());
75 const auto& p = trajectory.
points();
77 points.push_back(p.front());
80 for (
size_t i = 1; i < (p.size() - 1); i++)
82 const Eigen::Vector2f& prev =
85 const Eigen::Vector2f& next =
90 const float radius =
std::min(maxRadius, 300.
F);
92 const Eigen::Vector2f vIn = (prev - at).normalized();
93 const Eigen::Vector2f vOut = (next - at).normalized();
101 points.push_back(p.at(i));
105 const float l = radius / std::tan(phi / 2);
107 const Eigen::Vector2f prePos = at + Eigen::Vector2f(prev - at).normalized() * l;
108 const Eigen::Vector2f postPos = at + Eigen::Vector2f(next - at).normalized() * l;
119 points.push_back(tpProj(prePos));
120 points.push_back(tpProj(postPos));
124 points.push_back(p.back());