hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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#include <hpp/fcl/BV/kDOP.h>
42#include <hpp/fcl/BV/AABB.h>
43#include <hpp/fcl/BV/OBB.h>
44#include <hpp/fcl/BV/RSS.h>
45#include <hpp/fcl/BV/OBBRSS.h>
46#include <hpp/fcl/BV/kIOS.h>
48
50namespace hpp {
51namespace fcl {
52
54namespace details {
55
58template <typename BV1, typename BV2>
59struct Converter {
60 static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2);
61 static void convert(const BV1& bv1, BV2& bv2);
62};
63
65template <>
66struct Converter<AABB, AABB> {
67 static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) {
68 const Vec3f& center = bv1.center();
69 FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5;
70 const Vec3f center2 = tf1.transform(center);
71 bv2.min_ = center2 - Vec3f::Constant(r);
72 bv2.max_ = center2 + Vec3f::Constant(r);
73 }
74
75 static void convert(const AABB& bv1, AABB& bv2) { bv2 = bv1; }
76};
77
78template <>
79struct Converter<AABB, OBB> {
80 static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2) {
81 bv2.To = tf1.transform(bv1.center());
82 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
83 bv2.axes = tf1.getRotation();
84 }
85
86 static void convert(const AABB& bv1, OBB& bv2) {
87 bv2.To = bv1.center();
88 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
89 bv2.axes.setIdentity();
90 }
91};
92
93template <>
94struct Converter<OBB, OBB> {
95 static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2) {
96 bv2.extent = bv1.extent;
97 bv2.To = tf1.transform(bv1.To);
98 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
99 }
100
101 static void convert(const OBB& bv1, OBB& bv2) { bv2 = bv1; }
102};
103
104template <>
105struct Converter<OBBRSS, OBB> {
106 static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2) {
107 Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
108 }
109
110 static void convert(const OBBRSS& bv1, OBB& bv2) {
111 Converter<OBB, OBB>::convert(bv1.obb, bv2);
112 }
113};
114
115template <>
116struct Converter<RSS, OBB> {
117 static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) {
118 bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius,
119 bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
120 bv2.To = tf1.transform(bv1.Tr);
121 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
122 }
123
124 static void convert(const RSS& bv1, OBB& bv2) {
125 bv2.extent = Vec3f(bv1.length[0] * 0.5 + bv1.radius,
126 bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
127 bv2.To = bv1.Tr;
128 bv2.axes = bv1.axes;
129 }
130};
131
132template <typename BV1>
133struct Converter<BV1, AABB> {
134 static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) {
135 const Vec3f& center = bv1.center();
136 FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
137 const Vec3f center2 = tf1.transform(center);
138 bv2.min_ = center2 - Vec3f::Constant(r);
139 bv2.max_ = center2 + Vec3f::Constant(r);
140 }
141
142 static void convert(const BV1& bv1, AABB& bv2) {
143 const Vec3f& center = bv1.center();
144 FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
145 bv2.min_ = center - Vec3f::Constant(r);
146 bv2.max_ = center + Vec3f::Constant(r);
147 }
148};
149
150template <typename BV1>
151struct Converter<BV1, OBB> {
152 static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2) {
153 AABB bv;
154 Converter<BV1, AABB>::convert(bv1, bv);
155 Converter<AABB, OBB>::convert(bv, tf1, bv2);
156 }
157
158 static void convert(const BV1& bv1, OBB& bv2) {
159 AABB bv;
160 Converter<BV1, AABB>::convert(bv1, bv);
161 Converter<AABB, OBB>::convert(bv, bv2);
162 }
163};
164
165template <>
166struct Converter<OBB, RSS> {
167 static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2) {
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 static void convert(const OBB& bv1, RSS& bv2) {
177 bv2.Tr = bv1.To;
178 bv2.axes = bv1.axes;
179
180 bv2.radius = bv1.extent[2];
181 bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
182 bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
183 }
184};
185
186template <>
187struct Converter<RSS, RSS> {
188 static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2) {
189 bv2.Tr = tf1.transform(bv1.Tr);
190 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
191
192 bv2.radius = bv1.radius;
193 bv2.length[0] = bv1.length[0];
194 bv2.length[1] = bv1.length[1];
195 }
196
197 static void convert(const RSS& bv1, RSS& bv2) { bv2 = bv1; }
198};
199
200template <>
201struct Converter<OBBRSS, RSS> {
202 static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2) {
203 Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
204 }
205
206 static void convert(const OBBRSS& bv1, RSS& bv2) {
207 Converter<RSS, RSS>::convert(bv1.rss, bv2);
208 }
209};
210
211template <>
212struct Converter<AABB, RSS> {
213 static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2) {
214 bv2.Tr = tf1.transform(bv1.center());
215
217 FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth()};
218 Eigen::DenseIndex id[3] = {0, 1, 2};
219
220 for (Eigen::DenseIndex i = 1; i < 3; ++i) {
221 for (Eigen::DenseIndex j = i; j > 0; --j) {
222 if (d[j] > d[j - 1]) {
223 {
224 FCL_REAL tmp = d[j];
225 d[j] = d[j - 1];
226 d[j - 1] = tmp;
227 }
228 {
229 Eigen::DenseIndex tmp = id[j];
230 id[j] = id[j - 1];
231 id[j - 1] = tmp;
232 }
233 }
234 }
235 }
236
237 const Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
238 bv2.radius = extent[id[2]];
239 bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
240 bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
241
242 const Matrix3f& R = tf1.getRotation();
243 const bool left_hand = (id[0] == (id[1] + 1) % 3);
244 if (left_hand)
245 bv2.axes.col(0) = -R.col(id[0]);
246 else
247 bv2.axes.col(0) = R.col(id[0]);
248 bv2.axes.col(1) = R.col(id[1]);
249 bv2.axes.col(2) = R.col(id[2]);
250 }
251
252 static void convert(const AABB& bv1, RSS& bv2) {
253 convert(bv1, Transform3f(), bv2);
254 }
255};
256
257template <>
258struct Converter<AABB, OBBRSS> {
259 static void convert(const AABB& bv1, const Transform3f& tf1, OBBRSS& bv2) {
260 Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
261 Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
262 }
263
264 static void convert(const AABB& bv1, OBBRSS& bv2) {
265 Converter<AABB, OBB>::convert(bv1, bv2.obb);
266 Converter<AABB, RSS>::convert(bv1, bv2.rss);
267 }
268};
269
270} // namespace details
271
273
276template <typename BV1, typename BV2>
277static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2) {
278 details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
279}
280
283template <typename BV1, typename BV2>
284static inline void convertBV(const BV1& bv1, BV2& bv2) {
285 details::Converter<BV1, BV2>::convert(bv1, bv2);
286}
287
288} // namespace fcl
289
290} // namespace hpp
291
292#endif
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44