Gazebo Math

API Reference

8.1.0
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::math
25{
26 // Inline bracket to help doxygen filtering.
27 inline namespace GZ_MATH_VERSION_NAMESPACE {
28 //
32 template<typename T>
33 class Line3
34 {
36 public: Line3() = default;
37
42 {
43 this->Set(_ptA, _ptB);
44 }
45
51 public: Line3(const double _x1, const double _y1,
52 const double _x2, const double _y2)
53 {
54 this->Set(_x1, _y1, _x2, _y2);
55 }
56
64 public: Line3(const double _x1, const double _y1,
65 const double _z1, const double _x2,
66 const double _y2, const double _z2)
67 {
68 this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
69 }
70
74 public: void Set(const math::Vector3<T> &_ptA,
76 {
77 this->pts[0] = _ptA;
78 this->pts[1] = _ptB;
79 }
80
83 public: void SetA(const math::Vector3<T> &_ptA)
84 {
85 this->pts[0] = _ptA;
86 }
87
90 public: void SetB(const math::Vector3<T> &_ptB)
91 {
92 this->pts[1] = _ptB;
93 }
94
103 public: void Set(const double _x1, const double _y1,
104 const double _x2, const double _y2,
105 const double _z = 0)
106 {
107 this->pts[0].Set(_x1, _y1, _z);
108 this->pts[1].Set(_x2, _y2, _z);
109 }
110
118 public: void Set(const double _x1, const double _y1,
119 const double _z1, const double _x2,
120 const double _y2, const double _z2)
121 {
122 this->pts[0].Set(_x1, _y1, _z1);
123 this->pts[1].Set(_x2, _y2, _z2);
124 }
125
129 {
130 return (this->pts[1] - this->pts[0]).Normalize();
131 }
132
135 public: T Length() const
136 {
137 return this->pts[0].Distance(this->pts[1]);
138 }
139
150 public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
151 const double _epsilon = 1e-6) const
152 {
153 Vector3<T> p13 = this->pts[0] - _line[0];
154 Vector3<T> p43 = _line[1] - _line[0];
155
156 if (std::abs(p43.X()) < _epsilon && std::abs(p43.Y()) < _epsilon &&
157 std::abs(p43.Z()) < _epsilon)
158 {
159 return false;
160 }
161
162 Vector3<T> p21 = this->pts[1] - this->pts[0];
163
164 if (std::abs(p21.X()) < _epsilon && std::abs(p21.Y()) < _epsilon &&
165 std::abs(p21.Z()) < _epsilon)
166 {
167 return false;
168 }
169
170 double d1343 = p13.Dot(p43);
171 double d4321 = p43.Dot(p21);
172 double d1321 = p13.Dot(p21);
173 double d4343 = p43.Dot(p43);
174 double d2121 = p21.Dot(p21);
175
176 double denom = d2121 * d4343 - d4321 * d4321;
177
178 // In this case, we choose the first point in this line,
179 // and the closest point in the provided line.
180 if (std::abs(denom) < _epsilon)
181 {
182 double d1 = this->pts[0].Distance(_line[0]);
183 double d2 = this->pts[0].Distance(_line[1]);
184
185 double d3 = this->pts[1].Distance(_line[0]);
186 double d4 = this->pts[1].Distance(_line[1]);
187
188 if (d1 <= d2 && d1 <= d3 && d1 <= d4)
189 {
190 _result.SetA(this->pts[0]);
191 _result.SetB(_line[0]);
192 }
193 else if (d2 <= d3 && d2 <= d4)
194 {
195 _result.SetA(this->pts[0]);
196 _result.SetB(_line[1]);
197 }
198 else if (d3 <= d4)
199 {
200 _result.SetA(this->pts[1]);
201 _result.SetB(_line[0]);
202 }
203 else
204 {
205 _result.SetA(this->pts[1]);
206 _result.SetB(_line[1]);
207 }
208
209 return true;
210 }
211
212 double numer = d1343 * d4321 - d1321 * d4343;
213
214 double mua = clamp(numer / denom, 0.0, 1.0);
215 double mub = clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
216
217 _result.Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
218
219 return true;
220 }
221
225 public: T Distance(const Vector3<T> &_pt)
226 {
227 auto line = this->pts[1] - this->pts[0];
228 auto ptTo0 = _pt - this->pts[0];
229 auto ptTo1 = _pt - this->pts[1];
230
231 // Point is projected beyond pt0 or the line has length 0
232 if (ptTo0.Dot(line) <= 0.0)
233 {
234 return ptTo0.Length();
235 }
236
237 // Point is projected beyond pt1
238 if (ptTo1.Dot(line) >= 0.0)
239 {
240 return ptTo1.Length();
241 }
242
243 // Distance to point projected onto line
244 // line.Length() will have to be > 0 at this point otherwise it would
245 // return at line 244.
246 auto d = ptTo0.Cross(line);
247 auto lineLength = line.Length();
248 assert(lineLength > 0);
249 return d.Length() / lineLength;
250 }
251
257 public: bool Intersect(const Line3<T> &_line,
258 double _epsilon = 1e-6) const
259 {
260 math::Vector3<T> ignore;
261 return this->Intersect(_line, ignore, _epsilon);
262 }
263
269 public: bool Coplanar(const Line3<T> &_line,
270 const double _epsilon = 1e-6) const
271 {
272 return std::abs((_line[0] - this->pts[0]).Dot(
273 (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
274 <= _epsilon;
275 }
276
282 public: bool Parallel(const Line3<T> &_line,
283 const double _epsilon = 1e-6) const
284 {
285 return (this->pts[1] - this->pts[0]).Cross(
286 _line[1] - _line[0]).Length() <= _epsilon;
287 }
288
298 double _epsilon = 1e-6) const
299 {
300 // Handle special case when lines are parallel
301 if (this->Parallel(_line, _epsilon))
302 {
303 // Check if _line's starting point is on the line.
304 if (this->Within(_line[0], _epsilon))
305 {
306 _pt = _line[0];
307 return true;
308 }
309 // Check if _line's ending point is on the line.
310 else if (this->Within(_line[1], _epsilon))
311 {
312 _pt = _line[1];
313 return true;
314 }
315 // Otherwise return false.
316 else
317 return false;
318 }
319
320 // Get the line that is the shortest distance between this and _line
322 this->Distance(_line, distLine, _epsilon);
323
324 // If the length of the line is less than epsilon, then they
325 // intersect.
326 if (distLine.Length() < _epsilon)
327 {
328 _pt = distLine[0];
329 return true;
330 }
331
332 return false;
333 }
334
341 public: bool Within(const math::Vector3<T> &_pt,
342 double _epsilon = 1e-6) const
343 {
344 return _pt.X() <= std::max(this->pts[0].X(),
345 this->pts[1].X()) + _epsilon &&
346 _pt.X() >= std::min(this->pts[0].X(),
347 this->pts[1].X()) - _epsilon &&
348 _pt.Y() <= std::max(this->pts[0].Y(),
349 this->pts[1].Y()) + _epsilon &&
350 _pt.Y() >= std::min(this->pts[0].Y(),
351 this->pts[1].Y()) - _epsilon &&
352 _pt.Z() <= std::max(this->pts[0].Z(),
353 this->pts[1].Z()) + _epsilon &&
354 _pt.Z() >= std::min(this->pts[0].Z(),
355 this->pts[1].Z()) - _epsilon;
356 }
357
361 public: bool operator==(const Line3<T> &_line) const
362 {
363 return this->pts[0] == _line[0] && this->pts[1] == _line[1];
364 }
365
369 public: bool operator!=(const Line3<T> &_line) const
370 {
371 return !(*this == _line);
372 }
373
377 public: math::Vector3<T> operator[](const size_t _index) const
378 {
379 return this->pts[clamp(_index, GZ_ZERO_SIZE_T, GZ_ONE_SIZE_T)];
380 }
381
386 public: friend std::ostream &operator<<(
388 {
389 _out << _line[0] << " " << _line[1];
390 return _out;
391 }
392
394 private: math::Vector3<T> pts[2];
395 };
396
400 } // namespace GZ_MATH_VERSION_NAMESPACE_
401} // namespace gz::math
402#endif // GZ_MATH_LINE3_HH_