Gazebo Math

API Reference

7.4.0
gz/math/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
25{
26 namespace math
27 {
28 // Inline bracket to help doxygen filtering.
29 inline namespace GZ_MATH_VERSION_NAMESPACE {
30 //
34 template<typename T>
35 class Line2
36 {
40 public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
41 {
42 this->Set(_ptA, _ptB);
43 }
44
50 public: Line2(double _x1, double _y1, double _x2, double _y2)
51 {
52 this->Set(_x1, _y1, _x2, _y2);
53 }
54
58 public: void Set(const math::Vector2<T> &_ptA,
59 const math::Vector2<T> &_ptB)
60 {
61 this->pts[0] = _ptA;
62 this->pts[1] = _ptB;
63 }
64
70 public: void Set(double _x1, double _y1, double _x2, double _y2)
71 {
72 this->pts[0].Set(_x1, _y1);
73 this->pts[1].Set(_x2, _y2);
74 }
75
82 public: double CrossProduct(const Line2<T> &_line) const
83 {
84 return (this->pts[0].X() - this->pts[1].X()) *
85 (_line[0].Y() -_line[1].Y()) -
86 (this->pts[0].Y() - this->pts[1].Y()) *
87 (_line[0].X() - _line[1].X());
88 }
89
92 // (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
95 public: double CrossProduct(const Vector2<T> &_pt) const
96 {
97 return (_pt.Y() - this->pts[0].Y()) *
98 (this->pts[1].X() - this->pts[0].X()) -
99 (_pt.X() - this->pts[0].X()) *
100 (this->pts[1].Y() - this->pts[0].Y());
101 }
102
109 public: bool Collinear(const math::Vector2<T> &_pt,
110 double _epsilon = 1e-6) const
111 {
112 return math::equal(this->CrossProduct(_pt),
113 0., _epsilon);
114 }
115
123 public: bool Parallel(const math::Line2<T> &_line,
124 double _epsilon = 1e-6) const
125 {
126 return math::equal(this->CrossProduct(_line),
127 0., _epsilon);
128 }
129
137 public: bool Collinear(const math::Line2<T> &_line,
138 double _epsilon = 1e-6) const
139 {
140 return this->Parallel(_line, _epsilon) &&
141 this->Intersect(_line, _epsilon);
142 }
143
149 public: bool OnSegment(const math::Vector2<T> &_pt,
150 double _epsilon = 1e-6) const
151 {
152 return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
153 }
154
162 public: bool Within(const math::Vector2<T> &_pt,
163 double _epsilon = 1e-6) const
164 {
165 return _pt.X() <= std::max(this->pts[0].X(),
166 this->pts[1].X()) + _epsilon &&
167 _pt.X() >= std::min(this->pts[0].X(),
168 this->pts[1].X()) - _epsilon &&
169 _pt.Y() <= std::max(this->pts[0].Y(),
170 this->pts[1].Y()) + _epsilon &&
171 _pt.Y() >= std::min(this->pts[0].Y(),
172 this->pts[1].Y()) - _epsilon;
173 }
174
180 public: bool Intersect(const Line2<T> &_line,
181 double _epsilon = 1e-6) const
182 {
183 static math::Vector2<T> ignore;
184 return this->Intersect(_line, ignore, _epsilon);
185 }
186
195 public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
196 double _epsilon = 1e-6) const
197 {
198 double d = this->CrossProduct(_line);
199
200 // d is zero if the two line are collinear. Must check special
201 // cases.
202 if (math::equal(d, 0.0, _epsilon))
203 {
204 // Check if _line's starting point is on the line.
205 if (this->Within(_line[0], _epsilon))
206 {
207 _pt = _line[0];
208 return true;
209 }
210 // Check if _line's ending point is on the line.
211 else if (this->Within(_line[1], _epsilon))
212 {
213 _pt = _line[1];
214 return true;
215 }
216 // Other wise return false.
217 else
218 return false;
219 }
220
221 _pt.X((_line[0].X() - _line[1].X()) *
222 (this->pts[0].X() * this->pts[1].Y() -
223 this->pts[0].Y() * this->pts[1].X()) -
224 (this->pts[0].X() - this->pts[1].X()) *
225 (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
226
227 _pt.Y((_line[0].Y() - _line[1].Y()) *
228 (this->pts[0].X() * this->pts[1].Y() -
229 this->pts[0].Y() * this->pts[1].X()) -
230 (this->pts[0].Y() - this->pts[1].Y()) *
231 (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
232
233 _pt /= d;
234
235 if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
236 _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
237 _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
238 _pt.X() > std::max(_line[0].X(), _line[1].X()))
239 {
240 return false;
241 }
242
243 if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
244 _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
245 _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
246 _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
247 {
248 return false;
249 }
250
251 return true;
252 }
253
256 public: T Length() const
257 {
258 return sqrt((this->pts[0].X() - this->pts[1].X()) *
259 (this->pts[0].X() - this->pts[1].X()) +
260 (this->pts[0].Y() - this->pts[1].Y()) *
261 (this->pts[0].Y() - this->pts[1].Y()));
262 }
263
266 public: double Slope() const
267 {
268 if (math::equal(this->pts[1].X(), this->pts[0].X()))
269 return NAN_D;
270
271 return (this->pts[1].Y() - this->pts[0].Y()) /
272 static_cast<double>(this->pts[1].X() - this->pts[0].X());
273 }
274
278 public: bool operator==(const Line2<T> &_line) const
279 {
280 return this->pts[0] == _line[0] && this->pts[1] == _line[1];
281 }
282
286 public: bool operator!=(const Line2<T> &_line) const
287 {
288 return !(*this == _line);
289 }
290
294 public: math::Vector2<T> operator[](size_t _index) const
295 {
296 return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
297 }
298
303 public: friend std::ostream &operator<<(
304 std::ostream &_out, const Line2<T> &_line)
305 {
306 _out << _line[0] << " " << _line[1];
307 return _out;
308 }
309
310 private: math::Vector2<T> pts[2];
311 };
312
313
317 }
318 }
319}
320#endif