Gazebo Math
API Reference
8.1.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
src
gz-math
include
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
24
namespace
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
41
public
:
Line3
(
const
math::Vector3<T>
&
_ptA
,
const
math::Vector3<T>
&
_ptB
)
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
,
75
const
math::Vector3<T>
&
_ptB
)
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
128
public
:
math::Vector3<T>
Direction
()
const
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
= 1
e
-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
= 1
e
-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
= 1
e
-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
= 1
e
-6)
const
284
{
285
return
(this->pts[1] - this->pts[0]).Cross(
286
_line
[1] -
_line
[0]).Length() <=
_epsilon
;
287
}
288
297
public
:
bool
Intersect
(
const
Line3<T>
&
_line
,
math::Vector3<T>
&
_pt
,
298
double
_epsilon
= 1
e
-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
321
math::Line3<T>
distLine
;
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
= 1
e
-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<<
(
387
std::ostream
&
_out
,
const
Line3<T>
&
_line
)
388
{
389
_out
<<
_line
[0] <<
" "
<<
_line
[1];
390
return
_out
;
391
}
392
394
private
:
math::Vector3<T>
pts[2];
395
};
396
397
typedef
Line3<int>
Line3i
;
398
typedef
Line3<double>
Line3d
;
399
typedef
Line3<float>
Line3f
;
400
}
// namespace GZ_MATH_VERSION_NAMESPACE_
401
}
// namespace gz::math
402
#endif
// GZ_MATH_LINE3_HH_