23#ifndef _UTILS_MATH_LINES_H_
24#define _UTILS_MATH_LINES_H_
26#include <Eigen/Geometry>
41 const Eigen::Vector2f &l1_to,
42 const Eigen::Vector2f &l2_from,
43 const Eigen::Vector2f &l2_to)
45 const Eigen::ParametrizedLine<float, 2> edge_seg(
46 Eigen::ParametrizedLine<float, 2>::Through(l1_from, l1_to));
48 const Eigen::ParametrizedLine<float, 2> line_seg(
49 Eigen::ParametrizedLine<float, 2>::Through(l2_from, l2_to));
51 float k = edge_seg.direction().dot(line_seg.direction());
52 if (std::abs(k - 1.0) < std::numeric_limits<double>::epsilon()) {
56 if (edge_seg.distance(l2_from) > std::numeric_limits<float>::epsilon())
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.)
66 k = dir.dot(l2_to - l1_from) / dir_sn;
67 if (k >= 0. && k <= 1.)
71 dir = l2_to - l2_from;
72 dir_sn = dir.squaredNorm();
73 k = dir.dot(l1_from - l2_from) / dir_sn;
74 if (k >= 0. && k <= 1.)
77 k = dir.dot(l1_to - l2_from) / dir_sn;
78 if (k >= 0. && k <= 1.)
85 const float ipar = edge_seg.intersection(Eigen::Hyperplane<float, 2>(line_seg));
87 if (std::isfinite(ipar)) {
88#if EIGEN_VERSION_AT_LEAST(3, 2, 0)
89 const Eigen::Vector2f ip(edge_seg.pointAt(ipar));
91 const Eigen::Vector2f ip(edge_seg.origin() + (edge_seg.direction() * ipar));
94 Eigen::Vector2f dir_edge = l1_to - l1_from;
95 Eigen::Vector2f dir_line = l2_to - l2_from;
96 float k_edge = dir_edge.dot(ip - l1_from) / dir_edge.squaredNorm();
97 float k_line = dir_line.dot(ip - l2_from) / dir_line.squaredNorm();
98 return (k_edge >= 0. && k_edge <= 1. && k_line >= 0. && k_line <= 1.);
118 const Eigen::Vector2f &l1_to,
119 const Eigen::Vector2f &l2_from,
120 const Eigen::Vector2f &l2_to)
122 const Eigen::ParametrizedLine<float, 2> edge_seg(
123 Eigen::ParametrizedLine<float, 2>::Through(l1_from, l1_to));
125 const Eigen::ParametrizedLine<float, 2> line_seg(
126 Eigen::ParametrizedLine<float, 2>::Through(l2_from, l2_to));
128 float k = edge_seg.direction().dot(line_seg.direction());
129 if (std::abs(k - 1.0) < std::numeric_limits<double>::epsilon()) {
133 if (edge_seg.distance(l2_from) > std::numeric_limits<float>::epsilon())
134 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
135 std::numeric_limits<float>::quiet_NaN());
138 Eigen::Vector2f dir = l1_to - l1_from;
139 float dir_sn = dir.squaredNorm();
140 float k = dir.dot(l2_from - l1_from) / dir_sn;
141 if (k >= 0. && k <= 1.)
144 k = dir.dot(l2_to - l1_from) / dir_sn;
145 if (k >= 0. && k <= 1.)
149 dir = l2_to - l2_from;
150 dir_sn = dir.squaredNorm();
151 k = dir.dot(l1_from - l2_from) / dir_sn;
152 if (k >= 0. && k <= 1.)
155 k = dir.dot(l1_to - l2_from) / dir_sn;
156 if (k >= 0. && k <= 1.)
160 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
161 std::numeric_limits<float>::quiet_NaN());
164 const float ipar = edge_seg.intersection(Eigen::Hyperplane<float, 2>(line_seg));
166 if (std::isfinite(ipar)) {
167#if EIGEN_VERSION_AT_LEAST(3, 2, 0)
168 const Eigen::Vector2f ip(edge_seg.pointAt(ipar));
170 const Eigen::Vector2f ip(edge_seg.origin() + (edge_seg.direction() * ipar));
173 Eigen::Vector2f dir_edge = l1_to - l1_from;
174 Eigen::Vector2f dir_line = l2_to - l2_from;
175 float k_edge = dir_edge.dot(ip - l1_from) / dir_edge.squaredNorm();
176 float k_line = dir_line.dot(ip - l2_from) / dir_line.squaredNorm();
177 if (k_edge >= 0. && k_edge <= 1. && k_line >= 0. && k_line <= 1.) {
180 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
181 std::numeric_limits<float>::quiet_NaN());
185 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
186 std::numeric_limits<float>::quiet_NaN());
Fawkes library namespace.
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.
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.