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 #if 0 /* just to make Emacs auto-indent happy */
30 }
31 #endif
32 
33 /** Check if two line segments intersect.
34  * Line segments only intersect if the intersection point of the lines
35  * lies within both segment boundaries.
36  * @param l1_from line segment 1 first point
37  * @param l1_to line segment 1 second point
38  * @param l2_from line segment 2 first point
39  * @param l2_to line segment 2 second point
40  * @return true if the lines intersect, false otherwise
41  */
42 bool
43 line_segm_intersect(const Eigen::Vector2f l1_from, const Eigen::Vector2f l1_to,
44  const Eigen::Vector2f l2_from, const Eigen::Vector2f l2_to)
45 {
46  const Eigen::ParametrizedLine<float, 2>
47  edge_seg(Eigen::ParametrizedLine<float, 2>::Through(l1_from, l1_to));
48 
49  const Eigen::ParametrizedLine<float, 2>
50  line_seg(Eigen::ParametrizedLine<float, 2>::Through(l2_from, l2_to));
51 
52  float k = edge_seg.direction().dot(line_seg.direction());
53  if (std::abs(k - 1.0) < std::numeric_limits<double>::epsilon()) {
54  // lines are collinear, check overlap
55 
56  // lines are actually parallel with a non-zero distance
57  if (edge_seg.distance(l2_from) > std::numeric_limits<float>::epsilon()) 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.) return true;
64 
65  k = dir.dot(l2_to - l1_from) / dir_sn;
66  if (k >= 0. && k <= 1.) return true;
67 
68  // Check if the edge end points are in the l2_from--l2_to line segment
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;
73 
74  k = dir.dot(l1_to - l2_from) / dir_sn;
75  if (k >= 0. && k <= 1.) return true;
76 
77  // collinear, but not overlapping
78  return false;
79 
80  } else {
81  const float ipar =
82  edge_seg.intersection(Eigen::Hyperplane<float, 2>(line_seg));
83 
84  if (std::isfinite(ipar)) {
85 #if EIGEN_VERSION_AT_LEAST(3,2,0)
86  const Eigen::Vector2f ip(edge_seg.pointAt(ipar));
87 #else
88  const Eigen::Vector2f ip(edge_seg.origin() + (edge_seg.direction()*ipar));
89 #endif
90  // check if the intersection point is on the line segments
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.);
96 
97  } else {
98  return false;
99  }
100  }
101 }
102 
103 /** Get line segment intersection point.
104  * Line segments only intersect if the intersection point of the lines
105  * lies within both segment boundaries.
106  * @param l1_from line segment 1 first point
107  * @param l1_to line segment 1 second point
108  * @param l2_from line segment 2 first point
109  * @param l2_to line segment 2 second point
110  * @return point which is either the intersection point, or a point of NaNs if
111  * no intersection point exists.
112  */
113 Eigen::Vector2f
114 line_segm_intersection(const Eigen::Vector2f l1_from, const Eigen::Vector2f l1_to,
115  const Eigen::Vector2f l2_from, const Eigen::Vector2f l2_to)
116 {
117  const Eigen::ParametrizedLine<float, 2>
118  edge_seg(Eigen::ParametrizedLine<float, 2>::Through(l1_from, l1_to));
119 
120  const Eigen::ParametrizedLine<float, 2>
121  line_seg(Eigen::ParametrizedLine<float, 2>::Through(l2_from, l2_to));
122 
123  float k = edge_seg.direction().dot(line_seg.direction());
124  if (std::abs(k - 1.0) < std::numeric_limits<double>::epsilon()) {
125  // lines are collinear, check overlap
126 
127  // lines are actually parallel with a non-zero distance
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());
131 
132 
133  // Check if l2_from or l2_to is in the edge line segment
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;
138 
139  k = dir.dot(l2_to - l1_from) / dir_sn;
140  if (k >= 0. && k <= 1.) return l2_to;
141 
142  // Check if the edge end points are in the l2_from--l2_to line segment
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;
147 
148  k = dir.dot(l1_to - l2_from) / dir_sn;
149  if (k >= 0. && k <= 1.) return l1_to;
150 
151  // collinear, but not overlapping
152  return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
153  std::numeric_limits<float>::quiet_NaN());
154 
155  } else {
156  const float ipar =
157  edge_seg.intersection(Eigen::Hyperplane<float, 2>(line_seg));
158 
159  if (std::isfinite(ipar)) {
160 #if EIGEN_VERSION_AT_LEAST(3,2,0)
161  const Eigen::Vector2f ip(edge_seg.pointAt(ipar));
162 #else
163  const Eigen::Vector2f ip(edge_seg.origin() + (edge_seg.direction()*ipar));
164 #endif
165  // check if the intersection point is on the line segments
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.) {
171  return ip;
172  } else {
173  return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
174  std::numeric_limits<float>::quiet_NaN());
175  }
176 
177 
178  } else {
179  return Eigen::Vector2f(std::numeric_limits<float>::quiet_NaN(),
180  std::numeric_limits<float>::quiet_NaN());
181  }
182  }
183 
184 }
185 
186 
187 } // end namespace fawkes
188 
189 #endif
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:114
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.
Definition: lines.h:43