1 |
|
|
/* |
2 |
|
|
* Software License Agreement (BSD License) |
3 |
|
|
* |
4 |
|
|
* Copyright (c) 2011-2014, Willow Garage, Inc. |
5 |
|
|
* Copyright (c) 2014-2015, Open Source Robotics Foundation |
6 |
|
|
* All rights reserved. |
7 |
|
|
* |
8 |
|
|
* Redistribution and use in source and binary forms, with or without |
9 |
|
|
* modification, are permitted provided that the following conditions |
10 |
|
|
* are met: |
11 |
|
|
* |
12 |
|
|
* * Redistributions of source code must retain the above copyright |
13 |
|
|
* notice, this list of conditions and the following disclaimer. |
14 |
|
|
* * Redistributions in binary form must reproduce the above |
15 |
|
|
* copyright notice, this list of conditions and the following |
16 |
|
|
* disclaimer in the documentation and/or other materials provided |
17 |
|
|
* with the distribution. |
18 |
|
|
* * Neither the name of Open Source Robotics Foundation nor the names of its |
19 |
|
|
* contributors may be used to endorse or promote products derived |
20 |
|
|
* from this software without specific prior written permission. |
21 |
|
|
* |
22 |
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
23 |
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
24 |
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
25 |
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
26 |
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
27 |
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
28 |
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
29 |
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
30 |
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
31 |
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32 |
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33 |
|
|
* POSSIBILITY OF SUCH DAMAGE. |
34 |
|
|
*/ |
35 |
|
|
|
36 |
|
|
/** \author Jia Pan */ |
37 |
|
|
|
38 |
|
|
#ifndef HPP_FCL_RSS_H |
39 |
|
|
#define HPP_FCL_RSS_H |
40 |
|
|
|
41 |
|
|
#include <hpp/fcl/data_types.h> |
42 |
|
|
#include <boost/math/constants/constants.hpp> |
43 |
|
|
|
44 |
|
|
namespace hpp { |
45 |
|
|
namespace fcl { |
46 |
|
|
|
47 |
|
|
struct CollisionRequest; |
48 |
|
|
|
49 |
|
|
/// @addtogroup Bounding_Volume |
50 |
|
|
/// @{ |
51 |
|
|
|
52 |
|
|
/// @brief A class for rectangle sphere-swept bounding volume |
53 |
|
|
struct HPP_FCL_DLLAPI RSS { |
54 |
|
|
/// @brief Orientation of RSS. axis[i] is the ith column of the orientation |
55 |
|
|
/// matrix for the RSS; it is also the i-th principle direction of the RSS. We |
56 |
|
|
/// assume that axis[0] corresponds to the axis with the longest length, |
57 |
|
|
/// axis[1] corresponds to the shorter one and axis[2] corresponds to the |
58 |
|
|
/// shortest one. |
59 |
|
|
Matrix3f axes; |
60 |
|
|
|
61 |
|
|
/// @brief Origin of the rectangle in RSS |
62 |
|
|
Vec3f Tr; |
63 |
|
|
|
64 |
|
|
/// @brief Side lengths of rectangle |
65 |
|
|
FCL_REAL length[2]; |
66 |
|
|
|
67 |
|
|
/// @brief Radius of sphere summed with rectangle to form RSS |
68 |
|
|
FCL_REAL radius; |
69 |
|
|
|
70 |
|
|
/// @brief Default constructor with default values |
71 |
✓✗✓✗
|
2712452 |
RSS() : axes(Matrix3f::Zero()), Tr(Vec3f::Zero()), radius(-1) { |
72 |
|
2712452 |
length[0] = 0; |
73 |
|
2712452 |
length[1] = 0; |
74 |
|
2712452 |
} |
75 |
|
|
|
76 |
|
|
/// @brief Equality operator |
77 |
|
374509 |
bool operator==(const RSS& other) const { |
78 |
✓✗ |
749018 |
return axes == other.axes && Tr == other.Tr && |
79 |
✓✗✓✗
✓✗ |
1123527 |
length[0] == other.length[0] && length[1] == other.length[1] && |
80 |
✓✗ |
749018 |
radius == other.radius; |
81 |
|
|
} |
82 |
|
|
|
83 |
|
|
/// @brief Difference operator |
84 |
|
|
bool operator!=(const RSS& other) const { return !(*this == other); } |
85 |
|
|
|
86 |
|
|
/// @brief Check whether the RSS contains a point |
87 |
|
|
bool contain(const Vec3f& p) const; |
88 |
|
|
|
89 |
|
|
/// @brief Check collision between two RSS |
90 |
|
|
bool overlap(const RSS& other) const; |
91 |
|
|
|
92 |
|
|
/// Not implemented |
93 |
|
15645 |
bool overlap(const RSS& other, const CollisionRequest&, |
94 |
|
|
FCL_REAL& sqrDistLowerBound) const { |
95 |
|
15645 |
sqrDistLowerBound = sqrt(-1); |
96 |
|
15645 |
return overlap(other); |
97 |
|
|
} |
98 |
|
|
|
99 |
|
|
/// @brief the distance between two RSS; P and Q, if not NULL, return the |
100 |
|
|
/// nearest points |
101 |
|
|
FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; |
102 |
|
|
|
103 |
|
|
/// @brief A simple way to merge the RSS and a point, not compact. |
104 |
|
|
/// @todo This function may have some bug. |
105 |
|
|
RSS& operator+=(const Vec3f& p); |
106 |
|
|
|
107 |
|
|
/// @brief Merge the RSS and another RSS |
108 |
|
|
inline RSS& operator+=(const RSS& other) { |
109 |
|
|
*this = *this + other; |
110 |
|
|
return *this; |
111 |
|
|
} |
112 |
|
|
|
113 |
|
|
/// @brief Return the merged RSS of current RSS and the other one |
114 |
|
|
RSS operator+(const RSS& other) const; |
115 |
|
|
|
116 |
|
|
/// @brief Size of the RSS (used in BV_Splitter to order two RSSs) |
117 |
|
31542 |
inline FCL_REAL size() const { |
118 |
|
31542 |
return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + |
119 |
|
31542 |
2 * radius); |
120 |
|
|
} |
121 |
|
|
|
122 |
|
|
/// @brief The RSS center |
123 |
|
98004 |
inline const Vec3f& center() const { return Tr; } |
124 |
|
|
|
125 |
|
|
/// @brief Width of the RSS |
126 |
|
|
inline FCL_REAL width() const { return length[0] + 2 * radius; } |
127 |
|
|
|
128 |
|
|
/// @brief Height of the RSS |
129 |
|
|
inline FCL_REAL height() const { return length[1] + 2 * radius; } |
130 |
|
|
|
131 |
|
|
/// @brief Depth of the RSS |
132 |
|
|
inline FCL_REAL depth() const { return 2 * radius; } |
133 |
|
|
|
134 |
|
|
/// @brief Volume of the RSS |
135 |
|
|
inline FCL_REAL volume() const { |
136 |
|
|
return (length[0] * length[1] * 2 * radius + |
137 |
|
|
4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * |
138 |
|
|
radius); |
139 |
|
|
} |
140 |
|
|
|
141 |
|
|
/// @brief Check collision between two RSS and return the overlap part. |
142 |
|
|
/// For RSS, we return nothing, as the overlap part of two RSSs usually is not |
143 |
|
|
/// a RSS. |
144 |
|
|
bool overlap(const RSS& other, RSS& /*overlap_part*/) const { |
145 |
|
|
return overlap(other); |
146 |
|
|
} |
147 |
|
|
}; |
148 |
|
|
|
149 |
|
|
/// @brief distance between two RSS bounding volumes |
150 |
|
|
/// P and Q (optional return values) are the closest points in the rectangles, |
151 |
|
|
/// not the RSS. But the direction P - Q is the correct direction for cloest |
152 |
|
|
/// points Notice that P and Q are both in the local frame of the first RSS (not |
153 |
|
|
/// global frame and not even the local frame of object 1) |
154 |
|
|
HPP_FCL_DLLAPI FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, |
155 |
|
|
const RSS& b1, const RSS& b2, Vec3f* P = NULL, |
156 |
|
|
Vec3f* Q = NULL); |
157 |
|
|
|
158 |
|
|
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and |
159 |
|
|
/// b2 is in identity. |
160 |
|
|
HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, |
161 |
|
|
const RSS& b2); |
162 |
|
|
|
163 |
|
|
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and |
164 |
|
|
/// b2 is in identity. |
165 |
|
|
HPP_FCL_DLLAPI bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, |
166 |
|
|
const RSS& b2, const CollisionRequest& request, |
167 |
|
|
FCL_REAL& sqrDistLowerBound); |
168 |
|
|
|
169 |
|
|
} // namespace fcl |
170 |
|
|
|
171 |
|
|
} // namespace hpp |
172 |
|
|
|
173 |
|
|
#endif |