Loading...
Searching...
No Matches
Line3.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015 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_LINE3_HH_
18#define IGNITION_MATH_LINE3_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 Line3
35 {
37 public: Line3() = default;
38
41 public: Line3(const Line3<T> &_line)
42 {
43 this->pts[0] = _line[0];
44 this->pts[1] = _line[1];
45 }
46
50 public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
51 {
52 this->Set(_ptA, _ptB);
53 }
54
60 public: Line3(const double _x1, const double _y1,
61 const double _x2, const double _y2)
62 {
63 this->Set(_x1, _y1, _x2, _y2);
64 }
65
73 public: Line3(const double _x1, const double _y1,
74 const double _z1, const double _x2,
75 const double _y2, const double _z2)
76 {
77 this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
78 }
79
83 public: void Set(const math::Vector3<T> &_ptA,
84 const math::Vector3<T> &_ptB)
85 {
86 this->pts[0] = _ptA;
87 this->pts[1] = _ptB;
88 }
89
92 public: void SetA(const math::Vector3<T> &_ptA)
93 {
94 this->pts[0] = _ptA;
95 }
96
99 public: void SetB(const math::Vector3<T> &_ptB)
100 {
101 this->pts[1] = _ptB;
102 }
103
112 public: void Set(const double _x1, const double _y1,
113 const double _x2, const double _y2,
114 const double _z = 0)
115 {
116 this->pts[0].Set(_x1, _y1, _z);
117 this->pts[1].Set(_x2, _y2, _z);
118 }
119
127 public: void Set(const double _x1, const double _y1,
128 const double _z1, const double _x2,
129 const double _y2, const double _z2)
130 {
131 this->pts[0].Set(_x1, _y1, _z1);
132 this->pts[1].Set(_x2, _y2, _z2);
133 }
134
138 {
139 return (this->pts[1] - this->pts[0]).Normalize();
140 }
141
144 public: T Length() const
145 {
146 return this->pts[0].Distance(this->pts[1]);
147 }
148
158 public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
159 const double _epsilon = 1e-6) const
160 {
161 Vector3<T> p13 = this->pts[0] - _line[0];
162 Vector3<T> p43 = _line[1] - _line[0];
163
164 if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
165 std::abs(p43.Z()) < _epsilon)
166 {
167 return false;
168 }
169
170 Vector3<T> p21 = this->pts[1] - this->pts[0];
171
172 if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
173 std::abs(p21.Z()) < _epsilon)
174 {
175 return false;
176 }
177
178 double d1343 = p13.Dot(p43);
179 double d4321 = p43.Dot(p21);
180 double d1321 = p13.Dot(p21);
181 double d4343 = p43.Dot(p43);
182 double d2121 = p21.Dot(p21);
183
184 double denom = d2121 * d4343 - d4321 * d4321;
185
186 // In this case, we choose the first point in this line,
187 // and the closest point in the provided line.
188 if (std::abs(denom) < _epsilon)
189 {
190 double d1 = this->pts[0].Distance(_line[0]);
191 double d2 = this->pts[0].Distance(_line[1]);
192
193 double d3 = this->pts[1].Distance(_line[0]);
194 double d4 = this->pts[1].Distance(_line[1]);
195
196 if (d1 <= d2 && d1 <= d3 && d1 <= d4)
197 {
198 _result.SetA(this->pts[0]);
199 _result.SetB(_line[0]);
200 }
201 else if (d2 <= d3 && d2 <= d4)
202 {
203 _result.SetA(this->pts[0]);
204 _result.SetB(_line[1]);
205 }
206 else if (d3 <= d4)
207 {
208 _result.SetA(this->pts[1]);
209 _result.SetB(_line[0]);
210 }
211 else
212 {
213 _result.SetA(this->pts[1]);
214 _result.SetB(_line[1]);
215 }
216
217 return true;
218 }
219
220 double numer = d1343 * d4321 - d1321 * d4343;
221
222 double mua = clamp(numer / denom, 0.0, 1.0);
223 double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
224
225 _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
226
227 return true;
228 }
229
235 public: bool Intersect(const Line3<T> &_line,
236 double _epsilon = 1e-6) const
237 {
238 static math::Vector3<T> ignore;
239 return this->Intersect(_line, ignore, _epsilon);
240 }
241
247 public: bool Coplanar(const Line3<T> &_line,
248 const double _epsilon = 1e-6) const
249 {
250 return std::abs((_line[0] - this->pts[0]).Dot(
251 (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
252 <= _epsilon;
253 }
254
260 public: bool Parallel(const Line3<T> &_line,
261 const double _epsilon = 1e-6) const
262 {
263 return (this->pts[1] - this->pts[0]).Cross(
264 _line[1] - _line[0]).Length() <= _epsilon;
265 }
266
275 public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
276 double _epsilon = 1e-6) const
277 {
278 // Handle special case when lines are parallel
279 if (this->Parallel(_line, _epsilon))
280 {
281 // Check if _line's starting point is on the line.
282 if (this->Within(_line[0], _epsilon))
283 {
284 _pt = _line[0];
285 return true;
286 }
287 // Check if _line's ending point is on the line.
288 else if (this->Within(_line[1], _epsilon))
289 {
290 _pt = _line[1];
291 return true;
292 }
293 // Otherwise return false.
294 else
295 return false;
296 }
297
298 // Get the line that is the shortest distance between this and _line
299 math::Line3<T> distLine;
300 this->Distance(_line, distLine, _epsilon);
301
302 // If the length of the line is less than epsilon, then they
303 // intersect.
304 if (distLine.Length() < _epsilon)
305 {
306 _pt = distLine[0];
307 return true;
308 }
309
310 return false;
311 }
312
319 public: bool Within(const math::Vector3<T> &_pt,
320 double _epsilon = 1e-6) const
321 {
322 return _pt.X() <= std::max(this->pts[0].X(),
323 this->pts[1].X()) + _epsilon &&
324 _pt.X() >= std::min(this->pts[0].X(),
325 this->pts[1].X()) - _epsilon &&
326 _pt.Y() <= std::max(this->pts[0].Y(),
327 this->pts[1].Y()) + _epsilon &&
328 _pt.Y() >= std::min(this->pts[0].Y(),
329 this->pts[1].Y()) - _epsilon &&
330 _pt.Z() <= std::max(this->pts[0].Z(),
331 this->pts[1].Z()) + _epsilon &&
332 _pt.Z() >= std::min(this->pts[0].Z(),
333 this->pts[1].Z()) - _epsilon;
334 }
335
339 public: bool operator==(const Line3<T> &_line) const
340 {
341 return this->pts[0] == _line[0] && this->pts[1] == _line[1];
342 }
343
347 public: bool operator!=(const Line3<T> &_line) const
348 {
349 return !(*this == _line);
350 }
351
355 public: math::Vector3<T> operator[](const size_t _index) const
356 {
357 return this->pts[clamp(_index, IGN_ZERO_SIZE_T, IGN_ONE_SIZE_T)];
358 }
359
364 public: friend std::ostream &operator<<(
365 std::ostream &_out, const Line3<T> &_line)
366 {
367 _out << _line[0] << " " << _line[1];
368 return _out;
369 }
370
374 public: Line3 &operator=(const Line3<T> &_line)
375 {
376 this->pts[0] = _line[0];
377 this->pts[1] = _line[1];
378
379 return *this;
380 }
381
383 private: math::Vector3<T> pts[2];
384 };
385
389 }
390 }
391}
392#endif
A three dimensional line segment.
Definition Line3.hh:35
bool Parallel(const Line3< T > &_line, const double _epsilon=1e-6) const
Test if this line and the given line are parallel.
Definition Line3.hh:260
bool Within(const math::Vector3< T > &_pt, double _epsilon=1e-6) const
Check if the given point is between the start and end points of the line segment.
Definition Line3.hh:319
friend std::ostream & operator<<(std::ostream &_out, const Line3< T > &_line)
Stream extraction operator.
Definition Line3.hh:364
void Set(const double _x1, const double _y1, const double _x2, const double _y2, const double _z=0)
Set the start and end point of the line segment, assuming that both points have the same height.
Definition Line3.hh:112
void Set(const double _x1, const double _y1, const double _z1, const double _x2, const double _y2, const double _z2)
Set the start and end point of the line segment.
Definition Line3.hh:127
bool Distance(const Line3< T > &_line, Line3< T > &_result, const double _epsilon=1e-6) const
Get the shortest line between this line and the provided line.
Definition Line3.hh:158
Line3(const math::Vector3< T > &_ptA, const math::Vector3< T > &_ptB)
Constructor.
Definition Line3.hh:50
math::Vector3< T > operator[](const size_t _index) const
Get the start or end point.
Definition Line3.hh:355
Line3(const Line3< T > &_line)
Copy constructor.
Definition Line3.hh:41
bool Coplanar(const Line3< T > &_line, const double _epsilon=1e-6) const
Test if this line and the given line are coplanar.
Definition Line3.hh:247
void SetA(const math::Vector3< T > &_ptA)
Set the start point of the line segment.
Definition Line3.hh:92
bool Intersect(const Line3< T > &_line, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition Line3.hh:235
void Set(const math::Vector3< T > &_ptA, const math::Vector3< T > &_ptB)
Set the start and end point of the line segment.
Definition Line3.hh:83
bool operator!=(const Line3< T > &_line) const
Inequality operator.
Definition Line3.hh:347
bool Intersect(const Line3< T > &_line, math::Vector3< T > &_pt, double _epsilon=1e-6) const
Check if this line intersects the given line segment.
Definition Line3.hh:275
bool operator==(const Line3< T > &_line) const
Equality operator.
Definition Line3.hh:339
Line3(const double _x1, const double _y1, const double _z1, const double _x2, const double _y2, const double _z2)
Constructor.
Definition Line3.hh:73
T Length() const
Get the length of the line.
Definition Line3.hh:144
math::Vector3< T > Direction() const
Get the direction of the line.
Definition Line3.hh:137
Line3(const double _x1, const double _y1, const double _x2, const double _y2)
2D Constructor where Z coordinates are 0
Definition Line3.hh:60
void SetB(const math::Vector3< T > &_ptB)
Set the end point of the line segment.
Definition Line3.hh:99
Line3 & operator=(const Line3< T > &_line)
Assignment operator.
Definition Line3.hh:374
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
T Z() const
Get the z value.
Definition Vector3.hh:661
T Y() const
Get the y value.
Definition Vector3.hh:654
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition Vector3.hh:198
T X() const
Get the x value.
Definition Vector3.hh:647
Line3< float > Line3f
Definition Line3.hh:388
T clamp(T _v, T _min, T _max)
Simple clamping function.
Definition Helpers.hh:395
Line3< double > Line3d
Definition Line3.hh:387
Line3< int > Line3i
Definition Line3.hh:386
static const size_t IGN_ZERO_SIZE_T
size_t type with a value of 0
Definition Helpers.hh:216
static const size_t IGN_ONE_SIZE_T
size_t type with a value of 1
Definition Helpers.hh:219
Definition Angle.hh:40