38 #ifndef FCL_NARROWPHASE_DETAIL_HALFSPACE_INL_H
39 #define FCL_NARROWPHASE_DETAIL_HALFSPACE_INL_H
54 std::vector<ContactPoint<double>>* contacts);
61 std::vector<ContactPoint<double>>* contacts);
66 const Box<double>& s1,
const Transform3<double>& tf1,
72 const Box<double>& s1,
const Transform3<double>& tf1,
74 std::vector<ContactPoint<double>>* contacts);
81 std::vector<ContactPoint<double>>* contacts);
88 std::vector<ContactPoint<double>>* contacts);
95 std::vector<ContactPoint<double>>* contacts);
102 Vector3<double>* contact_points,
double* penetration_depth, Vector3<double>* normal);
108 std::vector<ContactPoint<double>>* contacts);
114 const Vector3<double>& P1,
const Vector3<double>& P2,
const Vector3<double>& P3,
const Transform3<double>& tf2,
115 Vector3<double>* contact_points,
double* penetration_depth, Vector3<double>* normal);
123 Vector3<double>& p, Vector3<double>& d,
124 double& penetration_depth,
133 double& penetration_depth,
141 Vector3<double>& p, Vector3<double>& d,
143 double& penetration_depth,
147 template <
typename S>
154 template <
typename S>
169 const S penetration_depth = depth;
171 contacts->emplace_back(normal,
point, penetration_depth);
183 template <
typename S>
195 const Vector3<S> normal2(std::pow(new_s2.
n[0], 2), std::pow(new_s2.
n[1], 2), std::pow(new_s2.
n[2], 2));
197 const S center_to_contact_plane = std::sqrt(normal2.dot(radii2));
200 const S depth = center_to_contact_plane + new_s2.
d;
207 const Vector3<S> normal = tf1.linear() * -new_s2.
n;
208 const Vector3<S> support_vector = (1.0/center_to_contact_plane) *
Vector3<S>(radii2[0]*new_s2.
n[0], radii2[1]*new_s2.
n[1], radii2[2]*new_s2.
n[2]);
209 const Vector3<S> point_in_halfspace_coords = support_vector * (0.5 * depth / new_s2.
n.dot(support_vector) - 1.0);
211 const S penetration_depth = depth;
213 contacts->emplace_back(normal,
point, penetration_depth);
225 template <
typename S>
243 template <
typename S>
264 if(depth < 0)
return false;
275 if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance<S>() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance<S>())
277 sign = (A[0] > 0) ? -1 : 1;
278 p += axis[0] * (0.5 * s1.
side[0] * sign);
280 else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance<S>() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance<S>())
282 sign = (A[1] > 0) ? -1 : 1;
283 p += axis[1] * (0.5 * s1.
side[1] * sign);
285 else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance<S>() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance<S>())
287 sign = (A[2] > 0) ? -1 : 1;
288 p += axis[2] * (0.5 * s1.
side[2] * sign);
292 for(std::size_t i = 0; i < 3; ++i)
294 sign = (A[i] > 0) ? -1 : 1;
295 p += axis[i] * (0.5 * s1.
side[i] * sign);
304 const S penetration_depth = depth;
306 contacts->emplace_back(normal,
point, penetration_depth);
314 template <
typename S>
326 S cosa = dir_z.dot(new_s2.
n);
327 if(std::abs(cosa) < halfspaceIntersectTolerance<S>())
330 S depth = s1.
radius - signed_dist;
331 if(depth < 0)
return false;
337 const S penetration_depth = depth;
339 contacts->emplace_back(normal,
point, penetration_depth);
346 int sign = (cosa > 0) ? -1 : 1;
350 S depth = s1.
radius - signed_dist;
351 if(depth < 0)
return false;
357 const S penetration_depth = depth;
359 contacts->emplace_back(normal,
point, penetration_depth);
367 template <
typename S>
378 S cosa = dir_z.dot(new_s2.
n);
380 if(std::abs(cosa) < halfspaceIntersectTolerance<S>())
383 S depth = s1.
radius - signed_dist;
384 if(depth < 0)
return false;
390 const S penetration_depth = depth;
392 contacts->emplace_back(normal,
point, penetration_depth);
400 if(std::abs(cosa + 1) < halfspaceIntersectTolerance<S>() || std::abs(cosa - 1) < halfspaceIntersectTolerance<S>())
409 int sign = (cosa > 0) ? -1 : 1;
413 if(depth < 0)
return false;
420 const S penetration_depth = depth;
422 contacts->emplace_back(normal,
point, penetration_depth);
431 template <
typename S>
442 S cosa = dir_z.dot(new_s2.
n);
444 if(cosa < halfspaceIntersectTolerance<S>())
447 S depth = s1.
radius - signed_dist;
448 if(depth < 0)
return false;
455 const S penetration_depth = depth;
457 contacts->emplace_back(normal,
point, penetration_depth);
466 if(std::abs(cosa + 1) < halfspaceIntersectTolerance<S>() || std::abs(cosa - 1) < halfspaceIntersectTolerance<S>())
481 if(d1 > 0 && d2 > 0)
return false;
486 const S penetration_depth = -
std::min(d1, d2);
488 const Vector3<S> point = ((d1 < d2) ? p1 : p2) + new_s2.
n * (0.5 * penetration_depth);
490 contacts->emplace_back(normal,
point, penetration_depth);
500 template <
typename S>
531 if(contact_points) *contact_points = v - new_s2.
n * (0.5 * depth);
534 if(penetration_depth) *penetration_depth = depth;
537 if(normal) *normal = -new_s2.
n;
545 template <
typename S>
563 if (signed_distance < min_signed_distance) {
564 min_signed_distance = signed_distance;
566 if (signed_distance <= 0 && contacts ==
nullptr)
return true;
570 const bool intersecting = min_signed_distance <= 0;
572 if (intersecting && contacts) {
573 const Vector3<S> normal_F = X_FH.linear() * half_space_H.
n;
577 const S depth = -min_signed_distance;
578 contacts->emplace_back(-normal_F, p_FV + normal_F * (0.5 * depth), depth);
585 template <
typename S>
613 if(penetration_depth) *penetration_depth = -depth;
614 if(normal) *normal = new_s1.
n;
615 if(contact_points) *contact_points = v - new_s1.
n * (0.5 * depth);
623 template <
typename S>
628 S& penetration_depth,
637 S dir_norm = dir.squaredNorm();
640 if((new_s1.
n).dot(new_s2.
n) > 0)
642 if(new_s1.
d < new_s2.
d)
644 penetration_depth = new_s2.
d - new_s1.
d;
654 if(new_s1.
d + new_s2.
d > 0)
658 penetration_depth = -(new_s1.
d + new_s2.
d);
668 origin *= (1.0 / dir_norm);
679 template <
typename S>
683 S& penetration_depth,
690 template <
typename S>
695 S& penetration_depth,
704 S dir_norm = dir.squaredNorm();
707 if((new_s1.
n).dot(new_s2.
n) > 0)
709 if(new_s1.
d < new_s2.
d)
725 if(new_s1.
d + new_s2.
d > 0)
730 penetration_depth = -(new_s1.
d + new_s2.
d);
738 origin *= (1.0 / dir_norm);