OBBRSS.h 5.11 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
52
53
/// @addtogroup Bounding_Volume
/// @{

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

59
  /// @brief OBB member, for rotation
jpan's avatar
 
jpan committed
60
  OBB obb;
61
62

  /// @brief RSS member, for distance
jpan's avatar
 
jpan committed
63
  RSS rss;
64

65
66
67
68
69
70
/// @brief Check whether the OBBRSS contains a point
  inline bool contain(const Vec3f& p) const
  {
    return obb.contain(p);
  }

71
  /// @brief Check collision between two OBBRSS
jpan's avatar
 
jpan committed
72
73
74
75
76
  bool overlap(const OBBRSS& other) const
  {
    return obb.overlap(other.obb);
  }

77
  /// Check collision between two OBBRSS
Lucile Remigy's avatar
Lucile Remigy committed
78
  /// @retval sqrDistLowerBound squared lower bound on distance between
79
  ///         objects if they do not overlap.
80
81
  bool overlap(const OBBRSS& other, const CollisionRequest& request,
               FCL_REAL& sqrDistLowerBound) const
82
  {
83
    return obb.overlap(other.obb, request, sqrDistLowerBound);
84
85
  }

86
87
  /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points
  FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
jpan's avatar
 
jpan committed
88
  {
89
    return rss.distance(other.rss, P, Q);
jpan's avatar
 
jpan committed
90
91
  }

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

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

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

116
117
118
119
120
121
122
123
124
125
126
127
    /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
  inline FCL_REAL size() const
  {
    return obb.size();
  }

  /// @brief Center of the OBBRSS
  inline const Vec3f& center() const
  {
    return obb.center();
  }

128
  /// @brief Width of the OBRSS
129
  inline FCL_REAL width() const
jpan's avatar
 
jpan committed
130
131
132
133
  {
    return obb.width();
  }

134
  /// @brief Height of the OBBRSS
135
  inline FCL_REAL height() const
jpan's avatar
 
jpan committed
136
137
138
139
  {
    return obb.height();
  }

140
  /// @brief Depth of the OBBRSS
141
  inline FCL_REAL depth() const
jpan's avatar
 
jpan committed
142
143
144
145
  {
    return obb.depth();
  }

146
  /// @brief Volume of the OBBRSS
147
  inline FCL_REAL volume() const
jpan's avatar
 
jpan committed
148
149
150
  {
    return obb.volume();
  }
151
152
};

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

159
/// Check collision between two OBBRSS
Lucile Remigy's avatar
Lucile Remigy committed
160
161
162
/// @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.
163
164
165
166
167
168
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);
}
169

170
/// @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
171
172
173
174
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
175
176
177
178

}


179
180
} // namespace hpp

jpan's avatar
 
jpan committed
181
#endif