Gazebo Math
API Reference
7.4.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
include
gz
math
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
30
{
31
namespace
math
32
{
33
// Inline bracket to help doxygen filtering.
34
inline
namespace
GZ_MATH_VERSION_NAMESPACE {
35
//
38
template
<
typename
T>
39
class
OrientedBox
40
{
42
public
:
OrientedBox
() : size(
Vector3
<T>::Zero), pose(
Pose3
<T>::Zero)
43
{
44
}
45
50
public
:
OrientedBox
(
const
Vector3<T>
&_size,
const
Pose3<T>
&_pose)
51
: size(_size.Abs()), pose(_pose)
52
{
53
}
54
60
public
:
OrientedBox
(
const
Vector3<T>
&_size,
const
Pose3<T>
&_pose,
61
const
Material
&_mat)
62
: size(_size.Abs()), pose(_pose), material(_mat)
63
{
64
}
65
69
public
:
explicit
OrientedBox
(
const
Vector3<T>
&_size)
70
: size(_size.Abs()), pose(
Pose3
<T>::Zero)
71
{
72
}
73
78
public
:
explicit
OrientedBox
(
const
Vector3<T>
&_size,
79
const
Material
&_mat)
80
: size(_size.Abs()), pose(
Pose3
<T>::Zero), material(_mat)
81
{
82
}
83
86
public
:
OrientedBox
(
const
OrientedBox<T>
&_b) =
default
;
87
89
public
:
~OrientedBox
() =
default
;
90
93
public
: T
XLength
()
const
94
{
95
return
this->size.X();
96
}
97
100
public
: T
YLength
()
const
101
{
102
return
this->size.Y();
103
}
104
107
public
: T
ZLength
()
const
108
{
109
return
this->size.Z();
110
}
111
114
public
:
const
Vector3<T>
&
Size
()
const
115
{
116
return
this->size;
117
}
118
121
public
:
const
Pose3<T>
&
Pose
()
const
122
{
123
return
this->pose;
124
}
125
129
public
:
void
Size
(
const
Vector3<T>
&_size)
130
{
131
// Enforce non-negative size
132
this->size = _size.
Abs
();
133
}
134
137
public
:
void
Pose
(
const
Pose3<T>
&_pose)
138
{
139
this->pose = _pose;
140
}
141
145
public
:
OrientedBox
&
operator=
(
const
OrientedBox<T>
&_b) =
default
;
146
150
public
:
bool
operator==
(
const
OrientedBox<T>
&_b)
const
151
{
152
return
this->size == _b.size && this->pose == _b.pose &&
153
this->material == _b.material;
154
}
155
159
public
:
bool
operator!=
(
const
OrientedBox<T>
&_b)
const
160
{
161
return
this->size != _b.size || this->pose != _b.pose ||
162
this->material != _b.material;
163
}
164
169
public
:
friend
std::ostream
&
operator<<
(
std::ostream
&_out,
170
const
OrientedBox<T>
&_b)
171
{
172
_out <<
"Size["
<< _b.
Size
() <<
"] Pose["
<< _b.
Pose
() <<
"] "
173
<<
"Material["
<< _b.
Material
().
Name
() <<
"]"
;
174
return
_out;
175
}
176
180
public
:
bool
Contains
(
const
Vector3d
&_p)
const
181
{
182
// Move point to box frame
183
auto
t =
Matrix4<T>
(this->pose).
Inverse
();
184
auto
p = t *_p;
185
186
return
p.
X
() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 &&
187
p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 &&
188
p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5;
189
}
190
193
public
:
const
gz::math::Material
&
Material
()
const
194
{
195
return
this->material;
196
}
197
200
public
:
void
SetMaterial
(
const
gz::math::Material
&_mat)
201
{
202
this->material = _mat;
203
}
204
207
public
: T
Volume
()
const
208
{
209
return
this->size.X() * this->size.Y() * this->size.Z();
210
}
211
220
public
: T
DensityFromMass
(
const
T _mass)
const
221
{
222
if
(this->size.Min() <= 0|| _mass <= 0)
223
return
-1.0;
224
225
return
_mass / this->Volume();
226
}
227
240
public
:
bool
SetDensityFromMass
(
const
T _mass)
241
{
242
T newDensity = this->DensityFromMass(_mass);
243
if
(newDensity > 0)
244
this->material.SetDensity(newDensity);
245
return
newDensity > 0;
246
}
247
254
public
:
bool
MassMatrix
(
MassMatrix3<T>
&_massMat)
const
255
{
256
return
_massMat.
SetFromBox
(this->material, this->size);
257
}
258
260
private
:
Vector3<T>
size;
261
263
private
:
Pose3<T>
pose;
264
266
private
:
gz::math::Material
material;
267
};
268
269
typedef
OrientedBox<double>
OrientedBoxd
;
270
typedef
OrientedBox<float>
OrientedBoxf
;
271
}
272
}
273
}
274
#endif