Loading...
Searching...
No Matches
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>
25#include <ignition/math/config.hh>
26
27namespace 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
A 4x4 matrix class.
Definition Matrix4.hh:37
Matrix4< T > Inverse() const
Return the inverse matrix.
Definition Matrix4.hh:479
Mathematical representation of a box which can be arbitrarily positioned and rotated.
Definition OrientedBox.hh:37
bool operator==(const OrientedBox< T > &_b) const
Equality test operator.
Definition OrientedBox.hh:140
OrientedBox(const Vector3< T > &_size)
Constructor which takes only the size.
Definition OrientedBox.hh:57
OrientedBox(const OrientedBox< T > &_b)
Copy constructor.
Definition OrientedBox.hh:66
T ZLength() const
Get the length along the z dimension.
Definition OrientedBox.hh:92
bool Contains(const Vector3d &_p) const
Check if a point lies inside the box.
Definition OrientedBox.hh:167
void Size(Vector3< T > &_size)
Set the box size.
Definition OrientedBox.hh:114
friend std::ostream & operator<<(std::ostream &_out, const OrientedBox< T > &_b)
Output operator.
Definition OrientedBox.hh:157
OrientedBox(const Vector3< T > &_size, const Pose3< T > &_pose)
Constructor which takes size and pose.
Definition OrientedBox.hh:47
const Vector3< T > & Size() const
Get the size of the box.
Definition OrientedBox.hh:99
T XLength() const
Get the length along the x dimension.
Definition OrientedBox.hh:78
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
T YLength() const
Get the length along the y dimension.
Definition OrientedBox.hh:85
OrientedBox()
Default constructor.
Definition OrientedBox.hh:39
bool operator!=(const OrientedBox< T > &_b) const
Inequality test operator.
Definition OrientedBox.hh:148
virtual ~OrientedBox()
Destructor.
Definition OrientedBox.hh:72
const Pose3< T > & Pose() const
Get the box pose, which is the pose of its center.
Definition OrientedBox.hh:106
Encapsulates a position and rotation in three space.
Definition Pose3.hh:34
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
Vector3 Abs() const
Get the absolute value of the vector.
Definition Vector3.hh:222
T X() const
Get the x value.
Definition Vector3.hh:647
OrientedBox< int > OrientedBoxi
Definition OrientedBox.hh:185
OrientedBox< double > OrientedBoxd
Definition OrientedBox.hh:186
OrientedBox< float > OrientedBoxf
Definition OrientedBox.hh:187
Definition Angle.hh:40