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 
28 namespace 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  */
39 bool
40 line_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  */
116 Eigen::Vector2f
117 line_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::line_segm_intersect
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:43
fawkes
fawkes::line_segm_intersection
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:120