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
Plane.hh
Go to the documentation of this file.
1
/*
2
* Copyright (C) 2012 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_PLANE_HH_
18
#define GZ_MATH_PLANE_HH_
19
20
#include <
gz/math/AxisAlignedBox.hh
>
21
#include <
gz/math/Vector2.hh
>
22
#include <
gz/math/Vector3.hh
>
23
#include <gz/math/config.hh>
24
#include <
gz/math/Line2.hh
>
25
#include <
gz/math/Quaternion.hh
>
26
#include <optional>
27
28
namespace
gz::math
29
{
30
// Inline bracket to help doxygen filtering.
31
inline
namespace
GZ_MATH_VERSION_NAMESPACE {
32
//
35
template
<
typename
T>
36
class
Plane
37
{
41
public
:
enum
PlaneSide
42
{
45
NEGATIVE_SIDE = 0,
46
49
POSITIVE_SIDE = 1,
50
52
NO_SIDE = 2,
53
55
BOTH_SIDE = 3
56
};
57
59
public
:
Plane
()
60
: d(0.0)
61
{
62
}
63
67
public
:
explicit
Plane
(
const
Vector3<T>
&
_normal
, T
_offset
= 0.0)
68
: normal(
_normal
), d(
_offset
)
69
{
70
}
71
76
public
:
Plane
(
const
Vector3<T>
&
_normal
,
const
Vector2<T>
&
_size
,
77
T
_offset
)
78
{
79
this->Set(
_normal
,
_size
,
_offset
);
80
}
81
85
public
:
void
Set
(
const
Vector3<T>
&
_normal
, T
_offset
)
86
{
87
this->normal =
_normal
;
88
this->d =
_offset
;
89
}
90
95
public
:
void
Set
(
const
Vector3<T>
&
_normal
,
const
Vector2<T>
&
_size
,
96
T
_offset
)
97
{
98
this->normal =
_normal
;
99
this->size =
_size
;
100
this->d =
_offset
;
101
}
102
109
public
: T
Distance
(
const
Vector3<T>
&
_point
)
const
110
{
111
return
this->normal.Dot(
_point
) - this->d;
112
}
113
122
public
: std::optional<Vector3<T>>
Intersection
(
123
const
Vector3<T>
&
_point
,
124
const
Vector3<T>
&
_gradient
,
125
const
double
&
_tolerance
= 1
e
-6)
const
126
{
127
if
(std::abs(this->Normal().Dot(
_gradient
)) <
_tolerance
)
128
{
129
return
std::nullopt;
130
}
131
auto
constant
= this->Offset() - this->Normal().Dot(
_point
);
132
auto
param =
constant
/ this->Normal().Dot(
_gradient
);
133
auto
intersection
=
_point
+
_gradient
*param;
134
135
if
(this->Size() ==
Vector2<T>
(0, 0))
136
return
intersection
;
137
138
// Check if the point is within the size bounds
139
// To do this we create a Quaternion using Angle, Axis constructor and
140
// rotate the Y and X axis the same amount as the normal.
141
auto
dotProduct
=
Vector3<T>::UnitZ
.
Dot
(this->Normal());
142
auto
angle
= acos(
dotProduct
/ this->Normal().Length());
143
auto
axis
=
Vector3<T>::UnitZ
.
Cross
(this->Normal().Normalized());
144
Quaternion<T>
rotation
(
axis
,
angle
);
145
146
Vector3<T>
rotatedXAxis
=
rotation
*
Vector3<T>::UnitX
;
147
Vector3<T>
rotatedYAxis
=
rotation
*
Vector3<T>::UnitY
;
148
149
auto
xBasis
=
rotatedXAxis
.Dot(
intersection
);
150
auto
yBasis
=
rotatedYAxis
.Dot(
intersection
);
151
152
if
(std::abs(
xBasis
) < this->Size().X() / 2 &&
153
std::abs(
yBasis
) < this->Size().Y() / 2)
154
{
155
return
intersection
;
156
}
157
return
std::nullopt;
158
}
159
166
public
:
PlaneSide
Side
(
const
Vector3<T>
&
_point
)
const
167
{
168
T
dist
= this->Distance(
_point
);
169
170
if
(
dist
< 0.0)
171
return
NEGATIVE_SIDE;
172
173
if
(
dist
> 0.0)
174
return
POSITIVE_SIDE;
175
176
return
NO_SIDE;
177
}
178
185
public
:
PlaneSide
Side
(
const
math::AxisAlignedBox
&
_box
)
const
186
{
187
double
dist
= this->Distance(
_box
.Center());
188
double
maxAbsDist
= this->normal.AbsDot(
_box
.Size()/2.0);
189
190
if
(
dist
< -
maxAbsDist
)
191
return
NEGATIVE_SIDE;
192
193
if
(
dist
>
maxAbsDist
)
194
return
POSITIVE_SIDE;
195
196
return
BOTH_SIDE;
197
}
198
203
public
: T
Distance
(
const
Vector3<T>
&
_origin
,
204
const
Vector3<T>
&
_dir
)
const
205
{
206
T
denom
= this->normal.Dot(
_dir
);
207
208
if
(std::abs(
denom
) < 1
e
-3)
209
{
210
// parallel
211
return
0;
212
}
213
else
214
{
215
T
nom
=
_origin
.Dot(this->normal) - this->d;
216
T t = -(
nom
/
denom
);
217
return
t;
218
}
219
}
220
222
public
:
inline
const
Vector2<T>
&
Size
()
const
223
{
224
return
this->size;
225
}
226
228
public
:
inline
Vector2<T>
&
Size
()
229
{
230
return
this->size;
231
}
232
234
public
:
inline
const
Vector3<T>
&
Normal
()
const
235
{
236
return
this->normal;
237
}
238
240
public
:
inline
Vector3<T>
&
Normal
()
241
{
242
return
this->normal;
243
}
244
246
public
:
inline
T
Offset
()
const
247
{
248
return
this->d;
249
}
250
252
private
:
Vector3<T>
normal;
253
255
private
:
Vector2<T>
size;
256
258
private
: T d;
259
};
260
261
typedef
Plane<int>
Planei
;
262
typedef
Plane<double>
Planed
;
263
typedef
Plane<float>
Planef
;
264
}
// namespace GZ_MATH_VERSION_NAMESPACE
265
}
// namespace gz::math
266
#endif
// GZ_MATH_PLANE_HH_