23 #ifndef __UTILS_MATH_LINES_H_ 24 #define __UTILS_MATH_LINES_H_ 26 #include <Eigen/Geometry> 44 const Eigen::Vector2f l2_from,
const Eigen::Vector2f l2_to)
46 const Eigen::ParametrizedLine<float, 2>
47 edge_seg(Eigen::ParametrizedLine<float, 2>::Through(l1_from, l1_to));
49 const Eigen::ParametrizedLine<float, 2>
50 line_seg(Eigen::ParametrizedLine<float, 2>::Through(l2_from, l2_to));
52 float k = edge_seg.direction().dot(line_seg.direction());
53 if (std::abs(k - 1.0) < std::numeric_limits<double>::epsilon()) {
57 if (edge_seg.distance(l2_from) > std::numeric_limits<float>::epsilon())
return false;
60 Eigen::Vector2f dir = l1_to - l1_from;
61 float dir_sn = dir.squaredNorm();
62 float k = dir.dot(l2_from - l1_from) / dir_sn;
63 if (k >= 0. && k <= 1.)
return true;
65 k = dir.dot(l2_to - l1_from) / dir_sn;
66 if (k >= 0. && k <= 1.)
return true;
69 dir = l2_to - l2_from;
70 dir_sn = dir.squaredNorm();
71 k = dir.dot(l1_from - l2_from) / dir_sn;
72 if (k >= 0. && k <= 1.)
return true;
74 k = dir.dot(l1_to - l2_from) / dir_sn;
75 if (k >= 0. && k <= 1.)
return true;
82 edge_seg.intersection(Eigen::Hyperplane<float, 2>(line_seg));
84 if (std::isfinite(ipar)) {
85 #if EIGEN_VERSION_AT_LEAST(3,2,0) 86 const Eigen::Vector2f ip(edge_seg.pointAt(ipar));
88 const Eigen::Vector2f ip(edge_seg.origin() + (edge_seg.direction()*ipar));
91 Eigen::Vector2f dir_edge = l1_to - l1_from;
92 Eigen::Vector2f dir_line = l2_to - l2_from;
93 float k_edge = dir_edge.dot(ip - l1_from) / dir_edge.squaredNorm();
94 float k_line = dir_line.dot(ip - l2_from) / dir_line.squaredNorm();
95 return (k_edge >= 0. && k_edge <= 1. && k_line >= 0. && k_line <= 1.);
115 const Eigen::Vector2f l2_from,
const Eigen::Vector2f l2_to)
117 const Eigen::ParametrizedLine<float, 2>
118 edge_seg(Eigen::ParametrizedLine<float, 2>::Through(l1_from, l1_to));
120 const Eigen::ParametrizedLine<float, 2>
121 line_seg(Eigen::ParametrizedLine<float, 2>::Through(l2_from, l2_to));
123 float k = edge_seg.direction().dot(line_seg.direction());
124 if (std::abs(k - 1.0) < std::numeric_limits<double>::epsilon()) {
128 if (edge_seg.distance(l2_from) > std::numeric_limits<float>::epsilon())
129 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
130 std::numeric_limits<float>::quiet_NaN());
134 Eigen::Vector2f dir = l1_to - l1_from;
135 float dir_sn = dir.squaredNorm();
136 float k = dir.dot(l2_from - l1_from) / dir_sn;
137 if (k >= 0. && k <= 1.)
return l2_from;
139 k = dir.dot(l2_to - l1_from) / dir_sn;
140 if (k >= 0. && k <= 1.)
return l2_to;
143 dir = l2_to - l2_from;
144 dir_sn = dir.squaredNorm();
145 k = dir.dot(l1_from - l2_from) / dir_sn;
146 if (k >= 0. && k <= 1.)
return l1_from;
148 k = dir.dot(l1_to - l2_from) / dir_sn;
149 if (k >= 0. && k <= 1.)
return l1_to;
152 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
153 std::numeric_limits<float>::quiet_NaN());
157 edge_seg.intersection(Eigen::Hyperplane<float, 2>(line_seg));
159 if (std::isfinite(ipar)) {
160 #if EIGEN_VERSION_AT_LEAST(3,2,0) 161 const Eigen::Vector2f ip(edge_seg.pointAt(ipar));
163 const Eigen::Vector2f ip(edge_seg.origin() + (edge_seg.direction()*ipar));
166 Eigen::Vector2f dir_edge = l1_to - l1_from;
167 Eigen::Vector2f dir_line = l2_to - l2_from;
168 float k_edge = dir_edge.dot(ip - l1_from) / dir_edge.squaredNorm();
169 float k_line = dir_line.dot(ip - l2_from) / dir_line.squaredNorm();
170 if (k_edge >= 0. && k_edge <= 1. && k_line >= 0. && k_line <= 1.) {
173 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
174 std::numeric_limits<float>::quiet_NaN());
179 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
180 std::numeric_limits<float>::quiet_NaN());
Eigen::Vector2f line_segm_intersection(const Eigen::Vector2f l1_from, const Eigen::Vector2f l1_to, const Eigen::Vector2f l2_from, const Eigen::Vector2f l2_to)
Get line segment intersection point.
Fawkes library namespace.
bool line_segm_intersect(const Eigen::Vector2f l1_from, const Eigen::Vector2f l1_to, const Eigen::Vector2f l2_from, const Eigen::Vector2f l2_to)
Check if two line segments intersect.