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

hpp-fcl 1.7.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
51 namespace hpp
52 {
53 namespace fcl
54 {
55
57 namespace details
58 {
59
61 template<typename BV1, typename BV2>
62 class Converter
63 {
64 private:
65 static void convert(const BV1& /*bv1*/, const Transform3f& /*tf1*/, BV2& /*bv2*/)
66 {
67 // should only use the specialized version, so it is private.
68 }
69 };
70
71
73 template<>
74 class Converter<AABB, AABB>
75 {
76 public:
77 static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2)
78 {
79 const Vec3f& center = bv1.center();
80 FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5;
81 Vec3f center2 = tf1.transform(center);
82 Vec3f delta(r, r, r);
83 bv2.min_ = center2 - delta;
84 bv2.max_ = center2 + delta;
85 }
86 };
87
88 template<>
89 class Converter<AABB, OBB>
90 {
91 public:
92 static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2)
93 {
94 bv2.To.noalias() = tf1.transform(bv1.center());
95 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
96 bv2.axes.noalias() = tf1.getRotation();
97 }
98 };
99
100 template<>
101 class Converter<OBB, OBB>
102 {
103 public:
104 static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2)
105 {
106 bv2.extent.noalias() = bv1.extent;
107 bv2.To.noalias() = tf1.transform(bv1.To);
108 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
109 }
110 };
111
112 template<>
113 class Converter<OBBRSS, OBB>
114 {
115 public:
116 static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2)
117 {
118 Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
119 }
120 };
121
122 template<>
123 class Converter<RSS, OBB>
124 {
125 public:
126 static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
127 {
128 bv2.extent.noalias() = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
129 bv2.To.noalias() = tf1.transform(bv1.Tr);
130 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
131 }
132 };
133
134
135 template<typename BV1>
136 class Converter<BV1, AABB>
137 {
138 public:
139 static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2)
140 {
141 const Vec3f& center = bv1.center();
142 FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
143 Vec3f delta(r, r, r);
144 Vec3f center2 = tf1.transform(center);
145 bv2.min_ = center2 - delta;
146 bv2.max_ = center2 + delta;
147 }
148 };
149
150 template<typename BV1>
151 class Converter<BV1, OBB>
152 {
153 public:
154 static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2)
155 {
156 AABB bv;
157 Converter<BV1, AABB>::convert(bv1, Transform3f(), bv);
158 Converter<AABB, OBB>::convert(bv, tf1, bv2);
159 }
160 };
161
162 template<>
163 class Converter<OBB, RSS>
164 {
165 public:
166 static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2)
167 {
168 bv2.Tr = tf1.transform(bv1.To);
169 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
170
171 bv2.radius = bv1.extent[2];
172 bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
173 bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
174 }
175 };
176
177 template<>
178 class Converter<RSS, RSS>
179 {
180 public:
181 static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2)
182 {
183 bv2.Tr.noalias() = tf1.transform(bv1.Tr);
184 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
185
186 bv2.radius = bv1.radius;
187 bv2.length[0] = bv1.length[0];
188 bv2.length[1] = bv1.length[1];
189 }
190 };
191
192 template<>
193 class Converter<OBBRSS, RSS>
194 {
195 public:
196 static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2)
197 {
198 Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
199 }
200 };
201
202 template<>
203 class Converter<AABB, RSS>
204 {
205 public:
206 static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2)
207 {
208 bv2.Tr = tf1.transform(bv1.center());
209
211 FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
212 std::size_t id[3] = {0, 1, 2};
213
214 for(std::size_t i = 1; i < 3; ++i)
215 {
216 for(std::size_t j = i; j > 0; --j)
217 {
218 if(d[j] > d[j-1])
219 {
220 {
221 FCL_REAL tmp = d[j];
222 d[j] = d[j-1];
223 d[j-1] = tmp;
224 }
225 {
226 std::size_t tmp = id[j];
227 id[j] = id[j-1];
228 id[j-1] = tmp;
229 }
230 }
231 }
232 }
233
234 Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
235 bv2.radius = extent[id[2]];
236 bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
237 bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
238
239 const Matrix3f& R = tf1.getRotation();
240 bool left_hand = (id[0] == (id[1] + 1) % 3);
241 if (left_hand) bv2.axes.col(0).noalias() = -R.col(id[0]);
242 else bv2.axes.col(0).noalias() = R.col(id[0]);
243 bv2.axes.col(1).noalias() = R.col(id[1]);
244 bv2.axes.col(2).noalias() = R.col(id[2]);
245 }
246 };
247
248 template<>
249 class Converter<AABB, OBBRSS>
250 {
251 public:
252 static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2)
253 {
254 Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
255 Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
256 }
257 };
258
259 }
260
262
263
265 template<typename BV1, typename BV2>
266 static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
267 {
268 details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
269 }
270
271 }
272
273 } // namespace hpp
274
275 #endif
AABB.h
OBBRSS.h
RSS.h
OBB.h
kDOP.h
kIOS.h
details
Definition: traversal_node_setup.h:776
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
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