intersection.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 #include <stdint.h>
31 
33 
34 #define HAVE_INT64_T_64 # Prevents conflict with OpenCV typedef of int64
35 #include <geos/geom/CoordinateArraySequence.h>
36 #include <geos/geom/GeometryFactory.h>
37 #include <geos/geom/Polygon.h>
38 #include <geos/util/TopologyException.h>
39 #undef HAVE_INT64_T_64
40 
41 namespace swri_geometry_util
42 {
44  const cv::Vec2d& p1,
45  const cv::Vec2d& p2,
46  const cv::Vec2d& p3,
47  const cv::Vec2d& p4,
48  cv::Vec2d& c)
49  {
50  double d = (p1[0] - p2[0]) * (p3[1] - p4[1]) - (p1[1] - p2[1]) * (p3[0] - p4[0]);
51  if (d == 0)
52  {
53  return false;
54  }
55 
56  double n_a = (p1[0] * p2[1] - p1[1] * p2[0]) * (p3[0] - p4[0]) - (p1[0] - p2[0]) * (p3[0] * p4[1] - p3[1] * p4[0]);
57  double n_b = (p1[0] * p2[1] - p1[1] * p2[0]) * (p3[1] - p4[1]) - (p1[1] - p2[1]) * (p3[0] * p4[1] - p3[1] * p4[0]);
58 
59  c[0] = n_a / d;
60  c[1] = n_b / d;
61 
62  return true;
63  }
64 
66  const cv::Vec2d& p1,
67  const cv::Vec2d& p2,
68  const cv::Vec2d& p3,
69  const cv::Vec2d& p4,
70  cv::Vec2d& c)
71  {
72  // See: "Intersection of two lines in three-space"
73  // by Ronald Goldman from Graphics Gems
74 
75  // Handle case of singe points.
76  if (p1 == p2)
77  {
78  if (PointOnLineSegment(p1, p3, p4))
79  {
80  c = p1;
81  return true;
82  }
83 
84  return false;
85  }
86  else if (p3 == p4)
87  {
88  if (PointOnLineSegment(p3, p1, p2))
89  {
90  c = p3;
91  return true;
92  }
93 
94  return false;
95  }
96 
97  cv::Point2d p(p1);
98  cv::Point2d r(p2 - p1);
99  cv::Point2d q(p3);
100  cv::Point2d s(p4 - p3);
101  cv::Point2d qp(q - p);
102 
103  double rs = r.cross(s);
104  if (std::fabs(rs) > std::numeric_limits<float>::epsilon())
105  {
106  // Explicitly divide components since cv::Point doesn't support division
107  // in indigo.
108  double t = qp.cross(cv::Point2d(s.x / rs, s.y / rs));
109  double u = (qp * -1).cross(cv::Point2d(r.x / -rs, r.y / -rs));
110 
111  if (u >= 0 && t >= 0 && u <= 1.0 && t <= 1.0)
112  {
113  // The lines intersect within the line segments.
114  c = p + t * r;
115  return true;
116  }
117  else
118  {
119  // The lines intersect, but outside the line segments.
120  return false;
121  }
122  }
123  else if (std::fabs(qp.cross(r)) > std::numeric_limits<float>::epsilon())
124  {
125  // The lines are parellel and non-intersecting.
126  return false;
127  }
128  else
129  {
130  // The lines are parellel and coincident.
131  double rlen = r.dot(r);
132  cv::Point2d unit_r(r.x / rlen, r.y / rlen);
133  double t0 = qp.dot(unit_r);
134  double t1 = t0 + s.dot(unit_r);
135 
136  if (t0 > t1)
137  {
138  std::swap(t0, t1);
139  }
140 
141  if (t0 <= 1.0 && t1 >= 0.0)
142  {
143  // The line segments overlap.
144 
145  double t = std::max(0.0, t0);
146  c = p + t * r;
147  return true;
148  }
149 
150  // The line segments don't overlap.
151  return false;
152  }
153  }
154 
156  const cv::Vec2d& p1,
157  const cv::Vec2d& p2,
158  const cv::Vec2d& p3)
159  {
160  // Check if the points are collinear.
161  if (((p2[0] - p1[0]) * (p3[1] - p1[0])) - ((p3[0] - p1[0]) * (p2[1] - p1[1])) > std::numeric_limits<float>::epsilon())
162  {
163  return false;
164  }
165 
166  if (p2[0] != p3[0])
167  {
168  return (p1[0] <= p3[0] && p2[0] <= p1[0]) || (p1[0] <= p2[0] && p3[0] <= p1[0]);
169  }
170  else
171  {
172  return (p1[1] <= p3[1] && p2[1] <= p1[1]) || (p1[1] <= p2[1] && p3[1] <= p1[1]);
173  }
174  }
175 
177  const std::vector<cv::Vec2d>& a,
178  const std::vector<cv::Vec2d>& b)
179  {
180  // Create GEOS polygon from vertices in vector a.
181  geos::geom::CoordinateSequence* a_coords = new geos::geom::CoordinateArraySequence();
182  for (size_t i = 0; i < a.size(); i++)
183  {
184  a_coords->add(geos::geom::Coordinate(a[i][0], a[i][1]));
185  }
186  a_coords->add(a_coords->front());
187 
188  geos::geom::LinearRing* a_ring = geos::geom::GeometryFactory::getDefaultInstance()->createLinearRing(a_coords);
189  geos::geom::Polygon* a_polygon = geos::geom::GeometryFactory::getDefaultInstance()->createPolygon(a_ring, 0);
190  a_polygon->normalize();
191 
192  // Create GEOS polygon from vertices in vector b.
193  geos::geom::CoordinateSequence* b_coords = new geos::geom::CoordinateArraySequence();
194  for (size_t i = 0; i < b.size(); i++)
195  {
196  b_coords->add(geos::geom::Coordinate(b[i][0], b[i][1]));
197  }
198  b_coords->add(b_coords->front());
199 
200  geos::geom::LinearRing* b_ring = geos::geom::GeometryFactory::getDefaultInstance()->createLinearRing(b_coords);
201  geos::geom::Polygon* b_polygon = geos::geom::GeometryFactory::getDefaultInstance()->createPolygon(b_ring, 0);
202  b_polygon->normalize();
203 
204  bool intersects = a_polygon->intersects(b_polygon);
205 
206  // Free polygon objects.
207  delete a_polygon;
208  delete b_polygon;
209 
210  return intersects;
211  }
212 
214  const std::vector<cv::Vec2d>& a,
215  const std::vector<cv::Vec2d>& b)
216  {
217  if (a.size() < 3 || b.size() < 3)
218  {
219  return 0;
220  }
221 
222  double area = 0;
223  // Create GEOS polygon from vertices in vector a.
224  geos::geom::CoordinateSequence* a_coords = new geos::geom::CoordinateArraySequence();
225  for (size_t i = 0; i < a.size(); i++)
226  {
227  a_coords->add(geos::geom::Coordinate(a[i][0], a[i][1]));
228  }
229  a_coords->add(a_coords->front());
230 
231  geos::geom::LinearRing* a_ring = geos::geom::GeometryFactory::getDefaultInstance()->createLinearRing(a_coords);
232  geos::geom::Polygon* a_polygon = geos::geom::GeometryFactory::getDefaultInstance()->createPolygon(a_ring, 0);
233  a_polygon->normalize();
234 
235  // Create GEOS polygon from vertices in vector b.
236  geos::geom::CoordinateSequence* b_coords = new geos::geom::CoordinateArraySequence();
237  for (size_t i = 0; i < b.size(); i++)
238  {
239  b_coords->add(geos::geom::Coordinate(b[i][0], b[i][1]));
240  }
241  b_coords->add(b_coords->front());
242 
243  geos::geom::LinearRing* b_ring = geos::geom::GeometryFactory::getDefaultInstance()->createLinearRing(b_coords);
244  geos::geom::Polygon* b_polygon = geos::geom::GeometryFactory::getDefaultInstance()->createPolygon(b_ring, 0);
245  b_polygon->normalize();
246 
247  try
248  {
249  if (a_polygon->intersects(b_polygon))
250  {
251  area = a_polygon->intersection(b_polygon)->getArea();
252  }
253  }
254  catch (const geos::util::TopologyException& e)
255  {
256  // TODO Fix this
257  }
258 
259  // Free polygon objects.
260  delete a_polygon;
261  delete b_polygon;
262 
263  return area;
264  }
265 }
d
bool LineSegmentIntersection(const cv::Vec2d &p1, const cv::Vec2d &p2, const cv::Vec2d &p3, const cv::Vec2d &p4, cv::Vec2d &c)
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
bool PolygonsIntersect(const std::vector< cv::Vec2d > &a, const std::vector< cv::Vec2d > &b)
XmlRpcServer s
bool LineIntersection(const cv::Vec2d &p1, const cv::Vec2d &p2, const cv::Vec2d &p3, const cv::Vec2d &p4, cv::Vec2d &c)
double PolygonIntersectionArea(const std::vector< cv::Vec2d > &a, const std::vector< cv::Vec2d > &b)
bool PointOnLineSegment(const cv::Vec2d &p1, const cv::Vec2d &p2, const cv::Vec2d &p3)


swri_geometry_util
Author(s): Marc Alban
autogenerated on Fri Jun 7 2019 22:05:39