Fawkes API Fawkes Development Version
lines.h
1
2/***************************************************************************
3 * lines.h - functions to operate on lines
4 *
5 * Created: Tue Apr 07 21:42:34 2015
6 * Copyright 2015 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version. A runtime exception applies to
13 * this software (see LICENSE.GPL_WRE file mentioned below for details).
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
21 */
22
23#ifndef _UTILS_MATH_LINES_H_
24#define _UTILS_MATH_LINES_H_
25
26#include <Eigen/Geometry>
27
28namespace fawkes {
29
30/** Check if two line segments intersect.
31 * Line segments only intersect if the intersection point of the lines
32 * lies within both segment boundaries.
33 * @param l1_from line segment 1 first point
34 * @param l1_to line segment 1 second point
35 * @param l2_from line segment 2 first point
36 * @param l2_to line segment 2 second point
37 * @return true if the lines intersect, false otherwise
38 */
39bool
40line_segm_intersect(const Eigen::Vector2f &l1_from,
41 const Eigen::Vector2f &l1_to,
42 const Eigen::Vector2f &l2_from,
43 const Eigen::Vector2f &l2_to)
44{
45 const Eigen::ParametrizedLine<float, 2> edge_seg(
46 Eigen::ParametrizedLine<float, 2>::Through(l1_from, l1_to));
47
48 const Eigen::ParametrizedLine<float, 2> line_seg(
49 Eigen::ParametrizedLine<float, 2>::Through(l2_from, l2_to));
50
51 float k = edge_seg.direction().dot(line_seg.direction());
52 if (std::abs(k - 1.0) < std::numeric_limits<double>::epsilon()) {
53 // lines are collinear, check overlap
54
55 // lines are actually parallel with a non-zero distance
56 if (edge_seg.distance(l2_from) > std::numeric_limits<float>::epsilon())
57 return false;
58
59 // Check if l2_from or l2_to is in the edge line segment
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.)
64 return true;
65
66 k = dir.dot(l2_to - l1_from) / dir_sn;
67 if (k >= 0. && k <= 1.)
68 return true;
69
70 // Check if the edge end points are in the l2_from--l2_to line segment
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.)
75 return true;
76
77 k = dir.dot(l1_to - l2_from) / dir_sn;
78 if (k >= 0. && k <= 1.)
79 return true;
80
81 // collinear, but not overlapping
82 return false;
83
84 } else {
85 const float ipar = edge_seg.intersection(Eigen::Hyperplane<float, 2>(line_seg));
86
87 if (std::isfinite(ipar)) {
88#if EIGEN_VERSION_AT_LEAST(3, 2, 0)
89 const Eigen::Vector2f ip(edge_seg.pointAt(ipar));
90#else
91 const Eigen::Vector2f ip(edge_seg.origin() + (edge_seg.direction() * ipar));
92#endif
93 // check if the intersection point is on the line segments
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.);
99
100 } else {
101 return false;
102 }
103 }
104}
105
106/** Get line segment intersection point.
107 * Line segments only intersect if the intersection point of the lines
108 * lies within both segment boundaries.
109 * @param l1_from line segment 1 first point
110 * @param l1_to line segment 1 second point
111 * @param l2_from line segment 2 first point
112 * @param l2_to line segment 2 second point
113 * @return point which is either the intersection point, or a point of NaNs if
114 * no intersection point exists.
115 */
116Eigen::Vector2f
117line_segm_intersection(const Eigen::Vector2f &l1_from,
118 const Eigen::Vector2f &l1_to,
119 const Eigen::Vector2f &l2_from,
120 const Eigen::Vector2f &l2_to)
121{
122 const Eigen::ParametrizedLine<float, 2> edge_seg(
123 Eigen::ParametrizedLine<float, 2>::Through(l1_from, l1_to));
124
125 const Eigen::ParametrizedLine<float, 2> line_seg(
126 Eigen::ParametrizedLine<float, 2>::Through(l2_from, l2_to));
127
128 float k = edge_seg.direction().dot(line_seg.direction());
129 if (std::abs(k - 1.0) < std::numeric_limits<double>::epsilon()) {
130 // lines are collinear, check overlap
131
132 // lines are actually parallel with a non-zero distance
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());
136
137 // Check if l2_from or l2_to is in the edge line segment
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.)
142 return l2_from;
143
144 k = dir.dot(l2_to - l1_from) / dir_sn;
145 if (k >= 0. && k <= 1.)
146 return l2_to;
147
148 // Check if the edge end points are in the l2_from--l2_to line segment
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.)
153 return l1_from;
154
155 k = dir.dot(l1_to - l2_from) / dir_sn;
156 if (k >= 0. && k <= 1.)
157 return l1_to;
158
159 // collinear, but not overlapping
160 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
161 std::numeric_limits<float>::quiet_NaN());
162
163 } else {
164 const float ipar = edge_seg.intersection(Eigen::Hyperplane<float, 2>(line_seg));
165
166 if (std::isfinite(ipar)) {
167#if EIGEN_VERSION_AT_LEAST(3, 2, 0)
168 const Eigen::Vector2f ip(edge_seg.pointAt(ipar));
169#else
170 const Eigen::Vector2f ip(edge_seg.origin() + (edge_seg.direction() * ipar));
171#endif
172 // check if the intersection point is on the line segments
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.) {
178 return ip;
179 } else {
180 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
181 std::numeric_limits<float>::quiet_NaN());
182 }
183
184 } else {
185 return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
186 std::numeric_limits<float>::quiet_NaN());
187 }
188 }
189}
190
191} // end namespace fawkes
192
193#endif
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.
Definition: lines.h:117
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.
Definition: lines.h:40