61 template<
typename BV1,
typename BV2>
64 static void convert(
const BV1& bv1,
const Transform3f& tf1, BV2& bv2);
65 static void convert(
const BV1& bv1, BV2& bv2);
71 struct Converter<AABB, AABB>
73 static void convert(
const AABB& bv1,
const Transform3f& tf1, AABB& bv2)
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);
82 static void convert(
const AABB& bv1, AABB& bv2)
89 struct Converter<AABB, OBB>
91 static void convert(
const AABB& bv1,
const Transform3f& tf1, OBB& bv2)
93 bv2.To = tf1.transform(bv1.center());
94 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
95 bv2.axes = tf1.getRotation();
98 static void convert(
const AABB& bv1, OBB& bv2)
100 bv2.To = bv1.center();
101 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
102 bv2.axes.setIdentity();
107 struct Converter<OBB, OBB>
109 static void convert(
const OBB& bv1,
const Transform3f& tf1, OBB& bv2)
111 bv2.extent = bv1.extent;
112 bv2.To = tf1.transform(bv1.To);
113 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
116 static void convert(
const OBB& bv1, OBB& bv2)
123 struct Converter<OBBRSS, OBB>
125 static void convert(
const OBBRSS& bv1,
const Transform3f& tf1, OBB& bv2)
127 Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
130 static void convert(
const OBBRSS& bv1, OBB& bv2)
132 Converter<OBB, OBB>::convert(bv1.obb, bv2);
137 struct Converter<RSS, OBB>
139 static void convert(
const RSS& bv1,
const Transform3f& tf1, OBB& bv2)
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;
146 static void convert(
const RSS& bv1, OBB& bv2)
148 bv2.extent =
Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
155 template<
typename BV1>
156 struct Converter<BV1, AABB>
158 static void convert(
const BV1& bv1,
const Transform3f& tf1, AABB& bv2)
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);
167 static void convert(
const BV1& bv1, AABB& bv2)
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);
176 template<
typename BV1>
177 struct Converter<BV1, OBB>
179 static void convert(
const BV1& bv1,
const Transform3f& tf1, OBB& bv2)
182 Converter<BV1, AABB>::convert(bv1, bv);
183 Converter<AABB, OBB>::convert(bv, tf1, bv2);
186 static void convert(
const BV1& bv1, OBB& bv2)
189 Converter<BV1, AABB>::convert(bv1, bv);
190 Converter<AABB, OBB>::convert(bv, bv2);
195 struct Converter<OBB, RSS>
197 static void convert(
const OBB& bv1,
const Transform3f& tf1, RSS& bv2)
199 bv2.Tr = tf1.transform(bv1.To);
200 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
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);
207 static void convert(
const OBB& bv1, RSS& bv2)
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);
219 struct Converter<RSS, RSS>
221 static void convert(
const RSS& bv1,
const Transform3f& tf1, RSS& bv2)
223 bv2.Tr = tf1.transform(bv1.Tr);
224 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
226 bv2.radius = bv1.radius;
227 bv2.length[0] = bv1.length[0];
228 bv2.length[1] = bv1.length[1];
231 static void convert(
const RSS& bv1, RSS& bv2)
238 struct Converter<OBBRSS, RSS>
240 static void convert(
const OBBRSS& bv1,
const Transform3f& tf1, RSS& bv2)
242 Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
245 static void convert(
const OBBRSS& bv1, RSS& bv2)
247 Converter<RSS, RSS>::convert(bv1.rss, bv2);
252 struct Converter<AABB, RSS>
254 static void convert(
const AABB& bv1,
const Transform3f& tf1, RSS& bv2)
256 bv2.Tr = tf1.transform(bv1.center());
259 FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
260 Eigen::DenseIndex
id[3] = {0, 1, 2};
262 for(Eigen::DenseIndex i = 1; i < 3; ++i)
264 for(Eigen::DenseIndex j = i; j > 0; --j)
274 Eigen::DenseIndex tmp =
id[j];
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;
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]);
295 static void convert(
const AABB& bv1, RSS& bv2)
297 convert(bv1, Transform3f(), bv2);
302 struct Converter<AABB, OBBRSS>
304 static void convert(
const AABB& bv1,
const Transform3f& tf1, OBBRSS& bv2)
306 Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
307 Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
310 static void convert(
const AABB& bv1, OBBRSS& bv2)
312 Converter<AABB, OBB>::convert(bv1, bv2.obb);
313 Converter<AABB, RSS>::convert(bv1, bv2.rss);
323 template<
typename BV1,
typename BV2>
324 static inline void convertBV(
const BV1& bv1,
const Transform3f& tf1, BV2& bv2)
326 details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
330 template<
typename BV1,
typename BV2>
331 static inline void convertBV(
const BV1& bv1, BV2& bv2)
333 details::Converter<BV1, BV2>::convert(bv1, bv2);
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