hpp-fcl  1.8.1
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 
51 namespace hpp
52 {
53 namespace fcl
54 {
55 
57 namespace details
58 {
59 
61 template<typename BV1, typename BV2>
62 struct 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 
70 template<>
71 struct 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 
88 template<>
89 struct 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 
106 template<>
107 struct 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 
122 template<>
123 struct 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 
136 template<>
137 struct 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 
155 template<typename BV1>
156 struct 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 
176 template<typename BV1>
177 struct 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 
194 template<>
195 struct 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 
218 template<>
219 struct 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 
237 template<>
238 struct 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 
251 template<>
252 struct 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 
301 template<>
302 struct 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 
323 template<typename BV1, typename BV2>
324 static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
325 {
326  details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
327 }
328 
330 template<typename BV1, typename BV2>
331 static 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
Main namespace.
Definition: AABB.h:43
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
double FCL_REAL
Definition: data_types.h:66
Definition: traversal_node_setup.h:851
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67