hpp-fcl: include/hpp/fcl/BV/BV.h Source File

hpp-fcl 1.8.0 HPP fork of FCL -- The Flexible Collision Library

BV.h
Go to the documentation of this file.
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
38#ifndef HPP_FCL_BV_H
39#define HPP_FCL_BV_H
40
41
42#include <hpp/fcl/BV/kDOP.h>
43#include <hpp/fcl/BV/AABB.h>
44#include <hpp/fcl/BV/OBB.h>
45#include <hpp/fcl/BV/RSS.h>
46#include <hpp/fcl/BV/OBBRSS.h>
47#include <hpp/fcl/BV/kIOS.h>
48#include <hpp/fcl/math/transform.h>
49
51namespace hpp
52{
53namespace fcl
54{
55
57namespace details
58{
59
61template<typename BV1, typename BV2>
62struct Converter
63{
64 static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2);
65 static void convert(const BV1& bv1, BV2& bv2);
66};
67
68
70template<>
71struct Converter<AABB, AABB>
72{
73 static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2)
74 {
75 const Vec3f& center = bv1.center();
76 FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5;
77 const Vec3f center2 = tf1.transform(center);
78 bv2.min_ = center2 - Vec3f::Constant(r);
79 bv2.max_ = center2 + Vec3f::Constant(r);
80 }
81
82 static void convert(const AABB& bv1, AABB& bv2)
83 {
84 bv2 = bv1;
85 }
86};
87
88template<>
89struct Converter<AABB, OBB>
90{
91 static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2)
92 {
93 bv2.To = tf1.transform(bv1.center());
94 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
95 bv2.axes = tf1.getRotation();
96 }
97
98 static void convert(const AABB& bv1, OBB& bv2)
99 {
100 bv2.To = bv1.center();
101 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
102 bv2.axes.setIdentity();
103 }
104};
105
106template<>
107struct Converter<OBB, OBB>
108{
109 static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2)
110 {
111 bv2.extent = bv1.extent;
112 bv2.To = tf1.transform(bv1.To);
113 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
114 }
115
116 static void convert(const OBB& bv1, OBB& bv2)
117 {
118 bv2 = bv1;
119 }
120};
121
122template<>
123struct Converter<OBBRSS, OBB>
124{
125 static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2)
126 {
127 Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
128 }
129
130 static void convert(const OBBRSS& bv1, OBB& bv2)
131 {
132 Converter<OBB, OBB>::convert(bv1.obb, bv2);
133 }
134};
135
136template<>
137struct Converter<RSS, OBB>
138{
139 static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
140 {
141 bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
142 bv2.To = tf1.transform(bv1.Tr);
143 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
144 }
145
146 static void convert(const RSS& bv1, OBB& bv2)
147 {
148 bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
149 bv2.To = bv1.Tr;
150 bv2.axes = bv1.axes;
151 }
152};
153
154
155template<typename BV1>
156struct Converter<BV1, AABB>
157{
158 static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2)
159 {
160 const Vec3f& center = bv1.center();
161 FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
162 const Vec3f center2 = tf1.transform(center);
163 bv2.min_ = center2 - Vec3f::Constant(r);
164 bv2.max_ = center2 + Vec3f::Constant(r);
165 }
166
167 static void convert(const BV1& bv1, AABB& bv2)
168 {
169 const Vec3f& center = bv1.center();
170 FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
171 bv2.min_ = center - Vec3f::Constant(r);
172 bv2.max_ = center + Vec3f::Constant(r);
173 }
174};
175
176template<typename BV1>
177struct Converter<BV1, OBB>
178{
179 static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2)
180 {
181 AABB bv;
182 Converter<BV1, AABB>::convert(bv1, bv);
183 Converter<AABB, OBB>::convert(bv, tf1, bv2);
184 }
185
186 static void convert(const BV1& bv1, OBB& bv2)
187 {
188 AABB bv;
189 Converter<BV1, AABB>::convert(bv1, bv);
190 Converter<AABB, OBB>::convert(bv, bv2);
191 }
192};
193
194template<>
195struct Converter<OBB, RSS>
196{
197 static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2)
198 {
199 bv2.Tr = tf1.transform(bv1.To);
200 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
201
202 bv2.radius = bv1.extent[2];
203 bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
204 bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
205 }
206
207 static void convert(const OBB& bv1, RSS& bv2)
208 {
209 bv2.Tr = bv1.To;
210 bv2.axes = bv1.axes;
211
212 bv2.radius = bv1.extent[2];
213 bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
214 bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
215 }
216};
217
218template<>
219struct Converter<RSS, RSS>
220{
221 static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2)
222 {
223 bv2.Tr = tf1.transform(bv1.Tr);
224 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
225
226 bv2.radius = bv1.radius;
227 bv2.length[0] = bv1.length[0];
228 bv2.length[1] = bv1.length[1];
229 }
230
231 static void convert(const RSS& bv1, RSS& bv2)
232 {
233 bv2 = bv1;
234 }
235};
236
237template<>
238struct Converter<OBBRSS, RSS>
239{
240 static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2)
241 {
242 Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
243 }
244
245 static void convert(const OBBRSS& bv1, RSS& bv2)
246 {
247 Converter<RSS, RSS>::convert(bv1.rss, bv2);
248 }
249};
250
251template<>
252struct Converter<AABB, RSS>
253{
254 static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2)
255 {
256 bv2.Tr = tf1.transform(bv1.center());
257
259 FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
260 Eigen::DenseIndex id[3] = {0, 1, 2};
261
262 for(Eigen::DenseIndex i = 1; i < 3; ++i)
263 {
264 for(Eigen::DenseIndex j = i; j > 0; --j)
265 {
266 if(d[j] > d[j-1])
267 {
268 {
269 FCL_REAL tmp = d[j];
270 d[j] = d[j-1];
271 d[j-1] = tmp;
272 }
273 {
274 Eigen::DenseIndex tmp = id[j];
275 id[j] = id[j-1];
276 id[j-1] = tmp;
277 }
278 }
279 }
280 }
281
282 const Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
283 bv2.radius = extent[id[2]];
284 bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
285 bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
286
287 const Matrix3f& R = tf1.getRotation();
288 const bool left_hand = (id[0] == (id[1] + 1) % 3);
289 if (left_hand) bv2.axes.col(0) = -R.col(id[0]);
290 else bv2.axes.col(0) = R.col(id[0]);
291 bv2.axes.col(1) = R.col(id[1]);
292 bv2.axes.col(2) = R.col(id[2]);
293 }
294
295 static void convert(const AABB& bv1, RSS& bv2)
296 {
297 convert(bv1, Transform3f(), bv2);
298 }
299};
300
301template<>
302struct Converter<AABB, OBBRSS>
303{
304 static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2)
305 {
306 Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
307 Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
308 }
309
310 static void convert(const AABB& bv1, OBBRSS& bv2)
311 {
312 Converter<AABB, OBB>::convert(bv1, bv2.obb);
313 Converter<AABB, RSS>::convert(bv1, bv2.rss);
314 }
315};
316
317}
318
320
321
323template<typename BV1, typename BV2>
324static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
325{
326 details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
327}
328
330template<typename BV1, typename BV2>
331static inline void convertBV(const BV1& bv1, BV2& bv2)
332{
333 details::Converter<BV1, BV2>::convert(bv1, bv2);
334}
335
336}
337
338} // namespace hpp
339
340#endif
AABB.h
OBBRSS.h
RSS.h
OBB.h
kDOP.h
kIOS.h
details
Definition: traversal_node_setup.h:852
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
transform.h