Gazebo Math

API Reference

7.5.1
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>
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: OrientedBox(const OrientedBox<T> &_b) = default;
85 
87  public: ~OrientedBox() = default;
88 
91  public: T XLength() const
92  {
93  return this->size.X();
94  }
95 
98  public: T YLength() const
99  {
100  return this->size.Y();
101  }
102 
105  public: T ZLength() const
106  {
107  return this->size.Z();
108  }
109 
112  public: const Vector3<T> &Size() const
113  {
114  return this->size;
115  }
116 
119  public: const Pose3<T> &Pose() const
120  {
121  return this->pose;
122  }
123 
127  public: void Size(const Vector3<T> &_size)
128  {
129  // Enforce non-negative size
130  this->size = _size.Abs();
131  }
132 
135  public: void Pose(const Pose3<T> &_pose)
136  {
137  this->pose = _pose;
138  }
139 
143  public: OrientedBox &operator=(const OrientedBox<T> &_b) = default;
144 
148  public: bool operator==(const OrientedBox<T> &_b) const
149  {
150  return this->size == _b.size && this->pose == _b.pose &&
151  this->material == _b.material;
152  }
153 
157  public: bool operator!=(const OrientedBox<T> &_b) const
158  {
159  return this->size != _b.size || this->pose != _b.pose ||
160  this->material != _b.material;
161  }
162 
167  public: friend std::ostream &operator<<(std::ostream &_out,
168  const OrientedBox<T> &_b)
169  {
170  _out << "Size[" << _b.Size() << "] Pose[" << _b.Pose() << "] "
171  << "Material[" << _b.Material().Name() << "]";
172  return _out;
173  }
174 
178  public: bool Contains(const Vector3d &_p) const
179  {
180  // Move point to box frame
181  auto t = Matrix4<T>(this->pose).Inverse();
182  auto p = t *_p;
183 
184  return p.X() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 &&
185  p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 &&
186  p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5;
187  }
188 
191  public: const gz::math::Material &Material() const
192  {
193  return this->material;
194  }
195 
198  public: void SetMaterial(const gz::math::Material &_mat)
199  {
200  this->material = _mat;
201  }
202 
205  public: T Volume() const
206  {
207  return this->size.X() * this->size.Y() * this->size.Z();
208  }
209 
218  public: T DensityFromMass(const T _mass) const
219  {
220  if (this->size.Min() <= 0|| _mass <= 0)
221  return -1.0;
222 
223  return _mass / this->Volume();
224  }
225 
238  public: bool SetDensityFromMass(const T _mass)
239  {
240  T newDensity = this->DensityFromMass(_mass);
241  if (newDensity > 0)
242  this->material.SetDensity(newDensity);
243  return newDensity > 0;
244  }
245 
252  public: bool MassMatrix(MassMatrix3<T> &_massMat) const
253  {
254  return _massMat.SetFromBox(this->material, this->size);
255  }
256 
258  private: Vector3<T> size;
259 
261  private: Pose3<T> pose;
262 
264  private: gz::math::Material material;
265  };
266 
269  } // namespace GZ_MATH_VERSION_NAMESPACE
270 } // namespace gz::math
271 #endif // GZ_MATH_ORIENTEDBOX_HH_