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 IGNITION_MATH_ORIENTEDBOX_HH_
18 #define IGNITION_MATH_ORIENTEDBOX_HH_
19 
20 #include <iostream>
21 #include <ignition/math/Helpers.hh>
22 #include <ignition/math/Matrix4.hh>
23 #include <ignition/math/Pose3.hh>
24 #include <ignition/math/Vector3.hh>
25 #include <ignition/math/config.hh>
26 
27 namespace ignition
28 {
29  namespace math
30  {
31  inline namespace IGNITION_MATH_VERSION_NAMESPACE
32  {
35  template<typename T>
37  {
39  public: OrientedBox() : size(Vector3<T>::Zero), pose(Pose3<T>::Zero)
40  {
41  }
42 
47  public: OrientedBox(const Vector3<T> &_size, const Pose3<T> &_pose)
48  : size(_size), pose(_pose)
49  {
50  // Enforce non-negative size
51  this->size = this->size.Abs();
52  }
53 
57  public: explicit OrientedBox(const Vector3<T> &_size)
58  : size(_size), pose(Pose3<T>::Zero)
59  {
60  // Enforce non-negative size
61  this->size = this->size.Abs();
62  }
63 
66  public: OrientedBox(const OrientedBox<T> &_b)
67  : size(_b.size), pose(_b.pose)
68  {
69  }
70 
72  public: virtual ~OrientedBox()
73  {
74  }
75 
78  public: T XLength() const
79  {
80  return this->size.X();
81  }
82 
85  public: T YLength() const
86  {
87  return this->size.Y();
88  }
89 
92  public: T ZLength() const
93  {
94  return this->size.Z();
95  }
96 
99  public: const Vector3<T> &Size() const
100  {
101  return this->size;
102  }
103 
106  public: const Pose3<T> &Pose() const
107  {
108  return this->pose;
109  }
110 
114  public: void Size(Vector3<T> &_size)
115  {
116  // Enforce non-negative size
117  this->size = _size.Abs();
118  }
119 
122  public: void Pose(Pose3<T> &_pose)
123  {
124  this->pose = _pose;
125  }
126 
131  {
132  this->size = _b.size;
133  this->pose = _b.pose;
134  return *this;
135  }
136 
140  public: bool operator==(const OrientedBox<T> &_b) const
141  {
142  return this->size == _b.size && this->pose == _b.pose;
143  }
144 
148  public: bool operator!=(const OrientedBox<T> &_b) const
149  {
150  return this->size != _b.size || this->pose != _b.pose;
151  }
152 
157  public: friend std::ostream &operator<<(std::ostream &_out,
158  const OrientedBox<T> &_b)
159  {
160  _out << "Size[" << _b.Size() << "] Pose[" << _b.Pose() << "]";
161  return _out;
162  }
163 
167  public: bool Contains(const Vector3d &_p) const
168  {
169  // Move point to box frame
170  auto t = Matrix4<T>(this->pose).Inverse();
171  auto p = t *_p;
172 
173  return p.X() >= -this->size.X()*0.5 && p.X() <= this->size.X()*0.5 &&
174  p.Y() >= -this->size.Y()*0.5 && p.Y() <= this->size.Y()*0.5 &&
175  p.Z() >= -this->size.Z()*0.5 && p.Z() <= this->size.Z()*0.5;
176  }
177 
179  private: Vector3<T> size;
180 
182  private: Pose3<T> pose;
183  };
184 
188  }
189  }
190 }
191 #endif
OrientedBox(const Vector3< T > &_size)
Constructor which takes only the size.
Definition: OrientedBox.hh:57
OrientedBox & operator=(const OrientedBox< T > &_b)
Assignment operator.
Definition: OrientedBox.hh:130
void Pose(Pose3< T > &_pose)
Set the box pose.
Definition: OrientedBox.hh:122
OrientedBox()
Default constructor.
Definition: OrientedBox.hh:39
bool operator!=(const OrientedBox< T > &_b) const
Inequality test operator.
Definition: OrientedBox.hh:148
OrientedBox(const OrientedBox< T > &_b)
Copy constructor.
Definition: OrientedBox.hh:66
OrientedBox(const Vector3< T > &_size, const Pose3< T > &_pose)
Constructor which takes size and pose.
Definition: OrientedBox.hh:47
Matrix4< T > Inverse() const
Return the inverse matrix.
Definition: Matrix4.hh:479
friend std::ostream & operator<<(std::ostream &_out, const OrientedBox< T > &_b)
Output operator.
Definition: OrientedBox.hh:157
OrientedBox< int > OrientedBoxi
Definition: OrientedBox.hh:185
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:33
const Pose3< T > & Pose() const
Get the box pose, which is the pose of its center.
Definition: OrientedBox.hh:106
Mathematical representation of a box which can be arbitrarily positioned and rotated.
Definition: OrientedBox.hh:36
void Size(Vector3< T > &_size)
Set the box size.
Definition: OrientedBox.hh:114
virtual ~OrientedBox()
Destructor.
Definition: OrientedBox.hh:72
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
OrientedBox< float > OrientedBoxf
Definition: OrientedBox.hh:187
T X() const
Get the x value.
Definition: Vector3.hh:647
T XLength() const
Get the length along the x dimension.
Definition: OrientedBox.hh:78
T ZLength() const
Get the length along the z dimension.
Definition: OrientedBox.hh:92
A 4x4 matrix class.
Definition: Matrix4.hh:36
const Vector3< T > & Size() const
Get the size of the box.
Definition: OrientedBox.hh:99
bool operator==(const OrientedBox< T > &_b) const
Equality test operator.
Definition: OrientedBox.hh:140
Vector3 Abs() const
Get the absolute value of the vector.
Definition: Vector3.hh:222
Definition: Angle.hh:39
bool Contains(const Vector3d &_p) const
Check if a point lies inside the box.
Definition: OrientedBox.hh:167
T YLength() const
Get the length along the y dimension.
Definition: OrientedBox.hh:85
OrientedBox< double > OrientedBoxd
Definition: OrientedBox.hh:186