61 template<
typename BV1,
typename BV2>
65 static void convert(
const BV1& ,
const Transform3f& , BV2& )
74 class Converter<AABB, AABB>
77 static void convert(
const AABB& bv1,
const Transform3f& tf1, AABB& bv2)
79 const Vec3f& center = bv1.center();
80 FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5;
81 Vec3f center2 = tf1.transform(center);
83 bv2.min_ = center2 - delta;
84 bv2.max_ = center2 + delta;
89 class Converter<AABB, OBB>
92 static void convert(
const AABB& bv1,
const Transform3f& tf1, OBB& bv2)
94 bv2.To.noalias() = tf1.transform(bv1.center());
95 bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
96 bv2.axes.noalias() = tf1.getRotation();
101 class Converter<OBB, OBB>
104 static void convert(
const OBB& bv1,
const Transform3f& tf1, OBB& bv2)
106 bv2.extent.noalias() = bv1.extent;
107 bv2.To.noalias() = tf1.transform(bv1.To);
108 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
113 class Converter<OBBRSS, OBB>
116 static void convert(
const OBBRSS& bv1,
const Transform3f& tf1, OBB& bv2)
118 Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
123 class Converter<RSS, OBB>
126 static void convert(
const RSS& bv1,
const Transform3f& tf1, OBB& bv2)
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;
135 template<
typename BV1>
136 class Converter<BV1, AABB>
139 static void convert(
const BV1& bv1,
const Transform3f& tf1, AABB& bv2)
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;
150 template<
typename BV1>
151 class Converter<BV1, OBB>
154 static void convert(
const BV1& bv1,
const Transform3f& tf1, OBB& bv2)
157 Converter<BV1, AABB>::convert(bv1, Transform3f(), bv);
158 Converter<AABB, OBB>::convert(bv, tf1, bv2);
163 class Converter<OBB, RSS>
166 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;
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);
178 class Converter<RSS, RSS>
181 static void convert(
const RSS& bv1,
const Transform3f& tf1, RSS& bv2)
183 bv2.Tr.noalias() = tf1.transform(bv1.Tr);
184 bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
186 bv2.radius = bv1.radius;
187 bv2.length[0] = bv1.length[0];
188 bv2.length[1] = bv1.length[1];
193 class Converter<OBBRSS, RSS>
196 static void convert(
const OBBRSS& bv1,
const Transform3f& tf1, RSS& bv2)
198 Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
203 class Converter<AABB, RSS>
206 static void convert(
const AABB& bv1,
const Transform3f& tf1, RSS& bv2)
208 bv2.Tr = tf1.transform(bv1.center());
211 FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
212 std::size_t
id[3] = {0, 1, 2};
214 for(std::size_t i = 1; i < 3; ++i)
216 for(std::size_t j = i; j > 0; --j)
226 std::size_t tmp =
id[j];
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;
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]);
249 class Converter<AABB, OBBRSS>
252 static void convert(
const AABB& bv1,
const Transform3f& tf1, OBBRSS& bv2)
254 Converter<AABB, OBB>::convert(bv1, tf1, bv2.obb);
255 Converter<AABB, RSS>::convert(bv1, tf1, bv2.rss);
265 template<
typename BV1,
typename BV2>
266 static inline void convertBV(
const BV1& bv1,
const Transform3f& tf1, BV2& bv2)
268 details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
Main namespace.
Definition: AABB.h:43
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:74
double FCL_REAL
Definition: data_types.h:68
Definition: traversal_node_setup.h:775
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73