Gazebo Math

API Reference

7.4.0
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>
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
29namespace 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>
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
271 }
272 }
273}
274#endif