AABB.h 7.31 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 <stdexcept>
42
#include <hpp/fcl/math/types.h>
sachinc's avatar
sachinc committed
43

44
45
namespace hpp
{
sachinc's avatar
sachinc committed
46
47
namespace fcl
{
48
  class CollisionRequest;
49
/// @brief A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points
sachinc's avatar
sachinc committed
50
51
52
class AABB
{
public:
53
  /// @brief The min point in the AABB
sachinc's avatar
sachinc committed
54
  Vec3f min_;
55
  /// @brief The max point in the AABB
sachinc's avatar
sachinc committed
56
57
  Vec3f max_;

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

61
  /// @brief Creating an AABB at position v with zero size
sachinc's avatar
sachinc committed
62
63
64
65
  AABB(const Vec3f& v) : min_(v), max_(v)
  {
  }

66
  /// @brief Creating an AABB with two endpoints a and b
67
68
  AABB(const Vec3f& a, const Vec3f&b) : min_(a.cwiseMin(b)),
                                        max_(a.cwiseMax(b))
sachinc's avatar
sachinc committed
69
70
71
  {
  }

72
73
74
  /// @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
75
76
77
  {
  }

78
  /// @brief Creating an AABB contains three points
79
80
  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
81
82
83
  {
  }

84
  /// @brief Check whether two AABB are overlap
sachinc's avatar
sachinc committed
85
86
87
88
89
90
91
92
93
94
95
  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
96
97
  }    

98
  /// Not implemented
99
100
  bool overlap(const AABB& other, const CollisionRequest& request,
               FCL_REAL& sqrDistLowerBound) const;
101

102
   /// @brief Check whether the AABB contains another AABB
jpan's avatar
   
jpan committed
103
104
105
  inline bool contain(const AABB& other) const
  {
    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
106
107
  }

108
  /// @brief Check whether two AABB are overlap and return the overlap part
sachinc's avatar
sachinc committed
109
110
111
  inline bool overlap(const AABB& other, AABB& overlap_part) const
  {
    if(!overlap(other))
jpan's avatar
jpan committed
112
    {
sachinc's avatar
sachinc committed
113
      return false;
jpan's avatar
jpan committed
114
    }
sachinc's avatar
sachinc committed
115
    
116
117
    overlap_part.min_ = min_.cwiseMax(other.min_);
    overlap_part.max_ = max_.cwiseMin(other.max_);
sachinc's avatar
sachinc committed
118
119
120
121
    return true;
  }


122
  /// @brief Check whether the AABB contains a point
sachinc's avatar
sachinc committed
123
124
125
126
127
128
129
130
131
  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;
  }

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

140
  /// @brief Merge the AABB and another AABB
sachinc's avatar
sachinc committed
141
142
  inline AABB& operator += (const AABB& other)
  {
Joseph Mirabel's avatar
Joseph Mirabel committed
143
144
    min_ = min_.cwiseMin(other.min_);
    max_ = max_.cwiseMax(other.max_);
sachinc's avatar
sachinc committed
145
146
147
    return *this;
  }

148
  /// @brief Return the merged AABB of current AABB and the other one
sachinc's avatar
sachinc committed
149
150
151
152
153
154
  inline AABB operator + (const AABB& other) const
  {
    AABB res(*this);
    return res += other;
  }

155
  /// @brief Width of the AABB
156
  inline FCL_REAL width() const
sachinc's avatar
sachinc committed
157
158
159
160
  {
    return max_[0] - min_[0];
  }

161
  /// @brief Height of the AABB
162
  inline FCL_REAL height() const
sachinc's avatar
sachinc committed
163
164
165
166
  {
    return max_[1] - min_[1];
  }

167
  /// @brief Depth of the AABB
168
  inline FCL_REAL depth() const
sachinc's avatar
sachinc committed
169
170
171
172
  {
    return max_[2] - min_[2];
  }

173
    /// @brief Volume of the AABB
174
  inline FCL_REAL volume() const
sachinc's avatar
sachinc committed
175
176
177
178
  {
    return width() * height() * depth();
  }  

179
  /// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
180
  inline FCL_REAL size() const
sachinc's avatar
sachinc committed
181
  {
182
    return (max_ - min_).squaredNorm();
sachinc's avatar
sachinc committed
183
184
  }

185
  /// @brief Center of the AABB
sachinc's avatar
sachinc committed
186
187
188
189
190
  inline  Vec3f center() const
  {
    return (min_ + max_) * 0.5;
  }

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

194
  /// @brief Distance between two AABBs
195
  FCL_REAL distance(const AABB& other) const;
jpan's avatar
jpan committed
196

197
  /// @brief expand the half size of the AABB by delta, and keep the center unchanged.
jpan's avatar
jpan committed
198
199
200
201
202
203
204
  inline AABB& expand(const Vec3f& delta)
  {
    min_ -= delta;
    max_ += delta;
    return *this;
  }

205
  /// @brief expand the aabb by increase the thickness of the plate by a ratio
206
  inline AABB& expand(const AABB& core, FCL_REAL ratio)
jpan's avatar
jpan committed
207
208
209
210
  {
    min_ = min_ * ratio - core.min_;
    max_ = max_ * ratio - core.max_;
    return *this;
jpan's avatar
   
jpan committed
211
  }  
sachinc's avatar
sachinc committed
212
213
};

214
/// @brief translate the center of AABB by t
jpan's avatar
   
jpan committed
215
216
217
218
219
220
221
222
static inline AABB translate(const AABB& aabb, const Vec3f& t)
{
  AABB res(aabb);
  res.min_ += t;
  res.max_ += t;
  return res;
}

223
224
225
226
227
228
229
230
231
232
233
234
235
236
static inline AABB rotate(const AABB& aabb, const Matrix3f& t)
{
  AABB res (t * aabb.min_);
  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];
    }
    res += t * corner;
  }
  return res;
}

Joseph Mirabel's avatar
Joseph Mirabel committed
237
238
239
240
241
242
243
244
/// @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);

/// @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,
             FCL_REAL& sqrDistLowerBound);

sachinc's avatar
sachinc committed
245
246
}

247
248
} // namespace hpp

sachinc's avatar
sachinc committed
249
#endif