Loading...
Searching...
No Matches
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 IGNITION_MATH_LINE2_HH_
18#define IGNITION_MATH_LINE2_HH_
19
20#include <algorithm>
22#include <ignition/math/config.hh>
23
24namespace ignition
25{
26 namespace math
27 {
28 inline namespace IGNITION_MATH_VERSION_NAMESPACE
29 {
33 template<typename T>
34 class Line2
35 {
39 public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB)
40 {
41 this->Set(_ptA, _ptB);
42 }
43
49 public: Line2(double _x1, double _y1, double _x2, double _y2)
50 {
51 this->Set(_x1, _y1, _x2, _y2);
52 }
53
57 public: void Set(const math::Vector2<T> &_ptA,
58 const math::Vector2<T> &_ptB)
59 {
60 this->pts[0] = _ptA;
61 this->pts[1] = _ptB;
62 }
63
69 public: void Set(double _x1, double _y1, double _x2, double _y2)
70 {
71 this->pts[0].Set(_x1, _y1);
72 this->pts[1].Set(_x2, _y2);
73 }
74
81 public: double CrossProduct(const Line2<T> &_line) const
82 {
83 return (this->pts[0].X() - this->pts[1].X()) *
84 (_line[0].Y() -_line[1].Y()) -
85 (this->pts[0].Y() - this->pts[1].Y()) *
86 (_line[0].X() - _line[1].X());
87 }
88
91 // (_pt.y - a.y) * (b.x - a.x) - (_pt.x - a.x) * (b.y - a.y)
94 public: double CrossProduct(const Vector2<T> &_pt) const
95 {
96 return (_pt.Y() - this->pts[0].Y()) *
97 (this->pts[1].X() - this->pts[0].X()) -
98 (_pt.X() - this->pts[0].X()) *
99 (this->pts[1].Y() - this->pts[0].Y());
100 }
101
108 public: bool Collinear(const math::Vector2<T> &_pt,
109 double _epsilon = 1e-6) const
110 {
111 return math::equal(this->CrossProduct(_pt),
112 static_cast<T>(0), _epsilon);
113 }
114
122 public: bool Parallel(const math::Line2<T> &_line,
123 double _epsilon = 1e-6) const
124 {
125 return math::equal(this->CrossProduct(_line),
126 static_cast<T>(0), _epsilon);
127 }
128
136 public: bool Collinear(const math::Line2<T> &_line,
137 double _epsilon = 1e-6) const
138 {
139 return this->Parallel(_line, _epsilon) &&
140 this->Intersect(_line, _epsilon);
141 }
142
148 public: bool OnSegment(const math::Vector2<T> &_pt,
149 double _epsilon = 1e-6) const
150 {
151 return this->Collinear(_pt, _epsilon) && this->Within(_pt, _epsilon);
152 }
153
161 public: bool Within(const math::Vector2<T> &_pt,
162 double _epsilon = 1e-6) const
163 {
164 return _pt.X() <= std::max(this->pts[0].X(),
165 this->pts[1].X()) + _epsilon &&
166 _pt.X() >= std::min(this->pts[0].X(),
167 this->pts[1].X()) - _epsilon &&
168 _pt.Y() <= std::max(this->pts[0].Y(),
169 this->pts[1].Y()) + _epsilon &&
170 _pt.Y() >= std::min(this->pts[0].Y(),
171 this->pts[1].Y()) - _epsilon;
172 }
173
179 public: bool Intersect(const Line2<T> &_line,
180 double _epsilon = 1e-6) const
181 {
182 static math::Vector2<T> ignore;
183 return this->Intersect(_line, ignore, _epsilon);
184 }
185
194 public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt,
195 double _epsilon = 1e-6) const
196 {
197 double d = this->CrossProduct(_line);
198
199 // d is zero if the two line are collinear. Must check special
200 // cases.
201 if (math::equal(d, 0.0, _epsilon))
202 {
203 // Check if _line's starting point is on the line.
204 if (this->Within(_line[0], _epsilon))
205 {
206 _pt = _line[0];
207 return true;
208 }
209 // Check if _line's ending point is on the line.
210 else if (this->Within(_line[1], _epsilon))
211 {
212 _pt = _line[1];
213 return true;
214 }
215 // Other wise return false.
216 else
217 return false;
218 }
219
220 _pt.X((_line[0].X() - _line[1].X()) *
221 (this->pts[0].X() * this->pts[1].Y() -
222 this->pts[0].Y() * this->pts[1].X()) -
223 (this->pts[0].X() - this->pts[1].X()) *
224 (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
225
226 _pt.Y((_line[0].Y() - _line[1].Y()) *
227 (this->pts[0].X() * this->pts[1].Y() -
228 this->pts[0].Y() * this->pts[1].X()) -
229 (this->pts[0].Y() - this->pts[1].Y()) *
230 (_line[0].X() * _line[1].Y() - _line[0].Y() * _line[1].X()));
231
232 _pt /= d;
233
234 if (_pt.X() < std::min(this->pts[0].X(), this->pts[1].X()) ||
235 _pt.X() > std::max(this->pts[0].X(), this->pts[1].X()) ||
236 _pt.X() < std::min(_line[0].X(), _line[1].X()) ||
237 _pt.X() > std::max(_line[0].X(), _line[1].X()))
238 {
239 return false;
240 }
241
242 if (_pt.Y() < std::min(this->pts[0].Y(), this->pts[1].Y()) ||
243 _pt.Y() > std::max(this->pts[0].Y(), this->pts[1].Y()) ||
244 _pt.Y() < std::min(_line[0].Y(), _line[1].Y()) ||
245 _pt.Y() > std::max(_line[0].Y(), _line[1].Y()))
246 {
247 return false;
248 }
249
250 return true;
251 }
252
255 public: T Length() const
256 {
257 return sqrt((this->pts[0].X() - this->pts[1].X()) *
258 (this->pts[0].X() - this->pts[1].X()) +
259 (this->pts[0].Y() - this->pts[1].Y()) *
260 (this->pts[0].Y() - this->pts[1].Y()));
261 }
262
265 public: double Slope() const
266 {
267 if (math::equal(this->pts[1].X(), this->pts[0].X()))
268 return NAN_D;
269
270 return (this->pts[1].Y() - this->pts[0].Y()) /
271 static_cast<double>(this->pts[1].X() - this->pts[0].X());
272 }
273
277 public: bool operator==(const Line2<T> &_line) const
278 {
279 return this->pts[0] == _line[0] && this->pts[1] == _line[1];
280 }
281
285 public: bool operator!=(const Line2<T> &_line) const
286 {
287 return !(*this == _line);
288 }
289
293 public: math::Vector2<T> operator[](size_t _index) const
294 {
295 return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
296 }
297
302 public: friend std::ostream &operator<<(
303 std::ostream &_out, const Line2<T> &_line)
304 {
305 _out << _line[0] << " " << _line[1];
306 return _out;
307 }
308
309 private: math::Vector2<T> pts[2];
310 };
311
312
316 }
317 }
318}
319#endif
A two dimensional line segment.
Definition Line2.hh:35
void Set(const math::Vector2< T > &_ptA, const math::Vector2< T > &_ptB)
Set the start and end point of the line segment.
Definition Line2.hh:57
bool Collinear(const math::Line2< T > &_line, double _epsilon=1e-6) const
Check if the given line is collinear with this line.
Definition Line2.hh:136
bool OnSegment(const math::Vector2< T > &_pt, double _epsilon=1e-6) const
Return whether the given point is on this line segment.
Definition Line2.hh:148
bool Intersect(const Line2< T > &_line, math::Vector2< T > &_pt, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition Line2.hh:194
bool operator==(const Line2< T > &_line) const
Equality operator.
Definition Line2.hh:277
Line2(double _x1, double _y1, double _x2, double _y2)
Constructor.
Definition Line2.hh:49
T Length() const
Get the length of the line.
Definition Line2.hh:255
bool Intersect(const Line2< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition Line2.hh:179
void Set(double _x1, double _y1, double _x2, double _y2)
Set the start and end point of the line segment.
Definition Line2.hh:69
double Slope() const
Get the slope of the line.
Definition Line2.hh:265
bool Parallel(const math::Line2< T > &_line, double _epsilon=1e-6) const
Check if the given line is parallel with this line.
Definition Line2.hh:122
double CrossProduct(const Line2< T > &_line) const
Return the cross product of this line and the given line.
Definition Line2.hh:81
Line2(const math::Vector2< T > &_ptA, const math::Vector2< T > &_ptB)
Constructor.
Definition Line2.hh:39
friend std::ostream & operator<<(std::ostream &_out, const Line2< T > &_line)
Stream extraction operator.
Definition Line2.hh:302
math::Vector2< T > operator[](size_t _index) const
Get the start or end point.
Definition Line2.hh:293
bool Collinear(const math::Vector2< T > &_pt, double _epsilon=1e-6) const
Check if the given point is collinear with this line.
Definition Line2.hh:108
bool operator!=(const Line2< T > &_line) const
Inequality operator.
Definition Line2.hh:285
bool Within(const math::Vector2< T > &_pt, double _epsilon=1e-6) const
Check if the given point is between the start and end points of the line segment.
Definition Line2.hh:161
double CrossProduct(const Vector2< T > &_pt) const
Return the cross product of this line and the given point.
Definition Line2.hh:94
Two dimensional (x, y) vector.
Definition Vector2.hh:33
T Y() const
Return the y value.
Definition Vector2.hh:398
T X() const
Return the x value.
Definition Vector2.hh:391
static const double NAN_D
Returns the representation of a quiet not a number (NAN)
Definition Helpers.hh:258
Line2< int > Line2i
Definition Line2.hh:313
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition Helpers.hh:395
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition Helpers.hh:545
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition Helpers.hh:216
Line2< double > Line2d
Definition Line2.hh:314
Line2< float > Line2f
Definition Line2.hh:315
static const size_t IGN_ONE_SIZE_T
size_t type with a value of 1
Definition Helpers.hh:219
Definition Angle.hh:40