17 #ifndef GZ_MATH_LINE3_HH_
18 #define GZ_MATH_LINE3_HH_
22 #include <gz/math/config.hh>
27 inline namespace GZ_MATH_VERSION_NAMESPACE {
36 public:
Line3() =
default;
47 this->Set(_ptA, _ptB);
55 public:
Line3(
const double _x1,
const double _y1,
56 const double _x2,
const double _y2)
58 this->Set(_x1, _y1, _x2, _y2);
68 public:
Line3(
const double _x1,
const double _y1,
69 const double _z1,
const double _x2,
70 const double _y2,
const double _z2)
72 this->Set(_x1, _y1, _z1, _x2, _y2, _z2);
107 public:
void Set(
const double _x1,
const double _y1,
108 const double _x2,
const double _y2,
111 this->pts[0].Set(_x1, _y1, _z);
112 this->pts[1].Set(_x2, _y2, _z);
122 public:
void Set(
const double _x1,
const double _y1,
123 const double _z1,
const double _x2,
124 const double _y2,
const double _z2)
126 this->pts[0].Set(_x1, _y1, _z1);
127 this->pts[1].Set(_x2, _y2, _z2);
134 return (this->pts[1] - this->pts[0]).Normalize();
141 return this->pts[0].Distance(this->pts[1]);
155 const double _epsilon = 1e-6)
const
160 if (std::abs(p43.
X()) < _epsilon && std::abs(p43.
Y()) < _epsilon &&
161 std::abs(p43.
Z()) < _epsilon)
168 if (std::abs(p21.
X()) < _epsilon && std::abs(p21.
Y()) < _epsilon &&
169 std::abs(p21.
Z()) < _epsilon)
174 double d1343 = p13.
Dot(p43);
175 double d4321 = p43.
Dot(p21);
176 double d1321 = p13.
Dot(p21);
177 double d4343 = p43.
Dot(p43);
178 double d2121 = p21.
Dot(p21);
180 double denom = d2121 * d4343 - d4321 * d4321;
184 if (std::abs(denom) < _epsilon)
186 double d1 = this->pts[0].Distance(_line[0]);
187 double d2 = this->pts[0].Distance(_line[1]);
189 double d3 = this->pts[1].Distance(_line[0]);
190 double d4 = this->pts[1].Distance(_line[1]);
192 if (d1 <= d2 && d1 <= d3 && d1 <= d4)
194 _result.
SetA(this->pts[0]);
195 _result.
SetB(_line[0]);
197 else if (d2 <= d3 && d2 <= d4)
199 _result.
SetA(this->pts[0]);
200 _result.
SetB(_line[1]);
204 _result.
SetA(this->pts[1]);
205 _result.
SetB(_line[0]);
209 _result.
SetA(this->pts[1]);
210 _result.
SetB(_line[1]);
216 double numer = d1343 * d4321 - d1321 * d4343;
218 double mua =
clamp(numer / denom, 0.0, 1.0);
219 double mub =
clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
221 _result.
Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
231 auto line = this->pts[1] - this->pts[0];
232 auto ptTo0 = _pt - this->pts[0];
233 auto ptTo1 = _pt - this->pts[1];
236 if (ptTo0.Dot(line) <= 0.0)
242 if (ptTo1.Dot(line) >= 0.0)
244 return ptTo1.Length();
250 auto d = ptTo0.Cross(line);
251 auto lineLength = line.Length();
252 assert(lineLength > 0);
253 return d.Length() / lineLength;
262 double _epsilon = 1e-6)
const
265 return this->Intersect(_line, ignore, _epsilon);
274 const double _epsilon = 1e-6)
const
276 return std::abs((_line[0] - this->pts[0]).Dot(
277 (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
287 const double _epsilon = 1e-6)
const
289 return (this->pts[1] - this->pts[0]).Cross(
290 _line[1] - _line[0]).Length() <= _epsilon;
302 double _epsilon = 1e-6)
const
305 if (this->Parallel(_line, _epsilon))
308 if (this->Within(_line[0], _epsilon))
314 else if (this->Within(_line[1], _epsilon))
326 this->Distance(_line, distLine, _epsilon);
330 if (distLine.
Length() < _epsilon)
346 double _epsilon = 1e-6)
const
348 return _pt.
X() <=
std::max(this->pts[0].X(),
349 this->pts[1].X()) + _epsilon &&
351 this->pts[1].X()) - _epsilon &&
353 this->pts[1].Y()) + _epsilon &&
355 this->pts[1].Y()) - _epsilon &&
357 this->pts[1].Z()) + _epsilon &&
359 this->pts[1].Z()) - _epsilon;
367 return this->pts[0] == _line[0] && this->pts[1] == _line[1];
375 return !(*
this == _line);
393 _out << _line[0] <<
" " << _line[1];
411 #endif // GZ_MATH_LINE3_HH_