OBBRSS.h 5.12 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
38
39
40
 *     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 */

#ifndef FCL_OBBRSS_H
#define FCL_OBBRSS_H

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
jpan's avatar
jpan committed
160
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
jpan's avatar
 
jpan committed
161

162
163
164
165
/// 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.
166
167
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1,
             const OBBRSS& b2, const CollisionRequest& request,
168
169
	     FCL_REAL& sqrDistLowerBound);

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
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
jpan's avatar
 
jpan committed
172
173
174
175

}


176
177
} // namespace hpp

jpan's avatar
 
jpan committed
178
#endif