17#ifndef IGNITION_MATH_LINE3_HH_
18#define IGNITION_MATH_LINE3_HH_
22#include <ignition/math/config.hh>
28 inline namespace IGNITION_MATH_VERSION_NAMESPACE
43 this->pts[0] = _line[0];
44 this->pts[1] = _line[1];
52 this->
Set(_ptA, _ptB);
60 public:
Line3(
const double _x1,
const double _y1,
61 const double _x2,
const double _y2)
63 this->
Set(_x1, _y1, _x2, _y2);
73 public:
Line3(
const double _x1,
const double _y1,
74 const double _z1,
const double _x2,
75 const double _y2,
const double _z2)
77 this->
Set(_x1, _y1, _z1, _x2, _y2, _z2);
112 public:
void Set(
const double _x1,
const double _y1,
113 const double _x2,
const double _y2,
116 this->pts[0].Set(_x1, _y1, _z);
117 this->pts[1].Set(_x2, _y2, _z);
127 public:
void Set(
const double _x1,
const double _y1,
128 const double _z1,
const double _x2,
129 const double _y2,
const double _z2)
131 this->pts[0].Set(_x1, _y1, _z1);
132 this->pts[1].Set(_x2, _y2, _z2);
139 return (this->pts[1] - this->pts[0]).Normalize();
146 return this->pts[0].Distance(this->pts[1]);
159 const double _epsilon = 1e-6)
const
164 if (std::abs(p43.
X()) < _epsilon && std::abs(p43.
Y()) < _epsilon &&
165 std::abs(p43.
Z()) < _epsilon)
172 if (std::abs(p21.
X()) < _epsilon && std::abs(p21.
Y()) < _epsilon &&
173 std::abs(p21.
Z()) < _epsilon)
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);
184 double denom = d2121 * d4343 - d4321 * d4321;
188 if (std::abs(denom) < _epsilon)
190 double d1 = this->pts[0].Distance(_line[0]);
191 double d2 = this->pts[0].Distance(_line[1]);
193 double d3 = this->pts[1].Distance(_line[0]);
194 double d4 = this->pts[1].Distance(_line[1]);
196 if (d1 <= d2 && d1 <= d3 && d1 <= d4)
198 _result.
SetA(this->pts[0]);
199 _result.
SetB(_line[0]);
201 else if (d2 <= d3 && d2 <= d4)
203 _result.
SetA(this->pts[0]);
204 _result.
SetB(_line[1]);
208 _result.
SetA(this->pts[1]);
209 _result.
SetB(_line[0]);
213 _result.
SetA(this->pts[1]);
214 _result.
SetB(_line[1]);
220 double numer = d1343 * d4321 - d1321 * d4343;
222 double mua =
clamp(numer / denom, 0.0, 1.0);
223 double mub =
clamp((d1343 + d4321 * mua) / d4343, 0.0, 1.0);
225 _result.
Set(this->pts[0] + (p21 * mua), _line[0] + (p43 * mub));
236 double _epsilon = 1e-6)
const
239 return this->
Intersect(_line, ignore, _epsilon);
248 const double _epsilon = 1e-6)
const
250 return std::abs((_line[0] - this->pts[0]).Dot(
251 (this->pts[1] - this->pts[0]).Cross(_line[1] - _line[0])))
261 const double _epsilon = 1e-6)
const
263 return (this->pts[1] - this->pts[0]).Cross(
264 _line[1] - _line[0]).Length() <= _epsilon;
276 double _epsilon = 1e-6)
const
279 if (this->
Parallel(_line, _epsilon))
282 if (this->
Within(_line[0], _epsilon))
288 else if (this->
Within(_line[1], _epsilon))
300 this->
Distance(_line, distLine, _epsilon);
304 if (distLine.
Length() < _epsilon)
320 double _epsilon = 1e-6)
const
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;
341 return this->pts[0] == _line[0] && this->pts[1] == _line[1];
349 return !(*
this == _line);
365 std::ostream &_out,
const Line3<T> &_line)
367 _out << _line[0] <<
" " << _line[1];
376 this->pts[0] = _line[0];
377 this->pts[1] = _line[1];
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
Line3()=default
Line Constructor.
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