AABB.h 7.53 KB
Newer Older
sachinc's avatar
sachinc committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
sachinc's avatar
sachinc committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
sachinc's avatar
sachinc committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */

Joseph Mirabel's avatar
Joseph Mirabel committed
38
39
#ifndef HPP_FCL_AABB_H
#define HPP_FCL_AABB_H
sachinc's avatar
sachinc committed
40

41
#include <hpp/fcl/data_types.h>
sachinc's avatar
sachinc committed
42

43
44
namespace hpp
{
sachinc's avatar
sachinc committed
45
46
namespace fcl
{
47
48
49

struct CollisionRequest;

Joseph Mirabel's avatar
Joseph Mirabel committed
50
51
/// @defgroup Bounding_Volume Bounding volumes
/// Classes of different types of bounding volume.
52
53
/// @{

54
/// @brief A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points
Guilhem Saurel's avatar
Guilhem Saurel committed
55
class HPP_FCL_DLLAPI AABB
sachinc's avatar
sachinc committed
56
57
{
public:
58
  /// @brief The min point in the AABB
sachinc's avatar
sachinc committed
59
  Vec3f min_;
60
  /// @brief The max point in the AABB
sachinc's avatar
sachinc committed
61
62
  Vec3f max_;

63
  /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf)
sachinc's avatar
sachinc committed
64
65
  AABB();

66
  /// @brief Creating an AABB at position v with zero size
sachinc's avatar
sachinc committed
67
68
69
70
  AABB(const Vec3f& v) : min_(v), max_(v)
  {
  }

71
  /// @brief Creating an AABB with two endpoints a and b
72
73
  AABB(const Vec3f& a, const Vec3f&b) : min_(a.cwiseMin(b)),
                                        max_(a.cwiseMax(b))
sachinc's avatar
sachinc committed
74
75
76
  {
  }

77
78
79
  /// @brief Creating an AABB centered as core and is of half-dimension delta
  AABB(const AABB& core, const Vec3f& delta) : min_(core.min_ - delta),
                                               max_(core.max_ + delta)
jpan's avatar
jpan committed
80
81
82
  {
  }

83
  /// @brief Creating an AABB contains three points
84
85
  AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(a.cwiseMin(b).cwiseMin(c)),
                                                         max_(a.cwiseMax(b).cwiseMax(c))
jpan's avatar
jpan committed
86
87
88
  {
  }

89
90
91
92
93
94
95
96
97
98
99
100
101
102
  /// @name Bounding volume API
  /// Common API to BVs.
  /// @{
  
  /// @brief Check whether the AABB contains a point
  inline bool contain(const Vec3f& p) const
  {
    if(p[0] < min_[0] || p[0] > max_[0]) return false;
    if(p[1] < min_[1] || p[1] > max_[1]) return false;
    if(p[2] < min_[2] || p[2] > max_[2]) return false;

    return true;
  }
  
103
  /// @brief Check whether two AABB are overlap
sachinc's avatar
sachinc committed
104
105
106
107
108
109
110
111
112
113
114
  inline bool overlap(const AABB& other) const
  {
    if(min_[0] > other.max_[0]) return false;
    if(min_[1] > other.max_[1]) return false;
    if(min_[2] > other.max_[2]) return false;

    if(max_[0] < other.min_[0]) return false;
    if(max_[1] < other.min_[1]) return false;
    if(max_[2] < other.min_[2]) return false;

    return true;
jpan's avatar
   
jpan committed
115
116
  }    

Joseph Mirabel's avatar
Joseph Mirabel committed
117
  /// @brief Check whether two AABB are overlap
118
119
  bool overlap(const AABB& other, const CollisionRequest& request,
               FCL_REAL& sqrDistLowerBound) const;
120

121
122
  /// @brief Distance between two AABBs
  FCL_REAL distance(const AABB& other) const;
sachinc's avatar
sachinc committed
123

124
125
  /// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points 
  FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
sachinc's avatar
sachinc committed
126

127
  /// @brief Merge the AABB and a point
sachinc's avatar
sachinc committed
128
129
  inline AABB& operator += (const Vec3f& p)
  {
Joseph Mirabel's avatar
Joseph Mirabel committed
130
131
    min_ = min_.cwiseMin(p);
    max_ = max_.cwiseMax(p);
sachinc's avatar
sachinc committed
132
133
134
    return *this;
  }

135
  /// @brief Merge the AABB and another AABB
sachinc's avatar
sachinc committed
136
137
  inline AABB& operator += (const AABB& other)
  {
Joseph Mirabel's avatar
Joseph Mirabel committed
138
139
    min_ = min_.cwiseMin(other.min_);
    max_ = max_.cwiseMax(other.max_);
sachinc's avatar
sachinc committed
140
141
142
    return *this;
  }

143
  /// @brief Return the merged AABB of current AABB and the other one
sachinc's avatar
sachinc committed
144
145
146
147
148
149
  inline AABB operator + (const AABB& other) const
  {
    AABB res(*this);
    return res += other;
  }

150
151
152
153
154
155
156
157
158
159
160
161
  /// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
  inline FCL_REAL size() const
  {
    return (max_ - min_).squaredNorm();
  }

