OBBRSS.h 5.18 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
50

  struct CollisionRequest;
51

52
53
54
/// @addtogroup Bounding_Volume
/// @{

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

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

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

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

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

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

87
88
  /// @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
89
  {
90
    return rss.distance(other.rss, P, Q);
jpan's avatar
 
jpan committed
91
92
  }

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

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

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

117
118
119
120
121
122
123
124
125
126
127
128
    /// @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();
  }

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

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

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

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

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

160
/// Check collision between two OBBRSS
Joseph Mirabel's avatar
Joseph Mirabel committed
161
/// @param  R0, T0 configuration of b1
Lucile Remigy's avatar
Lucile Remigy committed
162
163
/// @param  b1 first OBBRSS in configuration (R0, T0)
/// @param  b2 second OBBRSS in identity position
Joseph Mirabel's avatar
Joseph Mirabel committed
164
/// @retval sqrDistLowerBound squared lower bound on the distance if OBBRSS do not overlap.
165
166
167
168
169
170
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);
}
171

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

}


181
182
} // namespace hpp

jpan's avatar
 
jpan committed
183
#endif