Gazebo Math

API Reference

7.4.0
gz/math/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 GZ_MATH_LINE3_HH_
18#define GZ_MATH_LINE3_HH_
19
20#include <algorithm>
21#include <gz/math/Vector3.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 Line3
36 {
38 public: Line3() = default;
39
42 public: Line3(const Line3<T> &_line) = default;
43
47 public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB)
48 {
49 this->Set(_ptA, _ptB);
50 }
51
57 public: Line3(const double _x1, const double _y1,
58 const double _x2, const double _y2)
59 {
60 this->Set(_x1, _y1, _x2, _y2);
61 }
62
70 public: Line3(const double _x1, const double _y1,
71 const double _z1, const double _x2,
72 const double _y2, const double _z2)
73 {
74 this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
75 }
76
80 public: void Set(const math::Vector3<T> &_ptA,
81 const math::Vector3<T> &_ptB)
82 {
83 this->pts[0] = _ptA;
84 this->pts[1] = _ptB;
85 }
86
89 public: void SetA(const math::Vector3<T> &_ptA)
90 {
91 this->pts[0] = _ptA;
92 }
93
96 public: void SetB(const math::Vector3<T> &_ptB)
97 {
98 this->pts[1] = _ptB;
99 }
100
109 public: void Set(const double _x1, const double _y1,
110 const double _x2, const double _y2,
111 const double _z = 0)
112 {
113 this->pts[0].Set(_x1, _y1, _z);
114 this->pts[1].Set(_x2, _y2, _z);
115 }
116
124 public: void Set(const double _x1, const double _y1,
125 const double _z1, const double _x2,
126 const double _y2, const double _z2)
127 {
128 this->pts[0].Set(_x1, _y1, _z1);
129 this->pts[1].Set(_x2, _y2, _z2);
130 }
131
135 {
136 return (this->pts[1] - this->pts[0]).Normalize();
137 }
138
141 public: T Length() const
142 {
143 return this->pts[0].Distance(this->pts[1]);
144 }
145
156 public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
157 const double _epsilon = 1e-6) const
158 {
159 Vector3<T> p13 = this->pts[0] - _line[0];
160 Vector3<T> p43 = _line[1] - _line[0];
161
162 if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
163 std::abs(p43.Z()) < _epsilon)
164 {
165 return false;
166 }
167
168 Vector3<T> p21 = this->pts[1] - this->pts[0];
169
170 if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
171 std::abs(p21.Z()) < _epsilon)
172 {
173 return false;
174 }
175
176 double d1343 = p13.Dot(p43);
177 double d4321 = p43.Dot(p21);
178 double d1321 = p13.Dot(p21);
179 double d4343 = p43.Dot(p43);
180 double d2121 = p21.Dot(p21);
181
182 double denom = d2121 * d4343 - d4321 * d4321;
183
184 // In this case, we choose the first point in this line,
185 // and the closest point in the provided line.
186 if (std::abs(denom) < _epsilon)
187 {
188 double d1 = this->pts[0].Distance(_line[0]);
189 double d2 = this->pts[0].Distance(_line[1]);
190
191 double d3 = this->pts[1].Distance(_line[0]);
192 double d4 = this->pts[1].Distance(_line[1]);
193
194 if (d1 <= d2 && d1 <= d3 && d1 <= d4)
195 {
196 _result.SetA(this->pts[0]);
197 _result.SetB(_line[0]);
198 }
199 else if (d2 <= d3 && d2 <= d4)
200 {
201 _result.SetA(this->pts[0]);
202 _result.SetB(_line[1]);
203 }
204 else if (d3 <= d4)
205 {
206 _result.SetA(this->pts[1]);
207 _result.SetB(_line[0]);
208 }
209 else
210 {
211 _result.SetA(this->pts[1]);
212 _result.SetB(_line[1]);
213 }
214
215 return true;
216 }
217
218 double numer = d1343 * d4321 - d1321 * d4343;
219
220 double mua = clamp(numer / denom, 0.0, 1.0);
221 double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
222
223 _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
224
225 return true;
226 }
227
231 public: T Distance(const Vector3<T> &_pt)
232 {
233 auto line = this->pts[1] - this->pts[0];
234 auto ptTo0 = _pt - this->pts[0];
235 auto ptTo1 = _pt - this->pts[1];
236
237 // Point is projected beyond pt0 or the line has length 0
238 if (ptTo0.Dot(line) <= 0.0)
239 {
240 return ptTo0.Length();
241 }
242
243 // Point is projected beyond pt1
244 if (ptTo1.Dot(line) >= 0.0)
245 {
246 return ptTo1.Length();
247 }
248
249 // Distance to point projected onto line
250 // line.Length() will have to be > 0 at this point otherwise it would
251 // return at line 244.
252 auto d = ptTo0.Cross(line);
253 auto lineLength = line.Length();
254 assert(lineLength > 0);
255 return d.Length() / lineLength;
256 }
257
263 public: bool Intersect(const Line3<T> &_line,
264 double _epsilon = 1e-6) const
265 {
266 static math::Vector3<T> ignore;
267 return this->Intersect(_line, ignore, _epsilon);
268 }
269
275 public: bool Coplanar(const Line3<T> &_line,
276 const double _epsilon = 1e-6) const
277 {
278 return std::abs((_line[0] - this->pts[0]).Dot(
279 (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
280 <= _epsilon;
281 }
282
288 public: bool Parallel(const Line3<T> &_line,
289 const double _epsilon = 1e-6) const
290 {
291 return (this->pts[1] - this->pts[0]).Cross(
292 _line[1] - _line[0]).Length() <= _epsilon;
293 }
294
303 public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
304 double _epsilon = 1e-6) const
305 {
306 // Handle special case when lines are parallel
307 if (this->Parallel(_line, _epsilon))
308 {
309 // Check if _line's starting point is on the line.
310 if (this->Within(_line[0], _epsilon))
311 {
312 _pt = _line[0];
313 return true;
314 }
315 // Check if _line's ending point is on the line.
316 else if (this->Within(_line[1], _epsilon))
317 {
318 _pt = _line[1];
319 return true;
320 }
321 // Otherwise return false.
322 else
323 return false;
324 }
325
326 // Get the line that is the shortest distance between this and _line
327 math::Line3<T> distLine;
328 this->Distance(_line, distLine, _epsilon);
329
330 // If the length of the line is less than epsilon, then they
331 // intersect.
332 if (distLine.Length() < _epsilon)
333 {
334 _pt = distLine[0];
335 return true;
336 }
337
338 return false;
339 }
340
347 public: bool Within(const math::Vector3<T> &_pt,
348 double _epsilon = 1e-6) const
349 {
350 return _pt.X() <= std::max(this->pts[0].X(),
351 this->pts[1].X()) + _epsilon &&
352 _pt.X() >= std::min(this->pts[0].X(),
353 this->pts[1].X()) - _epsilon &&
354 _pt.Y() <= std::max(this->pts[0].Y(),
355 this->pts[1].Y()) + _epsilon &&
356 _pt.Y() >= std::min(this->pts[0].Y(),
357 this->pts[1].Y()) - _epsilon &&
358 _pt.Z() <= std::max(this->pts[0].Z(),
359 this->pts[1].Z()) + _epsilon &&
360 _pt.Z() >= std::min(this->pts[0].Z(),
361 this->pts[1].Z()) - _epsilon;
362 }
363
367 public: bool operator==(const Line3<T> &_line) const
368 {
369 return this->pts[0] == _line[0] && this->pts[1] == _line[1];
370 }
371
375 public: bool operator!=(const Line3<T> &_line) const
376 {
377 return !(*this == _line);
378 }
379
383 public: math::Vector3<T> operator[](const size_t _index) const
384 {
385 return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
386 }
387
392 public: friend std::ostream &operator<<(
393 std::ostream &_out, const Line3<T> &_line)
394 {
395 _out << _line[0] << " " << _line[1];
396 return _out;
397 }
398
402 public: Line3 &operator=(const Line3<T> &_line) = default;
403
405 private: math::Vector3<T> pts[2];
406 };
407
411 }
412 }
413}
414#endif