  /// @brief Center of the AABB
  inline  Vec3f center() const
  {
    return (min_ + max_) * 0.5;
  }

162
  /// @brief Width of the AABB
163
  inline FCL_REAL width() const
sachinc's avatar
sachinc committed
164
165
166
167
  {
    return max_[0] - min_[0];
  }

168
  /// @brief Height of the AABB
169
  inline FCL_REAL height() const
sachinc's avatar
sachinc committed
170
171
172
173
  {
    return max_[1] - min_[1];
  }

174
  /// @brief Depth of the AABB
175
  inline FCL_REAL depth() const
sachinc's avatar
sachinc committed
176
177
178
179
  {
    return max_[2] - min_[2];
  }

180
    /// @brief Volume of the AABB
181
  inline FCL_REAL volume() const
sachinc's avatar
sachinc committed
182
183
  {
    return width() * height() * depth();
184
  }
sachinc's avatar
sachinc committed
185

186
187
188
189
  /// @}

  /// @brief Check whether the AABB contains another AABB
  inline bool contain(const AABB& other) const
sachinc's avatar
sachinc committed
190
  {
191
    return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
sachinc's avatar
sachinc committed
192
193
  }

194
195
  /// @brief Check whether two AABB are overlap and return the overlap part
  inline bool overlap(const AABB& other, AABB& overlap_part) const
sachinc's avatar
sachinc committed
196
  {
197
198
199
200
201
202
203
204
    if(!overlap(other))
    {
      return false;
    }
    
    overlap_part.min_ = min_.cwiseMax(other.min_);
    overlap_part.max_ = max_.cwiseMin(other.max_);
    return true;
sachinc's avatar
sachinc committed
205
206
  }

207
  /// @brief expand the half size of the AABB by delta, and keep the center unchanged.
jpan's avatar
jpan committed
208
209
210
211
212
213
214
  inline AABB& expand(const Vec3f& delta)
  {
    min_ -= delta;
    max_ += delta;
    return *this;
  }

215
  /// @brief expand the aabb by increase the thickness of the plate by a ratio
216
  inline AABB& expand(const AABB& core, FCL_REAL ratio)
jpan's avatar
jpan committed
217
218
219
220
  {
    min_ = min_ * ratio - core.min_;
    max_ = max_ * ratio - core.max_;
    return *this;
jpan's avatar
   
jpan committed
221
  }  
sachinc's avatar
sachinc committed
222
223
};

224
/// @brief translate the center of AABB by t
jpan's avatar
   
jpan committed
225
226
227
228
229
230
231
232
static inline AABB translate(const AABB& aabb, const Vec3f& t)
{
  AABB res(aabb);
  res.min_ += t;
  res.max_ += t;
  return res;
}

Justin Carpentier's avatar
Justin Carpentier committed
233
static inline AABB rotate(const AABB& aabb, const Matrix3f& R)
234
{
Justin Carpentier's avatar
Justin Carpentier committed
235
  AABB res (R * aabb.min_);
236
237
238
239
240
241
  Vec3f corner (aabb.min_);
  const std::size_t bit[3] = { 1, 2, 4 };
  for (std::size_t ic = 1; ic < 8; ++ic) { // ic = 0 corresponds to aabb.min_. Skip it.
    for (std::size_t i = 0; i < 3; ++i) {
      corner[i] = (ic && bit[i]) ? aabb.max_[i] : aabb.min_[i];
    }
Justin Carpentier's avatar
Justin Carpentier committed
242
    res += R * corner;
243
244
245
246
  }
  return res;
}

Joseph Mirabel's avatar
Joseph Mirabel committed
247
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
Joseph Mirabel's avatar
Joseph Mirabel committed
248
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2) HPP_FCL_DLLAPI;
Joseph Mirabel's avatar
Joseph Mirabel committed
249
250
251
252

/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
	     const AABB& b2, const CollisionRequest& request,
Joseph Mirabel's avatar
Joseph Mirabel committed
253
             FCL_REAL& sqrDistLowerBound) HPP_FCL_DLLAPI;
sachinc's avatar
sachinc committed
254
255
}

256
257
} // namespace hpp

sachinc's avatar
sachinc committed
258
#endif