OBBRSS.h 5.07 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 whether the OBBRSS contains a point
jpan's avatar
 
jpan committed
78
79
80
81
82
  inline bool contain(const Vec3f& p) const
  {
    return obb.contain(p);
  }

83
  /// @brief Merge the OBBRSS and a point
jpan's avatar
 
jpan committed
84
85
86
87
88
89
90
  OBBRSS& operator += (const Vec3f& p) 
  {
    obb += p;
    rss += p;
    return *this;
  }

91
  /// @brief Merge two OBBRSS
jpan's avatar
 
jpan committed
92
93
94
95
96
97
  OBBRSS& operator += (const OBBRSS& other)
  {
    *this = *this + other;
    return *this;
  }

98
  /// @brief Merge two OBBRSS
jpan's avatar
 
jpan committed
99
100
101
102
103
104
105
106
  OBBRSS operator + (const OBBRSS& other) const
  {
    OBBRSS result;
    result.obb = obb + other.obb;
    result.rss = rss + other.rss;
    return result;
  }

107
  /// @brief Width of the OBRSS
108
  inline FCL_REAL width() const
jpan's avatar
 
jpan committed
109
110
111
112
  {
    return obb.width();
  }

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

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

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

131
  /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
132
  inline FCL_REAL size() const
jpan's avatar
 
jpan committed
133
134
135
136
  {
    return obb.size();
  }

137
  /// @brief Center of the OBBRSS
jpan's avatar
 
jpan committed
138
139
140
141
142
  inline const Vec3f& center() const
  {
    return obb.center();
  }

143
  /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points
144
  FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
jpan's avatar
 
jpan committed
145
146
147
148
149
  {
    return rss.distance(other.rss, P, Q);
  }
};

150
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
151
152
153
154
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
155

156
157
158
159
/// 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.
160
161
162
163
164
165
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);
}
166

167
/// @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
168
169
170
171
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
172
173
174
175

}


176
177
} // namespace hpp

jpan's avatar
 
jpan committed
178
#endif