OBBRSS.h 5.33 KB
Newer Older
jpan's avatar
 
jpan 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
jpan's avatar
 
jpan 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
jpan's avatar
 
jpan 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_OBBRSS_H
#define HPP_FCL_OBBRSS_H
jpan's avatar
 
jpan committed
40

jpan's avatar
jpan committed
41

42
43
#include <hpp/fcl/BV/OBB.h>
#include <hpp/fcl/BV/RSS.h>
jpan's avatar
 
jpan committed
44

45
46
namespace hpp
{
jpan's avatar
 
jpan committed
47
48
namespace fcl
{
49
  class CollisionRequest;
50
51

/// @brief Class merging the OBB and RSS, can handle collision and distance simultaneously
jpan's avatar
 
jpan committed
52
53
54
55
class OBBRSS
{
public:

56
  /// @brief OBB member, for rotation
jpan's avatar
 
jpan committed
57
  OBB obb;
58
59

  /// @brief RSS member, for distance
jpan's avatar
 
jpan committed
60
  RSS rss;
61
62

  /// @brief Check collision between two OBBRSS
jpan's avatar
 
jpan committed
63
64
65
66
67
  bool overlap(const OBBRSS& other) const
  {
    return obb.overlap(other.obb);
  }

68
69
70
  /// Check collision between two OBBRSS
  /// \retval sqrDistLowerBound squared lower bound on distance between
  ///         objects if they do not overlap.
71
72
  bool overlap(const OBBRSS& other, const CollisionRequest& request,
               FCL_REAL& sqrDistLowerBound) const
73
  {
74
    return obb.overlap(other.obb, request, sqrDistLowerBound);
75
76
  }

77
  /// @brief Check collision between two OBBRSS and return the overlap part.
Joseph Mirabel's avatar
Joseph Mirabel committed
78
  bool overlap(const OBBRSS& other, OBBRSS& /*overlap_part*/) const
jpan's avatar
 
jpan committed
79
80
81
82
  {
    return overlap(other);
  }

83
  /// @brief Check whether the OBBRSS contains a point
jpan's avatar
 
jpan committed
84
85
86
87
88
  inline bool contain(const Vec3f& p) const
  {
    return obb.contain(p);
  }

89
  /// @brief Merge the OBBRSS and a point
jpan's avatar
 
jpan committed
90
91
92
93
94
95
96
  OBBRSS& operator += (const Vec3f& p) 
  {
    obb += p;
    rss += p;
    return *this;
  }

97
  /// @brief Merge two OBBRSS
jpan's avatar
 
jpan committed
98
99
100
101
102
103
  OBBRSS& operator += (const OBBRSS& other)
  {
    *this = *this + other;
    return *this;
  }

104
  /// @brief Merge two OBBRSS
jpan's avatar
 
jpan committed
105
106
107
108
109
110
111
112
  OBBRSS operator + (const OBBRSS& other) const
  {
    OBBRSS result;
    result.obb = obb + other.obb;
    result.rss = rss + other.rss;
    return result;
  }

113
  /// @brief Width of the OBRSS
114
  inline FCL_REAL width() const
jpan's avatar
 
jpan committed
115
116
117
118
  {
    return obb.width();
  }

119
  /// @brief Height of the OBBRSS
120
  inline FCL_REAL height() const
jpan's avatar
 
jpan committed
121
122
123
124
  {
    return obb.height();
  }

125
  /// @brief Depth of the OBBRSS
126
  inline FCL_REAL depth() const
jpan's avatar
 
jpan committed
127
128
129
130
  {
    return obb.depth();
  }

131
  /// @brief Volume of the OBBRSS
132
  inline FCL_REAL volume() const
jpan's avatar
 
jpan committed
133
134
135
136
  {
    return obb.volume();
  }

137
  /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
138
  inline FCL_REAL size() const
jpan's avatar
 
jpan committed
139
140
141
142
  {
    return obb.size();
  }

143
  /// @brief Center of the OBBRSS
jpan's avatar
 
jpan committed
144
145
146
147
148
  inline const Vec3f& center() const
  {
    return obb.center();
  }

149
  /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points
150
  FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
jpan's avatar
 
jpan committed
151
152
153
154
155
  {
    return rss.distance(other.rss, P, Q);
  }
};

jpan's avatar
jpan committed
156
157
/// @brief Translate the OBBRSS bv
OBBRSS translate(const OBBRSS& bv, const Vec3f& t);
jpan's avatar
 
jpan committed
158

159
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
160
161
162
163
inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
{
  return overlap(R0, T0, b1.obb, b2.obb);
}
jpan's avatar
 
jpan committed
164

165
166
167
168
/// Check collision between two OBBRSS
/// \param  b1 first OBBRSS in configuration (R0, T0)
/// \param  b2 second OBBRSS in identity position
/// \retval squared lower bound on the distance if OBBRSS do not overlap.
169
170
171
172
173
174
inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
                    const OBBRSS& b2, const CollisionRequest& request,
	            FCL_REAL& sqrDistLowerBound)
{
  return overlap(R0, T0, b1.obb, b2.obb, request, sqrDistLowerBound);
}
175

176
/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points
177
178
179
180
inline FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL)
{
  return distance(R0, T0, b1.rss, b2.rss, P, Q);
}
jpan's avatar
 
jpan committed
181
182
183
184

}


185
186
} // namespace hpp

jpan's avatar
 
jpan committed
187
#endif