38 #ifndef FCL_BV_KIOS_INL_H
39 #define FCL_BV_KIOS_INL_H
56 S dist2 = d.squaredNorm();
57 S diff_r = s1.r - s0.r;
60 if(diff_r * diff_r >= dist2)
62 if(s1.r > s0.r)
return s1;
67 float dist = std::sqrt(dist2);
69 s.r = dist + s0.r + s1.r;
71 s.o = s0.o + d * ((s.r - s0.r) / dist);
82 for(
unsigned int i = 0; i < num_spheres; ++i)
86 S o_dist = (spheres[i].o - other.
spheres[j].o).squaredNorm();
87 S sum_r = spheres[i].r + other.
spheres[j].r;
88 if(o_dist > sum_r * sum_r)
93 return obb.overlap(other.
obb);
107 template <
typename S>
110 for(
unsigned int i = 0; i < num_spheres; ++i)
113 if((spheres[i].o - p).squaredNorm() >
r *
r)
121 template <
typename S>
124 for(
unsigned int i = 0; i < num_spheres; ++i)
127 S new_r_sqr = (p - spheres[i].o).squaredNorm();
128 if(new_r_sqr >
r *
r)
130 spheres[i].r = sqrt(new_r_sqr);
139 template <
typename S>
142 *
this = *
this + other;
147 template <
typename S>
152 for(
unsigned int i = 0; i < new_num_spheres; ++i)
159 result.
obb = obb + other.
obb;
165 template <
typename S>
172 template <
typename S>
179 template <
typename S>
186 template <
typename S>
193 template <
typename S>
200 template <
typename S>
207 template <
typename S>
214 int id_a = -1, id_b = -1;
215 for(
unsigned int i = 0; i < num_spheres; ++i)
217 for(
unsigned int j = 0; j < other.
num_spheres; ++j)
219 S d = (spheres[i].o - other.
spheres[j].o).norm() - (spheres[i].r + other.
spheres[j].r);
233 if(id_a != -1 && id_b != -1)
235 Vector3<S> v = spheres[id_a].o - spheres[id_b].o;
237 *P = spheres[id_a].o;
238 (*P).noalias() -= v * (spheres[id_a].r / len_v);
239 *Q = spheres[id_b].o;
240 (*Q).noalias() += v * (spheres[id_b].r / len_v);
248 template <
typename S,
typename DerivedA,
typename DerivedB>
250 const Eigen::MatrixBase<DerivedA>& R0,
251 const Eigen::MatrixBase<DerivedB>& T0,
255 for(
unsigned int i = 0; i < b2_temp.
num_spheres; ++i)
265 template <
typename S,
typename DerivedA,
typename DerivedB>
267 const Eigen::MatrixBase<DerivedA>& R0,
268 const Eigen::MatrixBase<DerivedB>& T0,
273 for(
unsigned int i = 0; i < b2_temp.
num_spheres; ++i)
280 template <
typename S>
290 for(
unsigned int i = 0; i < b2_temp.
num_spheres; ++i)
297 template <
typename S,
typename Derived>
299 const kIOS<S>& bv,
const Eigen::MatrixBase<Derived>& t)
302 Derived::RowsAtCompileTime == 3
303 && Derived::ColsAtCompileTime == 1,
304 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);