geometry_util.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 
31 
32 namespace swri_geometry_util
33 {
35  const tf::Vector3& plane_normal,
36  const tf::Vector3& plane_point,
37  const tf::Vector3& point)
38  {
39  return plane_normal.normalized().dot(point - plane_point);
40  }
41 
42  tf::Vector3 ProjectPointToPlane(
43  const tf::Vector3& plane_normal,
44  const tf::Vector3& plane_point,
45  const tf::Vector3& point)
46  {
47  double d = DistanceFromPlane(plane_normal, plane_point, point);
48  return point - plane_normal * d;
49  }
50 
52  const tf::Vector3& line_start,
53  const tf::Vector3& line_end,
54  const tf::Vector3& point)
55  {
56  return point.distance(ProjectToLineSegment(line_start, line_end, point));
57  }
58 
60  const cv::Vec2d& line_start,
61  const cv::Vec2d& line_end,
62  const cv::Vec2d& point)
63  {
64  const cv::Vec2d proj = ProjectToLineSegment(line_start, line_end, point);
65  return std::sqrt(
66  (point[0] - proj[0]) * (point[0] - proj[0]) +
67  (point[1] - proj[1]) * (point[1] - proj[1]));
68  }
69 
70  tf::Vector3 ProjectToLineSegment(
71  const tf::Vector3& line_start,
72  const tf::Vector3& line_end,
73  const tf::Vector3& point)
74  {
75  tf::Vector3 v = line_end - line_start;
76  tf::Vector3 r = point - line_start;
77 
78  double t = r.dot(v);
79  if (t <= 0)
80  {
81  return line_start;
82  }
83 
84  double b = v.dot(v);
85  if (t >= b)
86  {
87  return line_end;
88  }
89 
90  return line_start + (t / b) * v;
91  }
92 
93 
95  const cv::Vec2d& line_start,
96  const cv::Vec2d& line_end,
97  const cv::Vec2d& point)
98  {
99  cv::Point2d v(line_end - line_start);
100  cv::Point2d r(point - line_start);
101 
102  double t = r.dot(v);
103  if (t <= 0)
104  {
105  return line_start;
106  }
107 
108  double b = v.dot(v);
109  if (t >= b)
110  {
111  return line_end;
112  }
113 
114  // Explicitly multiply components since cv::Point doesn't support operation
115  // in indigo.
116  return line_start + cv::Vec2d(v.x * (t / b), v.y * (t / b));
117  }
118 
120  const std::vector<cv::Vec2d>& polygon,
121  const cv::Vec2d& point)
122  {
123  if (polygon.size() < 2)
124  {
125  return false;
126  }
127 
128  bool is_inside = false;
129  if (((polygon.front()[1] > point[1]) != (polygon.back()[1] > point[1])) &&
130  (point[0] < (polygon.back()[0] - polygon.front()[0]) * (point[1] - polygon.front()[1]) /
131  (polygon.back()[1] - polygon.front()[1]) + polygon.front()[0]))
132  {
133  is_inside = !is_inside;
134  }
135 
136  for (size_t i = 1; i < polygon.size(); i++)
137  {
138  if (((polygon[i][1] > point[1]) != (polygon[i - 1][1] > point[1])) &&
139  (point[0] < (polygon[i - 1][0] - polygon[i][0]) * (point[1] - polygon[i][1]) /
140  (polygon[i - 1][1] - polygon[i][1]) + polygon[i][0]))
141  {
142  is_inside = !is_inside;
143  }
144  }
145 
146  return is_inside;
147  }
148 
150  const std::vector<cv::Vec2d>& polygon,
151  const cv::Vec2d& point)
152  {
153  if (polygon.empty())
154  {
155  return -1;
156  }
157 
158  double dist = DistanceFromLineSegment(polygon.front(), polygon.back(), point);
159  for (size_t i = 1; i < polygon.size(); i++)
160  {
161  dist = std::min(dist, DistanceFromLineSegment(polygon[i], polygon[i - 1], point));
162  }
163 
164  return dist;
165  }
166 
168  const tf::Vector3& a1,
169  const tf::Vector3& a2,
170  const tf::Vector3& b1,
171  const tf::Vector3& b2,
172  tf::Vector3& point)
173  {
174  tf::Vector3 u = a1 - a2;
175  tf::Vector3 v = b1 - b2;
176  if (u.length() == 0 || v.length() == 0)
177  {
178  return false;
179  }
180  tf::Vector3 w = u.cross(v);
181  tf::Vector3 s = b1 - a1;
182  if (s.length() == 0)
183  {
184  point = a1;
185  return true;
186  }
187  double f = w.dot(w);
188  if (f == 0)
189  {
190  return false;
191  }
192  tf::Vector3 x = a1 + u * (s.cross(v).dot(w) / f);
193  tf::Vector3 y = b1 + v * (s.cross(u).dot(w) / f);
194  point = (x + y) / 2;
195  return true;
196  }
197 }
d
bool PointInPolygon(const std::vector< cv::Vec2d > &polygon, const cv::Vec2d &point)
f
XmlRpcServer s
double DistanceFromPolygon(const std::vector< cv::Vec2d > &polygon, const cv::Vec2d &point)
tf::Vector3 ProjectPointToPlane(const tf::Vector3 &plane_normal, const tf::Vector3 &plane_point, const tf::Vector3 &point)
tf::Vector3 ProjectToLineSegment(const tf::Vector3 &line_start, const tf::Vector3 &line_end, const tf::Vector3 &point)
double DistanceFromPlane(const tf::Vector3 &plane_normal, const tf::Vector3 &plane_point, const tf::Vector3 &point)
bool ClosestPointToLines(const tf::Vector3 &a1, const tf::Vector3 &a2, const tf::Vector3 &b1, const tf::Vector3 &b2, tf::Vector3 &point)
double DistanceFromLineSegment(const tf::Vector3 &line_start, const tf::Vector3 &line_end, const tf::Vector3 &point)


swri_geometry_util
Author(s): Marc Alban
autogenerated on Sat Jan 21 2023 03:13:10