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
OrientedBox.hh
Go to the documentation of this file.
1
/*
2
* Copyright (C) 2017 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_ORIENTEDBOX_HH_
18
#define GZ_MATH_ORIENTEDBOX_HH_
19
20
#include <ostream>
21
#include <
gz/math/Helpers.hh
>
22
#include <
gz/math/MassMatrix3.hh
>
23
#include <
gz/math/Material.hh
>
24
#include <
gz/math/Matrix4.hh
>
25
#include <
gz/math/Pose3.hh
>
26
#include <
gz/math/Vector3.hh
>
27
#include <gz/math/config.hh>
28
29
namespace
gz::math
30
{
31
// Inline bracket to help doxygen filtering.
32
inline
namespace
GZ_MATH_VERSION_NAMESPACE {
33
//
36
template
<
typename
T>
37
class
OrientedBox
38
{
40
public
:
OrientedBox
() : size(
Vector3
<T>::Zero), pose(
Pose3
<T>::Zero)
41
{
42
}
43
48
public
:
OrientedBox
(
const
Vector3<T>
&
_size
,
const
Pose3<T>
&
_pose
)
49
: size(
_size
.Abs()), pose(
_pose
)
50
{
51
}
52
58
public
:
OrientedBox
(
const
Vector3<T>
&
_size
,
const
Pose3<T>
&
_pose
,
59
const
Material
&
_mat
)
60
: size(
_size
.Abs()), pose(
_pose
), material(
_mat
)
61
{
62
}
63
67
public
:
explicit
OrientedBox
(
const
Vector3<T>
&
_size
)
68
: size(
_size
.Abs()), pose(
Pose3
<T>::Zero)
69
{
70
}
71
76
public
:
explicit
OrientedBox
(
const
Vector3<T>
&
_size
,
77
const
Material
&
_mat
)
78
: size(
_size
.Abs()), pose(
Pose3
<T>::Zero), material(
_mat
)
79
{
80
}
81
84
public
: T
XLength
()
const
85
{
86
return
this->size.X();
87
}
88
91
public
: T
YLength
()
const
92
{
93
return
this->size.Y();
94
}
95
98
public
: T
ZLength
()
const
99
{
100
return
this->size.Z();
101
}
102
105
public
:
const
Vector3<T>
&
Size
()
const
106
{
107
return
this->size;
108
}
109
112
public
:
const
Pose3<T>
&
Pose
()
const
113
{
114
return
this->pose;
115
}
116
120
public
:
void
Size
(
const
Vector3<T>
&
_size
)
121
{
122
// Enforce non-negative size
123
this->size =
_size
.Abs();
124
}
125
128
public
:
void
Pose
(
const
Pose3<T>
&
_pose
)
129
{
130
this->pose =
_pose
;
131
}
132
136
public
:
bool
operator==
(
const
OrientedBox<T>
&
_b
)
const
137
{
138
return
this->size ==
_b
.size && this->pose ==
_b
.pose &&
139
this->material ==
_b
.material;
140
}
141
145
public
:
bool
operator!=
(
const
OrientedBox<T>
&
_b
)
const
146
{
147
return
this->size !=
_b
.size || this->pose !=
_b
.pose ||
148
this->material !=
_b
.material;
149
}
150
155
public
:
friend
std::ostream
&
operator<<
(
std::ostream
&
_out
,
156
const
OrientedBox<T>
&
_b
)
157
{
158
_out
<<
"Size["
<<
_b
.Size() <<
"] Pose["
<<
_b
.Pose() <<
"] "
159
<<
"Material["
<<
_b
.Material().Name() <<
"]"
;
160
return
_out
;
161
}
162
166
public
:
bool
Contains
(
const
Vector3d
&
_p
)
const
167
{
168
// Move point to box frame
169
auto
t =
Matrix4<T>
(this->pose).Inverse();
170
auto
p = t *
_p
;
171
172
return
p.X() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 &&
173
p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 &&
174
p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5;
175
}
176
179
public
:
const
gz::math::Material
&
Material
()
const
180
{
181
return
this->material;
182
}
183
186
public
:
void
SetMaterial
(
const
gz::math::Material
&
_mat
)
187
{
188
this->material =
_mat
;
189
}
190
193
public
: T
Volume
()
const
194
{
195
return
this->size.X() * this->size.Y() * this->size.Z();
196
}
197
206
public
: T
DensityFromMass
(
const
T
_mass
)
const
207
{
208
if
(this->size.Min() <= 0||
_mass
<= 0)
209
return
-1.0;
210
211
return
_mass
/ this->Volume();
212
}
213
226
public
:
bool
SetDensityFromMass
(
const
T
_mass
)
227
{
228
T
newDensity
= this->DensityFromMass(
_mass
);
229
if
(
newDensity
> 0)
230
this->material.SetDensity(
newDensity
);
231
return
newDensity
> 0;
232
}
233
240
public
:
bool
MassMatrix
(
MassMatrix3<T>
&
_massMat
)
const
241
{
242
return
_massMat
.SetFromBox(this->material, this->size);
243
}
244
246
private
:
Vector3<T>
size;
247
249
private
:
Pose3<T>
pose;
250
252
private
:
gz::math::Material
material;
253
};
254
255
typedef
OrientedBox<double>
OrientedBoxd
;
256
typedef
OrientedBox<float>
OrientedBoxf
;
257
}
// namespace GZ_MATH_VERSION_NAMESPACE
258
}
// namespace gz::math
259
#endif
// GZ_MATH_ORIENTEDBOX_HH_