Gazebo Math

API Reference

8.1.0
Line2.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2014 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16*/
17#ifndef GZ_MATH_LINE2_HH_
18#define GZ_MATH_LINE2_HH_
19
20#include <algorithm>
21#include <gz/math/Vector2.hh>
22#include <gz/math/config.hh>
23
24namespace gz::math
25{
26 // Inline bracket to help doxygen filtering.
27 inline namespace GZ_MATH_VERSION_NAMESPACE {
28 //
32 template<typename T>
33 class Line2
34 {
39 {
40 this->Set(_ptA, _ptB);
41 }
42
48 public: Line2(double _x1, double _y1, double _x2, double _y2)
49 {
50 this->Set(_x1, _y1, _x2, _y2);
51 }
52
56 public: void Set(const math::Vector2<T> &_ptA,
58 {
59 this->pts[0] = _ptA;
60 this->pts[1] = _ptB;
61 }
62
68 public: void Set(double _x1, double _y1, double _x2, double _y2)
69 {
70 this->pts[0].Set(_x1, _y1);
71 this->pts[1].Set(_x2, _y2);
72 }
73
80 public: double CrossProduct(const Line2<T> &_line) const
81 {
82 return (this->pts[0].X() - this->pts[1].X()) *
83 (_line[0].Y() -_line[1].Y()) -
84 (this->pts[0].Y() - this->pts[1].Y()) *
85 (_line[0].X() - _line[1].X());
86 }
87
90 // (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
93 public: double CrossProduct(const Vector2<T> &_pt) const
94 {
95 return (_pt.Y() - this->pts[0].Y()) *
96 (this->pts[1].X() - this->pts[0].X()) -
97 (_pt.X() - this->pts[0].X()) *
98 (this->pts[1].Y() - this->pts[0].Y());
99 }
100
107 public: bool Collinear(const math::Vector2<T> &_pt,
108 double _epsilon = 1e-6) const
109 {
110 return math::equal(this->CrossProduct(_pt),
111 0., _epsilon);
112 }
113
121 public: bool Parallel(const math::Line2<T> &_line,
122 double _epsilon = 1e-6) const
123 {
124 return math::equal(this->CrossProduct(_line),
125 0., _epsilon);
126 }
127
135 public: bool Collinear(const math::Line2<T> &_line,
136 double _epsilon = 1e-6) const
137 {
138 return this->Parallel(_line, _epsilon) &&
139 this->Intersect(_line, _epsilon);
140 }
141
147 public: bool OnSegment(const math::Vector2<T> &_pt,
148 double _epsilon = 1e-6) const
149 {
150 return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
151 }
152
160 public: bool Within(const math::Vector2<T> &_pt,
161 double _epsilon = 1e-6) const
162 {
163 return _pt.X() <= std::max(this->pts[0].X(),
164 this->pts[1].X()) + _epsilon &&
165 _pt.X() >= std::min(this->pts[0].X(),
166 this->pts[1].X()) - _epsilon &&
167 _pt.Y() <= std::max(this->pts[0].Y(),
168 this->pts[1].Y()) + _epsilon &&
169 _pt.Y() >= std::min(this->pts[0].Y(),
170 this->pts[1].Y()) - _epsilon;
171 }
172
178 public: bool Intersect(const Line2<T> &_line,
179 double _epsilon = 1e-6) const
180 {
181 math::Vector2<T> ignore;
182 return this->Intersect(_line, ignore, _epsilon);
183 }
184
194 double _epsilon = 1e-6) const
195 {
196 double d = this->CrossProduct(_line);
197
198 // d is zero if the two line are collinear. Must check special
199 // cases.
200 if (math::equal(d, 0.0, _epsilon))
201 {
202 // Check if _line's starting point is on the line.
203 if (this->Within(_line[0], _epsilon))
204 {
205 _pt = _line[0];
206 return true;
207 }
208 // Check if _line's ending point is on the line.
209 else if (this->Within(_line[1], _epsilon))
210 {
211 _pt = _line[1];
212 return true;
213 }
214 // Other wise return false.
215 else
216 return false;
217 }
218
219 _pt.X((_line[0].X() - _line[1].X()) *
220 (this->pts[0].X() * this->pts[1].Y() -
221 this->pts[0].Y() * this->pts[1].X()) -
222 (this->pts[0].X() - this->pts[1].X()) *
223 (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
224
225 _pt.Y((_line[0].Y() - _line[1].Y()) *
226 (this->pts[0].X() * this->pts[1].Y() -
227 this->pts[0].Y() * this->pts[1].X()) -
228 (this->pts[0].Y() - this->pts[1].Y()) *
229 (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
230
231 _pt /= d;
232
233 if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
234 _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
235 _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
236 _pt.X() > std::max(_line[0].X(), _line[1].X()))
237 {
238 return false;
239 }
240
241 if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
242 _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
243 _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
244 _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
245 {
246 return false;
247 }
248
249 return true;
250 }
251
254 public: T Length() const
255 {
256 return sqrt((this->pts[0].X() - this->pts[1].X()) *
257 (this->pts[0].X() - this->pts[1].X()) +
258 (this->pts[0].Y() - this->pts[1].Y()) *
259 (this->pts[0].Y() - this->pts[1].Y()));
260 }
261
264 public: double Slope() const
265 {
266 if (math::equal(this->pts[1].X(), this->pts[0].X()))
267 return NAN_D;
268
269 return (this->pts[1].Y() - this->pts[0].Y()) /
270 static_cast<double>(this->pts[1].X() - this->pts[0].X());
271 }
272
276 public: bool operator==(const Line2<T> &_line) const
277 {
278 return this->pts[0] == _line[0] && this->pts[1] == _line[1];
279 }
280
284 public: bool operator!=(const Line2<T> &_line) const
285 {
286 return !(*this == _line);
287 }
288
292 public: math::Vector2<T> operator[](size_t _index) const
293 {
294 return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
295 }
296
301 public: friend std::ostream &operator<<(
303 {
304 _out << _line[0] << " " << _line[1];
305 return _out;
306 }
307
308 private: math::Vector2<T> pts[2];
309 };
310
311
315 } // namespace GZ_MATH_VERSION_NAMESPACE
316} // namespace gz::math
317#endif // GZ_MATH_LINE2_HH_