Gazebo Math

API Reference

6.15.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 <iostream>
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 ignition
30 {
31  namespace math
32  {
33  // Inline bracket to help doxygen filtering.
34  inline namespace IGNITION_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)
87  : size(_b.size), pose(_b.pose), material(_b.material)
88  {
89  }
90 
92  public: virtual ~OrientedBox()
93  {
94  }
95 
98  public: T XLength() const
99  {
100  return this->size.X();
101  }
102 
105  public: T YLength() const
106  {
107  return this->size.Y();
108  }
109 
112  public: T ZLength() const
113  {
114  return this->size.Z();
115  }
116 
119  public: const Vector3<T> &Size() const
120  {
121  return this->size;
122  }
123 
126  public: const Pose3<T> &Pose() const
127  {
128  return this->pose;
129  }
130 
134  public: void Size(Vector3<T> &_size)
135  {
136  // Enforce non-negative size
137  this->size = _size.Abs();
138  }
139 
142  public: void Pose(Pose3<T> &_pose)
143  {
144  this->pose = _pose;
145  }
146 
151  {
152  this->size = _b.size;
153  this->pose = _b.pose;
154  this->material = _b.material;
155  return *this;
156  }
157 
161  public: bool operator==(const OrientedBox<T> &_b) const
162  {
163  return this->size == _b.size && this->pose == _b.pose &&
164  this->material == _b.material;
165  }
166 
170  public: bool operator!=(const OrientedBox<T> &_b) const
171  {
172  return this->size != _b.size || this->pose != _b.pose ||
173  this->material != _b.material;
174  }
175 
180  public: friend std::ostream &operator<<(std::ostream &_out,
181  const OrientedBox<T> &_b)
182  {
183  _out << "Size[" << _b.Size() << "] Pose[" << _b.Pose() << "] "
184  << "Material[" << _b.Material().Name() << "]";
185  return _out;
186  }
187 
191  public: bool Contains(const Vector3d &_p) const
192  {
193  // Move point to box frame
194  auto t = Matrix4<T>(this->pose).Inverse();
195  auto p = t *_p;
196 
197  return p.X() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 &&
198  p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 &&
199  p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5;
200  }
201 
204  public: const gz::math::Material &Material() const
205  {
206  return this->material;
207  }
208 
211  public: void SetMaterial(const gz::math::Material &_mat)
212  {
213  this->material = _mat;
214  }
215 
218  public: T Volume() const
219  {
220  return this->size.X() * this->size.Y() * this->size.Z();
221  }
222 
231  public: T DensityFromMass(const T _mass) const
232  {
233  if (this->size.Min() <= 0|| _mass <= 0)
234  return -1.0;
235 
236  return _mass / this->Volume();
237  }
238 
251  public: bool SetDensityFromMass(const T _mass)
252  {
253  T newDensity = this->DensityFromMass(_mass);
254  if (newDensity > 0)
255  this->material.SetDensity(newDensity);
256  return newDensity > 0;
257  }
258 
265  public: bool MassMatrix(MassMatrix3<T> &_massMat) const
266  {
267  return _massMat.SetFromBox(this->material, this->size);
268  }
269 
271  private: Vector3<T> size;
272 
274  private: Pose3<T> pose;
275 
277  private: gz::math::Material material;
278  };
279 
283  }
284  }
285 }
286 #endif
A 4x4 matrix class.
Definition: gz/math/Matrix4.hh:38
Encapsulates a position and rotation in three space.
Definition: gz/math/Pose3.hh:34
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
bool SetDensityFromMass(const T _mass)
Set the density of this box based on a mass value. Density is computed using double DensityFromMass(c...
Definition: gz/math/OrientedBox.hh:251
OrientedBox(const Vector3< T > &_size)
Constructor which takes only the size.
Definition: gz/math/OrientedBox.hh:69
OrientedBox(const Vector3< T > &_size, const Material &_mat)
Constructor which takes only the size.
Definition: gz/math/OrientedBox.hh:78
Matrix4< T > Inverse() const
Return the inverse matrix. This is a non-destructive operation.
Definition: gz/math/Matrix4.hh:481
friend std::ostream & operator<<(std::ostream &_out, const OrientedBox< T > &_b)
Output operator.
Definition: gz/math/OrientedBox.hh:180
Contains information about a single material.
Definition: gz/math/Material.hh:65
T X() const
Get the x value.
Definition: gz/math/Vector3.hh:654
OrientedBox< float > OrientedBoxf
Definition: gz/math/OrientedBox.hh:282
OrientedBox(const OrientedBox< T > &_b)
Copy constructor.
Definition: gz/math/OrientedBox.hh:86
bool operator==(const OrientedBox< T > &_b) const
Equality test operator.
Definition: gz/math/OrientedBox.hh:161
OrientedBox(const Vector3< T > &_size, const Pose3< T > &_pose, const Material &_mat)
Constructor which takes size, pose, and material.
Definition: gz/math/OrientedBox.hh:60
T YLength() const
Get the length along the y dimension.
Definition: gz/math/OrientedBox.hh:105
OrientedBox< double > OrientedBoxd
Definition: gz/math/OrientedBox.hh:281
virtual ~OrientedBox()
Destructor.
Definition: gz/math/OrientedBox.hh:92
T ZLength() const
Get the length along the z dimension.
Definition: gz/math/OrientedBox.hh:112
void SetMaterial(const Material &_mat)
Set the material associated with this box.
Definition: gz/math/OrientedBox.hh:211
void Pose(Pose3< T > &_pose)
Set the box pose.
Definition: gz/math/OrientedBox.hh:142
STL class.
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
Mathematical representation of a box which can be arbitrarily positioned and rotated.
Definition: gz/math/OrientedBox.hh:39
bool Contains(const Vector3d &_p) const
Check if a point lies inside the box.
Definition: gz/math/OrientedBox.hh:191
const Material & Material() const
Get the material associated with this box.
Definition: gz/math/OrientedBox.hh:204
const Vector3< T > & Size() const
Get the size of the box.
Definition: gz/math/OrientedBox.hh:119
T XLength() const
Get the length along the x dimension.
Definition: gz/math/OrientedBox.hh:98
OrientedBox< int > OrientedBoxi
Definition: gz/math/OrientedBox.hh:280
const Pose3< T > & Pose() const
Get the box pose, which is the pose of its center.
Definition: gz/math/OrientedBox.hh:126
OrientedBox()
Default constructor.
Definition: gz/math/OrientedBox.hh:42
T DensityFromMass(const T _mass) const
Compute the box's density given a mass value. The box is assumed to be solid with uniform density....
Definition: gz/math/OrientedBox.hh:231
bool SetFromBox(const Material &_mat, const Vector3< T > &_size, const Quaternion< T > &_rot=Quaternion< T >::Identity)
Set inertial properties based on a Material and equivalent box.
Definition: gz/math/MassMatrix3.hh:1054
Vector3 Abs() const
Get the absolute value of the vector.
Definition: gz/math/Vector3.hh:229
OrientedBox & operator=(const OrientedBox< T > &_b)
Assignment operator. Set this box to the parameter.
Definition: gz/math/OrientedBox.hh:150
void Size(Vector3< T > &_size)
Set the box size.
Definition: gz/math/OrientedBox.hh:134
T Volume() const
Get the volume of the box in m^3.
Definition: gz/math/OrientedBox.hh:218
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: gz/math/MassMatrix3.hh:45
bool operator!=(const OrientedBox< T > &_b) const
Inequality test operator.
Definition: gz/math/OrientedBox.hh:170
void SetDensity(const double _density)
Set the density value of the material in kg/m^3.
bool MassMatrix(MassMatrix3< T > &_massMat) const
Get the mass matrix for this box. This function is only meaningful if the box's size and material hav...
Definition: gz/math/OrientedBox.hh:265
OrientedBox(const Vector3< T > &_size, const Pose3< T > &_pose)
Constructor which takes size and pose.
Definition: gz/math/OrientedBox.hh:50