Gazebo Math

API Reference

8.0.0
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::math
30{
31 // Inline bracket to help doxygen filtering.
32 inline namespace GZ_MATH_VERSION_NAMESPACE {
33 //
36 template<typename T>
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
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
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
257 } // namespace GZ_MATH_VERSION_NAMESPACE
258} // namespace gz::math
259#endif // GZ_MATH_ORIENTEDBOX_HH_